diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 513479fda..e60953750 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1195,6 +1195,46 @@ int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) return status->m_userDebugDrawArgs.m_debugItemUniqueId; } +b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_USER_DEBUG_DRAW; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle)command; +} + + + +void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + command->m_updateFlags |= USER_DEBUG_SET_CUSTOM_OBJECT_COLOR; + command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId; + command->m_userDebugDrawArgs.m_linkIndex = linkIndex; + command->m_userDebugDrawArgs.m_objectDebugColorRGB[0] = objectColorRGB[0]; + command->m_userDebugDrawArgs.m_objectDebugColorRGB[1] = objectColorRGB[1]; + command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2]; +} + +void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + command->m_updateFlags |= USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR; + command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId; + command->m_userDebugDrawArgs.m_linkIndex = linkIndex; + +} + + + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index de2a5a971..b9da28cd5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -89,9 +89,15 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); + +b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient); +void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]); +void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); + ///All debug items unique Ids are positive: a negative unique Id means failure. int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 147f3a316..18231566f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3536,7 +3536,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for( int i=0;igetRigidBodyByIndex(i); - if (colObj) { btRigidBody* rb = btRigidBody::upcast(colObj); @@ -3599,7 +3598,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; hasStatus = true; - + + if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) + { + int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body) + { + btCollisionObject* destColObj = 0; + + if (body->m_multiBody) + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1) + { + destColObj = body->m_multiBody->getBaseCollider(); + } + else + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks()) + { + destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider; + } + } + + } + if (body->m_rigidBody) + { + destColObj = body->m_rigidBody; + } + + if (destColObj) + { + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR) + { + destColObj->removeCustomDebugColor(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) + { + btVector3 objectColorRGB; + objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]); + destColObj->setCustomDebugColor(objectColorRGB); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + } + } if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 965db5233..5b3346433 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -526,7 +526,10 @@ enum EnumUserDebugDrawFlags USER_DEBUG_HAS_LINE=1, USER_DEBUG_HAS_TEXT=2, USER_DEBUG_REMOVE_ONE_ITEM=4, - USER_DEBUG_REMOVE_ALL=8 + USER_DEBUG_REMOVE_ALL=8, + USER_DEBUG_SET_CUSTOM_OBJECT_COLOR = 16, + USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32, + }; struct UserDebugDrawArgs @@ -543,6 +546,10 @@ struct UserDebugDrawArgs double m_textPositionXYZ[3]; double m_textColorRGB[3]; double m_textSize; + + double m_objectDebugColorRGB[3]; + int m_objectUniqueId; + int m_linkIndex; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index aa1c379ee..1947ec103 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1647,8 +1647,45 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args Py_INCREF(Py_None); return Py_None; } - +static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds) +{ + PyObject* objectColorRGBObj = 0; + double objectColorRGB[3]; + + int objectUniqueId = -1; + int linkIndex = -2; + + static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|O", kwlist, + &objectUniqueId, &linkIndex, &objectColorRGBObj)) + return NULL; + + if (objectColorRGBObj) + { + if (pybullet_internalSetVectord(objectColorRGBObj, objectColorRGB)) + { + b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm); + b3SetDebugObjectColor(commandHandle, objectUniqueId, linkIndex, objectColorRGB); + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + } + else + { + b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm); + b3RemoveDebugObjectColor(commandHandle, objectUniqueId, linkIndex); + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) { @@ -3427,6 +3464,10 @@ static PyMethodDef SpamMethods[] = { "remove all user debug draw items" }, + { "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS, + "Override the wireframe debug drawing color for a particular object unique id / link index." + "If you ommit the color, the custom color will be removed." + }, {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c285a83fb..0cae21000 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -122,6 +122,7 @@ protected: ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. int m_updateRevision; + btVector3 m_customDebugColorRGB; public: @@ -136,7 +137,8 @@ public: CF_CHARACTER_OBJECT = 16, CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing - CF_HAS_CONTACT_STIFFNESS_DAMPING = 128 + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, + CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, }; enum CollisionObjectTypes @@ -556,6 +558,26 @@ public: return m_updateRevision; } + void setCustomDebugColor(const btVector3& colorRGB) + { + m_customDebugColorRGB = colorRGB; + m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + void removeCustomDebugColor() + { + m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + bool getCustomDebugColor(btVector3& colorRGB) const + { + bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + if (hasCustomColor) + { + colorRGB = m_customDebugColorRGB; + } + return hasCustomColor; + } inline bool checkCollideWith(const btCollisionObject* co) const { diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 133994112..3bbf7586e 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1572,6 +1572,8 @@ void btCollisionWorld::debugDrawWorld() } }; + colObj->getCustomDebugColor(color); + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))