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/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