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

Methods to compute more accurate inertia tensor for btCompoundShape and btConvexTriangleMeshShape.

Browse Source 
Thanks to Ole K. for the fixes, see http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=2562
This commit is contained in:
erwin.coumans
2008-09-04 18:20:32 +00:00
parent 5d2720d267
commit 7380db7653
5 changed files with 268 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
src/BulletCollision/CollisionShapes/btCompoundShape.cpp
View File

@@ -149,3 +149,60 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
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]);
}

8
src/BulletCollision/CollisionShapes/btCompoundShape.h
View File

@@ -142,6 +142,14 @@ public:
return m_aabbTree;
}
///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 calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
private:
btScalar m_collisionMargin;
protected:

110
src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
View File

@@ -204,3 +204,113 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
{
return m_stridingMesh->getScaling();
}
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(&centerCallback, -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;
}

7
src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
View File

@@ -46,6 +46,13 @@ public:
virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const;
///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;
};

85
src/LinearMath/btMatrix3x3.h
View File

@@ -284,6 +284,91 @@ class btMatrix3x3 {
}
///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;
}
}
}
protected:
btScalar cofac(int r1, int c1, int r2, int c2) const