From 85fd7f560c64cf90ad7a30a57cae103708ef9f69 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 1 Sep 2016 13:30:07 -0700 Subject: [PATCH] add first draft of contact point query in shared memory API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData); Implemented for PhysicsClientSharedMemory, not for PhysicsDirect yet. Add btCollisionObject::setUserIndex2 --- examples/SharedMemory/PhysicsClient.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 45 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 5 + .../SharedMemory/PhysicsClientExample.cpp | 23 ++- .../PhysicsClientSharedMemory.cpp | 54 +++++++ .../SharedMemory/PhysicsClientSharedMemory.h | 2 + examples/SharedMemory/PhysicsDirect.cpp | 9 ++ examples/SharedMemory/PhysicsDirect.h | 2 + examples/SharedMemory/PhysicsLoopBack.cpp | 7 + examples/SharedMemory/PhysicsLoopBack.h | 2 + .../PhysicsServerCommandProcessor.cpp | 136 +++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 15 ++ examples/SharedMemory/SharedMemoryPublic.h | 38 ++++- examples/TinyRenderer/tgaimage.h | 6 +- .../CollisionDispatch/btCollisionObject.cpp | 1 + .../CollisionDispatch/btCollisionObject.h | 17 ++- .../Featherstone/btMultiBody.cpp | 4 +- src/BulletDynamics/Featherstone/btMultiBody.h | 36 +++++ 18 files changed, 395 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 07b10aa13..b76e7e18c 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -40,6 +40,8 @@ public: virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0; + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b0f66c6db..d57113e3e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1103,6 +1103,51 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage } } +///request an contact point information +b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION; + command->m_requestContactPointArguments.m_startingContactPointIndex = 0; + command->m_requestContactPointArguments.m_objectAIndexFilter = -1; + command->m_requestContactPointArguments.m_objectBIndexFilter = -1; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} + +void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA; +} + +void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB; +} + +void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); + + +void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + if (cl) + { + cl->getCachedContactPointInformation(contactPointData); + } +} + + b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 351c87677..4d6522576 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -80,6 +80,11 @@ void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); +///request an contact point information +b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); +void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); +void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); +void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 034215bd2..07ba317a4 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -274,7 +274,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_CREATE_BOX_COLLISION_SHAPE: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); - b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3); + b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-1.5); b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; @@ -415,6 +415,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_CALCULATE_INVERSE_DYNAMICS: { if (m_selectedBody >= 0) @@ -442,6 +443,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } break; } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle); + b3SetContactFilterBodyA(commandHandle,0); + b3SetContactFilterBodyB(commandHandle,1); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } default: { b3Error("Unknown buttonId"); @@ -530,6 +539,7 @@ void PhysicsClientExample::createButtons() createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger); + createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger); if (m_bodyUniqueIds.size()) { @@ -946,6 +956,17 @@ void PhysicsClientExample::stepSimulation(float deltaTime) } } + if (statusType == CMD_CONTACT_POINT_INFORMATION_FAILED) + { + b3Warning("Cannot get contact information"); + } + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + b3ContactInformation contactPointData; + b3GetContactPointInformation(m_physicsClientHandle, &contactPointData); + b3Printf("Num Contacts: %d\n", contactPointData.m_numContactPoints); + + } } } if (b3CanSubmitCommand(m_physicsClientHandle)) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 85c90bbd7..317902254 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -38,6 +38,9 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedCameraDepthBuffer; btAlignedObjectArray m_cachedSegmentationMaskBuffer; + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArraym_cachedContactPointDynamics; + btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -58,6 +61,7 @@ struct PhysicsClientSharedMemoryInternalData { m_counter(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), + m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), @@ -558,6 +562,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Inverse Dynamics computations failed"); break; } + case CMD_CONTACT_POINT_INFORMATION_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + m_data->m_cachedContactPointDynamics.resize(0); + + b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + break; + } + case CMD_CONTACT_POINT_INFORMATION_FAILED: + { + b3Warning("Contact Point Information Request failed"); + break; + } + default: { b3Error("Unknown server status\n"); btAssert(0); @@ -620,6 +651,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } } + if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED) + { + //todo: request remaining points, if needed + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + submitClientCommand(command); + return 0; + } + } + if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; @@ -716,6 +762,14 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0; } +void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactDynamicsData = 0; + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + +} + const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { if (m_data->m_debugLinesFrom.size()) { return &m_data->m_debugLinesFrom[0].m_x; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index a47fc551a..69e5ed64d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -48,6 +48,8 @@ public: virtual const float* getDebugLinesTo() const; virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(struct b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 948b90b90..e465aa6d9 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -487,3 +487,12 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; } } + +void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + if (contactPointData) + { + contactPointData->m_numContactPoints = 0; + } +} + diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 7e3ae2c94..ee3709ecf 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -64,6 +64,8 @@ public: virtual void getCachedCameraImage(b3CameraImageData* cameraData); + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + //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 ada37ce88..91ac0d2cc 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -105,10 +105,12 @@ const float* PhysicsLoopBack::getDebugLinesFrom() const { return m_data->m_physicsClient->getDebugLinesFrom(); } + const float* PhysicsLoopBack::getDebugLinesTo() const { return m_data->m_physicsClient->getDebugLinesTo(); } + const float* PhysicsLoopBack::getDebugLinesColor() const { return m_data->m_physicsClient->getDebugLinesColor(); @@ -118,3 +120,8 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData) { return m_data->m_physicsClient->getCachedCameraImage(cameraData); } + +void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData); +} diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 8c0e187b6..326e21128 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -53,6 +53,8 @@ public: virtual const float* getDebugLinesTo() const; virtual const float* getDebugLinesColor() const; virtual void getCachedCameraImage(struct b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 84d4554ca..daccc500c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -403,6 +403,8 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; + + btAlignedObjectArray m_cachedContactPoints; btAlignedObjectArray m_sdfRecentLoadedBodies; @@ -759,6 +761,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + u2b.setBodyUniqueId(bodyUniqueId); { btScalar mass = 0; @@ -782,9 +786,16 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe mb = creation.getBulletMultiBody(); rb = creation.getRigidBody(); + if (rb) + rb->setUserIndex2(bodyUniqueId); + + if (mb) + mb->setUserIndex2(bodyUniqueId); + if (mb) { bodyHandle->m_multiBody = mb; + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); @@ -856,6 +867,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto u2b.setBodyUniqueId(bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + { btScalar mass = 0; @@ -887,6 +900,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); + if (useMultiBody) { @@ -894,7 +908,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (mb) { - + mb->setUserIndex2(bodyUniqueId); bodyHandle->m_multiBody = mb; createJointMotors(mb); @@ -958,6 +972,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (rb) { bodyHandle->m_rigidBody = rb; + rb->setUserIndex2(bodyUniqueId); return true; } } @@ -2079,6 +2094,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; + rb->setUserIndex2(bodyUniqueId); bodyHandle->m_rootLocalInertialFrame.setIdentity(); bodyHandle->m_rigidBody = rb; hasStatus = true; @@ -2121,6 +2137,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; 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(); + m_data->m_cachedContactPoints.resize(0); + m_data->m_cachedContactPoints.reserve(numContactManifolds*4); + for (int i=0;im_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; + + int objectIndexB = -1; + + const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (mblB && mblB->m_multiBody) + { + 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) + { + objectIndexA = mblA->m_multiBody->getUserIndex2(); + } + + btAssert(bodyA || mblA); + + //apply the filter, if any + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) + { + if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) && + (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB)) + continue; + } + + 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++) + { + //if the point passes the optional filter, add it + + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0) + { + //one of the two unique Ids has to match... + + } + + b3ContactPointData pt; + pt.m_bodyUniqueIdA = -1; + pt.m_bodyUniqueIdB = -1; + const btManifoldPoint& srcPt = manifold->getContactPoint(p); + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_contactPointDynamicsIndex = -1; + pt.m_linkIndexA = -1; + pt.m_linkIndexB = -1; + 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]; + } + m_data->m_cachedContactPoints.push_back (pt); + } + } + } + + int numContactPoints = m_data->m_cachedContactPoints.size(); + + + //b3ContactPoint + //struct b3ContactPointDynamics + + int totalBytesPerContact = sizeof(b3ContactPointData); + int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1; + + b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; + + int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + int numContactPointBatch = btMin(numContactPoints,contactPointStorage); + + int endContactPointIndex = startContactPointIndex+numContactPointBatch; + + for (int i=startContactPointIndex;im_cachedContactPoints[i]; + b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; + destPt = srcPt; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; + } + + serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED, + hasStatus = true; + break; } case CMD_CALCULATE_INVERSE_DYNAMICS: { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ccbfb36fe..b718d40c7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -144,6 +144,12 @@ enum EnumRequestPixelDataUpdateFlags }; +struct RequestContactDataArgs +{ + int m_startingContactPointIndex; + int m_objectAIndexFilter; + int m_objectBIndexFilter; +}; struct SendDebugLinesArgs @@ -406,6 +412,7 @@ struct SharedMemoryCommand struct ExternalForceArgs m_externalForceArguments; struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments; struct CreateJointArgs m_createJointArguments; + struct RequestContactDataArgs m_requestContactPointArguments; }; }; @@ -414,6 +421,13 @@ struct RigidBodyCreateArgs int m_bodyUniqueId; }; +struct SendContactDataArgs +{ + int m_startingContactPointIndex; + int m_numContactPointsCopied; + int m_numRemainingContactPoints; +}; + struct SharedMemoryStatus { int m_type; @@ -430,6 +444,7 @@ struct SharedMemoryStatus struct SendPixelDataArgs m_sendPixelDataArguments; struct RigidBodyCreateArgs m_rigidBodyCreateArgs; struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs; + struct SendContactDataArgs m_sendContactPointArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9b50e4d17..231929ab6 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -29,8 +29,11 @@ enum EnumSharedMemoryClientCommand CMD_APPLY_EXTERNAL_FORCE, CMD_CALCULATE_INVERSE_DYNAMICS, CMD_CALCULATE_INVERSE_KINEMATICS, + CMD_CREATE_JOINT, + CMD_REQUEST_CONTACT_POINT_INFORMATION, + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, - CMD_CREATE_JOINT + }; enum EnumSharedMemoryServerStatus @@ -64,6 +67,8 @@ enum EnumSharedMemoryServerStatus CMD_INVALID_STATUS, CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, + CMD_CONTACT_POINT_INFORMATION_COMPLETED, + CMD_CONTACT_POINT_INFORMATION_FAILED, CMD_MAX_SERVER_COMMANDS }; @@ -131,6 +136,37 @@ struct b3CameraImageData const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints }; + +struct b3ContactPointData +{ + int m_contactFlags;//flag wether certain fields below are valid + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates + double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates + double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A + double m_contactDistance;//negative number is penetration, positive is distance. + int m_contactPointDynamicsIndex; +}; + +struct b3ContactPointDynamicsData +{ + double m_normalForce; + double m_linearFrictionDirection[3]; + double m_linearFrictionForce; + double m_angularFrictionDirection[3]; + double m_angularFrictionForce; +}; + +struct b3ContactInformation +{ + int m_numContactPoints; + struct b3ContactPointData* m_contactPointData; + struct b3ContactPointDynamicsData* m_contactDynamicsData; +}; + ///b3LinkState provides extra information such as the Cartesian world coordinates ///center of mass (COM) of the link, relative to the world reference frame. ///Orientation is a quaternion x,y,z,w diff --git a/examples/TinyRenderer/tgaimage.h b/examples/TinyRenderer/tgaimage.h index 9e2cbf877..80818dda7 100644 --- a/examples/TinyRenderer/tgaimage.h +++ b/examples/TinyRenderer/tgaimage.h @@ -30,20 +30,20 @@ struct TGAColor { bgra[i] = 0; } - TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) { + TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bytespp(4) { bgra[0] = B; bgra[1] = G; bgra[2] = R; bgra[3] = A; } - TGAColor(unsigned char v) : bgra(), bytespp(1) { + TGAColor(unsigned char v) : bytespp(1) { for (int i=0; i<4; i++) bgra[i] = 0; bgra[0] = v; } - TGAColor(const unsigned char *p, unsigned char bpp) : bgra(), bytespp(bpp) { + TGAColor(const unsigned char *p, unsigned char bpp) : bytespp(bpp) { for (int i=0; i<(int)bpp; i++) { bgra[i] = p[i]; } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 395df3a55..032a5828d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject() m_rollingFriction(0.0f), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), + m_userIndex2(-1), m_userIndex(-1), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c68402418..a96809f91 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -93,8 +93,10 @@ protected: ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - + void* m_userObjectPointer; + + int m_userIndex2; + int m_userIndex; ///time of impact calculation @@ -476,6 +478,12 @@ public: { return m_userIndex; } + + int getUserIndex2() const + { + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet void setUserPointer(void* userPointer) { @@ -487,6 +495,11 @@ public: { m_userIndex = index; } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } int getUpdateRevisionInternal() const { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index edef315b3..76903be98 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -106,7 +106,9 @@ btMultiBody::btMultiBody(int n_links, m_awake(true), m_canSleep(canSleep), m_sleepTimer(0), - + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index a4510d2ca..2b387df1d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -577,6 +577,38 @@ void addJointTorque(int i, btScalar Q); m_baseName = name; } + ///users can point to their objects, userPointer is not used by Bullet + void* getUserPointer() const + { + return m_userObjectPointer; + } + + int getUserIndex() const + { + return m_userIndex; + } + + int getUserIndex2() const + { + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void* userPointer) + { + m_userObjectPointer = userPointer; + } + + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -653,6 +685,10 @@ private: bool m_canSleep; btScalar m_sleepTimer; + void* m_userObjectPointer; + int m_userIndex2; + int m_userIndex; + int m_companionId; btScalar m_linearDamping; btScalar m_angularDamping;