AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #735 from erwincoumans/master

Browse Source 
set the camera, even in multi-threaded mode
This commit is contained in:
erwincoumans 2016-08-11 17:37:02 -07:00 committed by GitHub
commit fec13f2d0a
24 changed files with 293 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@ -46,7 +46,11 @@ struct GUIHelperInterface
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0;
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;
@ -107,7 +111,12 @@ struct DummyGUIHelper : public GUIHelperInterface
{
}
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int width, int height, int* numPixelsCopied)
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int width, int height, int* numPixelsCopied)
{
if (numPixelsCopied)
*numPixelsCopied = 0;

8
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@ -634,10 +634,12 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS];
GraphingTexture* m_gt[MAX_GRAPH_WINDOWS];
int m_curNumGraphWindows;
int m_curXpos;
QuickCanvas(GL3TexLoader* myTexLoader)
:m_myTexLoader(myTexLoader),
m_curNumGraphWindows(0)
m_curNumGraphWindows(0),
m_curXpos(0)
{
for (int i=0;i<MAX_GRAPH_WINDOWS;i++)
{
@ -661,7 +663,8 @@ struct QuickCanvas : public Common2dCanvasInterface
MyGraphInput input(gui2->getInternalData());
input.m_width=width;
input.m_height=height;
input.m_xPos = 10000;//GUI will clamp it to the right//300;
input.m_xPos = m_curXpos;//GUI will clamp it to the right//300;
m_curXpos+=width+20;
input.m_yPos = 10000;//GUI will clamp it to bottom
input.m_name=canvasName;
input.m_texName = canvasName;
@ -677,6 +680,7 @@ struct QuickCanvas : public Common2dCanvasInterface
}
virtual void destroyCanvas(int canvasId)
{
m_curXpos = 0;
btAssert(canvasId>=0);
delete m_gt[canvasId];
m_gt[canvasId] = 0;

7
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@ -338,7 +338,12 @@ void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float c
}
void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied)
{
int sourceWidth = m_data->m_glApp->m_window->getWidth()*m_data->m_glApp->m_window->getRetinaScale();
int sourceHeight = m_data->m_glApp->m_window->getHeight()*m_data->m_glApp->m_window->getRetinaScale();

7
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

@ -44,7 +44,12 @@ struct OpenGLGuiHelper : public GUIHelperInterface
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied);
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied);
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) ;

17
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -45,6 +45,7 @@ struct BulletURDFInternalData
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
char m_pathPrefix[1024];
int m_bodyId;
btHashMap<btHashInt,btVector4> m_linkColors;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
@ -208,6 +209,18 @@ const char* BulletURDFImporter::getPathPrefix()
return m_data->m_pathPrefix;
}
void BulletURDFImporter::setBodyUniqueId(int bodyId)
{
m_data->m_bodyId =bodyId;
}
int BulletURDFImporter::getBodyUniqueId() const
{
return m_data->m_bodyId;
}
BulletURDFImporter::~BulletURDFImporter()
{
@ -1017,13 +1030,13 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
return false;
}
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const
{
if (m_data->m_customVisualShapesConverter)
{
const UrdfModel& model = m_data->m_urdfParser.getModel();
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex);
}
}

5
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@ -25,7 +25,8 @@ public:
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
const char* getPathPrefix();
void printTree(); //for debugging
@ -50,7 +51,7 @@ public:
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const;
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation

2
examples/Importers/ImportURDFDemo/LinkVisualShapesConverter.h
View File

@ -3,7 +3,7 @@
struct LinkVisualShapesConverter
{
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj)=0;
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
};
#endif //LINK_VISUAL_SHAPES_CONVERTER_H

2
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -397,7 +397,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);

6
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@ -48,8 +48,10 @@ public:
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj) const { }
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
virtual void setBodyUniqueId(int bodyId) {}
virtual int getBodyUniqueId() const { return 0;}
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
};

5
examples/RenderingExamples/TinyRendererSetup.cpp
View File

