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

Merge pull request #1367 from erwincoumans/master

Browse Source 
Implement first draft of pybullet.createVisualShape, see Bullet/examples/pybullet/examples/createVisualShape.py
This commit is contained in:
erwincoumans 2017-10-07 19:52:14 -07:00 committed by GitHub
commit f230011028
12 changed files with 9014 additions and 6537 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14749
data/duck.obj
View File

File diff suppressed because it is too large Load Diff

2
data/plane100.urdf
View File

@ -18,7 +18,7 @@
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="10 10 0.001"/>
<box size="200 200 0.001"/>
</geometry>
</collision>
</link>

2
examples/ExampleBrowser/OpenGLGuiHelper.cpp
View File

@ -905,7 +905,7 @@ void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor
B3_PROFILE("write all InstanceTransformToCPU");
for (int i = 0; i<numCollisionObjects; i++)
{
B3_PROFILE("writeSingleInstanceTransformToCPU");
//B3_PROFILE("writeSingleInstanceTransformToCPU");
btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
btVector3 pos = colObj->getWorldTransform().getOrigin();
btQuaternion orn = colObj->getWorldTransform().getRotation();

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

@ -35,12 +35,6 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include <list>
#include "UrdfParser.h"
struct MyTexture
{
int m_width;
int m_height;
unsigned char* textureData;
};
ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
@ -820,7 +814,7 @@ upAxisMat.setIdentity();
}
static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<BulletURDFTexture>& texturesOut) const
{
BT_PROFILE("convertURDFToVisualShapeInternal");
@ -886,7 +880,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
if (meshData.m_textureImage)
{
MyTexture texData;
BulletURDFTexture texData;
texData.m_width = meshData.m_textureWidth;
texData.m_height = meshData.m_textureHeight;
texData.textureData = meshData.m_textureImage;
@ -1101,7 +1095,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<MyTexture> textures;
btAlignedObjectArray<BulletURDFTexture> textures;
const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);

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

@ -5,6 +5,13 @@
#include "LinkVisualShapesConverter.h"
struct BulletURDFTexture
{
int m_width;
int m_height;
unsigned char* textureData;
};
///BulletURDFImporter can deal with URDF and (soon) SDF files
class BulletURDFImporter : public URDFImporterInterface
@ -75,6 +82,9 @@ public:
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
virtual void setEnableTinyRenderer(bool enable);
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut) const;
};

261
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -775,7 +775,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3Ph
b3Assert(command);
command->m_type = CMD_CREATE_COLLISION_SHAPE;
command->m_updateFlags =0;
command->m_createCollisionShapeArgs.m_numCollisionShapes = 0;
command->m_createUserShapeArgs.m_numUserShapes = 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
@ -785,138 +785,170 @@ B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle co
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)
{
return b3CreateCollisionShapeAddSphere(commandHandle,radius);
}
B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2];
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2];
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/])
{
return b3CreateCollisionShapeAddBox(commandHandle,halfExtents);
}
B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
return b3CreateCollisionShapeAddCapsule(commandHandle,radius,height);
}
B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
return b3CreateCollisionShapeAddCylinder(commandHandle,radius,height);
}
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[2] = planeNormal[2];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeConstant = planeConstant;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[2] = planeNormal[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeConstant = planeConstant;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant)
{
return b3CreateCollisionShapeAddPlane(commandHandle,planeNormal,planeConstant);
}
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES && strlen(fileName)<VISUAL_SHAPE_MAX_PATH_LEN)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
strcpy(command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName);
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3])
B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES && strlen(fileName)<VISUAL_SHAPE_MAX_PATH_LEN)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
strcpy(command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName);
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
return shapeIndex;
}
}
return -1;
return b3CreateCollisionShapeAddMesh(commandHandle,fileName,meshScale);
}
B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
@ -924,38 +956,80 @@ B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandH
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createCollisionShapeArgs.m_numCollisionShapes)
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags;
}
}
}
B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
{
b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags);
}
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createCollisionShapeArgs.m_numCollisionShapes)
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2];
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3];
}
}
}
B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/])
{
b3CreateCollisionShapeSetChildTransform(commandHandle,shapeIndex,childPosition,childOrientation);
}
B3_SHARED_API void b3CreateVisualShapeSetRGBAColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double rgbaColor[/*4*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[0] = rgbaColor[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[1] = rgbaColor[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[2] = rgbaColor[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[3] = rgbaColor[3];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags |= GEOM_VISUAL_HAS_RGBA_COLOR;
}
}
}
B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double specularColor[/*3*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[0] = specularColor[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[1] = specularColor[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[2] = specularColor[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags |= GEOM_VISUAL_HAS_SPECULAR_COLOR;
}
}
}
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
@ -964,7 +1038,7 @@ B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle s
b3Assert(status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED);
if (status && status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
return status->m_createCollisionShapeResultArgs.m_collisionShapeUniqueId;
return status->m_createUserShapeResultArgs.m_userShapeUniqueId;
}
return -1;
}
@ -981,11 +1055,20 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3Physi
b3Assert(command);
command->m_type = CMD_CREATE_VISUAL_SHAPE;
command->m_updateFlags =0;
command->m_createUserShapeArgs.m_numUserShapes= 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@ -993,7 +1076,7 @@ B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle stat
b3Assert(status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED);
if (status && status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
{
return status->m_createVisualShapeResultArgs.m_visualShapeUniqueId;
return status->m_createUserShapeResultArgs.m_userShapeUniqueId;
}
return -1;
}

13
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -379,12 +379,21 @@ B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/]);
B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]);
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
B3_SHARED_API int b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/]);
B3_SHARED_API int b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/]);
B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]);
B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double specularColor[/*3*/]);
B3_SHARED_API void b3CreateVisualShapeSetRGBAColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double rgbaColor[/*4*/]);
B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);

