From dd3d55610b242482dbf3171b550285ff6c21cfea Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 30 Jun 2017 13:35:07 -0700 Subject: [PATCH] fixes in pybullet.loadTexture, changeVisualShape replacing texture. (also works for OpenGL3 renderer now) --- .../CommonGUIHelperInterface.h | 3 + .../CommonInterfaces/CommonRenderInterface.h | 3 + examples/ExampleBrowser/OpenGLGuiHelper.cpp | 12 ++ examples/ExampleBrowser/OpenGLGuiHelper.h | 3 + .../OpenGLWindow/GLInstancingRenderer.cpp | 20 +++ examples/OpenGLWindow/GLInstancingRenderer.h | 3 +- examples/SharedMemory/PhysicsClientC_API.cpp | 12 ++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 169 ++++++++++++------ .../SharedMemory/PhysicsServerExample.cpp | 43 +++++ examples/SharedMemory/SharedMemoryCommands.h | 6 + .../TinyRendererVisualShapeConverter.cpp | 43 +++-- examples/pybullet/pybullet.c | 7 +- src/LinearMath/btHashMap.h | 24 ++- 14 files changed, 275 insertions(+), 76 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 233f3002a..d552f1ebf 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -41,6 +41,9 @@ struct GUIHelperInterface virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} + virtual int getShapeIndexFromInstance(int instanceUid){return -1;} + virtual void replaceTexture(int shapeIndex, int textureUid){} + virtual Common2dCanvasInterface* get2dCanvasInterface()=0; diff --git a/examples/CommonInterfaces/CommonRenderInterface.h b/examples/CommonInterfaces/CommonRenderInterface.h index 851db8d5f..236f666c9 100644 --- a/examples/CommonInterfaces/CommonRenderInterface.h +++ b/examples/CommonInterfaces/CommonRenderInterface.h @@ -70,6 +70,9 @@ struct CommonRenderInterface virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true)=0; virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true)=0; virtual void activateTexture(int textureIndex)=0; + virtual void replaceTexture(int shapeIndex, int textureIndex){}; + + virtual int getShapeIndexFromInstance(int srcIndex) {return -1;} virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex)=0; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index a02b5dbca..dfc259542 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -318,6 +318,18 @@ void OpenGLGuiHelper::removeGraphicsInstance(int graphicsUid) }; } +int OpenGLGuiHelper::getShapeIndexFromInstance(int instanceUid) +{ + return m_data->m_glApp->m_renderer->getShapeIndexFromInstance(instanceUid); +} + +void OpenGLGuiHelper::replaceTexture(int shapeIndex, int textureUid) +{ + if (shapeIndex>=0) + { + m_data->m_glApp->m_renderer->replaceTexture(shapeIndex, textureUid); + }; +} void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { if (instanceUid>=0) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index ccc935dcb..ade554a8a 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -29,6 +29,9 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); + virtual int getShapeIndexFromInstance(int instanceUid); + virtual void replaceTexture(int shapeIndex, int textureUid); + virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape); virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld); diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 3796bf371..e756dec66 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -392,6 +392,15 @@ GLInstancingRenderer::~GLInstancingRenderer() +int GLInstancingRenderer::getShapeIndexFromInstance(int srcIndex) +{ + b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex); + if (pg) + { + return pg->m_shapeIndex; + } + return -1; +} @@ -955,6 +964,17 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width } +void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId) +{ + if (shapeIndex >=0 && shapeIndex < m_data->m_textureHandles.size()) + { + b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex]; + if (textureId>=0) + { + gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture; + } + } +} void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY) { diff --git a/examples/OpenGLWindow/GLInstancingRenderer.h b/examples/OpenGLWindow/GLInstancingRenderer.h index a066197b5..6d0ea2819 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.h +++ b/examples/OpenGLWindow/GLInstancingRenderer.h @@ -67,7 +67,8 @@ public: virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true); virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true); virtual void activateTexture(int textureIndex); - + virtual void replaceTexture(int shapeIndex, int textureId); + virtual int getShapeIndexFromInstance(int srcIndex); ///position x,y,z, quaternion x,y,z,w, color r,g,b,a, scaling x,y,z virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index cb3e69de4..5a89751bd 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2938,6 +2938,18 @@ b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, return (b3SharedMemoryCommandHandle) command; } +int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + int uid = -1; + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); + if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED) + { + uid = status->m_loadTextureResultArguments.m_textureUniqueId; + } + return uid; +} + b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index da7bdac42..9c7271275 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -217,6 +217,9 @@ b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientH void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); +int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle); + + b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]); void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c0aff3429..55fac1dc9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -20,7 +20,7 @@ #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "Bullet3Common/b3HashMap.h" #include "../Utils/ChromeTraceUtil.h" - +#include "stb_image/stb_image.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" #include "IKTrajectoryHelper.h" #include "btBulletDynamicsCommon.h" @@ -129,7 +129,7 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw } }; -struct InteralCollisionShapeData +struct InternalCollisionShapeData { btCollisionShape* m_collisionShape; b3AlignedObjectArray m_urdfCollisionObjects; @@ -139,7 +139,7 @@ struct InteralCollisionShapeData } }; -struct InteralBodyData +struct InternalBodyData { btMultiBody* m_multiBody; btRigidBody* m_rigidBody; @@ -152,7 +152,7 @@ struct InteralBodyData b3HashMap m_audioSources; #endif //B3_ENABLE_TINY_AUDIO - InteralBodyData() + InternalBodyData() { clear(); } @@ -183,8 +183,20 @@ struct InteralUserConstraintData } }; -typedef b3PoolBodyHandle InternalBodyHandle; -typedef b3PoolBodyHandle InternalCollisionShapeHandle; +struct InternalTextureData +{ + int m_tinyRendererTextureId; + int m_openglTextureId; + void clear() + { + m_tinyRendererTextureId = -1; + m_openglTextureId = -1; + } +}; + +typedef b3PoolBodyHandle InternalTextureHandle; +typedef b3PoolBodyHandle InternalBodyHandle; +typedef b3PoolBodyHandle InternalCollisionShapeHandle; class btCommandChunk { @@ -1145,6 +1157,7 @@ struct ContactPointsStateLogger : public InternalStateLogger struct PhysicsServerCommandProcessorInternalData { ///handle management + b3ResizablePool< InternalTextureHandle > m_textureHandles; b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; @@ -1269,7 +1282,7 @@ struct PhysicsServerCommandProcessorInternalData int handle = allocHandle(); bla.push_back(handle); InternalBodyHandle* body = getHandle(handle); - InteralBodyData* body2 = body; + InternalBodyData* body2 = body; } for (int i=0;i0)) { int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body) { if (body->m_multiBody) @@ -3256,7 +3269,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int i=0;im_bodyHandles.getHandle(usedHandle); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle); if (body && (body->m_multiBody || body->m_rigidBody)) { serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; @@ -3345,7 +3358,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { { int bodyUniqueId = sd.m_bodyUniqueIds[i]; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body) { if (body->m_multiBody) @@ -3959,7 +3972,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); } } @@ -4024,7 +4037,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); hasStatus = true; @@ -4096,7 +4109,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Processed CMD_CREATE_SENSOR"); } int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; @@ -4216,7 +4229,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { @@ -4406,7 +4419,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; hasStatus=true; int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) @@ -4507,7 +4520,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Sending the actual state (Q,U)"); } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) @@ -4824,7 +4837,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAssert(bodyUniqueId >= 0); btAssert(linkIndex >= -1); - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { @@ -5010,7 +5023,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { SharedMemoryStatus& serverCmd = serverStatusOut; @@ -5151,7 +5164,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Server Init Pose not implemented yet"); } int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); btVector3 baseLinVel(0, 0, 0); btVector3 baseAngVel(0, 0, 0); @@ -5745,7 +5758,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (bodyUniqueIdA >= 0) { - InteralBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); + InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); if (bodyA) { if (bodyA->m_multiBody) @@ -5779,7 +5792,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (bodyUniqueIdB>=0) { - InteralBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); + InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); if (bodyB) { if (bodyB->m_multiBody) @@ -6048,7 +6061,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) { - InteralBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); if (body && body->m_multiBody) @@ -6254,12 +6267,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) { btScalar defaultMaxForce = 500.0; - InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); + InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); if (parentBody && parentBody->m_multiBody) { if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) { - InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; //also create a constraint with just a single multibody/rigid body without child //if (childBody) { @@ -6413,7 +6426,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; if (parentBody && childBody) { @@ -6762,12 +6775,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE"); SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - + InternalTextureHandle* texHandle = 0; + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) { + texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) { - m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + if (texHandle) + { + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); + } } } @@ -6784,10 +6803,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (bodyHandle->m_multiBody->getBaseCollider()) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) @@ -6802,10 +6829,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) @@ -6828,25 +6863,59 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } - case CMD_LOAD_TEXTURE: - { + case CMD_LOAD_TEXTURE: + { BT_PROFILE("CMD_LOAD_TEXTURE"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - - int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); - - if (uid>=0) - { - serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; - } else - { - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - } - hasStatus = true; - - break; - } + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + + char relativeFileName[1024]; + char pathPrefix[1024]; + + if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024)) + { + b3FileUtils::extractPath(relativeFileName,pathPrefix,1024); + + int texHandle = m_data->m_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = -1; + texH->m_openglTextureId = -1; + + int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); + if(uid>=0) + { + int m_tinyRendererTextureId; + texH->m_tinyRendererTextureId = uid; + } + + { + int width,height,n; + unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3); + + if(imageData) + { + texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height); + free(imageData); + } + else + { + b3Warning("Unsupported texture image format [%s]\n",relativeFileName); + } + } + serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle; + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + } + } + else + { + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + } + hasStatus = true; + + break; + } case CMD_LOAD_BULLET: { @@ -7041,7 +7110,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm 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->m_bodyHandles.getHandle(bodyUniqueId); + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body) { btCollisionObject* destColObj = 0; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 294381583..0a28b9c31 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -131,6 +131,8 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperChangeGraphicsInstanceRGBAColor, eGUIHelperChangeGraphicsInstanceSpecularColor, eGUIHelperSetVisualizerFlag, + eGUIHelperChangeGraphicsInstanceTextureId, + eGUIHelperGetShapeIndexFromInstance, }; @@ -932,6 +934,29 @@ public: m_cs->setSharedParam(1,eGUIHelperRemoveGraphicsInstance); workerThreadWait(); } + + int m_getShapeIndex_instance; + int getShapeIndex_shapeIndex; + + virtual int getShapeIndexFromInstance(int instance) + { + m_getShapeIndex_instance = instance; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperGetShapeIndexFromInstance); + workerThreadWait(); + return getShapeIndex_shapeIndex; + } + + int m_graphicsInstanceChangeTextureId; + int m_graphicsInstanceChangeTextureShapeIndex; + virtual void replaceTexture(int shapeIndex, int textureUid) + { + m_graphicsInstanceChangeTextureShapeIndex = shapeIndex; + m_graphicsInstanceChangeTextureId = textureUid; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceTextureId); + workerThreadWait(); + } double m_rgbaColor[4]; int m_graphicsInstanceChangeColor; @@ -1913,6 +1938,24 @@ void PhysicsServerExample::updateGraphics() break; } + case eGUIHelperGetShapeIndexFromInstance: + { + m_multiThreadedHelper->getShapeIndex_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->getShapeIndexFromInstance(m_multiThreadedHelper->m_getShapeIndex_instance); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + + case eGUIHelperChangeGraphicsInstanceTextureId: + { + m_multiThreadedHelper->m_childGuiHelper->replaceTexture( + m_multiThreadedHelper->m_graphicsInstanceChangeTextureShapeIndex, + m_multiThreadedHelper->m_graphicsInstanceChangeTextureId); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + + + case eGUIHelperChangeGraphicsInstanceRGBAColor: { m_multiThreadedHelper->m_childGuiHelper->changeRGBAColor(m_multiThreadedHelper->m_graphicsInstanceChangeColor,m_multiThreadedHelper->m_rgbaColor); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 103dd5f96..4b9fe12b9 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -283,6 +283,11 @@ struct LoadTextureArgs char m_textureFileName[MAX_FILENAME_LENGTH]; }; +struct b3LoadTextureResultArgs +{ + int m_textureUniqueId; +}; + struct SendVisualShapeDataArgs { int m_bodyUniqueId; @@ -1015,6 +1020,7 @@ struct SharedMemoryStatus struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs; struct SendMouseEvents m_sendMouseEvents; + struct b3LoadTextureResultArgs m_loadTextureResultArguments; }; }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index e70e08e1f..f804dcb3f 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -1070,12 +1070,15 @@ void TinyRendererVisualShapeConverter::resetAll() void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) { btAssert(textureUniqueId < m_data->m_textures.size()); - TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId); - if (ptrptr && *ptrptr) - { - TinyRendererObjectArray* ptr = *ptrptr; - ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height); - } + if (textureUniqueId>=0 && textureUniqueIdm_textures.size()) + { + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId); + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height); + } + } } void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) @@ -1085,18 +1088,26 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, { if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex) { - start = i; - break; - } - } - - if (start >= 0) - { - if (start + shapeIndex < m_data->m_visualShapes.size()) - { - activateShapeTexture(start + shapeIndex, textureUniqueId); + if (shapeIndex<0) + { + activateShapeTexture(i, textureUniqueId); + } else + { + start = i; + break; + } } } + if (shapeIndex>=0) + { + if (start >= 0) + { + if (start + shapeIndex < m_data->m_visualShapes.size()) + { + activateShapeTexture(start + shapeIndex, textureUniqueId); + } + } + } } int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 882de968c..08aee99e3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4578,6 +4578,9 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* statusType = b3GetStatusType(statusHandle); if (statusType == CMD_LOAD_TEXTURE_COMPLETED) { + PyObject* item; + item = PyInt_FromLong(b3GetStatusTextureUniqueId(statusHandle)); + return item; } else { @@ -4586,8 +4589,8 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* } } - Py_INCREF(Py_None); - return Py_None; + PyErr_SetString(SpamError, "Error loading texture"); + return NULL; } static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr) diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 3e8b75b7d..a0a062750 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -389,28 +389,38 @@ protected: const Value* getAtIndex(int index) const { btAssert(index < m_valueArray.size()); - - return &m_valueArray[index]; + btAssert(index>=0); + if (index>=0 && index < m_valueArray.size()) + { + return &m_valueArray[index]; + } + return 0; } Value* getAtIndex(int index) { btAssert(index < m_valueArray.size()); - - return &m_valueArray[index]; + btAssert(index>=0); + if (index>=0 && index < m_valueArray.size()) + { + return &m_valueArray[index]; + } + return 0; } Key getKeyAtIndex(int index) { btAssert(index < m_keyArray.size()); - return m_keyArray[index]; + btAssert(index>=0); + return m_keyArray[index]; } const Key getKeyAtIndex(int index) const { btAssert(index < m_keyArray.size()); - return m_keyArray[index]; - } + btAssert(index>=0); + return m_keyArray[index]; + } Value* operator[](const Key& key) {