diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index d2550ac82..63ad3e30e 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -80,6 +80,7 @@ struct UrdfGeometry FILE_COLLADA = 2, FILE_OBJ = 3, FILE_CDF = 4, + MEMORY_VERTICES = 5, }; int m_meshFileType; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ad356d671..0f684bae6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1192,6 +1192,92 @@ B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle comm 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_shapes[shapeIndex].m_numVertices = 0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0; + + command->m_createUserShapeArgs.m_numUserShapes++; + return shapeIndex; + } + } + return -1; +} + +B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices) +{ + 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 && numVertices >= 0) + { + int i=0; + if (numVertices>B3_MAX_NUM_VERTICES) + numVertices=B3_MAX_NUM_VERTICES; + 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; + 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_shapes[shapeIndex].m_meshFileName[0]=0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0; + + for (i=0;im_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2]; + } + command->m_createUserShapeArgs.m_numUserShapes++; + return shapeIndex; + } + } + return -1; +} + +B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices) +{ + 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 && numVertices >= 0 && numIndices >=0) + { + int i=0; + if (numVertices>B3_MAX_NUM_VERTICES) + numVertices=B3_MAX_NUM_VERTICES; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH; + 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_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_shapes[shapeIndex].m_meshFileName[0]=0; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices; + + for (i=0;im_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1]; + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2]; + } + if (numIndices>B3_MAX_NUM_INDICES) + numIndices = B3_MAX_NUM_INDICES; + + command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices; + for (i=0;im_createUserShapeArgs.m_shapes[shapeIndex].m_indices[i]=indices[i]; + } command->m_createUserShapeArgs.m_numUserShapes++; return shapeIndex; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b50489b29..a556b9728 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -460,6 +460,8 @@ extern "C" B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height); B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant); B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]); + B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices); + B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices); B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags); B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]); B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 972269b42..651dde11b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4193,6 +4193,80 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str std::string out_found_filename; int out_type; + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices) + { + if (compound == 0) + { + compound = worldImporter->createCompoundShape(); + } + compound->setMargin(m_data->m_defaultCollisionMargin); + + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices) + { + BT_PROFILE("convert trimesh2"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + this->m_data->m_meshInterfaces.push_back(meshInterface); + { + BT_PROFILE("convert vertices2"); + + for (int j = 0; j < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices / 3; j++) + { + int i0 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+0]; + int i1 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+1]; + int i2 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+2]; + + btVector3 v0( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+2]); + btVector3 v1( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+2]); + btVector3 v2( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*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); + + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE) + { + btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap(); + btGenerateInternalEdgeInfo(trimesh, triangleInfoMap); + } + shape = trimesh; + if (compound) + { + compound->addChildShape(childTransform, shape); + shape->setMargin(m_data->m_defaultCollisionMargin); + } + } + } + else + { + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(m_data->m_defaultCollisionMargin); + + for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++) + { + + btVector3 pt( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+2]); + convexHull->addPoint(pt, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(childTransform, convexHull); + } + urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES; + break; + } bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); if (foundFile) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 63170d11b..cb75c0e46 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -38,6 +38,7 @@ typedef unsigned long long int smUint64_t; #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM #define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH + struct TmpFloat3 { float m_x; @@ -925,7 +926,10 @@ struct b3CreateUserShapeData double m_meshScale[3]; int m_collisionFlags; int m_visualFlags; - + int m_numVertices; + double m_vertices[B3_MAX_NUM_VERTICES*3]; + int m_numIndices; + int m_indices[B3_MAX_NUM_INDICES]; double m_rgbaColor[4]; double m_specularColor[3]; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 90ad43b76..b4f4aa6ca 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,8 @@ //Please don't replace an existing magic number: //instead, only ADD a new one at the top, comment-out previous one -#define SHARED_MEMORY_MAGIC_NUMBER 2018090300 +#define SHARED_MEMORY_MAGIC_NUMBER 201810250 +//#define SHARED_MEMORY_MAGIC_NUMBER 201809030 //#define SHARED_MEMORY_MAGIC_NUMBER 201809010 //#define SHARED_MEMORY_MAGIC_NUMBER 201807040 //#define SHARED_MEMORY_MAGIC_NUMBER 201806150 @@ -925,4 +926,8 @@ enum eFileIOTypes eCNSFileIO, }; +//limits for vertices/indices in PyBullet::createCollisionShape +#define B3_MAX_NUM_VERTICES 1024 +#define B3_MAX_NUM_INDICES 1024 + #endif //SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index e2ea84a22..399725ab2 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -320,6 +320,10 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa switch (visual->m_geometry.m_meshFileType) { + case UrdfGeometry::MEMORY_VERTICES: + { + break; + } case UrdfGeometry::FILE_OBJ: { //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); @@ -442,8 +446,10 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa } default: - // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) - btAssert(0); + { + // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) + btAssert(0); + } } if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices > 0)) @@ -458,7 +464,10 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa } else { - b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + if (visual->m_geometry.m_meshFileType !=UrdfGeometry::MEMORY_VERTICES) + { + b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + } } break; } // case mesh diff --git a/examples/pybullet/examples/createObstacleCourse.py b/examples/pybullet/examples/createObstacleCourse.py index 6baa636ff..1194a213a 100644 --- a/examples/pybullet/examples/createObstacleCourse.py +++ b/examples/pybullet/examples/createObstacleCourse.py @@ -12,10 +12,39 @@ p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]); p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) + sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) + +#a few different ways to create a mesh: + +#convex mesh from obj stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj") +#concave mesh from obj +stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH) + + +verts=[[-0.246350, -0.246483, -0.000624], + [ -0.151407, -0.176325, 0.172867], + [ -0.246350, 0.249205, -0.000624], + [ -0.151407, 0.129477, 0.172867], + [ 0.249338, -0.246483, -0.000624], + [ 0.154395, -0.176325, 0.172867], + [ 0.249338, 0.249205, -0.000624], + [ 0.154395, 0.129477, 0.172867]] +#convex mesh from vertices +stoneConvexId = p.createCollisionShape(p.GEOM_MESH,vertices=verts) + +indices=[0,3,2,3,6,2,7,4,6,5,0,4,6,0,2,3,5,7,0,1,3,3,7,6,7,5,4,5,1,0,6,4,0,3,1,5] + +#concave mesh from vertices+indices +stoneConcaveId = p.createCollisionShape(p.GEOM_MESH,vertices=verts, indices=indices) + +stoneId = stoneConvexId +#stoneId = stoneConcaveId + + boxHalfLength = 0.5 boxHalfWidth = 2.5 boxHalfHeight = 0.1 @@ -72,10 +101,10 @@ for i in range (segmentLength): p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) print(p.getNumJoints(boxId)) for joint in range (p.getNumJoints(boxId)): - targetVelocity = 1 + targetVelocity = 10 if (i%2): - targetVelocity =-1 - p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10) + targetVelocity =-10 + p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100) segmentStart=segmentStart-1.1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e44a5c350..5e61b5c89 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6284,6 +6284,75 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } + +static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices) +{ + int numVerticesOut=0; + + if (verticesObj) + { + PyObject* seqVerticesObj= PySequence_Fast(verticesObj, "expected a sequence of vertex positions"); + if (seqVerticesObj) + { + int numVerticesSrc = PySequence_Size(seqVerticesObj); + { + int i; + + if (numVerticesSrc > B3_MAX_NUM_VERTICES) + { + PyErr_SetString(SpamError, "Number of vertices exceeds the maximum."); + Py_DECREF(seqVerticesObj); + return 0; + } + for (i = 0; i < numVerticesSrc; i++) + { + PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i); + double vertex[3]; + if (pybullet_internalSetVectord(vertexObj, vertex)) + { + vertices[numVerticesOut*3+0]=vertex[0]; + vertices[numVerticesOut*3+1]=vertex[1]; + vertices[numVerticesOut*3+2]=vertex[2]; + numVerticesOut++; + } + } + } + } + } + return numVerticesOut; +} + +static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices) +{ + int numIndicesOut=0; + + if (indicesObj) + { + PyObject* seqIndicesObj= PySequence_Fast(indicesObj, "expected a sequence of indices"); + if (seqIndicesObj) + { + int numIndicesSrc = PySequence_Size(seqIndicesObj); + { + int i; + + if (numIndicesSrc > B3_MAX_NUM_INDICES) + { + PyErr_SetString(SpamError, "Number of indices exceeds the maximum."); + Py_DECREF(seqIndicesObj); + return 0; + } + for (i = 0; i < numIndicesSrc; i++) + { + int index = pybullet_internalGetIntFromSequence(seqIndicesObj,i); + indices[numIndicesOut]=index; + numIndicesOut++; + } + } + } + } + return numIndicesOut; +} + static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; @@ -6303,10 +6372,12 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P int flags = 0; PyObject* halfExtentsObj = 0; + PyObject* verticesObj = 0; + PyObject* indicesObj = 0; - 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)) + static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "vertices", "indices", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist, + &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &physicsClientId)) { return NULL; } @@ -6316,6 +6387,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + if (shapeType >= GEOM_SPHERE) { b3SharedMemoryStatusHandle statusHandle; @@ -6346,6 +6418,31 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P pybullet_internalSetVectord(meshScaleObj, meshScale); shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale); } + if (shapeType == GEOM_MESH && verticesObj) + { + int numVertices=0; + int numIndices=0; + + double vertices[B3_MAX_NUM_VERTICES*3]; + int indices[B3_MAX_NUM_INDICES]; + + numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES); + pybullet_internalSetVectord(meshScaleObj, meshScale); + + if (indicesObj) + { + numIndices = extractIndices(indicesObj, indices,B3_MAX_NUM_INDICES); + } + + if (numIndices) + { + shapeIndex = b3CreateCollisionShapeAddConcaveMesh(commandHandle, meshScale, vertices, numVertices, indices, numIndices); + } else + { + shapeIndex = b3CreateCollisionShapeAddConvexMesh(commandHandle, meshScale, vertices, numVertices); + } + } + if (shapeType == GEOM_PLANE) { double planeConstant = 0;