291
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1,5 +1,6 @@
#include "PhysicsServerCommandProcessor.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
@ -139,6 +140,24 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
}
};
struct InternalVisualShapeData
{
int m_tinyRendererVisualShapeIndex;
int m_OpenGLGraphicsIndex;
UrdfVisual m_visualShape;
btTransform m_localInertiaFrame;
std::string m_pathPrefix;
void clear()
{
m_tinyRendererVisualShapeIndex = 0;
m_OpenGLGraphicsIndex = 0;
m_localInertiaFrame.setIdentity();
m_pathPrefix = "";
}
};
struct InternalCollisionShapeData
{
btCollisionShape* m_collisionShape;
@ -214,6 +233,9 @@ struct InternalTextureData
typedef b3PoolBodyHandle<InternalTextureData> InternalTextureHandle;
typedef b3PoolBodyHandle<InternalBodyData> InternalBodyHandle;
typedef b3PoolBodyHandle<InternalCollisionShapeData> InternalCollisionShapeHandle;
typedef b3PoolBodyHandle<InternalVisualShapeData> InternalVisualShapeHandle;
class btCommandChunk
{
@ -1447,6 +1469,9 @@ struct PhysicsServerCommandProcessorInternalData
b3ResizablePool< InternalTextureHandle > m_textureHandles;
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
b3ResizablePool<InternalVisualShapeHandle> m_userVisualShapeHandles;
b3PluginManager m_pluginManager;
@ -1577,6 +1602,9 @@ struct PhysicsServerCommandProcessorInternalData
m_userCollisionShapeHandles.exitHandles();
m_userCollisionShapeHandles.initHandles();
m_userVisualShapeHandles.exitHandles();
m_userVisualShapeHandles.initHandles();
#if 0
btAlignedObjectArray<int> bla;
@ -1907,6 +1935,18 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
if (visHandle->m_visualShape.m_geometry.m_hasLocalMaterial)
{
matCol = visHandle->m_visualShape.m_geometry.m_localMaterial.m_matColor;
return true;
}
}
}
return false;
}
@ -2074,6 +2114,15 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
///quick hack: need to rethink the API/dependencies of this
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]>=0)
{
const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
if (visHandle)
{
return visHandle->m_OpenGLGraphicsIndex;
}
}
return -1;
}
@ -2081,6 +2130,9 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
{
//if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
UrdfModel model;// = m_data->m_urdfParser.getModel();
UrdfLink link;
int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
@ -4038,26 +4090,26 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btCompoundShape* compound = 0;
if (clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes>1)
if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1)
{
compound = worldImporter->createCompoundShape();
}
for (int i=0;i<clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes;i++)
for (int i=0;i<clientCmd.m_createUserShapeArgs.m_numUserShapes;i++)
{
UrdfCollision urdfColObj;
btTransform childTransform;
childTransform.setIdentity();
if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_hasChildTransform)
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_hasChildTransform)
{
childTransform.setOrigin(btVector3(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childPosition[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childPosition[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childPosition[2]));
childTransform.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[i].m_childPosition[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childPosition[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childPosition[2]));
childTransform.setRotation(btQuaternion(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[2],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_childOrientation[3]
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[2],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[3]
));
if (compound==0)
{
@ -4070,11 +4122,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
urdfColObj.m_name = "memory";
urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN;
switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type)
switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type)
{
case GEOM_SPHERE:
{
double radius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius;
double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
if (compound)
{
@ -4086,11 +4138,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case GEOM_BOX:
{
//double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius;
//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
btVector3 halfExtents(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
shape = worldImporter->createBoxShape(halfExtents);
if (compound)
{
@ -4102,37 +4154,37 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case GEOM_CAPSULE:
{
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight);
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
break;
}
case GEOM_CYLINDER:
{
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight);
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
break;
}
case GEOM_PLANE:
{
btVector3 planeNormal(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]);
btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
shape = worldImporter->createPlaneShape(planeNormal,0);
if (compound)
@ -4141,9 +4193,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
urdfColObj.m_geometry.m_planeNormal.setValue(
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]);
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
break;
}
@ -4151,13 +4203,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btScalar defaultCollisionMargin = 0.001;
btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1],
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]);
btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
const std::string& urdf_path="";
std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName;
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName;
urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
urdfColObj.m_geometry.m_meshFileName = fileName;
@ -4184,7 +4236,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
//create a convex hull for each shape, and store it in a btCompoundShape
if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
@ -4293,16 +4345,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
#if 0
shape = worldImporter->createCylinderShapeX(radius,height);
shape = worldImporter->createCylinderShapeY(radius,height);
shape = worldImporter->createCylinderShapeZ(radius,height);
shape = worldImporter->createCapsuleShapeX(radius,height);
shape = worldImporter->createCapsuleShapeY(radius,height);
shape = worldImporter->createCapsuleShapeZ(radius,height);
shape = worldImporter->createBoxShape(halfExtents);
#endif
if (compound && compound->getNumChildShapes())
{
shape = compound;
@ -4317,7 +4359,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
handle->m_urdfCollisionObjects.push_back(urdfCollisionObjects[i]);
}
serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid;
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid;
m_data->m_worldImporters.push_back(worldImporter);
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;
} else
@ -4332,6 +4374,136 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
{
double globalScaling = 1.f;
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btTransform localInertiaFrame;
localInertiaFrame.setIdentity();
btTransform childTrans;
childTrans.setIdentity();
const char* pathPrefix = "";
if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1)
{
int userShapeIndex = 0;
UrdfVisual visualShape;
visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type;
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (visualShape.m_geometry.m_type == URDF_GEOM_MESH)
{
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
visualShape.m_geometry.m_meshFileType = out_type;
visualShape.m_geometry.m_meshFileName=fileName;
visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
}
visualShape.m_name = "bla";
visualShape.m_materialName="";
visualShape.m_sourceFileLocation="blaat_line_10";
visualShape.m_linkLocalFrame.setIdentity();
visualShape.m_geometry.m_hasLocalMaterial = false;
btAlignedObjectArray<GLInstanceVertex> vertices;
btAlignedObjectArray<int> indices;
btTransform startTrans; startTrans.setIdentity();
btAlignedObjectArray<BulletURDFTexture> textures;
bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR)!=0;;
bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR)!=0;;
visualShape.m_geometry.m_hasLocalMaterial = hasRGBA|hasSpecular;
if (visualShape.m_geometry.m_hasLocalMaterial)
{
if (hasRGBA)
{
visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]);
} else
{
}
if (hasSpecular)
{
visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]);
} else
{
visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4,0.4,0.4);
}
}
if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform !=0)
{
childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[2]));
childTrans.setRotation(btQuaternion(
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[0],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[1],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[2],
clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3]));
}
u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
if (vertices.size() && indices.size())
{
if (1)
{
int textureIndex = -1;
if (textures.size())
{
textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
}
int graphicsIndex = -1;
{
B3_PROFILE("registerGraphicsShape");
graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
if (graphicsIndex>=0)
{
int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
visualHandle->m_OpenGLGraphicsIndex = graphicsIndex;
visualHandle->m_tinyRendererVisualShapeIndex = -1;
//tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
//store needed info for tinyrenderer
visualHandle->m_localInertiaFrame = localInertiaFrame;
visualHandle->m_visualShape = visualShape;
visualHandle->m_pathPrefix = pathPrefix ? pathPrefix : "";
serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
}
}
}
}
}
}
break;
}
case CMD_CREATE_MULTI_BODY:
@ -4342,33 +4514,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_sdfRecentLoadedBodies.clear();
#if 0
struct UrdfModel
{
std::string m_name;
std::string m_sourceFile;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
clientCmd.m_createMultiBodyArgs.
char m_bodyName[1024];
int m_baseLinkIndex;
double m_baseWorldPosition[3];
double m_baseWorldOrientation[4];
UrdfModel tmpModel;
tmpModel.m_bodyName =
#endif
ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data);
bool useMultiBody = true;

