diff --git a/data/quadruped/quadruped.urdf b/data/quadruped/quadruped.urdf
new file mode 100644
index 000000000..843371c32
--- /dev/null
+++ b/data/quadruped/quadruped.urdf
@@ -0,0 +1,728 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index 3dfe17dd0..aba0b3e03 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -135,7 +135,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (gFileNameArray.size()==0)
{
- gFileNameArray.push_back("r2d2.urdf");
+ gFileNameArray.push_back("quadruped/quadruped.urdf");
}
diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h
index f028f537f..e1c20cadb 100644
--- a/examples/SharedMemory/PhysicsClient.h
+++ b/examples/SharedMemory/PhysicsClient.h
@@ -48,6 +48,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
+ virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) = 0;
+
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
};
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 1739282bb..f95ff129a 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1274,7 +1274,69 @@ void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int body
command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
}
-void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+
+///compute the closest points between two bodies
+b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient)
+{
+ b3SharedMemoryCommandHandle commandHandle =b3InitRequestContactPointInformation(physClient);
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+ command->m_updateFlags = CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE;
+ command->m_requestContactPointArguments.m_mode = CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS;
+ return commandHandle;
+}
+
+void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
+{
+ b3SetContactFilterBodyA(commandHandle,bodyUniqueIdA);
+}
+
+void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
+{
+ b3SetContactFilterBodyB(commandHandle,bodyUniqueIdB);
+}
+
+void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+ command->m_updateFlags += CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
+ command->m_requestContactPointArguments.m_closestDistanceThreshold = distance;
+}
+
+
+///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
+b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])
+{
+ PhysicsClient* cl = (PhysicsClient*)physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+ command->m_type = CMD_REQUEST_AABB_OVERLAP;
+ command->m_updateFlags = 0;
+ command->m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = 0;
+ command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[0] = aabbMin[0];
+ command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[1] = aabbMin[1];
+ command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[2] = aabbMin[2];
+
+ command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[0] = aabbMax[0];
+ command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[1] = aabbMax[1];
+ command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[2] = aabbMax[2];
+ return (b3SharedMemoryCommandHandle)command;
+}
+
+void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
+{
+ PhysicsClient* cl = (PhysicsClient*)physClient;
+ if (cl)
+ {
+ cl->getCachedOverlappingObjects(data);
+ }
+}
+
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
@@ -1286,6 +1348,11 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con
}
}
+void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo)
+{
+ b3GetContactPointInformation(physClient,contactPointInfo);
+}
+
//request visual shape information
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 33e821759..7b4589e86 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -99,6 +99,17 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
+///compute the closest points between two bodies
+b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
+void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
+void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
+void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
+
+///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
+b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3],const double aabbMax[3]);
+void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data);
+
//request visual shape information
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
index 109b66b9c..0a3d198eb 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray m_cachedSegmentationMaskBuffer;
btAlignedObjectArray m_cachedContactPoints;
-
+ btAlignedObjectArray m_cachedOverlappingObjects;
btAlignedObjectArray m_cachedVisualShapes;
btAlignedObjectArray m_bodyIdsRequestInfo;
@@ -596,6 +596,30 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Inverse Dynamics computations failed");
break;
}
+ case CMD_REQUEST_AABB_OVERLAP_FAILED:
+ {
+ b3Warning("Overlapping object query failed");
+ break;
+ }
+ case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
+ {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Overlapping object request completed");
+ }
+
+ int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
+ int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
+ m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
+ b3OverlappingObject* objects = (b3OverlappingObject*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
+
+ for (int i = 0; i < numOverlapCopied; i++)
+ {
+ m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
+ }
+
+ break;
+ }
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
{
if (m_data->m_verboseOutput)
@@ -740,6 +764,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
}
}
+
+ if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)
+ {
+ SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+ if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
+ {
+ command.m_type = CMD_REQUEST_AABB_OVERLAP;
+ command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
+ submitClientCommand(command);
+ return 0;
+ }
+ }
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
@@ -873,6 +909,14 @@ void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3Contac
}
+void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
+{
+ overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
+ overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
+ &m_data->m_cachedOverlappingObjects[0] : 0;
+}
+
+
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h
index 1ebe5bccd..d5f6b3cff 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.h
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.h
@@ -57,6 +57,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+ virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
+
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index db408e529..b54485f66 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -44,7 +44,8 @@ struct PhysicsDirectInternalData
btAlignedObjectArray m_cachedSegmentationMask;
btAlignedObjectArray m_cachedContactPoints;
-
+ btAlignedObjectArray m_cachedOverlappingObjects;
+
btAlignedObjectArray m_cachedVisualShapes;
PhysicsCommandProcessorInterface* m_commandProcessor;
@@ -309,6 +310,60 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org
return m_data->m_hasStatus;
}
+bool PhysicsDirect::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand)
+{
+ SharedMemoryCommand command = orgCommand;
+
+ const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+ do
+ {
+ bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+
+ int timeout = 1024 * 1024 * 1024;
+ while ((!hasStatus) && (timeout-- > 0))
+ {
+ const SharedMemoryStatus* stat = processServerStatus();
+ if (stat)
+ {
+ hasStatus = true;
+ }
+ }
+
+
+ m_data->m_hasStatus = hasStatus;
+ if (hasStatus)
+ {
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Overlapping Objects Request OK\n");
+ }
+
+ int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
+ int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
+ m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
+ b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0];
+
+ for (int i = 0; i < numOverlapCopied; i++)
+ {
+ m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
+ }
+
+ if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
+ {
+ m_data->m_hasStatus = false;
+ command.m_type = CMD_REQUEST_AABB_OVERLAP;
+ command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
+ }
+
+ }
+ } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied);
+
+ return m_data->m_hasStatus;
+
+}
+
+
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
@@ -525,6 +580,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
switch (serverCmd.m_type)
{
+
case CMD_REQUEST_INTERNAL_DATA_COMPLETED:
{
if (serverCmd.m_numDataStreamBytes)
@@ -635,6 +691,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
{
return processVisualShapeData(command);
}
+ if (command.m_type == CMD_REQUEST_AABB_OVERLAP)
+ {
+ return processOverlappingObjects(command);
+ }
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
@@ -761,6 +821,14 @@ void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation
}
+void PhysicsDirect::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
+{
+ overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
+ overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
+ &m_data->m_cachedOverlappingObjects[0] : 0;
+}
+
+
void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h
index 968f8f560..963281f7e 100644
--- a/examples/SharedMemory/PhysicsDirect.h
+++ b/examples/SharedMemory/PhysicsDirect.h
@@ -20,6 +20,8 @@ protected:
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
+ bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
+
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
@@ -75,8 +77,11 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+ virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
+
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
+
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp
index f669dde30..3e9c3d77d 100644
--- a/examples/SharedMemory/PhysicsLoopBack.cpp
+++ b/examples/SharedMemory/PhysicsLoopBack.cpp
@@ -144,4 +144,10 @@ void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformati
void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
-}
\ No newline at end of file
+}
+
+void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
+{
+ return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects);
+}
+
diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h
index 42acca718..c9187c4b8 100644
--- a/examples/SharedMemory/PhysicsLoopBack.h
+++ b/examples/SharedMemory/PhysicsLoopBack.h
@@ -62,6 +62,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+ virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
+
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 7c0a7ed9f..28ed1a81d 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -318,6 +318,45 @@ struct SaveWorldObjectData
std::string m_fileName;
};
+struct MyBroadphaseCallback : public btBroadphaseAabbCallback
+{
+ b3AlignedObjectArray m_bodyUniqueIds;
+ b3AlignedObjectArray m_links;
+
+
+ MyBroadphaseCallback()
+ {
+ }
+ virtual ~MyBroadphaseCallback()
+ {
+ }
+ void clear()
+ {
+ m_bodyUniqueIds.clear();
+ m_links.clear();
+ }
+ virtual bool process(const btBroadphaseProxy* proxy)
+ {
+ btCollisionObject* colObj = (btCollisionObject*)proxy->m_clientObject;
+ btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj);
+ if (mbl)
+ {
+ int bodyUniqueId = mbl->m_multiBody->getUserIndex2();
+ m_bodyUniqueIds.push_back(bodyUniqueId);
+ m_links.push_back(mbl->m_link);
+ return true;
+ }
+ int bodyUniqueId = colObj->getUserIndex2();
+ if (bodyUniqueId >= 0)
+ {
+ m_bodyUniqueIds.push_back(bodyUniqueId);
+ m_links.push_back(mbl->m_link);
+ }
+ return true;
+ }
+};
+
+
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -453,6 +492,8 @@ struct PhysicsServerCommandProcessorInternalData
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
btAlignedObjectArray m_cachedContactPoints;
+ MyBroadphaseCallback m_cachedOverlappingObjects;
+
btAlignedObjectArray m_sdfRecentLoadedBodies;
@@ -2535,92 +2576,321 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
+ case CMD_REQUEST_AABB_OVERLAP:
+ {
+ SharedMemoryStatus& serverCmd = serverStatusOut;
+ int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
+
+ if (0== curObjectIndex)
+ {
+ //clientCmd.m_requestContactPointArguments.m_aabbQueryMin
+ btVector3 aabbMin, aabbMax;
+ aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0],
+ clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1],
+ clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]);
+ aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0],
+ clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1],
+ clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]);
+
+ m_data->m_cachedOverlappingObjects.clear();
+
+ m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects);
+ }
+
+
+ int totalBytesPerObject = sizeof(b3OverlappingObject);
+ int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1;
+ int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
+ int remainingObjects = numOverlap - curObjectIndex;
+
+ int curNumObjects = btMin(overlapCapacity, remainingObjects);
+
+ if (numOverlap < overlapCapacity)
+ {
+
+ b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient;
+ for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++)
+ {
+ overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i];
+ overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i];
+ }
+
+ serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED;
+
+ int m_startingOverlappingObjectIndex;
+ int m_numOverlappingObjectsCopied;
+ int m_numRemainingOverlappingObjects;
+ serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
+ serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
+ serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects;
+ }
+ else
+ {
+ serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED;
+ }
+
+ hasStatus = true;
+ break;
+ }
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
//make a snapshot of the contact manifolds into individual contact points
- if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0)
- {
- int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
+ if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0)
+ {
m_data->m_cachedContactPoints.resize(0);
- m_data->m_cachedContactPoints.reserve(numContactManifolds*4);
- for (int i=0;im_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
- int linkIndexA = -1;
- int linkIndexB = -1;
-
- int objectIndexB = -1;
- const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
- if (bodyB)
+ int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS;
+
+ if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE)
+ {
+ mode = clientCmd.m_requestContactPointArguments.m_mode;
+ }
+
+ switch (mode)
+ {
+ case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS:
+ {
+ int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
+ m_data->m_cachedContactPoints.reserve(numContactManifolds * 4);
+ for (int i = 0; i < numContactManifolds; i++)
{
- objectIndexB = bodyB->getUserIndex2();
- }
- const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
- if (mblB && mblB->m_multiBody)
- {
- linkIndexB = mblB->m_link;
- objectIndexB = mblB->m_multiBody->getUserIndex2();
- }
+ const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
+ int linkIndexA = -1;
+ int linkIndexB = -1;
- int objectIndexA = -1;
- const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
- if (bodyA)
- {
- objectIndexA = bodyA->getUserIndex2();
- }
- const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
- if (mblA && mblA->m_multiBody)
- {
- linkIndexA = mblA->m_link;
+ int objectIndexB = -1;
- objectIndexA = mblA->m_multiBody->getUserIndex2();
- }
-
- btAssert(bodyA || mblA);
-
- //apply the filter, if the user provides it
- if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
- {
- if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
- (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
- continue;
- }
-
- //apply the second object filter, if the user provides it
- if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
- {
- if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
- (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
- continue;
- }
-
- for (int p=0;pgetNumContacts();p++)
- {
-
- b3ContactPointData pt;
- pt.m_bodyUniqueIdA = objectIndexA;
- pt.m_bodyUniqueIdB = objectIndexB;
- const btManifoldPoint& srcPt = manifold->getContactPoint(p);
- pt.m_contactDistance = srcPt.getDistance();
- pt.m_contactFlags = 0;
- pt.m_linkIndexA = linkIndexA;
- pt.m_linkIndexB = linkIndexB;
- for (int j=0;j<3;j++)
+ const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
+ if (bodyB)
{
- pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
- pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
- pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
+ objectIndexB = bodyB->getUserIndex2();
+ }
+ const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+ if (mblB && mblB->m_multiBody)
+ {
+ linkIndexB = mblB->m_link;
+ objectIndexB = mblB->m_multiBody->getUserIndex2();
+ }
+
+ int objectIndexA = -1;
+ const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
+ if (bodyA)
+ {
+ objectIndexA = bodyA->getUserIndex2();
+ }
+ const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+ if (mblA && mblA->m_multiBody)
+ {
+ linkIndexA = mblA->m_link;
+
+ objectIndexA = mblA->m_multiBody->getUserIndex2();
+ }
+
+ btAssert(bodyA || mblA);
+
+ //apply the filter, if the user provides it
+ if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
+ {
+ if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
+ (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
+ continue;
+ }
+
+ //apply the second object filter, if the user provides it
+ if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
+ {
+ if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
+ (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
+ continue;
+ }
+
+ for (int p = 0; p < manifold->getNumContacts(); p++)
+ {
+
+ b3ContactPointData pt;
+ pt.m_bodyUniqueIdA = objectIndexA;
+ pt.m_bodyUniqueIdB = objectIndexB;
+ const btManifoldPoint& srcPt = manifold->getContactPoint(p);
+ pt.m_contactDistance = srcPt.getDistance();
+ pt.m_contactFlags = 0;
+ pt.m_linkIndexA = linkIndexA;
+ pt.m_linkIndexB = linkIndexB;
+ for (int j = 0; j < 3; j++)
+ {
+ pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
+ pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
+ pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
+ }
+ pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime;
+ // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
+ m_data->m_cachedContactPoints.push_back(pt);
}
- pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime;
-// pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
- m_data->m_cachedContactPoints.push_back (pt);
}
- }
- }
+ break;
+ }
+
+ case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS:
+ {
+ //todo(erwincoumans) compute closest points between all, and vs all, pair
+ btScalar closestDistanceThreshold = 0.f;
+
+ if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD)
+ {
+ closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold;
+ }
+
+ int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
+ int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter;
+
+ btAlignedObjectArray setA;
+ btAlignedObjectArray setB;
+ btAlignedObjectArray setALinkIndex;
+ btAlignedObjectArray setBLinkIndex;
+
+ if (bodyUniqueIdA >= 0)
+ {
+ InteralBodyData* bodyA = m_data->getHandle(bodyUniqueIdA);
+ if (bodyA)
+ {
+ if (bodyA->m_multiBody)
+ {
+ if (bodyA->m_multiBody->getBaseCollider())
+ {
+ setA.push_back(bodyA->m_multiBody->getBaseCollider());
+ setALinkIndex.push_back(-1);
+ }
+ for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
+ {
+ if (bodyA->m_multiBody->getLink(i).m_collider)
+ {
+ setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
+ setALinkIndex.push_back(i);
+ }
+ }
+ }
+ if (bodyA->m_rigidBody)
+ {
+ setA.push_back(bodyA->m_rigidBody);
+ setALinkIndex.push_back(-1);
+ }
+ }
+ }
+ if (bodyUniqueIdB>=0)
+ {
+ InteralBodyData* bodyB = m_data->getHandle(bodyUniqueIdB);
+ if (bodyB)
+ {
+ if (bodyB->m_multiBody)
+ {
+ if (bodyB->m_multiBody->getBaseCollider())
+ {
+ setB.push_back(bodyB->m_multiBody->getBaseCollider());
+ setBLinkIndex.push_back(-1);
+ }
+ for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
+ {
+ if (bodyB->m_multiBody->getLink(i).m_collider)
+ {
+ setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
+ setBLinkIndex.push_back(i);
+ }
+ }
+ }
+ if (bodyB->m_rigidBody)
+ {
+ setB.push_back(bodyB->m_rigidBody);
+ setBLinkIndex.push_back(-1);
+
+ }
+ }
+ }
+
+ {
+ ///ContactResultCallback is used to report contact points
+ struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
+ {
+ //short int m_collisionFilterGroup;
+ //short int m_collisionFilterMask;
+ int m_bodyUniqueIdA;
+ int m_bodyUniqueIdB;
+ int m_linkIndexA;
+ int m_linkIndexB;
+ btScalar m_deltaTime;
+
+ btAlignedObjectArray& m_cachedContactPoints;
+
+ MyContactResultCallback(btAlignedObjectArray& pointCache)
+ :m_cachedContactPoints(pointCache)
+ {
+ }
+
+ virtual ~MyContactResultCallback()
+ {
+ }
+
+ virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+ {
+ //bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+ //collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+ //return collides;
+ return true;
+ }
+
+ virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1)
+ {
+ b3ContactPointData pt;
+ pt.m_bodyUniqueIdA = m_bodyUniqueIdA;
+ pt.m_bodyUniqueIdB = m_bodyUniqueIdB;
+ const btManifoldPoint& srcPt = cp;
+ pt.m_contactDistance = srcPt.getDistance();
+ pt.m_contactFlags = 0;
+ pt.m_linkIndexA = m_linkIndexA;
+ pt.m_linkIndexB = m_linkIndexB;
+ for (int j = 0; j < 3; j++)
+ {
+ pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
+ pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
+ pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
+ }
+ pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime;
+ // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
+ m_cachedContactPoints.push_back(pt);
+
+ return 1;
+ }
+ };
+
+
+ MyContactResultCallback cb(m_data->m_cachedContactPoints);
+
+ cb.m_bodyUniqueIdA = bodyUniqueIdA;
+ cb.m_bodyUniqueIdB = bodyUniqueIdB;
+ cb.m_deltaTime = m_data->m_physicsDeltaTime;
+
+ for (int i = 0; i < setA.size(); i++)
+ {
+ cb.m_linkIndexA = setALinkIndex[i];
+ for (int j = 0; j < setB.size(); j++)
+ {
+ cb.m_linkIndexB = setBLinkIndex[j];
+ cb.m_closestDistanceThreshold = closestDistanceThreshold;
+ this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb);
+ }
+ }
+ }
+
+ break;
+ }
+ default:
+ {
+ b3Warning("Unknown contact query mode: %d", mode);
+ }
+
+ }
+ }
int numContactPoints = m_data->m_cachedContactPoints.size();
@@ -3588,6 +3858,8 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
+ btScalar avg = 0.f;
+
for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i))
@@ -3598,7 +3870,14 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
motor->setErp(0.2);
btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
btScalar maxPosTarget = 0.55;
-
+
+ btScalar correction = 0.f;
+
+ if (avg)
+ {
+ correction = m_data->m_gripperMultiBody->getJointPos(i) - avg;
+ }
+
if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
{
m_data->m_gripperMultiBody->setJointPos(i,0);
@@ -3608,10 +3887,19 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
}
- motor->setPositionTarget(posTarget, 1);
+ if (avg)
+ {
+ motor->setPositionTarget(avg, 1);
+ }
+ else
+ {
+ motor->setPositionTarget(posTarget, 1);
+ }
motor->setVelocityTarget(0, 0.5);
- btScalar maxImp = 1*m_data->m_physicsDeltaTime;
+ btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
+ avg = m_data->m_gripperMultiBody->getJointPos(i);
+
//motor->setRhsClamp(gRhsClamp);
}
}
@@ -3619,7 +3907,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
}
// Inverse kinematics for KUKA
- //if (0)
+ if (m_data->m_KukaId>=0)
{
InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 77eaa6cd9..eec1bdbfc 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -969,6 +969,23 @@ void PhysicsServerExample::renderScene()
static char line0[1024];
static char line1[1024];
+ //draw all user-debug-lines
+
+ //add array of lines
+
+ //draw all user- 'text3d' messages
+ if (m_guiHelper)
+ {
+ btVector3 from(0, 0, 0);
+ btVector3 toX(1, 1, 1);
+ btVector3 color(0, 1, 0);
+ double width = 2;
+ m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, width);
+
+ m_guiHelper->getAppInterface()->drawText3D("hi", 1, 1, 1, 1);
+ }
+
+
if (gEnableRealTimeSimVR)
{
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 8680acd18..bdc2dd96e 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -143,11 +143,26 @@ enum EnumRequestPixelDataUpdateFlags
};
+enum EnumRequestContactDataUpdateFlags
+{
+ CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE=1,
+ CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2,
+};
+
struct RequestContactDataArgs
{
int m_startingContactPointIndex;
int m_objectAIndexFilter;
int m_objectBIndexFilter;
+ double m_closestDistanceThreshold;
+ int m_mode;
+};
+
+struct RequestOverlappingObjectsArgs
+{
+ int m_startingOverlappingObjectIndex;
+ double m_aabbQueryMin[3];
+ double m_aabbQueryMax[3];
};
struct RequestVisualShapeDataArgs
@@ -510,6 +525,7 @@ struct SharedMemoryCommand
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
+ struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
struct LoadTextureArgs m_loadTextureArguments;
@@ -530,6 +546,13 @@ struct SendContactDataArgs
int m_numRemainingContactPoints;
};
+struct SendOverlappingObjectsArgs
+{
+ int m_startingOverlappingObjectIndex;
+ int m_numOverlappingObjectsCopied;
+ int m_numRemainingOverlappingObjects;
+};
+
struct SharedMemoryStatus
{
int m_type;
@@ -552,6 +575,7 @@ struct SharedMemoryStatus
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
+ struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
};
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 7b0a967bc..7f8179b6c 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -34,6 +34,7 @@ enum EnumSharedMemoryClientCommand
CMD_CALCULATE_JACOBIAN,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
+ CMD_REQUEST_AABB_OVERLAP,
CMD_SAVE_WORLD,
CMD_REQUEST_VISUAL_SHAPE_INFO,
CMD_UPDATE_VISUAL_SHAPE,
@@ -80,6 +81,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
+ CMD_REQUEST_AABB_OVERLAP_COMPLETED,
+ CMD_REQUEST_AABB_OVERLAP_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_SAVE_WORLD_COMPLETED,
@@ -158,6 +161,18 @@ struct b3DebugLines
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
+struct b3OverlappingObject
+{
+ int m_objectUniqueId;
+ int m_linkIndex;
+};
+
+struct b3AABBOverlapData
+{
+ int m_numOverlappingObjects;
+ struct b3OverlappingObject* m_overlappingObjects;
+};
+
struct b3CameraImageData
{
int m_pixelWidth;
@@ -192,6 +207,13 @@ struct b3ContactPointData
// double m_angularFrictionForce;
};
+enum
+{
+ CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS = 0,
+ CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
+};
+
+
struct b3ContactInformation
{
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index fd5355abe..a38d427fd 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -680,6 +680,11 @@ static int pybullet_internalGetBasePositionAndOrientation(
baseOrientation[2] = 0.;
baseOrientation[3] = 1.;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return 0;
+ }
+
{
{
b3SharedMemoryCommandHandle cmd_handle =
@@ -1352,6 +1357,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
int i;
PyObject* pyResultList = 0;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
if (size == 1)
{
if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) {
@@ -1450,6 +1459,11 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle;
int statusType;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
if (size == 4)
{
if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) {
@@ -1487,6 +1501,11 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
b3SharedMemoryStatusHandle statusHandle;
int statusType;
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
if (size == 1)
{
if (!PyArg_ParseTuple(args, "s", &filename)) {
@@ -1516,38 +1535,10 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args)
return Py_None;
}
-static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
- int size = PySequence_Size(args);
- int objectUniqueIdA = -1;
- int objectUniqueIdB = -1;
- b3SharedMemoryCommandHandle commandHandle;
- struct b3ContactInformation contactPointData;
- b3SharedMemoryStatusHandle statusHandle;
- int statusType;
- int i;
- PyObject* pyResultList = 0;
- if (size == 1) {
- if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA)) {
- PyErr_SetString(SpamError, "Error parsing object unique id");
- return NULL;
- }
- }
- if (size == 2) {
- if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA, &objectUniqueIdB)) {
- PyErr_SetString(SpamError, "Error parsing object unique id");
- return NULL;
- }
- }
-
- commandHandle = b3InitRequestContactPointInformation(sm);
- b3SetContactFilterBodyA(commandHandle, objectUniqueIdA);
- b3SetContactFilterBodyB(commandHandle, objectUniqueIdB);
-
- statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
- statusType = b3GetStatusType(statusHandle);
- if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
- /*
+static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPointPtr)
+{
+ /*
0 int m_contactFlags;
1 int m_bodyUniqueIdA;
2 int m_bodyUniqueIdB;
@@ -1565,66 +1556,208 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) {
15 double m_normalForce;
*/
- b3GetContactPointInformation(sm, &contactPointData);
- pyResultList = PyTuple_New(contactPointData.m_numContactPoints);
- for (i = 0; i < contactPointData.m_numContactPoints; i++) {
+ int i;
+
+ PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
+ for (i = 0; i < contactPointPtr->m_numContactPoints; i++) {
PyObject* contactObList = PyTuple_New(16); // see above 16 fields
PyObject* item;
item =
- PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
+ PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
PyTuple_SetItem(contactObList, 0, item);
item = PyInt_FromLong(
- contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
+ contactPointPtr->m_contactPointData[i].m_bodyUniqueIdA);
PyTuple_SetItem(contactObList, 1, item);
item = PyInt_FromLong(
- contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
+ contactPointPtr->m_contactPointData[i].m_bodyUniqueIdB);
PyTuple_SetItem(contactObList, 2, item);
item =
- PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
+ PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexA);
PyTuple_SetItem(contactObList, 3, item);
item =
- PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
+ PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
PyTuple_SetItem(contactObList, 4, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
+ contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
PyTuple_SetItem(contactObList, 5, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
+ contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
PyTuple_SetItem(contactObList, 6, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
+ contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
PyTuple_SetItem(contactObList, 7, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
+ contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
PyTuple_SetItem(contactObList, 8, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
+ contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
PyTuple_SetItem(contactObList, 9, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
+ contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
PyTuple_SetItem(contactObList, 10, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
+ contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
PyTuple_SetItem(contactObList, 11, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
+ contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
PyTuple_SetItem(contactObList, 12, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
+ contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
PyTuple_SetItem(contactObList, 13, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_contactDistance);
+ contactPointPtr->m_contactPointData[i].m_contactDistance);
PyTuple_SetItem(contactObList, 14, item);
item = PyFloat_FromDouble(
- contactPointData.m_contactPointData[i].m_normalForce);
+ contactPointPtr->m_contactPointData[i].m_normalForce);
PyTuple_SetItem(contactObList, 15, item);
PyTuple_SetItem(pyResultList, i, contactObList);
}
return pyResultList;
+}
+
+static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject *keywds)
+{
+ PyObject* aabbMinOb=0, *aabbMaxOb=0;
+ double aabbMin[3];
+ double aabbMax[3];
+ b3SharedMemoryCommandHandle commandHandle;
+ b3SharedMemoryStatusHandle statusHandle;
+ struct b3AABBOverlapData overlapData;
+ int i;
+
+ static char *kwlist[] = { "aabbMin", "aabbMax", NULL };
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist,
+ &aabbMinOb, &aabbMaxOb))
+ return NULL;
+
+ pybullet_internalSetVectord(aabbMinOb, aabbMin);
+ pybullet_internalSetVectord(aabbMaxOb, aabbMax);
+
+ commandHandle = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ b3GetAABBOverlapResults(sm, &overlapData);
+
+ if (overlapData.m_numOverlappingObjects)
+ {
+ PyObject* pyResultList = PyTuple_New(overlapData.m_numOverlappingObjects);
+ //For huge amount of overlap, we could use numpy instead (see camera pixel data)
+ //What would Python do with huge amount of data? Pass it onto TensorFlow!
+
+ for (i = 0; i < overlapData.m_numOverlappingObjects; i++) {
+ PyObject* overlap = PyTuple_New(2);//body unique id and link index
+
+ PyObject* item;
+ item =
+ PyInt_FromLong(overlapData.m_overlappingObjects[i].m_objectUniqueId);
+ PyTuple_SetItem(overlap, 0, item);
+ item =
+ PyInt_FromLong(overlapData.m_overlappingObjects[i].m_linkIndex);
+ PyTuple_SetItem(overlap, 1, item);
+ PyTuple_SetItem(pyResultList, i, overlap);
+ }
+
+ return pyResultList;
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+
+static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, PyObject *keywds)
+{
+ int size = PySequence_Size(args);
+ int bodyUniqueIdA = -1;
+ int bodyUniqueIdB = -1;
+ double distanceThreshold = 0.f;
+
+ b3SharedMemoryCommandHandle commandHandle;
+ struct b3ContactInformation contactPointData;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ PyObject* pyResultList = 0;
+
+
+ static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL };
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist,
+ &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold))
+ return NULL;
+
+
+ commandHandle = b3InitClosestDistanceQuery(sm);
+ b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
+ b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
+ b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
+
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
+
+
+ b3GetContactPointInformation(sm, &contactPointData);
+
+ return MyConvertContactPoint(&contactPointData);
+
+ }
+
+ Py_INCREF(Py_None);
+ return Py_None;
+}
+
+
+static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
+ int size = PySequence_Size(args);
+ int bodyUniqueIdA = -1;
+ int bodyUniqueIdB = -1;
+
+ b3SharedMemoryCommandHandle commandHandle;
+ struct b3ContactInformation contactPointData;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ PyObject* pyResultList = 0;
+
+
+ static char *kwlist[] = { "bodyA", "bodyB", NULL };
+
+ if (0 == sm) {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist,
+ &bodyUniqueIdA, &bodyUniqueIdB))
+ return NULL;
+
+
+ commandHandle = b3InitRequestContactPointInformation(sm);
+ b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
+ b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
+ //b3SetContactQueryMode(commandHandle, mode);
+
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+ statusType = b3GetStatusType(statusHandle);
+ if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
+
+ b3GetContactPointInformation(sm, &contactPointData);
+
+ return MyConvertContactPoint(&contactPointData);
+
}
Py_INCREF(Py_None);
@@ -2447,11 +2580,21 @@ static PyMethodDef SpamMethods[] = {
#endif
},
- {"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
- "Return the contact point information for all or some of pairwise "
- "object-object collisions. Optional arguments one or two object unique "
+ {"getContactPoints", pybullet_getContactPointData, METH_VARARGS | METH_KEYWORDS,
+ "Return existing contact points after the stepSimulation command. "
+ "Optional arguments one or two object unique "
"ids, that need to be involved in the contact."},
+ {"getClosestPoints", pybullet_getClosestPointData, METH_VARARGS | METH_KEYWORDS,
+ "Compute the closest points between two objects, if the distance is below a given threshold."
+ "Input is two objects unique ids and distance threshold."
+ },
+
+ { "getOverlappingObjects", pybullet_getOverlappingObjects, METH_VARARGS | METH_KEYWORDS,
+ "Return all the objects that have overlap with a given "
+ "axis-aligned bounding box volume (AABB)."
+ "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."
+ },
{"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
"Return the visual shape information for one object." },
@@ -2486,6 +2629,7 @@ static PyMethodDef SpamMethods[] = {
// loadSnapshot
// raycast info
// object names
+ // collision query
{NULL, NULL, 0, NULL} /* Sentinel */
};
@@ -2541,6 +2685,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME);
PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);
+ PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS);
+ PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS);
+
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index fa4cac660..ea7dc86c4 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -1252,6 +1252,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
if (algorithm)
{
btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);
+ contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold;
//discrete collision detection query
algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult);
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index be9eca61a..29d371116 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
@@ -412,10 +412,12 @@ public:
{
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
-
+ btScalar m_closestDistanceThreshold;
+
ContactResultCallback()
:m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
- m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+ m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+ m_closestDistanceThreshold(0)
{
}
diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
index ab2632ee3..fcaa9f851 100644
--- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
@@ -161,6 +161,13 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+ btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
+
+ aabbMin0 -= thresholdVec;
+ aabbMin1 -= thresholdVec;
+ aabbMax0 += thresholdVec;
+ aabbMax1 += thresholdVec;
+
if (gCompoundCompoundChildShapePairCallback)
{
if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
@@ -217,10 +224,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide
static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
- const btDbvtAabbMm& b, const btTransform& xform)
+ const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold)
{
btVector3 newmin,newmax;
btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
+ newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
+ newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold);
btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
return Intersect(a,newb);
}
@@ -229,7 +238,7 @@ static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a,
static inline void MycollideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
const btTransform& xform,
- btCompoundCompoundLeafCallback* callback)
+ btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold)
{
if(root0&&root1)
@@ -241,7 +250,7 @@ static inline void MycollideTT( const btDbvtNode* root0,
stkStack[0]=btDbvt::sStkNN(root0,root1);
do {
btDbvt::sStkNN p=stkStack[--depth];
- if(MyIntersect(p.a->volume,p.b->volume,xform))
+ if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold))
{
if(depth>treshold)
{
@@ -343,7 +352,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
- MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
+ MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold);
//printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
@@ -383,7 +392,9 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
newChildWorldTrans0 = orgTrans0*childTrans0 ;
childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
}
-
+ btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
+ aabbMin0 -= thresholdVec;
+ aabbMax0 += thresholdVec;
{
btTransform orgInterpolationTrans1;
const btCollisionShape* childShape1 = 0;
@@ -398,7 +409,8 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
}
-
+ aabbMin1 -= thresholdVec;
+ aabbMax1 += thresholdVec;
if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
{
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index 80d294895..bc23fdb98 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -365,7 +365,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
// input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
//} else
//{
- input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+ input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold;
// }
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 96c1eb5d5..be8e51d52 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -96,6 +96,7 @@ btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,con
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
+ , m_closestPointDistanceThreshold(0)
{
}
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 55b6380a5..86bbc3f72 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -49,17 +49,19 @@ protected:
int m_index0;
int m_index1;
-
+
public:
btManifoldResult()
-#ifdef DEBUG_PART_INDEX
:
+#ifdef DEBUG_PART_INDEX
+
m_partId0(-1),
m_partId1(-1),
m_index0(-1),
m_index1(-1)
#endif //DEBUG_PART_INDEX
+ m_closestPointDistanceThreshold(0)
{
}
@@ -142,6 +144,8 @@ public:
return m_body1Wrap->getCollisionObject();
}
+ btScalar m_closestPointDistanceThreshold;
+
/// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
index 36ba21f5b..41f37ddf2 100644
--- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
@@ -62,7 +62,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject
#endif
///iff distance positive, don't generate a new contact
- if ( len > (radius0+radius1))
+ if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold))
{
#ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints();