Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

catch invalid mass/inertia instead of division by zero/nan. also, avoid indexing <0

Browse Source
This commit is contained in:
Erwin Coumans
2017-11-07 19:25:14 -08:00
parent 9ce2a1f4b9
commit 344005a8f0
4 changed files with 53 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
data/multibody.bullet
View File

Binary file not shown.

4
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -128,7 +128,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
{ {
b3JointInfo jointInfo; b3JointInfo jointInfo;
sim->getJointInfo(m_data->m_quadrupedUniqueId,i,&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); m_data->m_jointNameToId.insert(jointInfo.m_jointName,i);
} }
@@ -137,4 +137,4 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
resetPose(sim); resetPose(sim);
return m_data->m_quadrupedUniqueId; return m_data->m_quadrupedUniqueId;
} }

8
src/BulletCollision/CollisionShapes/btConvexShape.cpp
View File

@@ -118,9 +118,13 @@ static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector
return supVec; return supVec;
#else #else
btScalar maxDot; btScalar maxDot;
long ptIndex = vec.maxDot( points, numPoints, maxDot); long ptIndex = vec.maxDot( points, numPoints, maxDot);
btAssert(ptIndex >= 0); btAssert(ptIndex >= 0);
if (ptIndex<0)
{
ptIndex = 0;
}
btVector3 supVec = points[ptIndex] * localScaling; btVector3 supVec = points[ptIndex] * localScaling;
return supVec; return supVec;
#endif //__SPU__ #endif //__SPU__

55
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -983,7 +983,13 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
case btMultibodyLink::ePrismatic: case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute: 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; break;
} }
case btMultibodyLink::eSpherical: case btMultibodyLink::eSpherical:
@@ -1266,12 +1272,29 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
if (num_links == 0) if (num_links == 0)
{ {
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier // 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]; if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
result[2] = rhs_bot[2] / m_baseInertia[2]; {
result[3] = rhs_top[0] / m_baseMass; result[0] = rhs_bot[0] / m_baseInertia[0];
result[4] = rhs_top[1] / m_baseMass; result[1] = rhs_bot[1] / m_baseInertia[1];
result[5] = rhs_top[2] / m_baseMass; 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 } else
{ {
if (!m_cachedInertiaValid) if (!m_cachedInertiaValid)
@@ -1322,9 +1345,21 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
if (num_links == 0) if (num_links == 0)
{ {
// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier // 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); if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
result.setLinear(rhs.getLinear() / m_baseMass); {
} else 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 /// Special routine for calculating the inverse of a spatial inertia matrix
///the 6x6 matrix is stored as 4 blocks of 3x3 matrices ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices