Allow to create concave collision meshes. Note that this is only supported for static (mass 0) multibodies.

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -716,6 +716,7 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan
 		b3Assert(command);
 		command->m_type = CMD_CREATE_COLLISION_SHAPE;
 		command->m_updateFlags =0;
+		command->m_createCollisionShapeArgs = {0};
 		command->m_createCollisionShapeArgs.m_numCollisionShapes = 0;
 		return (b3SharedMemoryCommandHandle) command;
 	}
@@ -733,6 +734,7 @@ int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,do
 		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++;
@@ -753,6 +755,7 @@ int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doubl
 		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];
@@ -775,6 +778,7 @@ int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,d
 		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;
@@ -796,6 +800,7 @@ int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,
 		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;
@@ -818,6 +823,7 @@ int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, do
 		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];
@@ -841,6 +847,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons
 		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];
@@ -856,6 +863,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons
 
 void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
 {
+	
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -14,7 +14,7 @@
 #include "../Importers/ImportURDFDemo/UrdfParser.h"
 #include "../Utils/b3ResourcePath.h"
 #include "Bullet3Common/b3FileUtils.h"
-
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -3704,50 +3704,102 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									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);
+										if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
-										//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
-										B3_PROFILE("createConvexHullFromShapes");
-										if (compound==0)
 										{
-											compound = worldImporter->createCompoundShape();
+											GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
-										}
+											
-										compound->setMargin(defaultCollisionMargin);
+											if (!glmesh || glmesh->m_numvertices<=0)
-
-										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)
 											{
+												b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
+												delete glmesh;
+												break;
+											}
+											btAlignedObjectArray<btVector3> convertedVerts;
+											convertedVerts.reserve(glmesh->m_numvertices);
+											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 pt;
+											for (int i=0; i<glmesh->m_numvertices; i++)
-												pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
+											{
-													shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
+												convertedVerts.push_back(btVector3(
-													shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+													glmesh->m_vertices->at(i).xyzw[0]*meshScale[0],
-			
+													glmesh->m_vertices->at(i).xyzw[1]*meshScale[1],
-												convexHull->addPoint(pt*meshScale,false);
+													glmesh->m_vertices->at(i).xyzw[2]*meshScale[2]));
-
-												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();
+											BT_PROFILE("convert trimesh");
-											convexHull->optimizeConvexHull();
+											btTriangleMesh* meshInterface = new btTriangleMesh();
-											compound->addChildShape(childTransform,convexHull);
+											{
+												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);
+												//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
+												shape = trimesh;
+												if (compound)
+												{
+													compound->addChildShape(childTransform,shape);
+												}
+											}
+										} 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);
+											}
 										}
 									}
 								}

examples/pybullet/examples/createMultiBodyLinks.py

@@ -49,5 +49,8 @@ for i in range (p.getNumJoints(sphereUid)):
 	p.getJointInfo(sphereUid,i)
 	
 while (1):
+	keys = p.getKeyboardEvents()
+	print(keys)
+
 	time.sleep(0.01)
 	

examples/pybullet/examples/createSphereMultiBodies.py

@@ -4,7 +4,11 @@ import time
 useMaximalCoordinates = 0
 
 p.connect(p.GUI)
-p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
+#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
+monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
+orn = p.getQuaternionFromEuler([1.5707963,0,0])
+p.createMultiBody (0,monastryId, baseOrientation=orn)
+
 sphereRadius = 0.05
 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
 colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
@@ -28,5 +32,7 @@ p.setGravity(0,0,-10)
 p.setRealTimeSimulation(1)
 
 while (1):
+	keys = p.getKeyboardEvents()
+	#print(keys)
 	time.sleep(0.01)
 	

examples/pybullet/pybullet.c

@@ -5008,7 +5008,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 			pybullet_internalSetVectord(planeNormalObj,planeNormal);
 			shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant);
 		}
-		if (shapeIndex && flags)
+		if (shapeIndex>=0 && flags)
 		{
 			b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags);
 		}
@@ -7275,6 +7275,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
 	PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
 
+	PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH);
+
 
 
 	SpamError = PyErr_NewException("pybullet.error", NULL, NULL);