Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add capsule, cylinder, plane, mesh support for pybullet.createCollisionShape

Browse Source 
preparation to add links to pybullet.createMultiBody
This commit is contained in:
Erwin Coumans
2017-06-19 10:14:26 -07:00
parent 5a8f12284a
commit f3c11b6f31
6 changed files with 648 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

192
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -721,7 +721,8 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan
}
return 0;
}
void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)
int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
@@ -735,11 +736,13 @@ void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,d
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
command->m_createCollisionShapeArgs.m_numCollisionShapes++;
return shapeIndex;
}
}
return -1;
}
void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3])
int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
@@ -755,6 +758,112 @@ void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doub
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++;
return shapeIndex;
}
}
return -1;
}
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)
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE;
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++;
return shapeIndex;
}
}
return -1;
}
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)
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER;
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++;
return shapeIndex;
}
}
return -1;
}
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)
{
int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE;
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++;
return shapeIndex;
}
}
return -1;
}
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);
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_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;
}
void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
if (command->m_type==CMD_CREATE_COLLISION_SHAPE)
{
if (shapeIndex<command->m_createCollisionShapeArgs.m_numCollisionShapes)
{
command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags;
}
}
}
@@ -857,14 +966,14 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass
int baseLinkIndex = numLinks;
command->m_updateFlags |=MULTI_BODY_HAS_BASE;
command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex;
command->m_createMultiBodyArgs.m_baseWorldPosition[0]=basePosition[0];
command->m_createMultiBodyArgs.m_baseWorldPosition[1]=basePosition[1];
command->m_createMultiBodyArgs.m_baseWorldPosition[2]=basePosition[2];
command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+0]=basePosition[0];
command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+1]=basePosition[1];
command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+2]=basePosition[2];
command->m_createMultiBodyArgs.m_baseWorldOrientation[0]=baseOrientation[0];
command->m_createMultiBodyArgs.m_baseWorldOrientation[1]=baseOrientation[1];
command->m_createMultiBodyArgs.m_baseWorldOrientation[2]=baseOrientation[2];
command->m_createMultiBodyArgs.m_baseWorldOrientation[3]=baseOrientation[3];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+0]=baseOrientation[0];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+1]=baseOrientation[1];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+2]=baseOrientation[2];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+3]=baseOrientation[3];
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass;
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass;
@@ -883,7 +992,7 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass
command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique;
command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId;
command->m_createMultiBodyArgs.m_linkMasses[command->m_createMultiBodyArgs.m_numLinks] = mass;
command->m_createMultiBodyArgs.m_linkMasses[baseLinkIndex] = mass;
command->m_createMultiBodyArgs.m_numLinks++;
}
return numLinks;
@@ -891,6 +1000,69 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass
return -2;
}
int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
double linkVisualShapeIndex,
double linkPosition[3],
double linkOrientation[4],
int linkParentIndex,
int linkJointType,
double linkJointAxis[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
int numLinks = command->m_createMultiBodyArgs.m_numLinks;
if (numLinks<MAX_CREATE_MULTI_BODY_LINKS)
{
int linkIndex = numLinks;
command->m_updateFlags |=MULTI_BODY_HAS_BASE;
command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+0]=linkPosition[0];
command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+1]=linkPosition[1];
command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+2]=linkPosition[2];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+0]=linkOrientation[0];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+1]=linkOrientation[1];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+2]=linkOrientation[2];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+3]=linkOrientation[3];
command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+0] = linkMass;
command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+1] = linkMass;
command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+2] = linkMass;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+0] = 0;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+1] = 0;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+2] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+0] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+1] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+2] = 0;
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+3] = 1;
command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex]= linkCollisionShapeIndex;
command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] = linkVisualShapeIndex;
command->m_createMultiBodyArgs.m_linkParentIndices[linkIndex] = linkParentIndex;
command->m_createMultiBodyArgs.m_linkJointTypes[linkIndex] = linkJointType;
command->m_createMultiBodyArgs.m_linkJointAxis[0] = linkJointAxis[0];
command->m_createMultiBodyArgs.m_linkJointAxis[1] = linkJointAxis[1];
command->m_createMultiBodyArgs.m_linkJointAxis[2] = linkJointAxis[2];
command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass;
command->m_createMultiBodyArgs.m_numLinks++;
return numLinks;
}
}
return -1;
}
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle)
{

19
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -326,9 +326,13 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]);
int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]);
int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant);
int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]);
void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]);
@@ -340,6 +344,15 @@ int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]);
int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
double linkVisualShapeIndex,
double linkPosition[3],
double linkOrientation[4],
int linkParentIndex,
int linkJointType,
double linkJointAxis[3]);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);

