One problem which I encountered is that Bullet's rigid bodies use a btVector3 instead of a btMatrix3x3 for the local moment of inertia, which implies that the local coordinate system has to be that of the principal axes of the inertia tensor. While this is true for primitive shapes like btSphereShape (by the way: there is an exception, for btConeShape the origin does not coincide with the center of mass), arbitrary shapes like btCompoundShape and btConvexTriangleMesh do not fulfill this condition. Given that one has computed the inertia matrix J, the principal axis transformation (R,c) (i.e., R^T J R is diagonal and c is the local center of mass) has to be computed and applied to the world transform of the collision object and (inversely) to the shape data. For a btCompoundShape, the latter can be done by applying individually to the child transformations, for a btConvexTriangleMesh one can transform the mesh vertices.
The rotation R can be computed by the robust Jacobi method. I added this method to btMatrix3x3:
Code: Select all
///diagonalizes this matrix by the Jacobi method. rot stores the rotation
///from the coordinate system in which the matrix is diagonal to the original
///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
///stops when all off-diagonal elements are less than the threshold multiplied
///by the sum of the absolute values of the diagonal, or when maxSteps have
///been executed. Note that this matrix is assumed to be symmetric.
void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
{
rot.setIdentity();
for (int step = maxSteps; step > 0; step--)
{
// find off-diagonal element [p][q] with largest magnitude
int p = 0;
int q = 1;
int r = 2;
btScalar max = btFabs(m_el[0][1]);
btScalar v = btFabs(m_el[0][2]);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = btFabs(m_el[1][2]);
if (v > max)
{
p = 1;
q = 2;
r = 0;
max = v;
}
btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
if (max <= t)
{
if (max <= SIMD_EPSILON * t)
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
btScalar mpq = m_el[p][q];
btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
btScalar theta2 = theta * theta;
btScalar cos;
btScalar sin;
if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
{
t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
: 1 / (theta - btSqrt(1 + theta2));
cos = 1 / btSqrt(1 + t * t);
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + btScalar(0.5) / theta2));
cos = 1 - btScalar(0.5) * t * t;
sin = cos * t;
}
// apply rotation to matrix (this = J^T * this * J)
m_el[p][q] = m_el[q][p] = 0;
m_el[p][p] -= t * mpq;
m_el[q][q] += t * mpq;
btScalar mrp = m_el[r][p];
btScalar mrq = m_el[r][q];
m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for (int i = 0; i < 3; i++)
{
btVector3& row = rot[i];
mrp = row[p];
mrq = row[q];
row[p] = cos * mrp - sin * mrq;
row[q] = cos * mrq + sin * mrp;
}
}
}
Code: Select all
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform.
void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
{
int n = m_children.size();
btScalar totalMass = 0;
btVector3 center(0, 0, 0);
for (int k = 0; k < n; k++)
{
center += m_children[k].m_transform.getOrigin() * masses[k];
totalMass += masses[k];
}
center /= totalMass;
principal.setOrigin(center);
btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
for (int k = 0; k < n; k++)
{
btVector3 i;
m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
const btTransform& t = m_children[k].m_transform;
btVector3 o = t.getOrigin() - center;
//compute inertia tensor in coordinate system of compound shape
btMatrix3x3 j = t.getBasis().transpose();
j[0] *= i[0];
j[1] *= i[1];
j[2] *= i[2];
j = t.getBasis() * j;
//add inertia tensor
tensor[0] += j[0];
tensor[1] += j[1];
tensor[2] += j[2];
//compute inertia tensor of pointmass at o
btScalar o2 = o.length2();
j[0].setValue(o2, 0, 0);
j[1].setValue(0, o2, 0);
j[2].setValue(0, 0, o2);
j[0] += o * -o.x();
j[1] += o * -o.y();
j[2] += o * -o.z();
//add inertia tensor of pointmass
tensor[0] += masses[k] * j[0];
tensor[1] += masses[k] * j[1];
tensor[2] += masses[k] * j[2];
}
tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
}
The computation for a convex triangle mesh is more complicated. It is done by considering a tetrahedron for each triangle with one corner at the center of the mesh, and adding up the contributions of all tetrahedra just like for the compound shape:
Code: Select all
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia"
///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
///of the collision object by the principal transform. This method also computes the volume of the convex mesh.
void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
{
class CenterCallback: public btInternalTriangleIndexCallback
{
bool first;
btVector3 ref;
btVector3 sum;
btScalar volume;
public:
CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{
(void) triangleIndex;
(void) partId;
if (first)
{
ref = triangle[0];
first = false;
}
else
{
btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
volume += vol;
}
}
btVector3 getCenter()
{
return (volume > 0) ? sum / volume : ref;
}
btScalar getVolume()
{
return volume * btScalar(1. / 6);
}
};
class InertiaCallback: public btInternalTriangleIndexCallback
{
btMatrix3x3 sum;
btVector3 center;
public:
InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{
(void) triangleIndex;
(void) partId;
btMatrix3x3 i;
btVector3 a = triangle[0] - center;
btVector3 b = triangle[1] - center;
btVector3 c = triangle[2] - center;
btVector3 abc = a + b + c;
btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
for (int j = 0; j < 3; j++)
{
for (int k = 0; k <= j; k++)
{
i[j][k] = i[k][j] = volNeg * (center[j] * center[k]
+ btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j])
+ btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+ btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
}
}
btScalar i00 = -i[0][0];
btScalar i11 = -i[1][1];
btScalar i22 = -i[2][2];
i[0][0] = i11 + i22;
i[1][1] = i22 + i00;
i[2][2] = i00 + i11;
sum[0] += i[0];
sum[1] += i[1];
sum[2] += i[2];
}
btMatrix3x3& getInertia()
{
return sum;
}
};
CenterCallback centerCallback;
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax);
btVector3 center = centerCallback.getCenter();
principal.setOrigin(center);
volume = centerCallback.getVolume();
InertiaCallback inertiaCallback(center);
m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
btMatrix3x3& i = inertiaCallback.getInertia();
i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertia.setValue(i[0][0], i[1][1], i[2][2]);
inertia /= volume;
}
