diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 95069900f..824bee9e5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8023,6 +8023,129 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc return hasStatus; } +bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd) +{ +#ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (!deformWorld) + { + return false; + } + int numSoftbodyContact = 0; + for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + { + numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size(); + } + int num_contact_points = m_data->m_cachedContactPoints.size(); + m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact); + + for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + { + btSoftBody* psb = deformWorld->getSoftBodyArray()[i]; + for (int c = 0; c < psb->m_faceRigidContacts.size(); c++) + { + const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[i]; + //convert rigidbody contact + int linkIndexA = -1; + int linkIndexB = -1; + int objectIndexA = psb->getUserIndex2(); + + int objectIndexB = -1; + const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj); + if (mblB && mblB->m_multiBody) + { + linkIndexB = mblB->m_link; + objectIndexB = mblB->m_multiBody->getUserIndex2(); + } + + //apply the filter, if the user provides it + bool swap = false; + 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 + { + continue; + } + } + + 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) + { + continue; + } + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + { + continue; + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) + { + continue; + } + b3ContactPointData pt; + pt.m_bodyUniqueIdA = objectIndexA; + pt.m_bodyUniqueIdB = objectIndexB; + pt.m_contactDistance = contact->m_cti.m_offset; + 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_cti.m_normal[j]; + pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j]; + pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j]; + pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j]; + pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j]; + } + } + pt.m_normalForce = 1; + pt.m_linearFrictionForce1 = 0; + pt.m_linearFrictionForce2 = 0; + for (int j = 0; j < 3; j++) + { + pt.m_linearFrictionDirection1[j] = 0; + pt.m_linearFrictionDirection2[j] = 0; + } + m_data->m_cachedContactPoints.push_back(pt); + } + } +#endif + return true; +} + bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -8165,6 +8288,10 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand m_data->m_cachedContactPoints.push_back(pt); } } + +#ifndef SKIP_DEFORMABLE_BODY + processRequestDeformableContactpointHelper(clientCmd); +#endif break; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index d456675df..e90ddb064 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -44,6 +44,7 @@ protected: bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); 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 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 e81680f01..699c6bcad 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -405,13 +405,12 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; - /* Clear contacts */ - psb->m_nodeRigidContacts.resize(0); - psb->m_faceRigidContacts.resize(0); - psb->m_faceNodeContacts.resize(0); - if (psb->isActive()) { + /* Clear contacts when softbody is active*/ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); // predict motion for collision detection predictDeformableMotion(psb, solverdt); } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index d1980ea6c..87d98a733 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -4095,7 +4095,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap case fCollision::SDF_RD: { btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); - if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) + if (this->isActive()) { const btTransform wtr = pcoWrap->getWorldTransform(); const btScalar timemargin = 0; @@ -4246,7 +4246,7 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) btSoftColliders::CollideCCD docollide; /* common */ docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin - docollide.dt = psb->m_sst.sdt; + docollide.dt = psb->m_sst.sd; /* psb0 nodes vs psb1 faces */ if (psb->m_tetras.size() > 0) docollide.useFaceNormal = true;