btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) { btCollisionShape* shape = 0; switch (collision->m_geometry.m_type) { case URDF_GEOM_CYLINDER: { btScalar cylRadius = collision->m_geometry.m_cylinderRadius; btScalar cylLength = collision->m_geometry.m_cylinderLength; btAlignedObjectArray<btVector3> vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); int numSteps = 32; for (int i=0;i<numSteps;i++) { btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.); vertices.push_back(vert); vert[2] = -cylLength/2.; vertices.push_back(vert); } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); cylZShape->setMargin(0.001); cylZShape->initializePolyhedralFeatures(); //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); shape = cylZShape; break; } case URDF_GEOM_BOX: { btVector3 extents = collision->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); shape = boxShape; shape ->setMargin(0.001); break; } case URDF_GEOM_SPHERE: { btScalar radius = collision->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); shape = sphereShape; shape ->setMargin(0.001); break; break; } case URDF_GEOM_MESH: { if (collision->m_name.length()) { //b3Printf("collision->name=%s\n",collision->m_name.c_str()); } if (1) { if (collision->m_geometry.m_meshFileName.length()) { const char* filename = collision->m_geometry.m_meshFileName.c_str(); //b3Printf("mesh->filename=%s\n",filename); char fullPath[1024]; int fileType = 0; sprintf(fullPath,"%s%s",urdfPathPrefix,filename); b3FileUtils::toLower(fullPath); char tmpPathPrefix[1024]; int maxPathLen = 1024; b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); char collisionPathPrefix[1024]; sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); if (strstr(fullPath,".dae")) { fileType = FILE_COLLADA; } if (strstr(fullPath,".stl")) { fileType = FILE_STL; } if (strstr(fullPath,".obj")) { fileType = FILE_OBJ; } sprintf(fullPath,"%s%s",urdfPathPrefix,filename); FILE* f = fopen(fullPath,"rb"); if (f) { fclose(f); GLInstanceGraphicsShape* glmesh = 0; switch (fileType) { case FILE_OBJ: { glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix); break; } case FILE_STL: { glmesh = LoadMeshFromSTL(fullPath); break; } case FILE_COLLADA: { btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btTransform upAxisTrans;upAxisTrans.setIdentity(); float unitMeterScaling=1; int upAxis = 2; LoadMeshFromCollada(fullPath, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis ); glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); for (int i=0;i<visualShapeInstances.size();i++) { ColladaGraphicsInstance* instance = &visualShapeInstances[i]; GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; b3AlignedObjectArray<GLInstanceVertex> verts; verts.resize(gfxShape->m_vertices->size()); int baseIndex = glmesh->m_vertices->size(); for (int i=0;i<gfxShape->m_vertices->size();i++) { verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; } int curNumIndices = glmesh->m_indices->size(); int additionalIndices = gfxShape->m_indices->size(); glmesh->m_indices->resize(curNumIndices+additionalIndices); for (int k=0;k<additionalIndices;k++) { glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex; } //compensate upAxisTrans and unitMeterScaling here btMatrix4x4 upAxisMat; upAxisMat.setIdentity(); //upAxisMat.setPureRotation(upAxisTrans.getRotation()); btMatrix4x4 unitMeterScalingMat; unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling)); btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat; //btMatrix4x4 worldMat = instance->m_worldTransform; int curNumVertices = glmesh->m_vertices->size(); int additionalVertices = verts.size(); glmesh->m_vertices->reserve(curNumVertices+additionalVertices); for(int v=0;v<verts.size();v++) { btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]); pos = worldMat*pos; verts[v].xyzw[0] = float(pos[0]); verts[v].xyzw[1] = float(pos[1]); verts[v].xyzw[2] = float(pos[2]); glmesh->m_vertices->push_back(verts[v]); } } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); //glmesh = LoadMeshFromCollada(fullPath); break; } default: { b3Warning("Unsupported file type in Collision: %s\n",fullPath); btAssert(0); } } if (glmesh && (glmesh->m_numvertices>0)) { //b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath); //int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size()); //convex->setUserIndex(shapeId); 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],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2])); } //btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex)); btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); //cylZShape->initializePolyhedralFeatures(); //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); cylZShape->setMargin(0.001); shape = cylZShape; } else { b3Warning("issue extracting mesh from STL file %s\n", fullPath); } delete glmesh; } else { b3Warning("mesh geometry not found %s\n",fullPath); } } } break; } default: { b3Warning("Error: unknown visual geometry type\n"); } } return shape; }
void RigidBodyFromObjExample::initPhysics() { m_guiHelper->setUpAxis(1); createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //if (m_dynamicsWorld->getDebugDrawer()) // m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); ///create a few basic rigid bodies btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); { btScalar mass(0.); createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); } //load our obj mesh const char* fileName = "teddy.obj";//sphere8.obj";//sponza_closed.obj";//sphere8.obj"; char relativeFileName[1024]; if (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) { char pathPrefix[1024]; b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, ""); printf("[INFO] Obj loaded: Extracted %d verticed from obj file [%s]\n", glmesh->m_numvertices, fileName); const GLInstanceVertex& v = glmesh->m_vertices->at(0); btConvexHullShape* shape = new btConvexHullShape((const btScalar*)(&(v.xyzw[0])), glmesh->m_numvertices, sizeof(GLInstanceVertex)); float scaling[4] = {0.1,0.1,0.1,1}; btVector3 localScaling(scaling[0],scaling[1],scaling[2]); shape->setLocalScaling(localScaling); if (m_options & OptimizeConvexObj) { shape->optimizeConvexHull(); } if (m_options & ComputePolyhedralFeatures) { shape->initializePolyhedralFeatures(); } //shape->setMargin(0.001); m_collisionShapes.push_back(shape); btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) shape->calculateLocalInertia(mass,localInertia); float color[4] = {1,1,1,1}; float orn[4] = {0,0,0,1}; float pos[4] = {0,3,0,0}; btVector3 position(pos[0],pos[1],pos[2]); startTransform.setOrigin(position); btRigidBody* body = createRigidBody(mass,startTransform,shape); bool useConvexHullForRendering = ((m_options & ObjUseConvexHullForRendering)!=0); if (!useConvexHullForRendering) { int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, &glmesh->m_indices->at(0), glmesh->m_numIndices, B3_GL_TRIANGLES, -1); shape->setUserIndex(shapeId); int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling); body->setUserIndex(renderInstance); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut) { GLInstanceGraphicsShape* glmesh = 0; btConvexShape* convexColShape = 0; switch (visual->m_geometry.m_type) { case URDF_GEOM_CYLINDER: { btAlignedObjectArray<btVector3> vertices; //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); int numSteps = 32; for (int i = 0; i<numSteps; i++) { btScalar cylRadius = visual->m_geometry.m_cylinderRadius; btScalar cylLength = visual->m_geometry.m_cylinderLength; btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); vertices.push_back(vert); vert[2] = -cylLength / 2.; vertices.push_back(vert); } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); cylZShape->setMargin(0.001); convexColShape = cylZShape; break; } case URDF_GEOM_BOX: { btVector3 extents = visual->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); convexColShape = boxShape; convexColShape->setMargin(0.001); break; } case URDF_GEOM_SPHERE: { btScalar radius = visual->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); convexColShape = sphereShape; convexColShape->setMargin(0.001); break; break; } case URDF_GEOM_MESH: { if (visual->m_name.length()) { //b3Printf("visual->name=%s\n", visual->m_name.c_str()); } if (1)//visual->m_geometry) { if (visual->m_geometry.m_meshFileName.length()) { const char* filename = visual->m_geometry.m_meshFileName.c_str(); //b3Printf("mesh->filename=%s\n", filename); char fullPath[1024]; int fileType = 0; char tmpPathPrefix[1024]; std::string xml_string; int maxPathLen = 1024; b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); char visualPathPrefix[1024]; sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); sprintf(fullPath, "%s%s", urdfPathPrefix, filename); b3FileUtils::toLower(fullPath); if (strstr(fullPath, ".dae")) { fileType = FILE_COLLADA; } if (strstr(fullPath, ".stl")) { fileType = FILE_STL; } if (strstr(fullPath,".obj")) { fileType = FILE_OBJ; } sprintf(fullPath, "%s%s", urdfPathPrefix, filename); FILE* f = fopen(fullPath, "rb"); if (f) { fclose(f); switch (fileType) { case FILE_OBJ: { glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); break; } case FILE_STL: { glmesh = LoadMeshFromSTL(fullPath); break; } case FILE_COLLADA: { btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes; btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances; btTransform upAxisTrans; upAxisTrans.setIdentity(); float unitMeterScaling = 1; int upAxis = 2; LoadMeshFromCollada(fullPath, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis); glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); for (int i = 0; i<visualShapeInstances.size(); i++) { ColladaGraphicsInstance* instance = &visualShapeInstances[i]; GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex]; b3AlignedObjectArray<GLInstanceVertex> verts; verts.resize(gfxShape->m_vertices->size()); int baseIndex = glmesh->m_vertices->size(); for (int i = 0; i<gfxShape->m_vertices->size(); i++) { verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; } int curNumIndices = glmesh->m_indices->size(); int additionalIndices = gfxShape->m_indices->size(); glmesh->m_indices->resize(curNumIndices + additionalIndices); for (int k = 0; k<additionalIndices; k++) { glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; } //compensate upAxisTrans and unitMeterScaling here btMatrix4x4 upAxisMat; upAxisMat.setIdentity(); // upAxisMat.setPureRotation(upAxisTrans.getRotation()); btMatrix4x4 unitMeterScalingMat; unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; //btMatrix4x4 worldMat = instance->m_worldTransform; int curNumVertices = glmesh->m_vertices->size(); int additionalVertices = verts.size(); glmesh->m_vertices->reserve(curNumVertices + additionalVertices); for (int v = 0; v<verts.size(); v++) { btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]); pos = worldMat*pos; verts[v].xyzw[0] = float(pos[0]); verts[v].xyzw[1] = float(pos[1]); verts[v].xyzw[2] = float(pos[2]); glmesh->m_vertices->push_back(verts[v]); } } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); //glmesh = LoadMeshFromCollada(fullPath); break; } default: { b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath); btAssert(0); } } if (glmesh && (glmesh->m_numvertices>0)) { } else { b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); } } else { b3Warning("mesh geometry not found %s\n", fullPath); } } } break; } default: { b3Warning("Error: unknown visual geometry type\n"); } } //if we have a convex, tesselate into localVertices/localIndices if ((glmesh==0) && convexColShape) { btShapeHull* hull = new btShapeHull(convexColShape); hull->buildHull(0.0); { // int strideInBytes = 9*sizeof(float); int numVertices = hull->numVertices(); int numIndices = hull->numIndices(); glmesh = new GLInstanceGraphicsShape; // int index = 0; glmesh->m_indices = new b3AlignedObjectArray<int>(); glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>(); for (int i = 0; i < numVertices; i++) { GLInstanceVertex vtx; btVector3 pos = hull->getVertexPointer()[i]; vtx.xyzw[0] = pos.x(); vtx.xyzw[1] = pos.y(); vtx.xyzw[2] = pos.z(); vtx.xyzw[3] = 1.f; pos.normalize(); vtx.normal[0] = pos.x(); vtx.normal[1] = pos.y(); vtx.normal[2] = pos.z(); vtx.uv[0] = 0.5f; vtx.uv[1] = 0.5f; glmesh->m_vertices->push_back(vtx); } btAlignedObjectArray<int> indices; for (int i = 0; i < numIndices; i++) { glmesh->m_indices->push_back(hull->getIndexPointer()[i]); } glmesh->m_numvertices = glmesh->m_vertices->size(); glmesh->m_numIndices = glmesh->m_indices->size(); } delete hull; delete convexColShape; convexColShape = 0; } if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) { int baseIndex = verticesOut.size(); for (int i = 0; i < glmesh->m_indices->size(); i++) { indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex); } for (int i = 0; i < glmesh->m_vertices->size(); i++) { GLInstanceVertex& v = glmesh->m_vertices->at(i); btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]); btVector3 vt = visualTransform*vert; v.xyzw[0] = vt[0]; v.xyzw[1] = vt[1]; v.xyzw[2] = vt[2]; btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]); triNormal = visualTransform.getBasis()*triNormal; v.normal[0] = triNormal[0]; v.normal[1] = triNormal[1]; v.normal[2] = triNormal[2]; verticesOut.push_back(v); } } delete glmesh; }