diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp index 488b24d54..6bd3cf948 100644 --- a/Extras/obj2sdf/obj2sdf.cpp +++ b/Extras/obj2sdf/obj2sdf.cpp @@ -13,6 +13,22 @@ #include "Bullet3Common/b3FileUtils.h" #include "../Utils/b3ResourcePath.h" #include "Bullet3Common/b3CommandLineArgs.h" +#include "Bullet3Common/b3HashMap.h" + +struct ShapeContainer +{ + std::string m_matName; + std::string m_shapeName; + tinyobj::material_t material; + std::vector positions; + std::vector normals; + std::vector texcoords; + std::vector indices; + + b3AlignedObjectArray m_shapeIndices; +}; + +b3HashMap gMaterialNames; #define MAX_PATH_LEN 1024 @@ -53,6 +69,8 @@ int main(int argc, char* argv[]) printf("Please use --fileName=\"pathToObj\"."); exit(0); } + bool mergeMaterials = args.CheckCmdLineFlag("mergeMaterials"); + char fileNameWithPath[MAX_PATH_LEN]; bool fileFound = (b3ResourcePath::findResourcePath(fileName,fileNameWithPath,MAX_PATH_LEN))>0; char materialPrefixPath[MAX_PATH_LEN]; @@ -72,141 +90,359 @@ int main(int argc, char* argv[]) fprintf(sdfFile, "\n\t\n\t0 0 -9.8\n"); - for (int s=0;s<(int)shapes.size();s++) + for (int s = 0; s < (int)shapes.size(); s++) { tinyobj::shape_t& shape = shapes[s]; - - if (shape.name.length()) - { - printf("object name = %s\n", shape.name.c_str()); - } - - char objFileName[MAX_PATH_LEN]; - if (strlen(materialPrefixPath)>0) - { - sprintf(objFileName,"%s/part%d.obj",materialPrefixPath,s); - } else - { - sprintf(objFileName,"part%d.obj",s); - } - FILE* f = fopen(objFileName,"w"); - if (f==0) - { - printf("Fatal error: cannot create part obj file %s\n",objFileName); - exit(0); - } - fprintf(f,"# Exported using automatic converter by Erwin Coumans\n"); - if (matLibName.length()) - { - fprintf(f,"mtllib %s.mtl\n", matLibName.c_str()); - } else - { - fprintf(f,"mtllib bedroom.mtl\n"); - - } - - - int faceCount = shape.mesh.indices.size(); - int vertexCount = shape.mesh.positions.size(); tinyobj::material_t mat = shape.material; - if (shape.name.length()) - { - const char* objName = shape.name.c_str(); - printf("mat.name = %s\n", objName); - fprintf(f,"#object %s\n\n",objName); - } - for (int v=0;vm_shapeIndices.push_back(s); + + int curPositions = shapeC->positions.size()/3; + int curNormals = shapeC->normals.size()/3; + int curTexcoords = shapeC->texcoords.size()/2; - fprintf(f,"\n"); - int numTexCoords = int(shape.mesh.texcoords.size()); - for (int vt = 0;vt=int(shape.mesh.indices.size())) + int faceCount = shape.mesh.indices.size(); + int vertexCount = shape.mesh.positions.size(); + for (int v = 0; v < vertexCount; v++) { - continue; + shapeC->positions.push_back(shape.mesh.positions[v]); + } + int numNormals = int(shape.mesh.normals.size()); + for (int vn = 0; vn < numNormals; vn++) + { + shapeC->normals.push_back(shape.mesh.normals[vn]); + } + int numTexCoords = int(shape.mesh.texcoords.size()); + for (int vt = 0; vt < numTexCoords; vt++) + { + shapeC->texcoords.push_back(shape.mesh.texcoords[vt]); + } + + for (int face = 0; face < faceCount; face += 3) + { + if (face < 0 && face >= int(shape.mesh.indices.size())) + { + continue; + } + + shapeC->indices.push_back(shape.mesh.indices[face] + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face+1] + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face + 2] + curPositions); } - fprintf(f,"f %d/%d/%d %d/%d/%d %d/%d/%d\n", - shape.mesh.indices[face]+1,shape.mesh.indices[face]+1,shape.mesh.indices[face]+1, - shape.mesh.indices[face+1]+1,shape.mesh.indices[face+1]+1,shape.mesh.indices[face+1]+1, - shape.mesh.indices[face+2]+1,shape.mesh.indices[face+2]+1,shape.mesh.indices[face+2]+1); } - fclose(f); + } + + printf("unique materials=%d\n", gMaterialNames.size()); - float kdRed=mat.diffuse[0]; - float kdGreen=mat.diffuse[1]; - float kdBlue=mat.diffuse[2]; + + if (mergeMaterials) + { + for (int m = 0; m < gMaterialNames.size();m++) + { + if (gMaterialNames.getAtIndex(m)->m_shapeIndices.size() == 0) + continue; - char objSdfPartFileName[MAX_PATH_LEN]; - sprintf(objSdfPartFileName,"part%d.obj",s); - fprintf(sdfFile,"\t\t\n" - "\t\t\t1\n" - "\t\t\t0 0 0 0 0 0\n" - "\t\t\t\n" - "\t\t\t\n" - "\t\t\t0\n" - "\t\t\t\n" - "\t\t\t0.166667\n" - "\t\t\t0\n" - "\t\t\t0\n" - "\t\t\t0.166667\n" - "\t\t\t0\n" - "\t\t\t0.166667\n" - "\t\t\t\n" - "\t\t\t\n" - "\t\t\t\n" - "\t\t\t\n" - "\t\t\t\n" - "\t\t\t1 1 1\n" - "\t\t\t\t%s\n" - "\t\t\t\n" - "\t\t\t\n" - "\t\t\t \n" - "\t\t\t\n" - "\t\t\t\t\n" - "\t\t\t\t\n" - "\t\t\t\t\t1 1 1\n" - "\t\t\t\t\t%s\n" - "\t\t\t\t\n" - "\t\t\t\t\n" - "\t\t\t\n" - "\t\t\t\t1 0 0 1\n" - "\t\t\t\t%f %f %f 1\n" - "\t\t\t\t0.1 0.1 0.1 1\n" - "\t\t\t\t0 0 0 0\n" - "\t\t\t \n" - "\t\t\t \n" - "\t\t\t \n" - "\t\t\t\n",objSdfPartFileName,s,s, - objSdfPartFileName,objSdfPartFileName, - kdRed, kdGreen, kdBlue); + ShapeContainer* shapeCon =gMaterialNames.getAtIndex(m); + + printf("object name = %s\n", shapeCon->m_shapeName.c_str()); + + char objSdfPartFileName[MAX_PATH_LEN]; + sprintf(objSdfPartFileName, "part%d.obj", m); + + char objFileName[MAX_PATH_LEN]; + if (strlen(materialPrefixPath) > 0) + { + sprintf(objFileName, "%s/part%d.obj", materialPrefixPath, m); + } + else + { + sprintf(objFileName, "part%d.obj", m); + } + FILE* f = fopen(objFileName, "w"); + if (f == 0) + { + printf("Fatal error: cannot create part obj file %s\n", objFileName); + exit(0); + } + fprintf(f, "# Exported using automatic converter by Erwin Coumans\n"); + if (matLibName.length()) + { + fprintf(f, "mtllib %s.mtl\n", matLibName.c_str()); + } + else + { + fprintf(f, "mtllib bedroom.mtl\n"); + + } + + int faceCount = shapeCon->indices.size(); + int vertexCount = shapeCon->positions.size(); + tinyobj::material_t mat = shapeCon->material; + if (shapeCon->m_matName.length()) + { + const char* objName = shapeCon->m_matName.c_str(); + printf("mat.name = %s\n", objName); + fprintf(f, "#object %s\n\n", objName); + } + for (int v = 0; v < vertexCount / 3; v++) + { + fprintf(f, "v %f %f %f\n", shapeCon->positions[v * 3 + 0], shapeCon->positions[v * 3 + 1], shapeCon->positions[v * 3 + 2]); + } + + if (mat.name.length()) + { + fprintf(f, "usemtl %s\n", mat.name.c_str()); + } + else + { + fprintf(f, "usemtl wire_028089177\n"); + } + + fprintf(f, "\n"); + int numNormals = int(shapeCon->normals.size()); + + for (int vn = 0; vn < numNormals / 3; vn++) + { + fprintf(f, "vn %f %f %f\n", shapeCon->normals[vn * 3 + 0], shapeCon->normals[vn * 3 + 1], shapeCon->normals[vn * 3 + 2]); + } + + fprintf(f, "\n"); + int numTexCoords = int(shapeCon->texcoords.size()); + for (int vt = 0; vt < numTexCoords / 2; vt++) + { + fprintf(f, "vt %f %f\n", shapeCon->texcoords[vt * 2 + 0], shapeCon->texcoords[vt * 2 + 1]); + } + + fprintf(f, "s off\n"); + + for (int face = 0; face < faceCount; face += 3) + { + if (face < 0 && face >= int(shapeCon->indices.size())) + { + continue; + } + fprintf(f, "f %d/%d/%d %d/%d/%d %d/%d/%d\n", + shapeCon->indices[face] + 1, shapeCon->indices[face] + 1, shapeCon->indices[face] + 1, + shapeCon->indices[face + 1] + 1, shapeCon->indices[face + 1] + 1, shapeCon->indices[face + 1] + 1, + shapeCon->indices[face + 2] + 1, shapeCon->indices[face + 2] + 1, shapeCon->indices[face + 2] + 1); + } + fclose(f); + + float kdRed = mat.diffuse[0]; + float kdGreen = mat.diffuse[1]; + float kdBlue = mat.diffuse[2]; + float transparency = mat.transparency; + + fprintf(sdfFile, "\t\t\n" + "\t\t\t1\n" + "\t\t\t0 0 0 0 0 0\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t0\n" + "\t\t\t\n" + "\t\t\t0.166667\n" + "\t\t\t0\n" + "\t\t\t0\n" + "\t\t\t0.166667\n" + "\t\t\t0\n" + "\t\t\t0.166667\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t1 1 1\n" + "\t\t\t\t%s\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t \n" + "\t\t\t\n" + "\t\t\t\t\n" + "\t\t\t\t\n" + "\t\t\t\t\t1 1 1\n" + "\t\t\t\t\t%s\n" + "\t\t\t\t\n" + "\t\t\t\t\n" + "\t\t\t\n" + "\t\t\t\t1 0 0 1\n" + "\t\t\t\t%f %f %f %f\n" + "\t\t\t\t0.1 0.1 0.1 1\n" + "\t\t\t\t0 0 0 0\n" + "\t\t\t \n" + "\t\t\t \n" + "\t\t\t \n" + "\t\t\t\n", objSdfPartFileName, m, m, + objSdfPartFileName, objSdfPartFileName, + kdRed, kdGreen, kdBlue, transparency); + + + } + + + + + + + + + } + else + { + + for (int s = 0; s < (int)shapes.size(); s++) + { + tinyobj::shape_t& shape = shapes[s]; + + if (shape.name.length()) + { + printf("object name = %s\n", shape.name.c_str()); + } + + char objFileName[MAX_PATH_LEN]; + if (strlen(materialPrefixPath) > 0) + { + sprintf(objFileName, "%s/part%d.obj", materialPrefixPath, s); + } + else + { + sprintf(objFileName, "part%d.obj", s); + } + FILE* f = fopen(objFileName, "w"); + if (f == 0) + { + printf("Fatal error: cannot create part obj file %s\n", objFileName); + exit(0); + } + fprintf(f, "# Exported using automatic converter by Erwin Coumans\n"); + if (matLibName.length()) + { + fprintf(f, "mtllib %s.mtl\n", matLibName.c_str()); + } + else + { + fprintf(f, "mtllib bedroom.mtl\n"); + + } + + + int faceCount = shape.mesh.indices.size(); + int vertexCount = shape.mesh.positions.size(); + tinyobj::material_t mat = shape.material; + if (shape.name.length()) + { + const char* objName = shape.name.c_str(); + printf("mat.name = %s\n", objName); + fprintf(f, "#object %s\n\n", objName); + } + for (int v = 0; v < vertexCount / 3; v++) + { + fprintf(f, "v %f %f %f\n", shape.mesh.positions[v * 3 + 0], shape.mesh.positions[v * 3 + 1], shape.mesh.positions[v * 3 + 2]); + } + + if (mat.name.length()) + { + fprintf(f, "usemtl %s\n", mat.name.c_str()); + } + else + { + fprintf(f, "usemtl wire_028089177\n"); + } + + fprintf(f, "\n"); + int numNormals = int(shape.mesh.normals.size()); + + for (int vn = 0; vn < numNormals / 3; vn++) + { + fprintf(f, "vn %f %f %f\n", shape.mesh.normals[vn * 3 + 0], shape.mesh.normals[vn * 3 + 1], shape.mesh.normals[vn * 3 + 2]); + } + + fprintf(f, "\n"); + int numTexCoords = int(shape.mesh.texcoords.size()); + for (int vt = 0; vt < numTexCoords / 2; vt++) + { + fprintf(f, "vt %f %f\n", shape.mesh.texcoords[vt * 2 + 0], shape.mesh.texcoords[vt * 2 + 1]); + } + + fprintf(f, "s off\n"); + + for (int face = 0; face < faceCount; face += 3) + { + if (face < 0 && face >= int(shape.mesh.indices.size())) + { + continue; + } + fprintf(f, "f %d/%d/%d %d/%d/%d %d/%d/%d\n", + shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, + shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, + shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1); + } + fclose(f); + + float kdRed = mat.diffuse[0]; + float kdGreen = mat.diffuse[1]; + float kdBlue = mat.diffuse[2]; + float transparency = mat.transparency; + char objSdfPartFileName[MAX_PATH_LEN]; + sprintf(objSdfPartFileName, "part%d.obj", s); + fprintf(sdfFile, "\t\t\n" + "\t\t\t1\n" + "\t\t\t0 0 0 0 0 0\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t0\n" + "\t\t\t\n" + "\t\t\t0.166667\n" + "\t\t\t0\n" + "\t\t\t0\n" + "\t\t\t0.166667\n" + "\t\t\t0\n" + "\t\t\t0.166667\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t1 1 1\n" + "\t\t\t\t%s\n" + "\t\t\t\n" + "\t\t\t\n" + "\t\t\t \n" + "\t\t\t\n" + "\t\t\t\t\n" + "\t\t\t\t\n" + "\t\t\t\t\t1 1 1\n" + "\t\t\t\t\t%s\n" + "\t\t\t\t\n" + "\t\t\t\t\n" + "\t\t\t\n" + "\t\t\t\t1 0 0 1\n" + "\t\t\t\t%f %f %f %f\n" + "\t\t\t\t0.1 0.1 0.1 1\n" + "\t\t\t\t0 0 0 0\n" + "\t\t\t \n" + "\t\t\t \n" + "\t\t\t \n" + "\t\t\t\n", objSdfPartFileName, s, s, + objSdfPartFileName, objSdfPartFileName, + kdRed, kdGreen, kdBlue, transparency); + + + } } fprintf(sdfFile,"\t\n\n"); diff --git a/data/multibody.bullet b/data/multibody.bullet index ebec1dad3..36a570fe5 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index e867d0583..13f6ddb0e 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -128,7 +128,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V { b3JointInfo jointInfo; sim->getJointInfo(m_data->m_quadrupedUniqueId,i,&jointInfo); - if (jointInfo.m_jointName) + if (jointInfo.m_jointName[0]) { m_data->m_jointNameToId.insert(jointInfo.m_jointName,i); } @@ -137,4 +137,4 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V resetPose(sim); return m_data->m_quadrupedUniqueId; -} \ No newline at end of file +} diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 33aac5e47..388a472d7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -32,7 +32,6 @@ #define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM -#define MAX_SDF_BODIES 512 struct TmpFloat3 { diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 03f3d79d8..a1f59ae45 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -355,6 +355,8 @@ enum b3VREventType #define MAX_KEYBOARD_EVENTS 256 #define MAX_MOUSE_EVENTS 256 +#define MAX_SDF_BODIES 1024 + enum b3VRButtonInfo { diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index d8e3102a8..129f5683a 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -328,6 +328,7 @@ void InitMaterial(material_t& material) { material.emission[i] = 0.f; } material.shininess = 1.f; + material.transparency = 1.f; } std::string LoadMtl ( @@ -465,6 +466,20 @@ std::string LoadMtl ( continue; } + // transparency + if (token[0] == 'T' && token[1] == 'r' && isSpace(token[2])) { + token += 2; + material.transparency = parseFloat(token); + continue; + } + + // transparency + if (token[0] == 'd' && isSpace(token[1])) { + token += 1; + material.transparency = parseFloat(token); + continue; + } + // ambient texture if ((0 == strncmp(token, "map_Ka", 6)) && isSpace(token[6])) { token += 7; diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h index bc3ad2eb7..16f918c46 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h @@ -22,6 +22,7 @@ typedef struct float transmittance[3]; float emission[3]; float shininess; + float transparency; std::string ambient_texname; std::string diffuse_texname; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2ea7e23c1..0825bff8b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1,5 +1,4 @@ - #include "../SharedMemory/PhysicsClientC_API.h" #include "../SharedMemory/PhysicsDirectC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" @@ -571,7 +570,6 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* ke return NULL; } -#define MAX_SDF_BODIES 512 static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* keywds) { const char* bulletFileName = ""; @@ -701,7 +699,9 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); + char str[1024]; + sprintf(str,"SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES); + PyErr_SetString(SpamError, str); return NULL; } @@ -1197,7 +1197,9 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); if (numBodies > MAX_SDF_BODIES) { - PyErr_SetString(SpamError, "SDF exceeds body capacity"); + char str[1024]; + sprintf(str,"SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES); + PyErr_SetString(SpamError, str); return NULL; } @@ -8191,6 +8193,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "COV_ENABLE_VR_TELEPORTING", COV_ENABLE_VR_TELEPORTING); PyModule_AddIntConstant(m, "COV_ENABLE_RENDERING", COV_ENABLE_RENDERING); PyModule_AddIntConstant(m, "COV_ENABLE_TINY_RENDERER", COV_ENABLE_TINY_RENDERER); + PyModule_AddIntConstant(m, "COV_ENABLE_Y_AXIS_UP", COV_ENABLE_Y_AXIS_UP); PyModule_AddIntConstant(m, "COV_ENABLE_VR_RENDER_CONTROLLERS", COV_ENABLE_VR_RENDER_CONTROLLERS); PyModule_AddIntConstant(m, "COV_ENABLE_KEYBOARD_SHORTCUTS", COV_ENABLE_KEYBOARD_SHORTCUTS); diff --git a/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 8d7fb054d..2f8485859 100644 --- a/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -118,9 +118,13 @@ static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector return supVec; #else - btScalar maxDot; - long ptIndex = vec.maxDot( points, numPoints, maxDot); + btScalar maxDot; + long ptIndex = vec.maxDot( points, numPoints, maxDot); btAssert(ptIndex >= 0); + if (ptIndex<0) + { + ptIndex = 0; + } btVector3 supVec = points[ptIndex] * localScaling; return supVec; #endif //__SPU__ diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 0f15a0c83..0c7da662e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -983,7 +983,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { - invDi[0] = 1.0f / D[0]; + if (D[0]>=SIMD_EPSILON) + { + invDi[0] = 1.0f / D[0]; + } else + { + invDi[0] = 0; + } break; } case btMultibodyLink::eSpherical: @@ -1266,12 +1272,29 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo if (num_links == 0) { // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result[0] = rhs_bot[0] / m_baseInertia[0]; - result[1] = rhs_bot[1] / m_baseInertia[1]; - result[2] = rhs_bot[2] / m_baseInertia[2]; - result[3] = rhs_top[0] / m_baseMass; - result[4] = rhs_top[1] / m_baseMass; - result[5] = rhs_top[2] / m_baseMass; + + if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON)) + { + result[0] = rhs_bot[0] / m_baseInertia[0]; + result[1] = rhs_bot[1] / m_baseInertia[1]; + result[2] = rhs_bot[2] / m_baseInertia[2]; + } else + { + result[0] = 0; + result[1] = 0; + result[2] = 0; + } + if (m_baseMass>=SIMD_EPSILON) + { + result[3] = rhs_top[0] / m_baseMass; + result[4] = rhs_top[1] / m_baseMass; + result[5] = rhs_top[2] / m_baseMass; + } else + { + result[3] = 0; + result[4] = 0; + result[5] = 0; + } } else { if (!m_cachedInertiaValid) @@ -1322,9 +1345,21 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV if (num_links == 0) { // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result.setAngular(rhs.getAngular() / m_baseInertia); - result.setLinear(rhs.getLinear() / m_baseMass); - } else + if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON)) + { + result.setAngular(rhs.getAngular() / m_baseInertia); + } else + { + result.setAngular(btVector3(0,0,0)); + } + if (m_baseMass>=SIMD_EPSILON) + { + result.setLinear(rhs.getLinear() / m_baseMass); + } else + { + result.setLinear(btVector3(0,0,0)); + } + } else { /// Special routine for calculating the inverse of a spatial inertia matrix ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices