diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2022-03-05 13:48:47 -0800 |
---|---|---|
committer | GitHub <noreply@github.com> | 2022-03-05 13:48:47 -0800 |
commit | e32c0674e77e405c654b2e050b040a5a3b792986 (patch) | |
tree | a5b80ba41d79b703b23e47db93de694d2e357ddf | |
parent | 34a471586e02e432b8ac98ebfdd267e3dfa7d9d3 (diff) | |
parent | 3fd61413df8bc0f58ab84adff8b8efff9b815276 (diff) | |
download | bullet3-e32c0674e77e405c654b2e050b040a5a3b792986.tar.gz |
Merge pull request #4172 from fuchuyuan/contact
Provide Deformable-Deformable contact for request contact info
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 120 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.h | 1 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.cpp | 1 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp | 5 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBody.h | 2 | ||||
-rw-r--r-- | src/BulletSoftBody/btSoftBodyInternals.h | 8 |
6 files changed, 133 insertions, 4 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index fcf04c778..faad9e634 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8104,6 +8104,125 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc return hasStatus; } +bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndexA, int& linkIndexB, int& objectIndexA, int& objectIndexB, bool& swap){ + + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) + { + swap = false; + } + else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) + { + swap = true; + } + else + { + return true; + } + } + + if (swap) + { + std::swap(objectIndexA, objectIndexB); + std::swap(linkIndexA, linkIndexB); + } + + //apply the second object filter, if the user provides it + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) + { + return true; + } + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + { + return true; + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) + { + return true; + } + + return false; +} + +bool PhysicsServerCommandProcessor::processRequestDeformableDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd){ +#ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (!deformWorld) + { + return false; + } + const int max_contacts_per_object = 4; + for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + { + int num_contacts_reported = 0; + btSoftBody* psb = deformWorld->getSoftBodyArray()[i]; + for (int c = 0; c < psb->m_faceNodeContacts.size(); c++) + { + const btSoftBody::DeformableFaceNodeContact* contact = &psb->m_faceNodeContacts[c]; + //apply the filter, if the user provides it + int linkIndexA = -1; + int linkIndexB = -1; + int objectIndexA = psb->getUserIndex2(); + int objectIndexB = -1; + const btSoftBody* bodyB = btSoftBody::upcast(contact->m_colObj); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + bool swap = false; + if(RequestFiltered(clientCmd, linkIndexA, linkIndexB, objectIndexA, objectIndexB, swap)==true){ + continue; + } + if (++num_contacts_reported > max_contacts_per_object) + { + break; + } + //Convert contact info + b3ContactPointData pt; + btVector3 l = contact->m_node->m_x - BaryEval(contact->m_face->m_n[0]->m_x, contact->m_face->m_n[1]->m_x, contact->m_face->m_n[2]->m_x, contact->m_normal); + pt.m_contactDistance = -contact->m_margin + contact->m_normal.dot(l); + pt.m_bodyUniqueIdA = objectIndexA; + pt.m_bodyUniqueIdB = objectIndexB; + pt.m_contactFlags = 0; + pt.m_linkIndexA = linkIndexA; + pt.m_linkIndexB = linkIndexB; + for (int j = 0; j < 3; j++) + { + if (swap) + { + pt.m_contactNormalOnBInWS[j] = -contact->m_normal[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = contact->m_normal[j]; + } + pt.m_positionOnAInWS[j] = contact->m_node->m_x[j]; + pt.m_positionOnBInWS[j] = contact->m_node->m_x[j]; + pt.m_linearFrictionDirection1[j] = 0; + pt.m_linearFrictionDirection2[j] = 0; + } + pt.m_normalForce = 0; + pt.m_linearFrictionForce1 = 0; + pt.m_linearFrictionForce2 = 0; + m_data->m_cachedContactPoints.push_back(pt); + } + } +#endif + return true; +} + + + bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd) { #ifndef SKIP_DEFORMABLE_BODY @@ -8372,6 +8491,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand #ifndef SKIP_DEFORMABLE_BODY processRequestDeformableContactpointHelper(clientCmd); + processRequestDeformableDeformableContactpointHelper(clientCmd); #endif break; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 985e3f469..fb23a1bb9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -46,6 +46,7 @@ protected: bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd); + bool processRequestDeformableDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd); bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 3fceb7525..4e9df5f83 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -412,6 +412,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) psb->m_nodeRigidContacts.resize(0); psb->m_faceRigidContacts.resize(0); psb->m_faceNodeContacts.resize(0); + psb->m_faceNodeContactsCCD.resize(0); // predict motion for collision detection predictDeformableMotion(psb, solverdt); } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 23967a67b..030cbaf90 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -201,7 +201,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim if (psb->isActive()) { // clear contact points in the previous iteration - psb->m_faceNodeContacts.clear(); + psb->m_faceNodeContactsCCD.clear(); // update m_q and normals for CCD calculation for (int j = 0; j < psb->m_nodes.size(); ++j) @@ -237,7 +237,8 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim btSoftBody* psb = m_softBodies[i]; if (psb->isActive()) { - penetration_count += psb->m_faceNodeContacts.size(); + penetration_count += psb->m_faceNodeContactsCCD.size(); + ; } } if (penetration_count == 0) diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 1016a9aa3..89842d70d 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -407,6 +407,7 @@ public: btScalar m_friction; // Friction btScalar m_imf; // inverse mass of the face at contact point btScalar m_c0; // scale of the impulse matrix; + const btCollisionObject* m_colObj; // Collision object to collide with. }; /* SContact */ @@ -824,6 +825,7 @@ public: btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; + btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContactsCCD; tSContactArray m_scontacts; // Soft contacts tJointArray m_joints; // Joints tMaterialArray m_materials; // Materials diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index a609258c7..137258675 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1960,6 +1960,7 @@ struct btSoftColliders c.m_weights = btVector3(0, 0, 0); c.m_imf = 0; c.m_c0 = 0; + c.m_colObj = psb[1]; psb[0]->m_faceNodeContacts.push_back(c); } } @@ -2035,6 +2036,7 @@ struct btSoftColliders c.m_weights = btVector3(0, 0, 0); c.m_imf = 0; c.m_c0 = 0; + c.m_colObj = psb[1]; psb[0]->m_faceNodeContacts.push_back(c); } } @@ -2066,7 +2068,8 @@ struct btSoftColliders c.m_margin = mrg; c.m_imf = 0; c.m_c0 = 0; - psb[0]->m_faceNodeContacts.push_back(c); + c.m_colObj = psb[1]; + psb[0]->m_faceNodeContactsCCD.push_back(c); } } void Process(const btDbvntNode* lface1, @@ -2130,7 +2133,8 @@ struct btSoftColliders c.m_margin = mrg; c.m_imf = 0; c.m_c0 = 0; - psb[0]->m_faceNodeContacts.push_back(c); + c.m_colObj = psb[1]; + psb[0]->m_faceNodeContactsCCD.push_back(c); } } } |