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

Start of a urdfEditor.py, limited support to extract a URDF from a PyBullet body.

Browse Source 
Use btCylinderShapeZ for URDF cylinder, instead of converting it to a btConvexHullShape.
Implement PyBullet.getCollisionShapeData
Extend PyBullet.getDynamicsInfo / b3GetDynamicsInfo, remove flag (don't rely on API returning a fixed number of elements in a list!)
Extend PyBullet.getJointInfo: add parentIndex
This commit is contained in:
Erwin Coumans
2018-01-03 19:17:28 -08:00
parent a1492f50fc
commit 79d78a325a
16 changed files with 858 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -67,6 +67,32 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisComOffset.m_floats[0];
info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisComOffset.m_floats[1];
info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisComOffset.m_floats[2];
info.m_parentFrame[3] = mb->m_links[link].m_zeroRotParentToThis.m_floats[0];
info.m_parentFrame[4] = mb->m_links[link].m_zeroRotParentToThis.m_floats[1];
info.m_parentFrame[5] = mb->m_links[link].m_zeroRotParentToThis.m_floats[2];
info.m_parentFrame[6] = mb->m_links[link].m_zeroRotParentToThis.m_floats[3];
info.m_jointAxis[0] = 0;
info.m_jointAxis[1] = 0;
info.m_jointAxis[2] = 0;
info.m_parentIndex = mb->m_links[link].m_parentIndex;
if (info.m_jointType == eRevoluteType)
{
info.m_jointAxis[0] = mb->m_links[link].m_jointAxisTop[0].m_floats[0];
info.m_jointAxis[1] = mb->m_links[link].m_jointAxisTop[0].m_floats[1];
info.m_jointAxis[2] = mb->m_links[link].m_jointAxisTop[0].m_floats[2];
}
if (info.m_jointType == ePrismaticType)
{
info.m_jointAxis[0] = mb->m_links[link].m_jointAxisBottom[0].m_floats[0];
info.m_jointAxis[1] = mb->m_links[link].m_jointAxisBottom[0].m_floats[1];
info.m_jointAxis[2] = mb->m_links[link].m_jointAxisBottom[0].m_floats[2];
}
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -58,6 +58,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) = 0;
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0;
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) = 0;

37
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2141,13 +2141,12 @@ B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
return false;
info->m_mass = dynamicsInfo.m_mass;
info->m_localInertialDiagonal[0] = dynamicsInfo.m_localInertialDiagonal[0];
info->m_localInertialDiagonal[1] = dynamicsInfo.m_localInertialDiagonal[1];
info->m_localInertialDiagonal[2] = dynamicsInfo.m_localInertialDiagonal[2];
info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
return true;
if (info)
{
*info = dynamicsInfo;
return true;
}
return false;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
@@ -3418,6 +3417,30 @@ B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_COLLISION_SHAPE_INFO;
command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA;
command->m_requestCollisionShapeDataArguments.m_linkIndex = linkIndex;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedCollisionShapeInformation(collisionShapeInfo);
}
}
//request visual shape information
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -259,6 +259,11 @@ B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, str
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);

30
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -44,6 +44,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
@@ -1253,6 +1255,27 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_COLLISION_SHAPE_INFO_FAILED:
{
b3Warning("getCollisionShapeData failed");
break;
}
case CMD_COLLISION_SHAPE_INFO_COMPLETED:
{
B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Collision Shape Information Request OK\n");
}
int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
b3CollisionShapeData* shapeData = (b3CollisionShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numCollisionShapesCopied; i++)
{
m_data->m_cachedCollisionShapes[i] = shapeData[i];
}
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@@ -1595,6 +1618,13 @@ void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualS
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}
void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
{
collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes? &m_data->m_cachedCollisionShapes[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -68,6 +68,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

31
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -54,6 +54,8 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
@@ -1011,6 +1013,27 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
break;
}
case CMD_COLLISION_SHAPE_INFO_FAILED:
{
b3Warning("getCollisionShapeData failed");
break;
}
case CMD_COLLISION_SHAPE_INFO_COMPLETED:
{
B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Collision Shape Information Request OK\n");
}
int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i = 0; i < numCollisionShapesCopied; i++)
{
m_data->m_cachedCollisionShapes[i] = shapeData[i];
}
break;
}
case CMD_RESTORE_STATE_FAILED:
{
b3Warning("restoreState failed");
@@ -1242,6 +1265,14 @@ void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformat
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}
void PhysicsDirect::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
{
collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0;
}
void PhysicsDirect::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -91,6 +91,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