185
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -11,8 +11,9 @@
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h"
#include "../Importers/ImportURDFDemo/UrdfParser.h"
#include "../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3FileUtils.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
@@ -1620,6 +1621,14 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
for (int i=0;i<m_createBodyArgs.m_numLinks;i++)
{
if (m_createBodyArgs.m_linkParents[i] == urdfLinkIndex)
{
childLinkIndices.push_back(i);
}
}
}
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
@@ -1637,15 +1646,17 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
int baseLinkIndex = m_createBodyArgs.m_baseLinkIndex;
rootTransformInWorld.setOrigin(btVector3(
m_createBodyArgs.m_baseWorldPosition[0],
m_createBodyArgs.m_baseWorldPosition[1],
m_createBodyArgs.m_baseWorldPosition[2]));
m_createBodyArgs.m_linkPositions[baseLinkIndex*3+0],
m_createBodyArgs.m_linkPositions[baseLinkIndex*3+1],
m_createBodyArgs.m_linkPositions[baseLinkIndex*3+2]));
rootTransformInWorld.setRotation(btQuaternion(
m_createBodyArgs.m_baseWorldOrientation[0],
m_createBodyArgs.m_baseWorldOrientation[1],
m_createBodyArgs.m_baseWorldOrientation[2],
m_createBodyArgs.m_baseWorldOrientation[3]));
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+0],
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+1],
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+2],
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+3]));
return true;
}
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld)
@@ -1661,6 +1672,19 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
{
#if 0
if (m_data->m_customVisualShapesConverter)
{
const UrdfModel& model = m_data->m_urdfParser.getModel();
UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (linkPtr)
{
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
}
}
}
#endif
}
virtual void setBodyUniqueId(int bodyId)
{
@@ -3493,6 +3517,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
btCollisionShape* shape = 0;
btCompoundShape* compound = 0;
@@ -3547,6 +3572,121 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
case GEOM_CAPSULE:
{
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
break;
}
case GEOM_CYLINDER:
{
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
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]);
shape = worldImporter->createPlaneShape(planeNormal,0);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
break;
}
case GEOM_MESH:
{
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]);
const std::string& urdf_path="";
std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName;
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
if (foundFile)
{
if (out_type==UrdfGeometry::FILE_OBJ)
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
//create a convex hull for each shape, and store it in a btCompoundShape
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
B3_PROFILE("createConvexHullFromShapes");
if (compound==0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f<faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt*meshScale,false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform,convexHull);
}
}
}
break;
}
default:
{
}
}
}
#if 0
@@ -3593,6 +3733,33 @@ 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;

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -816,10 +816,13 @@ struct b3CreateCollisionShape
double m_capsuleFrom[3];
double m_capsuleTo[3];
double m_planeNormal[3];
double m_planeConstant;
int m_meshFileType;
char m_meshFileName[1024];
double m_meshScale;
char m_meshFileName[VISUAL_SHAPE_MAX_PATH_LEN];
double m_meshScale[3];
int m_collisionFlags;
};
#define MAX_COMPOUND_COLLISION_SHAPES 16
@@ -847,17 +850,22 @@ struct b3CreateMultiBodyArgs
char m_bodyName[1024];
int m_baseLinkIndex;
double m_baseWorldPosition[3];
double m_baseWorldOrientation[4];
double m_linkPositions[3*MAX_CREATE_MULTI_BODY_LINKS];
double m_linkOrientations[4*MAX_CREATE_MULTI_BODY_LINKS];
int m_numLinks;
int m_linkParents[MAX_CREATE_MULTI_BODY_LINKS];
double m_linkMasses[MAX_CREATE_MULTI_BODY_LINKS];
double m_linkInertias[MAX_CREATE_MULTI_BODY_LINKS*3];
double m_linkInertialFramePositions[MAX_CREATE_MULTI_BODY_LINKS*3];
double m_linkInertialFrameOrientations[MAX_CREATE_MULTI_BODY_LINKS*4];
int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS];
int m_linkCollisionShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS];
int m_linkVisualShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS];
int m_linkParentIndices[MAX_CREATE_MULTI_BODY_LINKS];
int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS];
double m_linkJointAxis[3*MAX_CREATE_MULTI_BODY_LINKS];
#if 0
std::string m_name;

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -594,6 +594,12 @@ enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
GEOM_UNKNOWN,
};
enum eUrdfCollisionFlags
{
GEOM_FORCE_CONCAVE_TRIMESH=1,
};
#endif//SHARED_MEMORY_PUBLIC_H