@ -23,6 +23,7 @@ struct TinyRendererSetupInternalData
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> m_depthBuffer;
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
int m_width;
@ -185,7 +186,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
m_internalData->m_shapePtr.push_back(0);
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer);
m_internalData->m_depthBuffer,
m_internalData->m_segmentationMaskBuffer,
m_internalData->m_renderObjects.size(),0);
//ob->loadModel("cube.obj");
const int* indices = &meshData.m_gfxShape->m_indices->at(0);
ob->registerMeshShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],

13
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@ -352,12 +352,19 @@ public:
int m_rgbaBufferSizeInPixels;
float* m_depthBuffer;
int m_depthBufferSizeInPixels;
int* m_segmentationMaskBuffer;
int m_segmentationMaskBufferSizeInPixels;
int m_startPixelIndex;
int m_destinationWidth;
int m_destinationHeight;
int* m_numPixelsCopied;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied)
{
m_cs->lock();
for (int i=0;i<16;i++)
@ -369,6 +376,8 @@ public:
m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
m_depthBuffer = depthBuffer;
m_depthBufferSizeInPixels = depthBufferSizeInPixels;
m_segmentationMaskBuffer = segmentationMaskBuffer;
m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
m_startPixelIndex = startPixelIndex;
m_destinationWidth = destinationWidth;
m_destinationHeight = destinationHeight;
@ -532,6 +541,8 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
m_data->m_multiThreadedHelper->m_depthBuffer,
m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels,
m_data->m_multiThreadedHelper->m_segmentationMaskBuffer,
m_data->m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
m_data->m_multiThreadedHelper->m_startPixelIndex,
m_data->m_multiThreadedHelper->m_destinationWidth,
m_data->m_multiThreadedHelper->m_destinationHeight,

144
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -44,7 +44,9 @@ protected:
int m_selectedBody;
int m_prevSelectedBody;
struct Common2dCanvasInterface* m_canvas;
int m_canvasIndex;
int m_canvasRGBIndex;
int m_canvasDepthIndex;
int m_canvasSegMaskIndex;
void createButton(const char* name, int id, bool isTrigger );
@ -248,7 +250,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_LOAD_SDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf");
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");//kuka_iiwa/model.sdf");
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
@ -460,7 +462,11 @@ m_prevSelectedBody(-1),
m_numMotors(0),
m_options(options),
m_isOptionalServerConnected(false),
m_canvas(0)
m_canvas(0),
m_canvasRGBIndex(-1),
m_canvasDepthIndex(-1),
m_canvasSegMaskIndex(-1)
{
b3Printf("Started PhysicsClientExample\n");
}
@ -479,9 +485,15 @@ PhysicsClientExample::~PhysicsClientExample()
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas && (m_canvasIndex>=0))
if (m_canvas)
{
m_canvas->destroyCanvas(m_canvasIndex);
if (m_canvasRGBIndex>=0)
m_canvas->destroyCanvas(m_canvasRGBIndex);
if (m_canvasDepthIndex>=0)
m_canvas->destroyCanvas(m_canvasDepthIndex);
if (m_canvasSegMaskIndex>=0)
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
b3Printf("~PhysicsClientExample\n");
}
@ -613,9 +625,10 @@ void PhysicsClientExample::initPhysics()
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
{
m_canvasIndex = m_canvas->createCanvas("Synthetic Camera",camVisualizerWidth, camVisualizerHeight);
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",camVisualizerWidth, camVisualizerHeight);
m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",camVisualizerWidth, camVisualizerHeight);
m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",camVisualizerWidth, camVisualizerHeight);
for (int i=0;i<camVisualizerWidth;i++)
{
@ -631,10 +644,16 @@ void PhysicsClientExample::initPhysics()
green=0;
blue=0;
}
m_canvas->setPixel(m_canvasIndex,i,j,red,green,blue,alpha);
m_canvas->setPixel(m_canvasRGBIndex,i,j,red,green,blue,alpha);
m_canvas->setPixel(m_canvasDepthIndex,i,j,red,green,blue,alpha);
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,red,green,blue,alpha);
}
}
m_canvas->refreshImageData(m_canvasIndex);
m_canvas->refreshImageData(m_canvasRGBIndex);
m_canvas->refreshImageData(m_canvasDepthIndex);
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
@ -698,8 +717,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
if (m_canvas)
{
//compute depth image range
float minDepthValue = 1e20f;
float maxDepthValue = -1e20f;
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<camVisualizerHeight;j++)
{
int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
if (m_canvasDepthIndex >=0)
{
int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues4[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue>-1e20)
{
maxDepthValue = btMax(maxDepthValue,depthValue);
minDepthValue = btMin(minDepthValue,depthValue);
}
}
}
}
for (int i=0;i<camVisualizerWidth;i++)
{
for (int j=0;j<camVisualizerHeight;j++)
@ -710,15 +757,76 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4; //RGBA
int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,i,j,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255); //alpha set to 255
if (m_canvasRGBIndex >=0)
{
int rgbPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasRGBIndex,i,j,
imageData.m_rgbColorData[rgbPixelIndex],
imageData.m_rgbColorData[rgbPixelIndex+1],
imageData.m_rgbColorData[rgbPixelIndex+2],
255); //alpha set to 255
}
if (m_canvasDepthIndex >=0)
{
int depthPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues4[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue>-1e20)
{
int rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue)));
if (rgb<0 || rgb>255)
{
printf("rgb=%d\n",rgb);
}
m_canvas->setPixel(m_canvasDepthIndex,i,j,
rgb,
rgb,
255, 255); //alpha set to 255
} else
{
m_canvas->setPixel(m_canvasDepthIndex,i,j,
0,
0,
0, 255); //alpha set to 255
}
}
if (m_canvasSegMaskIndex>=0 && (0!=imageData.m_segmentationMaskValues))
{
int segmentationMaskPixelIndex = (xIndex+yIndex*imageData.m_pixelWidth);
int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex];
btVector4 palette[4] = {btVector4(32,255,32,255),
btVector4(32,32,255,255),
btVector4(255,255,32,255),
btVector4(32,255,255,255)};
if (segmentationMask>=0)
{
btVector4 rgb = palette[segmentationMask&3];
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
rgb.x(),
rgb.y(),
rgb.z(), 255); //alpha set to 255
} else
{
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
0,
0,
0, 255); //alpha set to 255
}
}
}
}
m_canvas->refreshImageData(m_canvasIndex);
if (m_canvasRGBIndex >=0)
m_canvas->refreshImageData(m_canvasRGBIndex);
if (m_canvasDepthIndex >=0)
m_canvas->refreshImageData(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >=0)
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
// b3Printf(msg);