187
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6237,6 +6237,29 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getBaseInertia()[2];
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction();
serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_rootLocalInertialFrame.getOrigin()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_rootLocalInertialFrame.getOrigin()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_rootLocalInertialFrame.getRotation()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_rootLocalInertialFrame.getRotation()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3];
serverCmd.m_dynamicsInfo.m_restitution = mb->getBaseCollider()->getRestitution();
serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getBaseCollider()->getRollingFriction();
serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getBaseCollider()->getSpinningFriction();
if (mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
{
serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getBaseCollider()->getContactStiffness();
serverCmd.m_dynamicsInfo.m_contactDamping = mb->getBaseCollider()->getContactDamping();
}
else
{
serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
serverCmd.m_dynamicsInfo.m_contactDamping = -1;
}
}
else
{
@@ -6245,9 +6268,31 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[3];
if (mb->getLinkCollider(linkIndex))
{
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();
serverCmd.m_dynamicsInfo.m_restitution = mb->getLinkCollider(linkIndex)->getRestitution();
serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getLinkCollider(linkIndex)->getRollingFriction();
serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getLinkCollider(linkIndex)->getSpinningFriction();
if (mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
{
serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getLinkCollider(linkIndex)->getContactStiffness();
serverCmd.m_dynamicsInfo.m_contactDamping = mb->getLinkCollider(linkIndex)->getContactDamping();
}
else
{
serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
serverCmd.m_dynamicsInfo.m_contactDamping = -1;
}
}
else
{
@@ -7967,7 +8012,144 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
return hasStatus;
}
// PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
// PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
// PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
// PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
// PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
// PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape* colShape, const btTransform& transform, b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes)
{
if (maxCollisionShapes <= 0)
{
b3Warning("No space in buffer");
return 0;
}
int numConverted = 0;
collisionShapeBuffer[0].m_localCollisionFrame[0] = transform.getOrigin()[0];
collisionShapeBuffer[0].m_localCollisionFrame[1] = transform.getOrigin()[1];
collisionShapeBuffer[0].m_localCollisionFrame[2] = transform.getOrigin()[2];
collisionShapeBuffer[0].m_localCollisionFrame[3] = transform.getRotation()[0];
collisionShapeBuffer[0].m_localCollisionFrame[4] = transform.getRotation()[1];
collisionShapeBuffer[0].m_localCollisionFrame[5] = transform.getRotation()[2];
collisionShapeBuffer[0].m_localCollisionFrame[6] = transform.getRotation()[3];
collisionShapeBuffer[0].m_meshAssetFileName[0] = 0;
switch (colShape->getShapeType())
{
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
sprintf( collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
collisionShapeBuffer[0].m_dimensions[0] = 1;
collisionShapeBuffer[0].m_dimensions[1] = 1;
collisionShapeBuffer[0].m_dimensions[2] = 1;
numConverted++;
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShapeZ* cyl = (btCylinderShapeZ*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CYLINDER;
collisionShapeBuffer[0].m_dimensions[0] = 2.*cyl->getHalfExtentsWithMargin().getZ();
collisionShapeBuffer[0].m_dimensions[1] = cyl->getHalfExtentsWithMargin().getX();
collisionShapeBuffer[0].m_dimensions[2] = 0;
numConverted++;
break;
}
case BOX_SHAPE_PROXYTYPE:
{
btBoxShape* box = (btBoxShape*)colShape;
btVector3 halfExtents = box->getHalfExtentsWithMargin();
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_BOX;
collisionShapeBuffer[0].m_dimensions[0] = 2.*halfExtents[0];
collisionShapeBuffer[0].m_dimensions[1] = 2.*halfExtents[1];
collisionShapeBuffer[0].m_dimensions[2] = 2.*halfExtents[2];
numConverted++;
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape* sphere = (btSphereShape*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_SPHERE;
collisionShapeBuffer[0].m_dimensions[0] = sphere->getRadius();
collisionShapeBuffer[0].m_dimensions[1] = sphere->getRadius();
collisionShapeBuffer[0].m_dimensions[2] = sphere->getRadius();
numConverted++;
break;
}
case COMPOUND_SHAPE_PROXYTYPE:
{
//recurse, accumulate childTransform
btCompoundShape* compound = (btCompoundShape*)colShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
btTransform childTrans = transform*compound->getChildTransform(i);
int remain = maxCollisionShapes - numConverted;
int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
numConverted += converted;
}
break;
}
default:
{
b3Warning("Unexpected collision shape type in PhysicsServerCommandProcessor::extractCollisionShapes");
}
};
return numConverted;
}
bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REQUEST_COLLISION_SHAPE_INFO");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_FAILED;
int bodyUniqueId = clientCmd.m_requestCollisionShapeDataArguments.m_bodyUniqueId;
int linkIndex = clientCmd.m_requestCollisionShapeDataArguments.m_linkIndex;
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (bodyHandle)
{
if (bodyHandle->m_multiBody)
{
b3CollisionShapeData* collisionShapeStoragePtr = (b3CollisionShapeData*)bufferServerToClient;
int totalBytesPerObject = sizeof(b3CollisionShapeData);
int maxNumColObjects = bufferSizeInBytes / totalBytesPerObject - 1;
btTransform childTrans;
childTrans.setIdentity();
serverCmd.m_sendCollisionShapeArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendCollisionShapeArgs.m_linkIndex = linkIndex;
if (linkIndex == -1)
{
if (bodyHandle->m_multiBody->getBaseCollider())
{
//extract shape info from base collider
int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
}
}
else
{
if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks() && bodyHandle->m_multiBody->getLinkCollider(linkIndex))
{
int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getLinkCollider(linkIndex)->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
}
}
}
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -8677,6 +8859,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_COLLISION_SHAPE_INFO:
{
hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_UPDATE_VISUAL_SHAPE:
{
hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);

3
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -74,6 +74,7 @@ protected:
bool processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -83,6 +84,8 @@ protected:
bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& childTransform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,

18
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -2542,14 +2542,14 @@ void PhysicsServerExample::drawUserDebugLines()
}
float colorRGBA[4] = {
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
1.};
(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
(float)1.};
float pos[3] = {m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
float pos[3] = { (float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
int graphicsIndex = m_multiThreadedHelper->m_userDebugText[i].m_trackingVisualShapeIndex;
if (graphicsIndex>=0)
@@ -2604,7 +2604,9 @@ void PhysicsServerExample::drawUserDebugLines()
offset.setIdentity();
offset.setOrigin(btVector3(0,-float(t)*sz,0));
btTransform result = tr*offset;
float newpos[3] = {result.getOrigin()[0],result.getOrigin()[1],result.getOrigin()[2]};
float newpos[3] = { (float)result.getOrigin()[0],
(float)result.getOrigin()[1],
(float)result.getOrigin()[2]};
m_guiHelper->getAppInterface()->drawText3D(pieces[t].c_str(),
newpos,orientation,colorRGBA,

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -303,6 +303,12 @@ struct RequestVisualShapeDataArgs
int m_startingVisualShapeIndex;
};
struct RequestCollisionShapeDataArgs
{
int m_bodyUniqueId;
int m_linkIndex;
};
enum EnumUpdateVisualShapeData
{
CMD_UPDATE_VISUAL_SHAPE_TEXTURE=1,
@@ -338,7 +344,12 @@ struct SendVisualShapeDataArgs
int m_numRemainingVisualShapes;
};
struct SendCollisionShapeDataArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_numCollisionShapes;
};
struct SendDebugLinesArgs
{
@@ -993,6 +1004,7 @@ struct SharedMemoryCommand
struct b3SearchPathfArgs m_searchPathArgs;
struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments;
struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;
};
};
@@ -1068,6 +1080,7 @@ struct SharedMemoryStatus
struct b3CustomCommandResultArgs m_customCommandResultArgs;
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
struct b3StateSerializationArguments m_saveStateResultArgs;
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
};
};

32
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -80,6 +80,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
CMD_SAVE_STATE,
CMD_RESTORE_STATE,
CMD_REQUEST_COLLISION_SHAPE_INFO,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -185,6 +186,8 @@ enum EnumSharedMemoryServerStatus
CMD_SAVE_STATE_COMPLETED,
CMD_RESTORE_STATE_FAILED,
CMD_RESTORE_STATE_COMPLETED,
CMD_COLLISION_SHAPE_INFO_COMPLETED,
CMD_COLLISION_SHAPE_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -217,10 +220,6 @@ enum JointType {
eGearType=6
};
enum b3RequestDynamicsInfoFlags
{
eDYNAMICS_INFO_REPORT_INERTIA=1,
};
enum b3JointInfoFlags
{
@@ -247,6 +246,7 @@ struct b3JointInfo
double m_parentFrame[7]; // position and orientation (quaternion)
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
int m_parentIndex;
};
@@ -278,7 +278,14 @@ struct b3DynamicsInfo
{
double m_mass;
double m_localInertialDiagonal[3];
double m_localInertialFrame[7];
double m_lateralFrictionCoeff;
double m_rollingFrictionCoeff;
double m_spinningFrictionCoeff;
double m_restitution;
double m_contactStiffness;
double m_contactDamping;
};
// copied from btMultiBodyLink.h
@@ -531,6 +538,23 @@ struct b3VisualShapeInformation
struct b3VisualShapeData* m_visualShapeData;
};
struct b3CollisionShapeData
{
int m_objectUniqueId;
int m_linkIndex;
int m_collisionGeometryType;//GEOM_BOX, GEOM_SPHERE etc
double m_dimensions[3];//meaning depends on m_visualGeometryType GEOM_BOX: extents, GEOM_SPHERE: radius, GEOM_CAPSULE:
double m_localCollisionFrame[7];//pos[3], orn[4]
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
};
struct b3CollisionShapeInformation
{
int m_numCollisionShapes;
struct b3CollisionShapeData* m_collisionShapeData;
};
enum eLinkStateFlags
{
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1,