diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index de1e2e2d9..1bd6504d6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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 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 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 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 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 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 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cbddd1ddc..04610f6cd 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 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& shapes, const btVector3& geomScale) - B3_PROFILE("createConvexHullFromShapes"); - if (compound==0) + if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) { - 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; fm_numvertices<=0) { + b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); + delete glmesh; + break; + } + btAlignedObjectArray 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; - 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); + for (int i=0; im_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])); } - convexHull->recalcLocalAabb(); - convexHull->optimizeConvexHull(); - compound->addChildShape(childTransform,convexHull); + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + { + BT_PROFILE("convert vertices"); + + for (int i=0; im_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 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& 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; faddPoint(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); + } } } } diff --git a/examples/pybullet/examples/createMultiBodyLinks.py b/examples/pybullet/examples/createMultiBodyLinks.py index 9c776cab6..bdb7f6739 100644 --- a/examples/pybullet/examples/createMultiBodyLinks.py +++ b/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) \ No newline at end of file diff --git a/examples/pybullet/examples/createSphereMultiBodies.py b/examples/pybullet/examples/createSphereMultiBodies.py index fb24febb1..f2b0fb59e 100644 --- a/examples/pybullet/examples/createSphereMultiBodies.py +++ b/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) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e7b83649c..c690d128a 100644 --- a/examples/pybullet/pybullet.c +++ b/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);