11
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -36,6 +36,7 @@ struct PhysicsClientSharedMemoryInternalData {
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
SharedMemoryStatus m_tempBackupServerStatus;
@ -514,6 +515,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
@ -522,12 +524,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
@ -704,8 +712,9 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
{
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_depthValues4 = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {

11
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -37,6 +37,8 @@ struct PhysicsDirectInternalData
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
btAlignedObjectArray<int> m_cachedSegmentationMask;
PhysicsServerCommandProcessor* m_commandProcessor;
@ -205,6 +207,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedSegmentationMask.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
@ -212,6 +215,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
(unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
// printf("pixel = %d\n", rgbaPixelsReceived[0]);
@ -219,6 +223,10 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
{
m_data->m_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
}
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
@ -456,7 +464,8 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
{
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_depthValues4 = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
}
}

18
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -751,7 +751,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
u2b.setBodyUniqueId(bodyUniqueId);
{
btScalar mass = 0;
bodyHandle->m_rootLocalInertialFrame.setIdentity();
@ -845,6 +845,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (bodyUniqueIdPtr)
*bodyUniqueIdPtr= bodyUniqueId;
u2b.setBodyUniqueId(bodyUniqueId);
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
{
@ -1165,15 +1166,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (numRemainingPixels>0)
{
int maxNumPixels = bufferSizeInBytes/8-1;
int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask
int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1;
unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels);
float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4);
int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8);
if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0)
{
m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,width,height,&numPixelsCopied);
m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,width,height,&numPixelsCopied);
} else
{
@ -1194,7 +1201,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied);
m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,&width,&height,&numPixelsCopied);
}
//each pixel takes 4 RGBA values and 1 float = 8 bytes

