Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

pybullet getClosestPoints

Browse Source
This commit is contained in:
erwincoumans
2016-11-09 21:01:04 -08:00
parent 2bb53b311d
commit 0d47d61007
15 changed files with 1373 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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

11
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

384
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;i<numContactManifolds;i++)
{
const btPersistentManifold* manifold = m_data->m_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;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++)
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<int> m_bodyUniqueIds;
b3AlignedObjectArray<int> 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<btCollisionObject*> setA;
btAlignedObjectArray<btCollisionObject*> setB;
btAlignedObjectArray<int> setALinkIndex;
btAlignedObjectArray<int> 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<b3ContactPointData>& m_cachedContactPoints;
MyContactResultCallback(btAlignedObjectArray<b3ContactPointData>& 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();

17
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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)
{

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

17
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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
{