From 7790ee2f029cc8133cb5166e648b5c4348441a23 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 31 Aug 2016 16:05:42 -0700 Subject: [PATCH 1/3] fix compile issue with pybullet.c on Visual Studio --- examples/pybullet/pybullet.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fdc753443..f7f062073 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -617,14 +617,16 @@ static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); const int status_type = b3GetStatusType(status_handle); + const double* actualStateQ; + // const double* jointReactionForces[]; + int i; + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { PyErr_SetString(SpamError, "getBasePositionAndOrientation failed."); return 0; } - const double* actualStateQ; - // const double* jointReactionForces[]; - int i; + b3GetStatusActualState(status_handle, 0/* body_unique_id */, 0/* num_degree_of_freedom_q */, 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/, From 1c65cae6d0b0e91faf93ca5bc2a06cf44b64152d Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Thu, 1 Sep 2016 10:45:14 -0700 Subject: [PATCH 2/3] [SharedMemory] Calculate inverse dynamics now uses world gravity. Small change that takes the world gravity and applies it to the body during the call to inverse dynamics in the shared memory interface. Otherwise the gravity vector is left at the default value and there is currently no interface to set the gravity for the inverse dynamics system. --- .../PhysicsServerCommandProcessor.cpp | 48 ++++++++++--------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 84d4554ca..914cce573 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -507,7 +507,7 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH } m_data->m_guiHelper = guiHelper; - + } @@ -518,7 +518,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() createEmptyDynamicsWorld(); m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; - + } PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() @@ -887,7 +887,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto btMultiBody* mb = creation.getBulletMultiBody(); btRigidBody* rb = creation.getRigidBody(); - + if (useMultiBody) { @@ -1024,7 +1024,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) { - + bool hasStatus = false; { @@ -1154,7 +1154,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; int numPixelsCopied = 0; - + if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) { @@ -1481,10 +1481,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) { kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; - } - + } + motor->setVelocityTarget(desiredVelocity,kd); - + btScalar kp = 0.f; motor->setPositionTarget(0,kp); hasDesiredVelocity = true; @@ -1565,7 +1565,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm motor->setVelocityTarget(desiredVelocity,kd); motor->setPositionTarget(desiredPosition,kp); - + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) @@ -1805,7 +1805,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); @@ -1897,7 +1897,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[5], clientCmd.m_initPoseArgs.m_initialStateQ[6]); - + mb->setWorldToBaseRot(invOrn.inverse()); } if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) @@ -1936,10 +1936,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_visualConverter.resetAll(); } - + deleteDynamicsWorld(); createEmptyDynamicsWorld(); - + m_data->exitHandles(); m_data->initHandles(); @@ -2128,7 +2128,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - btInverseDynamics::MultiBodyTree** treePtrPtr = + btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody); btInverseDynamics::MultiBodyTree* tree = 0; serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; @@ -2140,12 +2140,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm else { btInverseDynamics::btMultiBodyTreeCreator id_creator; - if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false)) + if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false)) { b3Error("error creating tree\n"); serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } - else + else { tree = btInverseDynamics::CreateMultiBodyTree(id_creator); m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree); @@ -2163,8 +2163,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; } - - if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) && + -1 != tree->setGravityInWorldFrame(id_grav)) { serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; @@ -2179,7 +2181,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; } } - + } else { @@ -2203,7 +2205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBody* mb = body->m_multiBody; bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); - + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) { btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], @@ -2213,7 +2215,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_externalForceArguments.m_positions[i*3+0], clientCmd.m_externalForceArguments.m_positions[i*3+1], clientCmd.m_externalForceArguments.m_positions[i*3+2]); - + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) { btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; @@ -2236,7 +2238,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) { btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; @@ -2252,7 +2254,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; From 85fd7f560c64cf90ad7a30a57cae103708ef9f69 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 1 Sep 2016 13:30:07 -0700 Subject: [PATCH 3/3] 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;