14
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -391,6 +391,7 @@ public:
}
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
{
m_childGuiHelper->resetCamera(camDist,pitch,yaw,camPosX,camPosY,camPosZ);
}
float m_viewMatrix[16];
@ -399,12 +400,19 @@ public:
int m_rgbaBufferSizeInPixels;
float* m_depthBuffer;
int m_depthBufferSizeInPixels;
int* m_segmentationMaskBuffer;
int m_segmentationMaskBufferSizeInPixels;
int m_startPixelIndex;
int m_destinationWidth;
int m_destinationHeight;
int* m_numPixelsCopied;
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth,
int destinationHeight, int* numPixelsCopied)
{
m_cs->lock();
for (int i=0;i<16;i++)
@ -416,6 +424,8 @@ public:
m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
m_depthBuffer = depthBuffer;
m_depthBufferSizeInPixels = depthBufferSizeInPixels;
m_segmentationMaskBuffer = segmentationMaskBuffer;
m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
m_startPixelIndex = startPixelIndex;
m_destinationWidth = destinationWidth;
m_destinationHeight = destinationHeight;
@ -767,6 +777,8 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
m_multiThreadedHelper->m_depthBuffer,
m_multiThreadedHelper->m_depthBufferSizeInPixels,
m_multiThreadedHelper->m_segmentationMaskBuffer,
m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
m_multiThreadedHelper->m_startPixelIndex,
m_multiThreadedHelper->m_destinationWidth,
m_multiThreadedHelper->m_destinationHeight,

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -122,7 +122,8 @@ struct b3CameraImageData
int m_pixelWidth;
int m_pixelHeight;
const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
const float* m_depthValues4;//m_pixelWidth*m_pixelHeight floats
const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
};
///b3LinkState provides extra information such as the Cartesian world coordinates

23
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@ -66,6 +66,8 @@ struct TinyRendererVisualShapeConverterInternalData
int m_swHeight;
TGAImage m_rgbColorBuffer;
b3AlignedObjectArray<float> m_depthBuffer;
b3AlignedObjectArray<int> m_segmentationMaskBuffer;
SimpleCamera m_camera;
TinyRendererVisualShapeConverterInternalData()
@ -75,6 +77,7 @@ struct TinyRendererVisualShapeConverterInternalData
m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB)
{
m_depthBuffer.resize(m_swWidth*m_swHeight);
m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1);
}
};
@ -440,7 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj)
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex)
{
@ -487,7 +490,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, m_data->m_segmentationMaskBuffer, objectIndex,0);
unsigned char* textureImage=0;
int textureWidth=0;
int textureHeight=0;
@ -535,6 +538,7 @@ void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor)
{
m_data->m_rgbColorBuffer.set(x,y,clearColor);
m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f;
m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1;
}
}
@ -624,7 +628,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
// printf("flipped!\n");
m_data->m_rgbColorBuffer.flip_vertically();
//flip z-buffer
//flip z-buffer and segmentation Buffer
{
int half = m_data->m_swHeight>>1;
for (int j=0; j<half; j++)
@ -634,6 +638,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
for (int i=0;i<m_data->m_swWidth;i++)
{
btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]);
btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]);
}
}
}
@ -652,11 +657,16 @@ void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
m_data->m_swHeight = height;
m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_segmentationMaskBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
}
void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,
int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
{
int w = m_data->m_rgbColorBuffer.get_width();
int h = m_data->m_rgbColorBuffer.get_height();
@ -682,6 +692,11 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
{
depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
}
if (segmentationMaskBuffer)
{
segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
}
if (pixelsRGBA)
{
pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];

4
examples/SharedMemory/TinyRendererVisualShapeConverter.h
View File

@ -13,7 +13,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape);
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
void setUpAxis(int axis);
@ -26,7 +26,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
void getWidthAndHeight(int& width, int& height);
void setWidthAndHeight(int width, int height);
void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
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]);

