From df89ce6f924a3e3c327bb72da410b300447c09aa Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 16 Jan 2018 17:58:19 -0800 Subject: [PATCH] small refactor of LinkVisualShapesConverter / TinyRendererVisualShapeConverter, it allows to use it in a Python module and in a PyBullet plugin. --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 4 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 2 +- .../LinkVisualShapesConverter.h | 41 ++- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 247 +++++++++++------- .../TinyRendererVisualShapeConverter.cpp | 67 +++-- .../TinyRendererVisualShapeConverter.h | 50 ++-- 7 files changed, 260 insertions(+), 155 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index daa3a0c59..e95a6c112 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -21,7 +21,7 @@ #include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include "../ImportMeshUtility/b3ImportMeshUtility.h" - +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" @@ -2277,7 +2277,7 @@ void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, if (m_data->m_customVisualShapesConverter) { const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex); } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 0c50643ab..9189c268e 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1248,7 +1248,7 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); if (linkPtr) { - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId); } } } diff --git a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h index edc7559ba..0010964f7 100644 --- a/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h +++ b/examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h @@ -8,9 +8,46 @@ class btCollisionObject; struct LinkVisualShapesConverter { - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0; + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex) =0; - virtual void removeVisualShape(class btCollisionObject* colObj)=0; + virtual void removeVisualShape(int shapeUid)=0; + + virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling)=0; + + virtual int getNumVisualShapes(int bodyUniqueId)=0; + + virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData)=0; + + virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, const double rgbaColor[4])=0; + + virtual void setUpAxis(int axis)=0; + + virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ)=0; + + virtual void clearBuffers(struct TGAColor& clearColor)=0; + + virtual void resetAll()=0; + + virtual void getWidthAndHeight(int& width, int& height)=0; + virtual void setWidthAndHeight(int width, int height)=0; + virtual void setLightDirection(float x, float y, float z)=0; + virtual void setLightColor(float x, float y, float z)=0; + virtual void setLightDistance(float dist)=0; + virtual void setLightAmbientCoeff(float ambientCoeff)=0; + virtual void setLightDiffuseCoeff(float diffuseCoeff)=0; + virtual void setLightSpecularCoeff(float specularCoeff)=0; + virtual void setShadow(bool hasShadow)=0; + virtual void setFlags(int flags)=0; + + virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)=0; + + virtual void render()=0; + virtual void render(const float viewMat[16], const float projMat[16])=0; + + virtual int loadTextureFile(const char* filename)=0; + virtual int registerTexture(unsigned char* texels, int width, int height)=0; + + virtual void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)=0; }; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 5382682a2..25bae7c86 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -566,7 +566,9 @@ void ConvertURDF2BulletInternal( } } else { - //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape); + int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex); + //u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId()); + u2b.convertLinkVisualShapes2(-1,urdfLinkIndex,pathPrefix,localInertialFrame,linkRigidBody,u2b.getBodyUniqueId()); } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3ca3a8a54..60b98da82 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1564,7 +1564,8 @@ struct PhysicsServerCommandProcessorInternalData btVector3 m_hitPos; btScalar m_oldPickingDist; bool m_prevCanSleep; - TinyRendererVisualShapeConverter m_visualConverter; + TinyRendererVisualShapeConverter m_tempVisualConverter;//we may move this into a plugin + LinkVisualShapesConverter* m_visualConverterPtr; #ifdef B3_ENABLE_TINY_AUDIO b3SoundEngine m_soundEngine; #endif @@ -1598,7 +1599,7 @@ struct PhysicsServerCommandProcessorInternalData m_pickingMultiBodyPoint2Point(0) { - + m_visualConverterPtr=&m_tempVisualConverter; { //register static plugins: @@ -2120,7 +2121,10 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface //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); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, &link, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId); + } } virtual void setBodyUniqueId(int bodyId) { @@ -2704,7 +2708,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS m_data->m_sdfRecentLoadedBodies.clear(); - BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, flags); + BulletMJCFImporter u2b(m_data->m_guiHelper, m_data->m_visualConverterPtr, flags); bool useFixedBase = false; MyMJCFLogger2 logger; @@ -2727,7 +2731,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe m_data->m_sdfRecentLoadedBodies.clear(); - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags); + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_visualConverterPtr, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); bool forceFixedBase = false; @@ -2759,7 +2763,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags); + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_visualConverterPtr, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); bool loadOk = u2b.loadURDF(fileName, useFixedBase); @@ -3076,15 +3080,21 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) { - m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); + } } int flags = 0; if (clientCmd.m_updateFlags&REQUEST_PIXEL_ARGS_HAS_FLAGS) { flags = clientCmd.m_requestPixelDataArguments.m_flags; } - m_data->m_visualConverter.setFlags(flags); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->setFlags(flags); + } int numTotalPixels = width*height; int numRemainingPixels = numTotalPixels - startPixelIndex; @@ -3158,82 +3168,92 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc } if (!handled) { - - if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) + if (m_data->m_visualConverterPtr) { - // printf("-------------------------------\nRendering\n"); + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) + { + // printf("-------------------------------\nRendering\n"); - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) - { - m_data->m_visualConverter.setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) - { - m_data->m_visualConverter.setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) - { - m_data->m_visualConverter.setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) - { - m_data->m_visualConverter.setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow!=0)); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) - { - m_data->m_visualConverter.setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) - { - m_data->m_visualConverter.setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) - { - m_data->m_visualConverter.setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) - { - m_data->m_visualConverter.render( - clientCmd.m_requestPixelDataArguments.m_viewMatrix, - clientCmd.m_requestPixelDataArguments.m_projectionMatrix); - } else - { - b3OpenGLVisualizerCameraInfo tmpCamResult; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &tmpCamResult.m_width, - &tmpCamResult.m_height, - tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix, - tmpCamResult.m_camUp, - tmpCamResult.m_camForward, - tmpCamResult.m_horizontal, - tmpCamResult.m_vertical, - &tmpCamResult.m_yaw, - &tmpCamResult.m_pitch, - &tmpCamResult.m_dist, - tmpCamResult.m_target); - if (result) + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) { - m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix); + m_data->m_visualConverterPtr->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) + { + m_data->m_visualConverterPtr->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) + { + m_data->m_visualConverterPtr->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) + { + m_data->m_visualConverterPtr->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow!=0)); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) + { + m_data->m_visualConverterPtr->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) + { + m_data->m_visualConverterPtr->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) + { + m_data->m_visualConverterPtr->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); + } + + for (int i=0;im_dynamicsWorld->getNumCollisionObjects();i++) + { + const btCollisionObject* colObj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; + m_data->m_visualConverterPtr->syncTransform(colObj->getBroadphaseHandle()->getUid(),colObj->getWorldTransform(),colObj->getCollisionShape()->getLocalScaling()); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) + { + m_data->m_visualConverterPtr->render( + clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix); } else { - m_data->m_visualConverter.render(); + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + m_data->m_visualConverterPtr->render(tmpCamResult.m_viewMatrix,tmpCamResult.m_projectionMatrix); + } else + { + m_data->m_visualConverterPtr->render(); + } } } } - m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->copyCameraImageData(pixelRGBA,numRequestedPixels, depthBuffer,numRequestedPixels, segmentationMaskBuffer, numRequestedPixels, startPixelIndex,&width,&height,&numPixelsCopied); + } m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels, @@ -3869,7 +3889,7 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; double globalScaling = 1.f; int flags=0; - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags); + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_visualConverterPtr, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); btTransform localInertiaFrame; localInertiaFrame.setIdentity(); @@ -7430,7 +7450,10 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared if (bodyHandle->m_multiBody->getBaseCollider()) { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()->getBroadphaseHandle()->getUid()); + } m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); @@ -7440,7 +7463,10 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared if (bodyHandle->m_multiBody->getLink(link).m_collider) { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider->getBroadphaseHandle()->getUid()); + } m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); @@ -7457,7 +7483,10 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared } if (bodyHandle->m_rigidBody) { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_rigidBody); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->removeVisualShape(bodyHandle->m_rigidBody->getBroadphaseHandle()->getUid()); + } serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; if (m_data->m_pickedConstraint && m_data->m_pickedBody==bodyHandle->m_rigidBody) @@ -8372,24 +8401,27 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED; //retrieve the visual shape information for a specific body - int totalNumVisualShapes = m_data->m_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); - //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); - //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; - b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; + if (m_data->m_visualConverterPtr) + { + int totalNumVisualShapes = m_data->m_visualConverterPtr->getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); + //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); + //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; + b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; - int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, - shapeIndex, - visualShapeStoragePtr); - if (success) { - serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; - serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; - serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; - serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; + int success = m_data->m_visualConverterPtr->getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, + shapeIndex, + visualShapeStoragePtr); + if (success) { + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; + serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; + serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; + } } return hasStatus; } @@ -8412,7 +8444,10 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct { 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); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); + } } } } @@ -8441,7 +8476,10 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->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) @@ -8467,7 +8505,10 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->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) @@ -8493,7 +8534,10 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct } if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->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) @@ -8550,7 +8594,11 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share texH->m_tinyRendererTextureId = -1; texH->m_openglTextureId = -1; - int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); + int uid = -1; + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->loadTextureFile(relativeFileName); + } if(uid>=0) { texH->m_tinyRendererTextureId = uid; @@ -9519,7 +9567,10 @@ void PhysicsServerCommandProcessor::resetSimulation() } if (m_data) { - m_data->m_visualConverter.resetAll(); + if (m_data->m_visualConverterPtr) + { + m_data->m_visualConverterPtr->resetAll(); + } for (int i = 0; i < m_data->m_savedStates.size(); i++) { delete m_data->m_savedStates[i].m_bulletFile; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index aa7756416..e5c595f3c 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -49,6 +49,14 @@ struct TinyRendererObjectArray btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects; int m_objectUniqueId; int m_linkIndex; + btTransform m_worldTransform; + btVector3 m_localScaling; + + TinyRendererObjectArray() + { + m_worldTransform.setIdentity(); + m_localScaling.setValue(1,1,1); + } }; #define START_WIDTH 640 @@ -57,7 +65,7 @@ struct TinyRendererObjectArray struct TinyRendererVisualShapeConverterInternalData { - btHashMap m_swRenderInstances; + btHashMap m_swRenderInstances; btAlignedObjectArray m_visualShapes; @@ -542,7 +550,7 @@ static btVector4 sColors[4] = void TinyRendererVisualShapeConverter::convertVisualShapes( int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, - class btCollisionObject* colObj, int bodyUniqueId) + int shapeUid, int bodyUniqueId) { btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines) if (linkPtr) @@ -579,15 +587,16 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( - int colorIndex = colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0; - + int colorIndex = linkIndex;//colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0; + if (colorIndex<0) + colorIndex=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 (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE) + //{ + // color.setValue(1,1,1,1); + //} if (model) { if (useVisual) @@ -618,12 +627,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } } - TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj]; + TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[shapeUid]; if (visualsPtr==0) { - m_data->m_swRenderInstances.insert(colObj,new TinyRendererObjectArray); + m_data->m_swRenderInstances.insert(shapeUid,new TinyRendererObjectArray); } - visualsPtr = m_data->m_swRenderInstances[colObj]; + visualsPtr = m_data->m_swRenderInstances[shapeUid]; btAssert(visualsPtr); TinyRendererObjectArray* visuals = *visualsPtr; @@ -822,6 +831,19 @@ void TinyRendererVisualShapeConverter::render() render(viewMat,projMat); } + +void TinyRendererVisualShapeConverter::syncTransform(int shapeUid, const btTransform& worldTransform, const btVector3& localScaling) +{ + TinyRendererObjectArray** renderObjPtr = m_data->m_swRenderInstances[shapeUid]; + if (renderObjPtr) + { + TinyRendererObjectArray* renderObj = *renderObjPtr; + renderObj->m_worldTransform = worldTransform; + renderObj->m_localScaling = localScaling; + } +} + + void TinyRendererVisualShapeConverter::render(const float viewMat[16], const float projMat[16]) { //clear the color buffer @@ -902,11 +924,6 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo continue;//can this ever happen? TinyRendererObjectArray* visualArray = *visualArrayPtr; - btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n); - - - const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer(); - for (int v=0;vm_renderObjects.size();v++) { @@ -914,7 +931,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo //sync the object transform - const btTransform& tr = colObj->getWorldTransform(); + const btTransform& tr = visualArray->m_worldTransform; tr.getOpenGLMatrix(modelMat); for (int i=0;i<4;i++) @@ -927,7 +944,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; } } - renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling(); + renderObj->m_localScaling = visualArray->m_localScaling; renderObj->m_lightDirWorld = lightDirWorld; renderObj->m_lightColor = lightColor; renderObj->m_lightDistance = lightDistance; @@ -946,10 +963,6 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo continue;//can this ever happen? TinyRendererObjectArray* visualArray = *visualArrayPtr; - btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(n); - - - const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer(); for (int v=0;vm_renderObjects.size();v++) { @@ -958,7 +971,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo //sync the object transform - const btTransform& tr = colObj->getWorldTransform(); + const btTransform& tr = visualArray->m_worldTransform; tr.getOpenGLMatrix(modelMat); for (int i=0;i<4;i++) @@ -971,7 +984,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; } } - renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling(); + renderObj->m_localScaling = visualArray->m_localScaling; renderObj->m_lightDirWorld = lightDirWorld; renderObj->m_lightColor = lightColor; renderObj->m_lightDistance = lightDistance; @@ -1096,9 +1109,9 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels } } -void TinyRendererVisualShapeConverter::removeVisualShape(class btCollisionObject* colObj) +void TinyRendererVisualShapeConverter::removeVisualShape(int shapeUid) { - TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances[colObj]; + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances[shapeUid]; if (ptrptr && *ptrptr) { TinyRendererObjectArray* ptr = *ptrptr; @@ -1110,7 +1123,7 @@ void TinyRendererVisualShapeConverter::removeVisualShape(class btCollisionObject } } delete ptr; - m_data->m_swRenderInstances.remove(colObj); + m_data->m_swRenderInstances.remove(shapeUid); } } diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 4c5abbfbb..df70ea0a3 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -16,45 +16,47 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter virtual ~TinyRendererVisualShapeConverter(); - virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex); - + virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int shapeUid, int objectIndex); + virtual int getNumVisualShapes(int bodyUniqueId); virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData); virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, const double rgbaColor[4]); - virtual void removeVisualShape(class btCollisionObject* colObj); + virtual void removeVisualShape(int shapeUid); - void setUpAxis(int axis); + virtual void setUpAxis(int axis); - void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ); + virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ); - void clearBuffers(struct TGAColor& clearColor); + virtual void clearBuffers(struct TGAColor& clearColor); - void resetAll(); + virtual void resetAll(); - void getWidthAndHeight(int& width, int& height); - void setWidthAndHeight(int width, int height); - void setLightDirection(float x, float y, float z); - void setLightColor(float x, float y, float z); - void setLightDistance(float dist); - void setLightAmbientCoeff(float ambientCoeff); - void setLightDiffuseCoeff(float diffuseCoeff); - void setLightSpecularCoeff(float specularCoeff); - void setShadow(bool hasShadow); - void setFlags(int flags); + virtual void getWidthAndHeight(int& width, int& height); + virtual void setWidthAndHeight(int width, int height); + virtual void setLightDirection(float x, float y, float z); + virtual void setLightColor(float x, float y, float z); + virtual void setLightDistance(float dist); + virtual void setLightAmbientCoeff(float ambientCoeff); + virtual void setLightDiffuseCoeff(float diffuseCoeff); + virtual void setLightSpecularCoeff(float specularCoeff); + virtual void setShadow(bool hasShadow); + virtual void setFlags(int flags); - void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); + virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); - void render(); - void render(const float viewMat[16], const float projMat[16]); + virtual void render(); + virtual void render(const float viewMat[16], const float projMat[16]); - int loadTextureFile(const char* filename); - int registerTexture(unsigned char* texels, int width, int height); + virtual int loadTextureFile(const char* filename); + virtual int registerTexture(unsigned char* texels, int width, int height); - void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); - + virtual void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); + + virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling); + };