From 0d47d61007e34f703ec1914dae4b5c8aa2d965c3 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 9 Nov 2016 21:01:04 -0800 Subject: [PATCH] pybullet getClosestPoints --- data/quadruped/quadruped.urdf | 728 ++++++++++++++++++ .../ImportURDFDemo/ImportURDFSetup.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 66 +- examples/SharedMemory/PhysicsClientC_API.h | 11 + .../PhysicsServerCommandProcessor.cpp | 384 +++++++-- .../SharedMemory/PhysicsServerExample.cpp | 17 + examples/SharedMemory/SharedMemoryCommands.h | 10 + examples/SharedMemory/SharedMemoryPublic.h | 17 + examples/pybullet/pybullet.c | 234 ++++-- .../CollisionDispatch/btCollisionWorld.cpp | 1 + .../CollisionDispatch/btCollisionWorld.h | 6 +- .../btCompoundCompoundCollisionAlgorithm.cpp | 24 +- .../btConvexConvexAlgorithm.cpp | 2 +- .../CollisionDispatch/btManifoldResult.h | 8 +- .../btSphereSphereCollisionAlgorithm.cpp | 2 +- 15 files changed, 1373 insertions(+), 139 deletions(-) create mode 100644 data/quadruped/quadruped.urdf 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/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1739282bb..8dcfdebd1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1274,7 +1274,66 @@ 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]) +{ + 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_AABB_OVERLAP; + + command->m_requestContactPointArguments.m_aabbQueryMin[0] = aabbMin[0]; + command->m_requestContactPointArguments.m_aabbQueryMin[1] = aabbMin[1]; + command->m_requestContactPointArguments.m_aabbQueryMin[2] = aabbMin[2]; + + command->m_requestContactPointArguments.m_aabbQueryMax[0] = aabbMax[0]; + command->m_requestContactPointArguments.m_aabbQueryMax[1] = aabbMax[1]; + command->m_requestContactPointArguments.m_aabbQueryMax[2] = aabbMax[2]; + + return commandHandle; +} + +void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data) +{ + data->m_numOverlappingObjects = 0; +// data->m_objectUniqueIds +} + void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData) @@ -1286,6 +1345,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/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7c0a7ed9f..402cdd84d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2541,86 +2541,326 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm 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_AABB_OVERLAP: + { + //clientCmd.m_requestContactPointArguments.m_aabbQueryMin + btVector3 aabbMin,aabbMax; + aabbMin.setValue(clientCmd.m_requestContactPointArguments.m_aabbQueryMin[0], + clientCmd.m_requestContactPointArguments.m_aabbQueryMin[1], + clientCmd.m_requestContactPointArguments.m_aabbQueryMin[2]); + aabbMax.setValue(clientCmd.m_requestContactPointArguments.m_aabbQueryMax[0], + clientCmd.m_requestContactPointArguments.m_aabbQueryMax[1], + clientCmd.m_requestContactPointArguments.m_aabbQueryMax[2]); + + struct MyBroadphaseCallback : public btBroadphaseAabbCallback + { + b3AlignedObjectArray m_bodyUniqueIds; + b3AlignedObjectArray m_links; + + + MyBroadphaseCallback() + { + } + virtual ~MyBroadphaseCallback() + { + } + 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; + } + }; + + MyBroadphaseCallback callback; + + m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin,aabbMax,callback); + + int totalBytesPerObject = 2 * sizeof(int); + int pairCapacity = bufferSizeInBytes / totalBytesPerObject - 1; + if (callback.m_bodyUniqueIds.size() < pairCapacity) + { + serverCmd.m_type = CMD_AABB_OVERLAP_COMPLETED; + int* pairStorage = (int*)bufferServerToClient; + for (int i = 0; i < callback.m_bodyUniqueIds.size(); i++) + { + pairStorage[i * 2] = callback.m_bodyUniqueIds[i]; + pairStorage[i * 2+1] = callback.m_links[i]; + } + } + else + { + serverCmd.m_type = CMD_AABB_OVERLAP_FAILED; + } + + hasStatus = true; + 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(); 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..2ba38f232 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -143,11 +143,21 @@ 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; + double m_aabbQueryMin[3]; + double m_aabbQueryMax[3]; + int m_mode; }; struct RequestVisualShapeDataArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7b0a967bc..4f55d5520 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -80,6 +80,8 @@ enum EnumSharedMemoryServerStatus CMD_CALCULATED_JACOBIAN_FAILED, CMD_CONTACT_POINT_INFORMATION_COMPLETED, CMD_CONTACT_POINT_INFORMATION_FAILED, + CMD_AABB_OVERLAP_COMPLETED, + CMD_AABB_OVERLAP_FAILED, CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED, CMD_CALCULATE_INVERSE_KINEMATICS_FAILED, CMD_SAVE_WORLD_COMPLETED, @@ -158,6 +160,13 @@ struct b3DebugLines const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'. }; +struct b3AABBOverlapData +{ + int m_numOverlappingObjects; + int* m_objectUniqueIds; + int* m_links; +}; + struct b3CameraImageData { int m_pixelWidth; @@ -192,6 +201,14 @@ struct b3ContactPointData // double m_angularFrictionForce; }; +enum +{ + CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS = 0, + CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1, + CONTACT_QUERY_MODE_AABB_OVERLAP = 2, +}; + + struct b3ContactInformation { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index af8eacdf7..f68538a58 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -680,6 +680,12 @@ static int pybullet_internalGetBasePositionAndOrientation( baseOrientation[2] = 0.; baseOrientation[3] = 1.; + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + Py_INCREF(Py_None); + return Py_None; + } + { { b3SharedMemoryCommandHandle cmd_handle = @@ -1352,6 +1358,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 +1460,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 +1502,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 +1536,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 +1557,188 @@ 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; + + 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); + + + 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; + int i; + 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 +2561,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 +2610,7 @@ static PyMethodDef SpamMethods[] = { // loadSnapshot // raycast info // object names + // collision query {NULL, NULL, 0, NULL} /* Sentinel */ }; @@ -2541,6 +2666,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.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();