diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 479fae693..bb198f23e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1458,7 +1458,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; - bodyJoints->m_baseName = "baseLink"; + bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName; if (bf.ok()) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index b8a4eb57c..cceb55a10 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1218,7 +1218,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; - bodyJoints->m_baseName = "baseLink"; + bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName; break; } case CMD_SYNC_USER_DATA_FAILED: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 437584951..af85e9c04 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3463,8 +3463,6 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); ser.startSerialization(); - ser.registerNameForPointer(sb, bodyHandle->m_bodyName.c_str()); - int len = sb->calculateSerializeBufferSize(); btChunk* chunk = ser.allocate(len, 1); const char* structType = sb->serialize(chunk->m_oldPtr, &ser); @@ -7259,6 +7257,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage); serverCmd.m_sendActualStateArgs.m_stateDetails = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = @@ -7275,11 +7274,12 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3]; + btVector3 center_of_mass(sb->getCenterOfMass()); btTransform tr = sb->getWorldTransform(); //base position in world space, cartesian - stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; - stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; - stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; + stateDetails->m_actualStateQ[0] = center_of_mass[0]; + stateDetails->m_actualStateQ[1] = center_of_mass[1]; + stateDetails->m_actualStateQ[2] = center_of_mass[2]; //base orientation, quaternion x,y,z,w, in world space, cartesian stateDetails->m_actualStateQ[3] = tr.getRotation()[0]; @@ -8227,6 +8227,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; + int pos = strlen(relativeFileName)-1; + while(pos>=0 && relativeFileName[pos]!='/') { pos--;} + btAssert(strlen(relativeFileName)-pos-5>0); + std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos)); + bodyHandle->m_bodyName = object_name; + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 908785895..5764e961a 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -14,7 +14,6 @@ */ #include "btDeformableContactConstraint.h" - /* ================ Deformable Node Anchor =================== */ btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a) : m_anchor(&a) @@ -216,12 +215,11 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() btVector3 impulse = m_contact->m_c0 * vr; const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; - btVector3 old_total_tangent_dv = m_total_tangent_dv; // m_c2 is the inverse mass of the deformable node/face m_total_normal_dv -= impulse_normal * m_contact->m_c2; m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; - + if (m_total_normal_dv.dot(cti.m_normal) < 0) { // separating in the normal direction @@ -236,13 +234,13 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() // dynamic friction // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. m_static = false; - if (m_total_tangent_dv.norm() < SIMD_EPSILON) + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) { m_total_tangent_dv = btVector3(0,0,0); } else { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_c3; + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; } impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); } @@ -255,7 +253,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() impulse = impulse_normal + impulse_tangent; // apply impulse to deformable nodes involved and change their velocities applyImpulse(impulse); - // apply impulse to the rigid/multibodies involved and change their velocities if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -361,12 +358,32 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul const btSoftBody::DeformableFaceRigidContact* contact = getContact(); btVector3 dv = impulse * contact->m_c2; btSoftBody::Face* face = contact->m_face; - if (face->m_n[0]->m_im > 0) - face->m_n[0]->m_v -= dv * contact->m_weights[0]; - if (face->m_n[1]->m_im > 0) - face->m_n[1]->m_v -= dv * contact->m_weights[1]; - if (face->m_n[2]->m_im > 0) - face->m_n[2]->m_v -= dv * contact->m_weights[2]; + + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + v0 -= dv * contact->m_weights[0]; + if (im1 > 0) + v1 -= dv * contact->m_weights[1]; + if (im2 > 0) + v2 -= dv * contact->m_weights[2]; + + // apply strain limiting to prevent undamped modes + btScalar m01 = (btScalar(1)/(im0 + im1)); + btScalar m02 = (btScalar(1)/(im0 + im2)); + btScalar m12 = (btScalar(1)/(im1 + im2)); + + btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); + btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); + btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); + + v0 += dv0; + v1 += dv1; + v2 += dv2; } /* ================ Face vs. Node =================== */ @@ -449,13 +466,13 @@ btScalar btDeformableFaceNodeContactConstraint::solveConstraint() // dynamic friction // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. m_static = false; - if (m_total_tangent_dv.norm() < SIMD_EPSILON) + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) { m_total_tangent_dv = btVector3(0,0,0); } else { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction; + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; } impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); } @@ -482,16 +499,33 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls } btSoftBody::Face* face = contact->m_face; - if (face->m_n[0]->m_im > 0) + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) { - face->m_n[0]->m_v -= dvb * contact->m_weights[0]; + v0 -= dvb * contact->m_weights[0]; } - if (face->m_n[1]->m_im > 0) + if (im1 > 0) { - face->m_n[1]->m_v -= dvb * contact->m_weights[1]; + v1 -= dvb * contact->m_weights[1]; } - if (face->m_n[2]->m_im > 0) + if (im2 > 0) { - face->m_n[2]->m_v -= dvb * contact->m_weights[2]; + v2 -= dvb * contact->m_weights[2]; } + // todo: Face node constraints needs more work +// btScalar m01 = (btScalar(1)/(im0 + im1)); +// btScalar m02 = (btScalar(1)/(im0 + im2)); +// btScalar m12 = (btScalar(1)/(im1 + im2)); +// +// btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); +// btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); +// btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); +// v0 += dv0; +// v1 += dv1; +// v2 += dv2; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 51f0dad21..d894d5cf5 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -20,6 +20,7 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" #include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btVector3.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletCollision/CollisionShapes/btConcaveShape.h" @@ -973,6 +974,16 @@ public: /* Return the volume */ btScalar getVolume() const; /* Cluster count */ + btVector3 getCenterOfMass() const + { + btVector3 com(0, 0, 0); + for (int i = 0; i < m_nodes.size(); i++) + { + com += (m_nodes[i].m_x * this->getMass(i)); + } + com /= this->getTotalMass(); + return com; + } int clusterCount() const; /* Cluster center of mass */ static btVector3 clusterCom(const Cluster* cluster);