34
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -834,7 +834,7 @@ enum
URDF_GEOM_HAS_RADIUS = 1,
};
struct b3CreateCollisionShape
struct b3CreateUserShapeData
{
int m_type;//see UrdfGeomTypes
@ -856,23 +856,27 @@ struct b3CreateCollisionShape
char m_meshFileName[VISUAL_SHAPE_MAX_PATH_LEN];
double m_meshScale[3];
int m_collisionFlags;
int m_visualFlags;
double m_rgbaColor[4];
double m_specularColor[3];
};
#define MAX_COMPOUND_COLLISION_SHAPES 16
struct b3CreateCollisionShapeArgs
struct b3CreateUserShapeArgs
{
int m_numCollisionShapes;
b3CreateCollisionShape m_shapes[MAX_COMPOUND_COLLISION_SHAPES];
int m_numUserShapes;
b3CreateUserShapeData m_shapes[MAX_COMPOUND_COLLISION_SHAPES];
};
struct b3CreateVisualShapeArgs
struct b3CreateUserShapeResultArgs
{
int m_visualShapeUniqueId;
int m_userShapeUniqueId;
};
#define MAX_CREATE_MULTI_BODY_LINKS 128
enum eCreateMultiBodyEnum
{
@ -910,16 +914,6 @@ struct b3CreateMultiBodyArgs
#endif
};
struct b3CreateCollisionShapeResultArgs
{
int m_collisionShapeUniqueId;
};
struct b3CreateVisualShapeResultArgs
{
int m_visualShapeUniqueId;
};
struct b3CreateMultiBodyResultArgs
{
int m_bodyUniqueId;
@ -980,8 +974,7 @@ struct SharedMemoryCommand
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
struct b3ObjectArgs m_removeObjectArgs;
struct b3Profile m_profile;
struct b3CreateCollisionShapeArgs m_createCollisionShapeArgs;
struct b3CreateVisualShapeArgs m_createVisualShapeArgs;
struct b3CreateUserShapeArgs m_createUserShapeArgs;
struct b3CreateMultiBodyArgs m_createMultiBodyArgs;
struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs;
struct b3ChangeTextureArgs m_changeTextureArgs;
@ -1053,8 +1046,7 @@ struct SharedMemoryStatus
struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs;
struct b3ObjectArgs m_removeObjectArgs;
struct b3DynamicsInfo m_dynamicsInfo;
struct b3CreateCollisionShapeResultArgs m_createCollisionShapeResultArgs;
struct b3CreateVisualShapeResultArgs m_createVisualShapeResultArgs;
struct b3CreateUserShapeResultArgs m_createUserShapeResultArgs;
struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs;
struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs;
struct SendMouseEvents m_sendMouseEvents;

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -624,6 +624,13 @@ enum eUrdfCollisionFlags
GEOM_FORCE_CONCAVE_TRIMESH=1,
};
enum eUrdfVisualFlags
{
GEOM_VISUAL_HAS_RGBA_COLOR=1,
GEOM_VISUAL_HAS_SPECULAR_COLOR=2,
};
enum eStateLoggingFlags
{
STATE_LOG_JOINT_MOTOR_TORQUES=1,

32
examples/pybullet/examples/createVisualShape.py Normal file
View File

@ -0,0 +1,32 @@
import pybullet as p
import time
p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0,-0.02,0]
meshScale=[0.1,0.1,0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
rangex = 40
rangey = 40
for i in range (rangex):
for j in range (rangey ):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.stopStateLogging(logId)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-10)
time.sleep(1)

