diff --git a/data/tex256.png b/data/tex256.png new file mode 100644 index 000000000..130d4aade Binary files /dev/null and b/data/tex256.png differ diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 233f3002a..cbd52aa6f 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -40,7 +40,11 @@ struct GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} + virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height){} + 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..49b717b5a 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -292,6 +292,12 @@ int OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int return textureId; } +void OpenGLGuiHelper::changeTexture(int textureUniqueId, const unsigned char* texels, int width, int height) +{ + bool flipPixelsY = true; + m_data->m_glApp->m_renderer->updateTexture(textureUniqueId, texels,flipPixelsY); +} + int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) { @@ -318,6 +324,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..52a522e0e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -28,6 +28,10 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); + virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height); + + virtual int getShapeIndexFromInstance(int instanceUid); + virtual void replaceTexture(int shapeIndex, int textureUid); virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 817536916..33fa0d49b 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -156,7 +156,8 @@ struct UrdfLink UrdfLink() :m_parentLink(0), - m_parentJoint(0) + m_parentJoint(0), + m_linkIndex(-2) { } diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 3796bf371..92a11f417 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -117,13 +117,14 @@ GLint lineWidthRange[2]={1,1}; enum { - eGfxTransparency=1 + eGfxTransparency=1, }; + struct b3GraphicsInstance { GLuint m_cube_vao; GLuint m_index_vbo; - GLuint m_texturehandle; + GLuint m_textureIndex; int m_numIndices; int m_numVertices; @@ -141,7 +142,7 @@ struct b3GraphicsInstance b3GraphicsInstance() :m_cube_vao(-1), m_index_vbo(-1), - m_texturehandle(0), + m_textureIndex(-1), m_numIndices(-1), m_numVertices(-1), m_numGraphicsInstances(0), @@ -188,6 +189,7 @@ struct InternalTextureHandle GLuint m_glTexture; int m_width; int m_height; + int m_enableFiltering; }; struct b3PublicGraphicsInstanceData @@ -392,6 +394,15 @@ GLInstancingRenderer::~GLInstancingRenderer() +int GLInstancingRenderer::getShapeIndexFromInstance(int srcIndex) +{ + b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex); + if (pg) + { + return pg->m_shapeIndex; + } + return -1; +} @@ -945,7 +956,7 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width h.m_glTexture = textureHandle; h.m_width = width; h.m_height = height; - + h.m_enableFiltering = true; m_data->m_textureHandles.push_back(h); if (texels) { @@ -955,14 +966,23 @@ 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_textureIndex = textureId; + + } + } +} void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY) { if (textureIndex>=0) { - - - glActiveTexture(GL_TEXTURE0); b3Assert(glGetError() ==GL_NO_ERROR); InternalTextureHandle& h = m_data->m_textureHandles[textureIndex]; @@ -991,7 +1011,10 @@ void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&texels[0]); } b3Assert(glGetError() ==GL_NO_ERROR); - glGenerateMipmap(GL_TEXTURE_2D); + if (h.m_enableFiltering) + { + glGenerateMipmap(GL_TEXTURE_2D); + } b3Assert(glGetError() ==GL_NO_ERROR); } } @@ -1028,7 +1051,7 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices, if (textureId>=0) { - gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture; + gfxObj->m_textureIndex = textureId; } gfxObj->m_primitiveType = primitiveType; @@ -2181,10 +2204,26 @@ b3Assert(glGetError() ==GL_NO_ERROR); { glActiveTexture(GL_TEXTURE0); GLuint curBindTexture = 0; - if (gfxObj->m_texturehandle) - curBindTexture = gfxObj->m_texturehandle; + if (gfxObj->m_textureIndex>=0) + { + curBindTexture = m_data->m_textureHandles[gfxObj->m_textureIndex].m_glTexture; + + glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_BASE_LEVEL,0); + + if (m_data->m_textureHandles[gfxObj->m_textureIndex].m_enableFiltering) + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } else + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + } + } else + { curBindTexture = m_data->m_defaultTexturehandle; + } //disable lazy evaluation, it just leads to bugs //if (lastBindTexture != curBindTexture) 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..22ce287a0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2918,6 +2918,25 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu } } +b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CHANGE_TEXTURE; + + command->m_changeTextureArgs.m_textureUniqueId = textureUniqueId; + command->m_changeTextureArgs.m_width = width; + command->m_changeTextureArgs.m_height = height; + int numPixels = width*height; + cl->uploadBulletFileToSharedMemory(rgbPixels,numPixels*3); + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} + + b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -2938,6 +2957,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..a0ce40f7b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -217,6 +217,10 @@ b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientH void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); +int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels); + 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/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 0877b1d0a..12585e13b 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1368,7 +1368,9 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } else { for (int i = 0; i < len; i++) { - m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + //m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; + m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i] = data[i]; + } } } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index afbf92f0a..d26ff70da 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1095,6 +1095,14 @@ void PhysicsDirect::setSharedMemoryKey(int key) void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) { + if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i=0;im_bulletStreamDataServerToClient[i] = data[i]; + } //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1c07479cc..2719f2788 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,16 +129,17 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw } }; -struct InteralCollisionShapeData +struct InternalCollisionShapeData { btCollisionShape* m_collisionShape; + b3AlignedObjectArray m_urdfCollisionObjects; void clear() { m_collisionShape=0; } }; -struct InteralBodyData +struct InternalBodyData { btMultiBody* m_multiBody; btRigidBody* m_rigidBody; @@ -151,7 +152,7 @@ struct InteralBodyData b3HashMap m_audioSources; #endif //B3_ENABLE_TINY_AUDIO - InteralBodyData() + InternalBodyData() { clear(); } @@ -182,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 { @@ -1144,6 +1157,7 @@ struct ContactPointsStateLogger : public InternalStateLogger struct PhysicsServerCommandProcessorInternalData { ///handle management + b3ResizablePool< InternalTextureHandle > m_textureHandles; b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; @@ -1268,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;im_customVisualShapesConverter) + //if there is a visual, use it, otherwise convert collision shape back into UrdfCollision... + + UrdfModel model;// = m_data->m_urdfParser.getModel(); + UrdfLink link; + int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex]; + if (colShapeUniqueId>=0) + { + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); + if (handle) { - const UrdfModel& model = m_data->m_urdfParser.getModel(); - UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); - if (linkPtr) + for (int i=0;im_urdfCollisionObjects.size();i++) { - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]); } } } - #endif + //UrdfVisual vis; + //link.m_visualArray.push_back(vis); + //UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); + m_data->m_visualConverter.convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, &link, &model, colObj, bodyUniqueId); } virtual void setBodyUniqueId(int bodyId) { @@ -2671,7 +2692,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) { 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) @@ -3248,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; @@ -3337,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) @@ -3588,6 +3609,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); btCollisionShape* shape = 0; + b3AlignedObjectArray urdfCollisionObjects; btCompoundShape* compound = 0; @@ -3597,6 +3619,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } for (int i=0;iaddChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE; + urdfColObj.m_geometry.m_sphereRadius = radius; break; } case GEOM_BOX: { //double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; - shape = worldImporter->createBoxShape(btVector3( + btVector3 halfExtents( clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0], clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1], - clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2])); + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]); + shape = worldImporter->createBoxShape(halfExtents); if (compound) { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_BOX; + urdfColObj.m_geometry.m_boxSize = 2.*halfExtents; break; } case GEOM_CAPSULE: @@ -3649,6 +3683,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight; + break; } case GEOM_CYLINDER: @@ -3659,6 +3697,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight; + break; } case GEOM_PLANE: @@ -3672,6 +3714,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { compound->addChildShape(childTransform,shape); } + urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE; + urdfColObj.m_geometry.m_planeNormal.setValue( + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]); + break; } case GEOM_MESH: @@ -3685,7 +3733,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm const std::string& urdf_path=""; std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName; - + urdfColObj.m_geometry.m_type = URDF_GEOM_MESH; + urdfColObj.m_geometry.m_meshFileName = fileName; + + urdfColObj.m_geometry.m_meshScale = meshScale; char relativeFileName[1024]; char pathPrefix[1024]; pathPrefix[0] = 0; @@ -3702,6 +3753,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); if (foundFile) { + urdfColObj.m_geometry.m_meshFileType = out_type; + if (out_type==UrdfGeometry::FILE_OBJ) { //create a convex hull for each shape, and store it in a btCompoundShape @@ -3718,10 +3771,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } btAlignedObjectArray convertedVerts; convertedVerts.reserve(glmesh->m_numvertices); - btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0], - clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1], - clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]); - + for (int i=0; im_numvertices; i++) { convertedVerts.push_back(btVector3( @@ -3809,7 +3859,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { } } + if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN) + { + urdfCollisionObjects.push_back(urdfColObj); + } } + + #if 0 shape = worldImporter->createCylinderShapeX(radius,height); shape = worldImporter->createCylinderShapeY(radius,height); @@ -3829,6 +3885,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); handle->m_collisionShape = shape; + for (int i=0;im_urdfCollisionObjects.push_back(urdfCollisionObjects[i]); + } serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid; m_data->m_worldImporters.push_back(worldImporter); serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; @@ -3912,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()); } } @@ -3977,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; @@ -4049,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; @@ -4169,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) { @@ -4359,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) @@ -4460,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) @@ -4777,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) { @@ -4963,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; @@ -5104,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); @@ -5698,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) @@ -5732,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) @@ -6001,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) @@ -6207,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) { @@ -6366,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) { @@ -6715,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); + } } } @@ -6737,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) @@ -6755,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) @@ -6781,25 +6863,77 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } - 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; - } + + case CMD_CHANGE_TEXTURE: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED; + + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId); + if(texH) + { + int gltex = texH->m_openglTextureId; + m_data->m_guiHelper->changeTexture(gltex, + (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height); + + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + } hasStatus = true; - break; - } + } + case CMD_LOAD_TEXTURE: + { + BT_PROFILE("CMD_LOAD_TEXTURE"); + 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: { @@ -6994,7 +7128,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..80968c880 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -131,6 +131,9 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperChangeGraphicsInstanceRGBAColor, eGUIHelperChangeGraphicsInstanceSpecularColor, eGUIHelperSetVisualizerFlag, + eGUIHelperChangeGraphicsInstanceTextureId, + eGUIHelperGetShapeIndexFromInstance, + eGUIHelperChangeTexture, }; @@ -932,6 +935,46 @@ 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(); + } + + + int m_changeTextureUniqueId; + const unsigned char* m_changeTextureRgbTexels; + int m_changeTextureWidth; + int m_changeTextureHeight; + + virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) + { + m_changeTextureUniqueId = textureUniqueId; + m_changeTextureRgbTexels = rgbTexels; + m_changeTextureWidth = width; + m_changeTextureHeight = height; + m_cs->lock(); + m_cs->setSharedParam(1,eGUIHelperChangeTexture); + workerThreadWait(); + } double m_rgbaColor[4]; int m_graphicsInstanceChangeColor; @@ -1913,6 +1956,34 @@ 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 eGUIHelperChangeTexture: + { + m_multiThreadedHelper->m_childGuiHelper->changeTexture( + m_multiThreadedHelper->m_changeTextureUniqueId, + m_multiThreadedHelper->m_changeTextureRgbTexels, + m_multiThreadedHelper->m_changeTextureWidth, + m_multiThreadedHelper->m_changeTextureHeight); + 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..a5db4cd71 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; @@ -893,6 +898,12 @@ struct b3CreateMultiBodyResultArgs int m_bodyUniqueId; }; +struct b3ChangeTextureArgs +{ + int m_textureUniqueId; + int m_width; + int m_height; +}; struct SharedMemoryCommand { @@ -945,6 +956,7 @@ struct SharedMemoryCommand struct b3CreateVisualShapeArgs m_createVisualShapeArgs; struct b3CreateMultiBodyArgs m_createMultiBodyArgs; struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs; + struct b3ChangeTextureArgs m_changeTextureArgs; }; }; @@ -1015,6 +1027,7 @@ struct SharedMemoryStatus struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs; struct SendMouseEvents m_sendMouseEvents; + struct b3LoadTextureResultArgs m_loadTextureResultArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index eaf3de675..05875d9b5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -67,6 +67,7 @@ enum EnumSharedMemoryClientCommand CMD_CREATE_MULTI_BODY, CMD_REQUEST_COLLISION_INFO, CMD_REQUEST_MOUSE_EVENTS_DATA, + CMD_CHANGE_TEXTURE, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -161,6 +162,7 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_COLLISION_INFO_COMPLETED, CMD_REQUEST_COLLISION_INFO_FAILED, CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, + CMD_CHANGE_TEXTURE_COMMAND_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index f18becf58..f804dcb3f 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -506,6 +506,15 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi } +static btVector4 sColors[4] = +{ + btVector4(60./256.,186./256.,84./256.,1), + btVector4(244./256.,194./256.,13./256.,1), + btVector4(219./256.,50./256.,54./256.,1), + btVector4(72./256.,133./256.,237./256.,1), + + //btVector4(1,1,0,1), +}; void TinyRendererVisualShapeConverter::convertVisualShapes( @@ -546,7 +555,17 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } btTransform childTrans = vis->m_linkLocalFrame; - float rgbaColor[4] = {1,1,1,1}; + + + int colorIndex = colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0; + + btVector4 color; + color = sColors[colorIndex]; + float rgbaColor[4] = {color[0],color[1],color[2],color[3]}; + if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) + { + color.setValue(1,1,1,1); + } if (model && useVisual) { btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str()); @@ -1051,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) @@ -1066,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/examples/changeTexture.py b/examples/pybullet/examples/changeTexture.py new file mode 100644 index 000000000..c9e592f88 --- /dev/null +++ b/examples/pybullet/examples/changeTexture.py @@ -0,0 +1,43 @@ +import pybullet as p +import time +p.connect(p.GUI) +planeUidA = p.loadURDF("plane_transparent.urdf",[0,0,0]) +planeUid = p.loadURDF("plane_transparent.urdf",[0,0,-1]) + +texUid = p.loadTexture("tex256.png") + +p.changeVisualShape(planeUidA,-1,rgbaColor=[1,1,1,0.5]) +p.changeVisualShape(planeUid,-1,rgbaColor=[1,1,1,0.5]) +p.changeVisualShape(planeUid,-1, textureUniqueId = texUid) + +width = 256 +height = 256 +pixels = [255]*width*height*3 +colorR = 0 +colorG = 0 +colorB = 0 + + +#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + +blue=0 +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json") +for i in range (100000): + p.stepSimulation() + for i in range (width): + for j in range(height): + pixels[(i+j*width)*3+0]=i + pixels[(i+j*width)*3+1]=(j+blue)%256 + pixels[(i+j*width)*3+2]=blue + blue=blue+1 + p.changeTexture(texUid, pixels,width,height) + start = time.time() + p.getCameraImage(300,300,renderer=p.ER_BULLET_HARDWARE_OPENGL) + end = time.time() + print("rendering duraction") + print(end-start) +p.stopStateLogging(logId) +#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +#p.configureDebugVisualizer(p.COV_ENABLE_GUI,1) + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 882de968c..4cf3eb6ac 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1433,7 +1433,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar int i; for (i = 0; i < numControlledDofs; i++) { - int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i); + int jointIndex = pybullet_internalGetIntFromSequence(jointIndicesSeq, i); if ((jointIndex >= numJoints) || (jointIndex < 0)) { Py_DECREF(jointIndicesSeq); @@ -4551,6 +4551,75 @@ static PyObject* pybullet_changeVisualShape(PyObject* self, PyObject* args, PyOb return Py_None; } + +static PyObject* pybullet_changeTexture(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3SharedMemoryCommandHandle commandHandle = 0; + b3SharedMemoryStatusHandle statusHandle=0; + int statusType = -1; + int textureUniqueId = -1; + int physicsClientId = 0; + int width=-1; + int height=-1; + + PyObject* pixelsObj = 0; + + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"textureUniqueId", "pixels", "width", "height", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOii|i", kwlist, &textureUniqueId, &pixelsObj, &width, &height, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (textureUniqueId>=0 && width>=0 && height>=0 && pixelsObj) + { + PyObject* seqPixels = PySequence_Fast(pixelsObj, "expected a sequence"); + PyObject* item; + int i; + int numPixels = width*height; + unsigned char* pixelBuffer = (char*) malloc (numPixels*3); + if (PyList_Check(seqPixels)) + { + for (i=0;i=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) {