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

further work on urdfEditor.py, fix some serialization issues

Browse Source
This commit is contained in:
Erwin Coumans
2018-01-08 12:25:56 -08:00
parent a85a4f387b
commit e97b751781
23 changed files with 1926 additions and 1455 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -68,9 +68,9 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisComOffset.m_floats[0];
info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisComOffset.m_floats[1];
info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisComOffset.m_floats[2];
info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[0];
info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[1];
info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[2];
info.m_parentFrame[3] = mb->m_links[link].m_zeroRotParentToThis.m_floats[0];
info.m_parentFrame[4] = mb->m_links[link].m_zeroRotParentToThis.m_floats[1];
info.m_parentFrame[5] = mb->m_links[link].m_zeroRotParentToThis.m_floats[2];

546
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -29,6 +29,7 @@
#include "LinearMath/btTransform.h"
#include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
#include "../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "LinearMath/btSerializer.h"
@@ -1510,6 +1511,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback;
@@ -2132,11 +2134,25 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
if (colShapeUniqueId>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
if (handle && handle->m_collisionShape)
{
btTransform childTrans;
childTrans.setIdentity();
compound->addChildShape(localInertiaFrame.inverse()*childTrans,handle->m_collisionShape);
if (handle->m_collisionShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* childCompound = (btCompoundShape*)handle->m_collisionShape;
for (int c = 0; c < childCompound->getNumChildShapes(); c++)
{
btTransform childTrans = childCompound->getChildTransform(c);
btCollisionShape* childShape = childCompound->getChildShape(c);
btTransform tr = localInertiaFrame.inverse()*childTrans;
compound->addChildShape(tr, childShape);
}
}
else
{
btTransform childTrans;
childTrans.setIdentity();
compound->addChildShape(localInertiaFrame.inverse()*childTrans, handle->m_collisionShape);
}
}
}
m_allocatedCollisionShapes.push_back(compound);
@@ -2378,6 +2394,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
}
m_data->m_meshInterfaces.clear();
m_data->m_collisionShapes.clear();
m_data->m_bulletCollisionShape2UrdfCollision.clear();
delete m_data->m_dynamicsWorld;
m_data->m_dynamicsWorld=0;
@@ -2620,6 +2637,25 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
{
btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
m_data->m_collisionShapes.push_back(shape);
UrdfCollision urdfCollision;
if (u2b.getUrdfFromCollisionShape(shape, urdfCollision))
{
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, urdfCollision);
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)shape;
for (int c = 0; c < compound->getNumChildShapes(); c++)
{
btCollisionShape* childShape = compound->getChildShape(c);
if (u2b.getUrdfFromCollisionShape(childShape, urdfCollision))
{
m_data->m_bulletCollisionShape2UrdfCollision.insert(childShape, urdfCollision);
}
}
}
}
m_data->m_saveWorldBodyData.push_back(sd);
@@ -3474,7 +3510,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
btScalar defaultCollisionMargin = 0.001;
btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
btCollisionShape* shape = 0;
@@ -3486,8 +3523,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
compound = worldImporter->createCompoundShape();
}
for (int i=0;i<clientCmd.m_createUserShapeArgs.m_numUserShapes;i++)
for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++)
{
GLInstanceGraphicsShape* glmesh = 0;
char pathPrefix[1024] = "";
char relativeFileName[1024] = "";
UrdfCollision urdfColObj;
btTransform childTransform;
@@ -3503,7 +3543,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[2],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[3]
));
if (compound==0)
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
@@ -3516,227 +3556,274 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type)
{
case GEOM_SPHERE:
case GEOM_SPHERE:
{
double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
if (compound)
{
double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
urdfColObj.m_geometry.m_sphereRadius = radius;
break;
compound->addChildShape(childTransform, shape);
}
case GEOM_BOX:
urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
urdfColObj.m_geometry.m_sphereRadius = radius;
break;
}
case GEOM_BOX:
{
//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
btVector3 halfExtents(
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)
{
//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
btVector3 halfExtents(
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)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
break;
compound->addChildShape(childTransform, shape);
}
case GEOM_CAPSULE:
urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
break;
}
case GEOM_CAPSULE:
{
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
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_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
break;
compound->addChildShape(childTransform, shape);
}
case GEOM_CYLINDER:
{
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_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
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;
break;
}
case GEOM_CYLINDER:
{
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
0.5*clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform, shape);
}
case GEOM_PLANE:
{
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]);
urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
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;
shape = worldImporter->createPlaneShape(planeNormal,0);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
urdfColObj.m_geometry.m_planeNormal.setValue(
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;
break;
}
case GEOM_PLANE:
{
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)
{
compound->addChildShape(childTransform, shape);
}
case GEOM_MESH:
{
btScalar defaultCollisionMargin = 0.001;
urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
urdfColObj.m_geometry.m_planeNormal.setValue(
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]);
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]);
break;
}
case GEOM_MESH:
{
const std::string& urdf_path="";
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]);
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;
urdfColObj.m_geometry.m_meshScale = meshScale;
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
const std::string& urdf_path = "";
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
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;
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
if (foundFile)
{
urdfColObj.m_geometry.m_meshFileType = out_type;
if (out_type==UrdfGeometry::FILE_OBJ)
{
//create a convex hull for each shape, and store it in a btCompoundShape
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
if (!glmesh || glmesh->m_numvertices<=0)
{
b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
delete glmesh;
break;
}
btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i=0; i<glmesh->m_numvertices; i++)
{
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0]*meshScale[0],
glmesh->m_vertices->at(i).xyzw[1]*meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*meshScale[2]));
}
BT_PROFILE("convert trimesh");
btTriangleMesh* meshInterface = new btTriangleMesh();
this->m_data->m_meshInterfaces.push_back(meshInterface);
{
BT_PROFILE("convert vertices");
for (int i=0; i<glmesh->m_numIndices/3; i++)
{
const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)];
const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)];
const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)];
meshInterface->addTriangle(v0,v1,v2);
}
}
{
BT_PROFILE("create btBvhTriangleMeshShape");
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
m_data->m_collisionShapes.push_back(trimesh);
//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
shape = trimesh;
if (compound)
{
compound->addChildShape(childTransform,shape);
}
}
delete glmesh;
} else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
//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]);
urdfColObj.m_geometry.m_meshScale = meshScale;
convexHull->addPoint(pt*meshScale,false);
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
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);
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
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);
}
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform,convexHull);
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (foundFile)
{
urdfColObj.m_geometry.m_meshFileType = out_type;
if (out_type == UrdfGeometry::FILE_STL)
{
glmesh = LoadMeshFromSTL(relativeFileName);
}
if (out_type == UrdfGeometry::FILE_OBJ)
{
//create a convex hull for each shape, and store it in a btCompoundShape
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str());
//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:
{
}
break;
}
default:
{
}
}
if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN)
{
urdfCollisionObjects.push_back(urdfColObj);
}
}
if (glmesh)
{
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]);
if (!glmesh || glmesh->m_numvertices <= 0)
{
b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
delete glmesh;
}
else
{
btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i = 0; i < glmesh->m_numvertices; i++)
{
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0] * meshScale[0],
glmesh->m_vertices->at(i).xyzw[1] * meshScale[1],
glmesh->m_vertices->at(i).xyzw[2] * meshScale[2]));
}
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
BT_PROFILE("convert trimesh");
btTriangleMesh* meshInterface = new btTriangleMesh();
this->m_data->m_meshInterfaces.push_back(meshInterface);
{
BT_PROFILE("convert vertices");
for (int i = 0; i < glmesh->m_numIndices / 3; i++)
{
const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i * 3)];
const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i * 3 + 1)];
const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i * 3 + 2)];
meshInterface->addTriangle(v0, v1, v2);
}
}
{
BT_PROFILE("create btBvhTriangleMeshShape");
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
m_data->m_collisionShapes.push_back(trimesh);
//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
shape = trimesh;
if (compound)
{
compound->addChildShape(childTransform, shape);
}
}
delete glmesh;
}
else
{
//convex mesh
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
for (int v = 0; v < convertedVerts.size(); v++)
{
btVector3 pt = convertedVerts[v];
convexHull->addPoint(pt, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform, convexHull);
}
}
}
}
}
if (compound && compound->getNumChildShapes())
{
shape = compound;
@@ -8050,11 +8137,35 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape
{
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
sprintf( collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
collisionShapeBuffer[0].m_dimensions[0] = 1;
collisionShapeBuffer[0].m_dimensions[1] = 1;
collisionShapeBuffer[0].m_dimensions[2] = 1;
UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
if (urdfCol && (urdfCol->m_geometry.m_type == GEOM_MESH))
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
numConverted += 1;
}
else
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
sprintf(collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
collisionShapeBuffer[0].m_dimensions[0] = 1;
collisionShapeBuffer[0].m_dimensions[1] = 1;
collisionShapeBuffer[0].m_dimensions[2] = 1;
numConverted++;
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShapeZ* capsule = (btCapsuleShapeZ*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CAPSULE;
collisionShapeBuffer[0].m_dimensions[0] = 2.*capsule->getHalfHeight();
collisionShapeBuffer[0].m_dimensions[1] = capsule->getRadius();
collisionShapeBuffer[0].m_dimensions[2] = 0;
numConverted++;
break;
}
@@ -8091,14 +8202,29 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape
}
case COMPOUND_SHAPE_PROXYTYPE:
{
//recurse, accumulate childTransform
btCompoundShape* compound = (btCompoundShape*)colShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
//it could be a compound mesh from a wavefront OBJ, check it
UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
if (urdfCol && (urdfCol->m_geometry.m_type == GEOM_MESH))
{
btTransform childTrans = transform*compound->getChildTransform(i);
int remain = maxCollisionShapes - numConverted;
int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
numConverted += converted;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
numConverted += 1;
}
else
{
//recurse, accumulate childTransform
btCompoundShape* compound = (btCompoundShape*)colShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
btTransform childTrans = transform*compound->getChildTransform(i);
int remain = maxCollisionShapes - numConverted;
int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
numConverted += converted;
}
}
break;
}

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201801010
#define SHARED_MEMORY_MAGIC_NUMBER 201801080
//#define SHARED_MEMORY_MAGIC_NUMBER 201801010
//#define SHARED_MEMORY_MAGIC_NUMBER 201710180
//#define SHARED_MEMORY_MAGIC_NUMBER 201710050
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
@@ -544,7 +545,7 @@ struct b3CollisionShapeData
int m_objectUniqueId;
int m_linkIndex;
int m_collisionGeometryType;//GEOM_BOX, GEOM_SPHERE etc
double m_dimensions[3];//meaning depends on m_visualGeometryType GEOM_BOX: extents, GEOM_SPHERE: radius, GEOM_CAPSULE:
double m_dimensions[3];//meaning depends on m_visualGeometryType GEOM_BOX: extents, GEOM_SPHERE: radius, GEOM_CAPSULE+GEOM_CYLINDER:length, radius, GEOM_MESH: mesh scale
double m_localCollisionFrame[7];//pos[3], orn[4]
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
};