138
examples/pybullet/pybullet.c
View File

@ -5226,15 +5226,18 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
double meshScale[3] = {1,1,1};
PyObject* planeNormalObj=0;
double planeNormal[3] = {0,0,1};
PyObject* collisionFramePositionObj=0;
double collisionFramePosition[3]={0,0,0};
PyObject* collisionFrameOrientationObj=0;
double collisionFrameOrientation[4]={0,0,0,1};
char* fileName=0;
int flags = 0;
PyObject* halfExtentsObj=0;
static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOii", kwlist,
&shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &physicsClientId))
static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOi", kwlist,
&shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags,&collisionFramePositionObj, &collisionFrameOrientationObj, &physicsClientId))
{
return NULL;
}
@ -5284,7 +5287,20 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
{
b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags);
}
if (shapeIndex>=0)
{
if (collisionFramePositionObj)
{
pybullet_internalSetVectord(collisionFramePositionObj,collisionFramePosition);
}
if (collisionFrameOrientationObj)
{
pybullet_internalSetVectord(collisionFrameOrientationObj,collisionFrameOrientation);
}
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition,collisionFrameOrientation);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
@ -5297,41 +5313,124 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
PyErr_SetString(SpamError, "createCollisionShape failed.");
return NULL;
}
#if 0
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle);
#endif
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int test=-1;
static char* kwlist[] = {"test",NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &test,
&physicsClientId))
int shapeType=-1;
double radius=0.5;
double height = 1;
PyObject* meshScaleObj=0;
double meshScale[3] = {1,1,1};
PyObject* planeNormalObj=0;
double planeNormal[3] = {0,0,1};
PyObject* rgbaColorObj=0;
double rgbaColor[4] = {1,1,1,1};
PyObject* specularColorObj=0;
double specularColor[3] = {1,1,1};
char* fileName=0;
int flags = 0;
PyObject* visualFramePositionObj=0;
double visualFramePosition[3]={0,0,0};
PyObject* visualFrameOrientationObj=0;
double visualFrameOrientation[4]={0,0,0,1};
PyObject* halfExtentsObj=0;
static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
&shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (test>=0)
if (shapeType>=GEOM_SPHERE)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm);
int shapeIndex = -1;
if (shapeType==GEOM_SPHERE && radius>0)
{
shapeIndex = b3CreateVisualShapeAddSphere(commandHandle,radius);
}
if (shapeType==GEOM_BOX && halfExtentsObj)
{
double halfExtents[3] = {1,1,1};
pybullet_internalSetVectord(halfExtentsObj,halfExtents);
shapeIndex = b3CreateVisualShapeAddBox(commandHandle,halfExtents);
}
if (shapeType==GEOM_CAPSULE && radius>0 && height>=0)
{
shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,height);
}
if (shapeType==GEOM_CYLINDER && radius>0 && height>=0)
{
shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,height);
}
if (shapeType==GEOM_MESH && fileName)
{
pybullet_internalSetVectord(meshScaleObj,meshScale);
shapeIndex = b3CreateVisualShapeAddMesh(commandHandle, fileName,meshScale);
}
if (shapeType==GEOM_PLANE)
{
double planeConstant=0;
pybullet_internalSetVectord(planeNormalObj,planeNormal);
shapeIndex = b3CreateVisualShapeAddPlane(commandHandle, planeNormal, planeConstant);
}
if (shapeIndex>=0 && flags)
{
b3CreateVisualSetFlag(commandHandle,shapeIndex,flags);
}
if (shapeIndex>=0)
{
double rgbaColor[4] = {1,1,1,1};
double specularColor[3] = {1,1,1};
if (rgbaColorObj)
{
pybullet_internalSetVector4d(rgbaColorObj,rgbaColor);
}
b3CreateVisualShapeSetRGBAColor(commandHandle,shapeIndex, rgbaColor);
if (specularColorObj)
{
pybullet_internalSetVectord(specularColorObj,specularColor);
}
b3CreateVisualShapeSetSpecularColor(commandHandle,shapeIndex,specularColor);
if (visualFramePositionObj)
{
pybullet_internalSetVectord(visualFramePositionObj,visualFramePosition);
}
if (visualFrameOrientationObj)
{
pybullet_internalSetVectord(visualFrameOrientationObj,visualFrameOrientation);
}
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, visualFramePosition,visualFrameOrientation);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
@ -5341,7 +5440,6 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
return ob;
}
}
PyErr_SetString(SpamError, "createVisualShape failed.");
return NULL;
}