9
examples/TinyRenderer/TinyRenderer.cpp
View File

@ -88,12 +88,14 @@ struct Shader : public IShader {
};
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>& segmentationMaskBuffer, int objectIndex,int objectIndex2)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_segmentationMaskBuffer(segmentationMaskBuffer),
m_model(0),
m_userData(0),
m_userIndex(-1)
m_userIndex(-1),
m_objectIndex(objectIndex)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
@ -247,6 +249,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
b3AlignedObjectArray<int>& segmentationMaskBuffer = renderData.m_segmentationMaskBuffer;
TGAImage& frame = renderData.m_rgbColorBuffer;
@ -264,7 +267,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
for (int j=0; j<3; j++) {
shader.vertex(i, j);
}
triangle(shader.varying_tri, shader, frame, &zbuffer[0], renderData.m_viewportMatrix);
triangle(shader.varying_tri, shader, frame, &zbuffer[0], &segmentationMaskBuffer[0], renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}

6
examples/TinyRenderer/TinyRenderer.h
View File

@ -28,8 +28,9 @@ struct TinyRenderObjectData
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
b3AlignedObjectArray<int>& m_segmentationMaskBuffer;
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>& segmentationMaskBuffer,int objectIndex,int objectIndex2);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);
@ -41,6 +42,7 @@ struct TinyRenderObjectData
void* m_userData;
int m_userIndex;
int m_objectIndex;
};

3
examples/TinyRenderer/our_gl.cpp
View File

@ -59,7 +59,7 @@ Vec3f barycentric(Vec2f A, Vec2f B, Vec2f C, Vec2f P) {
return Vec3f(-1,1,1); // in this case generate negative coordinates, it will be thrown away by the rasterizator
}
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix) {
void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
mat<3,4,float> pts = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
@ -100,6 +100,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
bool discard = shader.fragment(bc_clip, color);
if (!discard) {
zbuffer[P.x+P.y*image.get_width()] = frag_depth;
segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
image.set(P.x, P.y, color);
}
}

2
examples/TinyRenderer/our_gl.h
View File

@ -17,6 +17,6 @@ struct IShader {
};
//void triangle(Vec4f *pts, IShader &shader, TGAImage &image, float *zbuffer);
void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, const Matrix& viewPortMatrix);
void triangle(mat<4,3,float> &pts, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex);
#endif //__OUR_GL_H__

19
examples/pybullet/pybullet.c
View File

@ -1205,11 +1205,13 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
PyObject *pylistRGB;
PyObject* pylistDep;
PyObject* pylistSeg;
int i, j, p;
b3GetCameraImageData(sm, &imageData);
//TODO(hellojas): error handling if image size is 0
pyResultList = PyTuple_New(4);
pyResultList = PyTuple_New(5);
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
@ -1221,15 +1223,23 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
pylistRGB = PyTuple_New(num);
pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
pylistSeg = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
for (i=0;i<imageData.m_pixelWidth;i++)
{
for (j=0;j<imageData.m_pixelHeight;j++)
{
// TODO(hellojas): validate depth values make sense
int depIndex = i+j*imageData.m_pixelWidth;
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
{
item = PyFloat_FromDouble(imageData.m_depthValues4[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
}
{
item2 = PyLong_FromLong(imageData.m_segmentationMaskValues[depIndex]);
PyTuple_SetItem(pylistSeg, depIndex, item2);
}
for (p=0; p<bytesPerPixel; p++)
{
int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
@ -1242,6 +1252,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
PyTuple_SetItem(pyResultList, 2,pylistRGB);
PyTuple_SetItem(pyResultList, 3,pylistDep);
PyTuple_SetItem(pyResultList, 4,pylistSeg);
return pyResultList;
}
}