From 6ef96a4027b665ff46826e1a9a0c583dde3e2e3f Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 20 Oct 2016 10:56:44 -0700 Subject: [PATCH 1/9] Add programmatic render API and a basic test. --- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsClientExample.cpp | 17 +++++++-- .../PhysicsClientSharedMemory.cpp | 9 +++++ .../PhysicsServerCommandProcessor.cpp | 12 +++++++ examples/SharedMemory/SharedMemoryPublic.h | 3 ++ .../TinyRendererVisualShapeConverter.cpp | 36 ++++++++++++++++++- .../TinyRendererVisualShapeConverter.h | 4 +++ examples/TinyRenderer/model.cpp | 9 +++-- 9 files changed, 110 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 813457225..f9f94c047 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1252,7 +1252,32 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu } } +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient) +{ + /* + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_UPDATE_VISUAL_SHAPE; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle) command; + */ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_UPDATE_VISUAL_SHAPE; + //command->m_requestVisualShapeDataArguments.m_bodyUniqueId = 0; + //command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2d2a4460c..80ed8ad06 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -99,6 +99,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA); void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index afb0e1fb8..be4b1fcf0 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -481,6 +481,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } break; } + case CMD_UPDATE_VISUAL_SHAPE: + { + b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } default: { b3Error("Unknown buttonId"); @@ -560,6 +566,7 @@ void PhysicsClientExample::createButtons() createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger); createButton("Get Visual Shape Info",CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger); + createButton("Update Visual Shape",CMD_UPDATE_VISUAL_SHAPE, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); if (m_options!=eCLIENTEXAMPLE_SERVER) { @@ -997,14 +1004,20 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { b3Warning("Cannot get visual shape information"); } - + if (statusType == CMD_VISUAL_SHAPE_UPDATE_FAILED) + { + b3Warning("Cannot update visual shape"); + } if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { b3VisualShapeInformation shapeInfo; b3GetVisualShapeInformation(m_physicsClientHandle, &shapeInfo); b3Printf("Num visual shapes: %d", shapeInfo.m_numVisualShapes); } - + if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) + { + b3Printf("Visual shape update completed."); + } if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { b3ContactInformation contactPointData; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 9dda6aca1..3f031384c 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -657,6 +657,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Visual Shape Info Request failed"); break; } + case CMD_VISUAL_SHAPE_UPDATE_COMPLETED: + { + break; + } + case CMD_VISUAL_SHAPE_UPDATE_FAILED: + { + b3Warning("Visual Shape Update failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 613bc0234..75a9b1334 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3001,6 +3001,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_UPDATE_VISUAL_SHAPE: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; + + m_data->m_visualConverter.activateShapeTexture(0, 0); + + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; + hasStatus = true; + + break; + } default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index c668e5fe8..1607ec451 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -35,6 +35,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_CONTACT_POINT_INFORMATION, CMD_SAVE_WORLD, CMD_REQUEST_VISUAL_SHAPE_INFO, + CMD_UPDATE_VISUAL_SHAPE, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -81,6 +82,8 @@ enum EnumSharedMemoryServerStatus CMD_SAVE_WORLD_FAILED, CMD_VISUAL_SHAPE_INFO_COMPLETED, CMD_VISUAL_SHAPE_INFO_FAILED, + CMD_VISUAL_SHAPE_UPDATE_COMPLETED, + CMD_VISUAL_SHAPE_UPDATE_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 4ef4b0efc..2fd57b31a 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -33,6 +33,7 @@ subject to the following restrictions: #include #include "../Importers/ImportURDFDemo/UrdfParser.h" #include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData +#include "../TinyRenderer/model.h" enum MyFileType @@ -68,6 +69,7 @@ struct TinyRendererVisualShapeConverterInternalData int m_swWidth; int m_swHeight; TGAImage m_rgbColorBuffer; + TGAImage m_texture; b3AlignedObjectArray m_depthBuffer; b3AlignedObjectArray m_segmentationMaskBuffer; @@ -77,7 +79,8 @@ struct TinyRendererVisualShapeConverterInternalData :m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), - m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB) + m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB), + m_texture(START_WIDTH,START_HEIGHT,TGAImage::RGB) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1); @@ -827,3 +830,34 @@ void TinyRendererVisualShapeConverter::resetAll() m_data->m_swRenderInstances.clear(); } +// Get shapeUniqueId from getVisualShapesData? +void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) +{ + /* + // Use shapeUniqueId? + int objectArrayIndex = 0; + int objectIndex = 0; + TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(objectArrayIndex); + TinyRendererObjectArray* visualArray = *visualArrayPtr; + unsigned char textureImage[3] = {10, 10, 255}; + int textureWidth = 1000; + int textureHeight = 1000; + visualArray->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); + */ + + int objectArrayIndex = 0; + int objectIndex = 0; + for (int i=0;im_swRenderInstances.size();i++) + { + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + unsigned char textureImage[3] = {255, 10, 10}; + int textureWidth = 1000; + int textureHeight = 1000; + ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); + } + } + +} diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index df490716b..86455eefb 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -37,6 +37,10 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void render(); void render(const float viewMat[16], const float projMat[16]); + + int registerTexture(unsigned char* texels, int width, int height); + void activateShapeTexture(int shapeUniqueId, int textureUniqueId); + void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); }; diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 8864d0a2f..7bbd5b357 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -57,9 +57,12 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid for (int j=0;j Date: Thu, 20 Oct 2016 14:20:09 -0700 Subject: [PATCH 2/9] Change texture for one body. --- .../SharedMemory/PhysicsClientExample.cpp | 2 +- .../TinyRendererVisualShapeConverter.cpp | 34 +++++++++++-------- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index be4b1fcf0..fdb5bbe87 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -242,7 +242,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { case CMD_LOAD_URDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model_white.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 2fd57b31a..58e164ca7 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -833,20 +833,26 @@ void TinyRendererVisualShapeConverter::resetAll() // Get shapeUniqueId from getVisualShapesData? void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) { - /* - // Use shapeUniqueId? - int objectArrayIndex = 0; - int objectIndex = 0; - TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(objectArrayIndex); - TinyRendererObjectArray* visualArray = *visualArrayPtr; - unsigned char textureImage[3] = {10, 10, 255}; - int textureWidth = 1000; - int textureHeight = 1000; - visualArray->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); - */ + // Use shapeUniqueId? + int objectArrayIndex = 1; + int objectIndex = 0; + printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(objectArrayIndex); + if (ptrptr && *ptrptr) + { + TinyRendererObjectArray* ptr = *ptrptr; + unsigned char textureImage[3] = {255, 0, 0}; + int textureWidth = 1; + int textureHeight = 1; + ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); + } + + + /* int objectArrayIndex = 0; int objectIndex = 0; + printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); for (int i=0;im_swRenderInstances.size();i++) { TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); @@ -854,10 +860,10 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, i { TinyRendererObjectArray* ptr = *ptrptr; unsigned char textureImage[3] = {255, 10, 10}; - int textureWidth = 1000; - int textureHeight = 1000; + int textureWidth = 1; + int textureHeight = 1; ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); } } - + */ } From 1c04da23dbcd91a0d91582f3bd97a5edd97a9d48 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 20 Oct 2016 21:40:44 -0700 Subject: [PATCH 3/9] Change texture with loaded texture file. --- examples/SharedMemory/PhysicsClientExample.cpp | 6 ++++++ .../TinyRendererVisualShapeConverter.cpp | 11 +++++++++-- examples/TinyRenderer/model.cpp | 12 ++++++------ 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index fdb5bbe87..dee00ed27 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -256,8 +256,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { + /* b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + */ + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "sphere_big.urdf"); + //setting the initial position, orientation and other arguments are optional + b3LoadUrdfCommandSetStartPosition(commandHandle,0.5,0.5,0.5); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 58e164ca7..73d6935a4 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -34,6 +34,7 @@ subject to the following restrictions: #include "../Importers/ImportURDFDemo/UrdfParser.h" #include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData #include "../TinyRenderer/model.h" +#include "../ThirdPartyLibs/stb_image/stb_image.h" enum MyFileType @@ -833,9 +834,14 @@ void TinyRendererVisualShapeConverter::resetAll() // Get shapeUniqueId from getVisualShapesData? void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) { + int width,height,n; + const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; + unsigned char* image=0; + + image = stbi_load(filename, &width, &height, &n, 3); // Use shapeUniqueId? - int objectArrayIndex = 1; + int objectArrayIndex = 8; int objectIndex = 0; printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(objectArrayIndex); @@ -845,7 +851,8 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, i unsigned char textureImage[3] = {255, 0, 0}; int textureWidth = 1; int textureHeight = 1; - ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); + //ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); + ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(image,width,height); } diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 7bbd5b357..f0ef25563 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -57,12 +57,12 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid for (int j=0;j Date: Thu, 20 Oct 2016 22:11:38 -0700 Subject: [PATCH 4/9] Add API to load texture file. --- .../PhysicsServerCommandProcessor.cpp | 2 + .../TinyRendererVisualShapeConverter.cpp | 54 ++++++++----------- .../TinyRendererVisualShapeConverter.h | 1 + 3 files changed, 24 insertions(+), 33 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 75a9b1334..8c275af6e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3006,6 +3006,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; + const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; + m_data->m_visualConverter.loadTextureFile(filename); m_data->m_visualConverter.activateShapeTexture(0, 0); serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 73d6935a4..471ad81f5 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -70,7 +70,9 @@ struct TinyRendererVisualShapeConverterInternalData int m_swWidth; int m_swHeight; TGAImage m_rgbColorBuffer; - TGAImage m_texture; + unsigned char* m_texture; + int m_textureWidth; + int m_textureHeight; b3AlignedObjectArray m_depthBuffer; b3AlignedObjectArray m_segmentationMaskBuffer; @@ -80,8 +82,7 @@ struct TinyRendererVisualShapeConverterInternalData :m_upAxis(2), m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), - m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB), - m_texture(START_WIDTH,START_HEIGHT,TGAImage::RGB) + m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1); @@ -834,12 +835,6 @@ void TinyRendererVisualShapeConverter::resetAll() // Get shapeUniqueId from getVisualShapesData? void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) { - int width,height,n; - const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; - unsigned char* image=0; - - image = stbi_load(filename, &width, &height, &n, 3); - // Use shapeUniqueId? int objectArrayIndex = 8; int objectIndex = 0; @@ -848,29 +843,22 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, i if (ptrptr && *ptrptr) { TinyRendererObjectArray* ptr = *ptrptr; - unsigned char textureImage[3] = {255, 0, 0}; - int textureWidth = 1; - int textureHeight = 1; - //ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); - ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(image,width,height); + ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(m_data->m_texture,m_data->m_textureWidth,m_data->m_textureHeight); } - - - /* - int objectArrayIndex = 0; - int objectIndex = 0; - printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); - for (int i=0;im_swRenderInstances.size();i++) - { - TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i); - if (ptrptr && *ptrptr) - { - TinyRendererObjectArray* ptr = *ptrptr; - unsigned char textureImage[3] = {255, 10, 10}; - int textureWidth = 1; - int textureHeight = 1; - ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(textureImage,textureWidth,textureHeight); - } - } - */ } + +int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) +{ + m_data->m_texture = texels; + m_data->m_textureWidth = width; + m_data->m_textureHeight = height; + return 0; +} + +void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) +{ + int width,height,n; + unsigned char* image=0; + image = stbi_load(filename, &width, &height, &n, 3); + registerTexture(image, width, height); +} \ No newline at end of file diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 86455eefb..afd0a2ae6 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -38,6 +38,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void render(); void render(const float viewMat[16], const float projMat[16]); + void loadTextureFile(const char* filename); int registerTexture(unsigned char* texels, int width, int height); void activateShapeTexture(int shapeUniqueId, int textureUniqueId); void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); From 4911b14271f132ec6f3a93d2ca13989f178abd3f Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 20 Oct 2016 23:40:30 -0700 Subject: [PATCH 5/9] Add loading texture API. --- examples/SharedMemory/PhysicsClientC_API.cpp | 39 +++++++++++-------- examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../SharedMemory/PhysicsClientExample.cpp | 10 ++++- .../PhysicsClientSharedMemory.cpp | 9 +++++ .../PhysicsServerCommandProcessor.cpp | 17 ++++++-- examples/SharedMemory/SharedMemoryCommands.h | 13 +++++++ examples/SharedMemory/SharedMemoryPublic.h | 3 ++ .../TinyRendererVisualShapeConverter.cpp | 23 +++++------ 8 files changed, 82 insertions(+), 35 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f9f94c047..d05847313 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1252,31 +1252,38 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu } } -b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename) { - /* - PhysicsClient* cl = (PhysicsClient* ) physClient; - b3Assert(cl); - b3Assert(cl->canSubmitCommand()); - - struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); - b3Assert(command); - command->m_type = CMD_UPDATE_VISUAL_SHAPE; - command->m_updateFlags = 0; - - return (b3SharedMemoryCommandHandle) command; - */ PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); - command->m_type = CMD_UPDATE_VISUAL_SHAPE; - //command->m_requestVisualShapeDataArguments.m_bodyUniqueId = 0; - //command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0; + command->m_type = CMD_LOAD_TEXTURE; + int len = strlen(filename); + if (lenm_loadTextureArguments.m_textureFileName,filename); + } else + { + command->m_loadTextureArguments.m_textureFileName[0] = 0; + } command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; +} +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int textureUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_UPDATE_VISUAL_SHAPE; + command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId; + command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; } b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 80ed8ad06..d76a947a4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -99,7 +99,8 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA); void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); -b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient); +b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int textureUniqueId); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index dee00ed27..97bc9884e 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -489,7 +489,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } case CMD_UPDATE_VISUAL_SHAPE: { - b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle); + b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle,8,0); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } + case CMD_LOAD_TEXTURE: + { + const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; + b3SharedMemoryCommandHandle commandHandle = b3InitLoadTexture(m_physicsClientHandle, filename); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -572,6 +579,7 @@ void PhysicsClientExample::createButtons() createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger); createButton("Get Visual Shape Info",CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger); + createButton("Load Texture",CMD_LOAD_TEXTURE, isTrigger); createButton("Update Visual Shape",CMD_UPDATE_VISUAL_SHAPE, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); if (m_options!=eCLIENTEXAMPLE_SERVER) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 3f031384c..334806ff5 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -666,6 +666,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Visual Shape Update failed"); break; } + case CMD_LOAD_TEXTURE_COMPLETED: + { + break; + } + case CMD_LOAD_TEXTURE_FAILED: + { + b3Warning("Load texture failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8c275af6e..a3a46af75 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3006,15 +3006,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; - m_data->m_visualConverter.loadTextureFile(filename); - m_data->m_visualConverter.activateShapeTexture(0, 0); + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; hasStatus = true; break; } + case CMD_LOAD_TEXTURE: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + + m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName); + + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + hasStatus = true; + + break; + } + default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5a1141821..c4e6af179 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -157,6 +157,17 @@ struct RequestVisualShapeDataArgs int m_startingVisualShapeIndex; }; +struct UpdateVisualShapeDataArgs +{ + int m_bodyUniqueId; + int m_textureUniqueId; +}; + +struct LoadTextureArgs +{ + char m_textureFileName[MAX_FILENAME_LENGTH]; +}; + struct SendVisualShapeDataArgs { int m_bodyUniqueId; @@ -477,6 +488,8 @@ struct SharedMemoryCommand struct CreateJointArgs m_createJointArguments; struct RequestContactDataArgs m_requestContactPointArguments; struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; + struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments; + struct LoadTextureArgs m_loadTextureArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1607ec451..b435a38a5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -36,6 +36,7 @@ enum EnumSharedMemoryClientCommand CMD_SAVE_WORLD, CMD_REQUEST_VISUAL_SHAPE_INFO, CMD_UPDATE_VISUAL_SHAPE, + CMD_LOAD_TEXTURE, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -84,6 +85,8 @@ enum EnumSharedMemoryServerStatus CMD_VISUAL_SHAPE_INFO_FAILED, CMD_VISUAL_SHAPE_UPDATE_COMPLETED, CMD_VISUAL_SHAPE_UPDATE_FAILED, + CMD_LOAD_TEXTURE_COMPLETED, + CMD_LOAD_TEXTURE_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 471ad81f5..ea0db8f95 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -70,9 +70,7 @@ struct TinyRendererVisualShapeConverterInternalData int m_swWidth; int m_swHeight; TGAImage m_rgbColorBuffer; - unsigned char* m_texture; - int m_textureWidth; - int m_textureHeight; + b3AlignedObjectArray m_textures; b3AlignedObjectArray m_depthBuffer; b3AlignedObjectArray m_segmentationMaskBuffer; @@ -832,27 +830,24 @@ void TinyRendererVisualShapeConverter::resetAll() m_data->m_swRenderInstances.clear(); } -// Get shapeUniqueId from getVisualShapesData? void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId) { - // Use shapeUniqueId? - int objectArrayIndex = 8; - int objectIndex = 0; - printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); - TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(objectArrayIndex); + TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId); if (ptrptr && *ptrptr) { TinyRendererObjectArray* ptr = *ptrptr; - ptr->m_renderObjects[objectIndex]->m_model->setDiffuseTextureFromData(m_data->m_texture,m_data->m_textureWidth,m_data->m_textureHeight); + 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); } } int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) { - m_data->m_texture = texels; - m_data->m_textureWidth = width; - m_data->m_textureHeight = height; - return 0; + MyTexture2 texData; + texData.m_width = width; + texData.m_height = height; + texData.textureData = texels; + m_data->m_textures.push_back(texData); + return m_data->m_textures.size()-1; } void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) From 3d799617259dc516d942e43ce56ebfc9fcf81858 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 21 Oct 2016 11:55:27 -0700 Subject: [PATCH 6/9] Add API to change texture with object id and link index. --- examples/SharedMemory/PhysicsClientC_API.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../SharedMemory/PhysicsClientExample.cpp | 40 +++++++++++++++---- .../PhysicsServerCommandProcessor.cpp | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 2 + .../TinyRendererVisualShapeConverter.cpp | 24 +++++++++++ 6 files changed, 63 insertions(+), 11 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d05847313..583f38ba9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1272,7 +1272,7 @@ b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, return (b3SharedMemoryCommandHandle) command; } -b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int textureUniqueId) +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1281,6 +1281,8 @@ b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physCl b3Assert(command); command->m_type = CMD_UPDATE_VISUAL_SHAPE; command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId; + command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex; + command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex; command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId; command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d76a947a4..830717467 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -100,7 +100,7 @@ b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientH void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); -b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int textureUniqueId); +b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 97bc9884e..4ab986d17 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -25,6 +25,9 @@ struct MyMotorInfo2 int camVisualizerWidth = 320;//1024/3; int camVisualizerHeight = 240;//768/3; +int loadTextureCount = 0; +int setTextureCount = 0; + enum CustomCommands { CMD_CUSTOM_SET_REALTIME_SIMULATION = CMD_MAX_CLIENT_COMMANDS+1, @@ -242,7 +245,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { case CMD_LOAD_URDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model_white.urdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; @@ -260,9 +263,9 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); */ - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "sphere_big.urdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "table/table.urdf"); //setting the initial position, orientation and other arguments are optional - b3LoadUrdfCommandSetStartPosition(commandHandle,0.5,0.5,0.5); + b3LoadUrdfCommandSetStartPosition(commandHandle,2.0,2.0,0.5); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -489,15 +492,36 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } case CMD_UPDATE_VISUAL_SHAPE: { - b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle,8,0); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + if (setTextureCount == 0) + { + b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle,1,0,0,0); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + } + else if (setTextureCount == 1) + { + b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle,1,0,0,1); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + } + setTextureCount++; + if (setTextureCount >=2) + setTextureCount = 0; break; } case CMD_LOAD_TEXTURE: { - const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; - b3SharedMemoryCommandHandle commandHandle = b3InitLoadTexture(m_physicsClientHandle, filename); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + if (loadTextureCount == 0) + { + const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/cube.png"; + b3SharedMemoryCommandHandle commandHandle = b3InitLoadTexture(m_physicsClientHandle, filename); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + } + else if (loadTextureCount ==1) + { + const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; + b3SharedMemoryCommandHandle commandHandle = b3InitLoadTexture(m_physicsClientHandle, filename); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + } + loadTextureCount++; break; } default: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a3a46af75..e56ed672e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3006,7 +3006,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + 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); serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; hasStatus = true; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c4e6af179..50b099816 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -160,6 +160,8 @@ struct RequestVisualShapeDataArgs struct UpdateVisualShapeDataArgs { int m_bodyUniqueId; + int m_jointIndex; + int m_shapeIndex; int m_textureUniqueId; }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index ea0db8f95..c6cb3fd29 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -832,6 +832,7 @@ 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) { @@ -840,6 +841,29 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, i } } +void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) +{ + int start = -1; + //find first one, then count how many + for (int i = 0; i < m_data->m_visualShapes.size(); i++) + { + printf("object id: %d\n", m_data->m_visualShapes[i].m_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); + } + } +} + int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height) { MyTexture2 texData; From 1b312dab3c795d8bdd1739877685b69af6ffdef2 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 21 Oct 2016 12:42:42 -0700 Subject: [PATCH 7/9] Remove the temporary test code. --- .../SharedMemory/PhysicsClientExample.cpp | 45 ------------------- .../TinyRendererVisualShapeConverter.cpp | 2 - examples/TinyRenderer/model.cpp | 3 -- 3 files changed, 50 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 4ab986d17..b0fe2be97 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -25,9 +25,6 @@ struct MyMotorInfo2 int camVisualizerWidth = 320;//1024/3; int camVisualizerHeight = 240;//768/3; -int loadTextureCount = 0; -int setTextureCount = 0; - enum CustomCommands { CMD_CUSTOM_SET_REALTIME_SIMULATION = CMD_MAX_CLIENT_COMMANDS+1, @@ -259,14 +256,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { - /* b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - */ - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "table/table.urdf"); - //setting the initial position, orientation and other arguments are optional - b3LoadUrdfCommandSetStartPosition(commandHandle,2.0,2.0,0.5); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: @@ -490,40 +481,6 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } break; } - case CMD_UPDATE_VISUAL_SHAPE: - { - if (setTextureCount == 0) - { - b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle,1,0,0,0); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - } - else if (setTextureCount == 1) - { - b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape(m_physicsClientHandle,1,0,0,1); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - } - setTextureCount++; - if (setTextureCount >=2) - setTextureCount = 0; - break; - } - case CMD_LOAD_TEXTURE: - { - if (loadTextureCount == 0) - { - const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/cube.png"; - b3SharedMemoryCommandHandle commandHandle = b3InitLoadTexture(m_physicsClientHandle, filename); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - } - else if (loadTextureCount ==1) - { - const char filename[] = "/Users/yunfeibai/Documents/dev/bullet-change-texture/bullet3/data/checker_huge.gif"; - b3SharedMemoryCommandHandle commandHandle = b3InitLoadTexture(m_physicsClientHandle, filename); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - } - loadTextureCount++; - break; - } default: { b3Error("Unknown buttonId"); @@ -603,8 +560,6 @@ void PhysicsClientExample::createButtons() createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger); createButton("Get Visual Shape Info",CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger); - createButton("Load Texture",CMD_LOAD_TEXTURE, isTrigger); - createButton("Update Visual Shape",CMD_UPDATE_VISUAL_SHAPE, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); if (m_options!=eCLIENTEXAMPLE_SERVER) { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index c6cb3fd29..9bf123413 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -844,10 +844,8 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, i void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) { int start = -1; - //find first one, then count how many for (int i = 0; i < m_data->m_visualShapes.size(); i++) { - printf("object id: %d\n", m_data->m_visualShapes[i].m_objectUniqueId); if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex) { start = i; diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index f0ef25563..8864d0a2f 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -60,9 +60,6 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid color.bgra[0] = textureImage[(i+j*textureWidth)*3+0]; color.bgra[1] = textureImage[(i+j*textureWidth)*3+1]; color.bgra[2] = textureImage[(i+j*textureWidth)*3+2]; - //color.bgra[0] = textureImage[0]; - //color.bgra[1] = textureImage[1]; - //color.bgra[2] = textureImage[2]; color.bgra[3] = 255; color.bytespp = 3; From 4847e570694033237d6d860ada52688df48d330e Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 21 Oct 2016 15:54:39 -0700 Subject: [PATCH 8/9] Add script for pybullet. --- build_and_run_cmake_pybullet_double.sh | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100755 build_and_run_cmake_pybullet_double.sh diff --git a/build_and_run_cmake_pybullet_double.sh b/build_and_run_cmake_pybullet_double.sh new file mode 100755 index 000000000..e05d3f06d --- /dev/null +++ b/build_and_run_cmake_pybullet_double.sh @@ -0,0 +1,6 @@ +#!/bin/sh +rm CMakeCache.txt +mkdir build_cmake +cd build_cmake +cmake -DBUILD_PYBULLET=ON -DBUILD_PYBULLET_NUMPY=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release .. +make -j12 From 9c00b4d9f4a01cd3dca27dafad1a77651d5e64c2 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 21 Oct 2016 17:48:06 -0700 Subject: [PATCH 9/9] Add texture reset API to pybullet. --- examples/pybullet/pybullet.c | 84 +++++++++++++++++++++++++++++++++++- 1 file changed, 82 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1af256b47..2cbd35530 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1358,8 +1358,82 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) return Py_None; } +static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + int objectUniqueId = -1; + int jointIndex = -1; + int shapeIndex = -1; + int textureUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (size == 4) + { + if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) { + PyErr_SetString(SpamError, "Error parsing object unique id, or joint index, or shape index, or texture unique id"); + return NULL; + } + + commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error resetting visual shape info"); + return NULL; + } + } + else + { + PyErr_SetString(SpamError, "setVisualShapeData requires 4 argument"); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} - +static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + const char* filename = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + if (size == 1) + { + if (!PyArg_ParseTuple(args, "s", &filename)) { + PyErr_SetString(SpamError, "Error parsing file name"); + return NULL; + } + + commandHandle = b3InitLoadTexture(sm, filename); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_LOAD_TEXTURE_COMPLETED) + { + } + else + { + PyErr_SetString(SpamError, "Error loading texture"); + return NULL; + } + } + else + { + PyErr_SetString(SpamError, "loadTexture requires 1 argument"); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { int size = PySequence_Size(args); @@ -2294,9 +2368,15 @@ static PyMethodDef SpamMethods[] = { "object-object collisions. Optional arguments one or two object unique " "ids, that need to be involved in the contact."}, - { "getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, + {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, "Return the visual shape information for one object." }, + {"resetVisualShapeData", pybullet_resetVisualShapeData, METH_VARARGS, + "Reset part of the visual shape information for one object." }, + + {"loadTexture", pybullet_loadTexture, METH_VARARGS, + "Load texture file." }, + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " "quaternion [x,y,z,w]"},