add collision info

5 files changed, 128 insertions, 0 deletions

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index aade5904c..dc2e89b66 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8077,6 +8077,118 @@ 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;
+    }
+    for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
+    {
+        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;
+            }
+            //Convert contact info
+            b3ContactPointData pt;
+            pt.m_bodyUniqueIdA = objectIndexA;
+            pt.m_bodyUniqueIdB = objectIndexB;
+            pt.m_contactDistance = 0;
+            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
@@ -8345,6 +8457,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 ad6c403aa..2a2722132 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/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index c6226a008..366c8a477 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -4288,6 +4288,14 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
 		}
 	}
 }
+void btSoftBody::approximateCollisionHandler(btSoftBody* psb)
+{
+    if (psb->isActive() || this->isActive())
+    {
+        // test aabb collision
+        }
+}
+
 
 void btSoftBody::setWindVelocity(const btVector3& velocity)
 {
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index d5b01faf3..641883dec 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		*/
@@ -1090,6 +1091,7 @@ public:
 	/* defaultCollisionHandlers												*/
 	void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
 	void defaultCollisionHandler(btSoftBody* psb);
+    void approximateCollisionHandler(btSoftBody* psb)
 	void setSelfCollision(bool useSelfCollision);
 	bool useSelfCollision();
 	void updateDeactivation(btScalar timeStep);
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index e52b1c875..96db018de 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -1944,6 +1944,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);
 				}
 			}
@@ -2019,6 +2020,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);
 			}
 		}
@@ -2050,6 +2052,7 @@ struct btSoftColliders
 				c.m_margin = mrg;
 				c.m_imf = 0;
 				c.m_c0 = 0;
+                c.m_colObj = psb[1];
 				psb[0]->m_faceNodeContacts.push_back(c);
 			}
 		}
@@ -2114,6 +2117,7 @@ struct btSoftColliders
 					c.m_margin = mrg;
 					c.m_imf = 0;
 					c.m_c0 = 0;
+                    c.m_colObj = psb[1];
 					psb[0]->m_faceNodeContacts.push_back(c);
 				}
 			}