AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

rewrite hull inertia

Browse Source 
Rewrite the algorithm that calculates the inertia tensor for a hull, inner loops

Also add a reference explaining the derivation
This commit is contained in:
Irlan 2018-04-13 22:06:04 -03:00
parent 4570c971c0
commit 151ce1f385
2 changed files with 134 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
src/bounce/collision/shapes/hull.cpp
View File

@ -79,32 +79,31 @@ void b3Hull::Validate(const b3HalfEdge* e) const
} while (e != begin); } while (e != begin);
} }
static B3_FORCE_INLINE void b3Subexpressions(float32& w0, float32& w1, float32& w2,
float32& f1, float32& f2, float32& f3)
{
float32 t0 = w0 + w1;
float32 t1 = w0 * w0;
float32 t2 = t1 + w1 * t0;
f1 = t0 + w2;
f2 = t2 + w2 * f1;
f3 = w0 * t1 + w1 * t2 + w2 * f2;
}
b3Vec3 b3Hull::GetCentroid() const b3Vec3 b3Hull::GetCentroid() const
{ {
Validate(); Validate();
b3Vec3 c(0.0f, 0.0f, 0.0f);
float32 volume = 0.0f;
// Pick reference point not too away from the origin
// to minimize floating point rounding errors.
b3Vec3 p1(0.0f, 0.0f, 0.0f);
// Put it inside the hull.
for (u32 i = 0; i < vertexCount; ++i)
{
p1 += vertices[i];
}
p1 /= float32(vertexCount);
const float32 inv4 = 0.25f;
const float32 inv6 = 1.0f / 6.0f; const float32 inv6 = 1.0f / 6.0f;
const float32 inv60 = 1.0f / 60.0f; const float32 inv24 = 1.0f / 24.0f;
const float32 inv120 = 1.0f / 120.0f;
b3Vec3 diag(0.0f, 0.0f, 0.0f); float32 intgex = 0.0f;
b3Vec3 offDiag(0.0f, 0.0f, 0.0f);
float32 intgcx = 0.0f;
float32 intgcy = 0.0f;
float32 intgcz = 0.0f;
// Triangulate convex polygons
for (u32 i = 0; i < faceCount; ++i) for (u32 i = 0; i < faceCount; ++i)
{ {
const b3Face* face = GetFace(i); const b3Face* face = GetFace(i);
@ -113,34 +112,51 @@ b3Vec3 b3Hull::GetCentroid() const
const b3HalfEdge* edge = GetEdge(begin->next); const b3HalfEdge* edge = GetEdge(begin->next);
do do
{ {
const b3HalfEdge* next = GetEdge(edge->next);
u32 i1 = begin->origin; u32 i1 = begin->origin;
u32 i2 = edge->origin; u32 i2 = edge->origin;
const b3HalfEdge* next = GetEdge(edge->next);
u32 i3 = next->origin; u32 i3 = next->origin;
b3Vec3 p2 = vertices[i1]; b3Vec3 p1 = GetVertex(i1);
b3Vec3 p3 = vertices[i2]; b3Vec3 p2 = GetVertex(i2);
b3Vec3 p4 = vertices[i3]; b3Vec3 p3 = GetVertex(i3);
b3Vec3 e1 = p2 - p1; b3Vec3 e1 = p2 - p1;
b3Vec3 e2 = p3 - p1; b3Vec3 e2 = p3 - p1;
b3Vec3 e3 = p4 - p1;
float32 D = b3Det(e1, e2, e3); b3Vec3 d = b3Cross(e1, e2);
float32 tetraVolume = inv6 * D; float32 f1x, f1y, f1z;
volume += tetraVolume; float32 f2x, f2y, f2z;
float32 f3x, f3y, f3z;
// Volume weighted centroid b3Subexpressions(p1.x, p2.x, p3.x, f1x, f2x, f3x);
c += tetraVolume * inv4 * (e1 + e2 + e3); b3Subexpressions(p1.y, p2.y, p3.y, f1y, f2y, f3y);
b3Subexpressions(p1.z, p2.z, p3.z, f1z, f2z, f3z);
intgex += d.x * f1x;
intgcx += d.x * f2x;
intgcy += d.y * f2y;
intgcz += d.z * f2z;
edge = next; edge = next;
} while (GetEdge(edge->next) != begin); } while (GetEdge(edge->next) != begin);
} }
// Centroid // Apply constants
B3_ASSERT(volume > B3_EPSILON); intgex *= inv6;
c /= volume;
c += p1; intgcx *= inv24;
return c; intgcy *= inv24;
intgcz *= inv24;
// Center of volume
B3_ASSERT(intgex > B3_EPSILON);
intgcx /= intgex;
intgcy /= intgex;
intgcz /= intgex;
return b3Vec3(intgcx, intgcy, intgcz);
} }

127
src/bounce/dynamics/shapes/hull_shape.cpp
View File

@ -37,21 +37,33 @@ void b3HullShape::Swap(const b3HullShape& other)
m_hull = other.m_hull; m_hull = other.m_hull;
} }
void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const static B3_FORCE_INLINE void b3Subexpressions(float32& w0, float32& w1, float32& w2,
float32& f1, float32& f2, float32& f3,
float32& g0, float32& g1, float32& g2)
{ {
// Compute mass data float32 t0 = w0 + w1;
b3Vec3 center(0.0f, 0.0f, 0.0f); float32 t1 = w0 * w0;
float32 volume = 0.0f; float32 t2 = t1 + w1 * t0;
b3Mat33 I;
I.SetZero();
const float32 inv4 = 0.25f; f1 = t0 + w2;
f2 = t2 + w2 * f1;
f3 = w0 * t1 + w1 * t2 + w2 * f2;
g0 = f2 + w0 * (f1 + w0);
g1 = f2 + w1 * (f1 + w1);
g2 = f2 + w2 * (f1 + w2);
}
// For explanation, see Polyhedral Mass Properties - David Eberly
void b3HullShape::ComputeMass(b3MassData* data, float32 density) const
{
const float32 inv6 = 1.0f / 6.0f; const float32 inv6 = 1.0f / 6.0f;
const float32 inv24 = 1.0f / 24.0f;
const float32 inv60 = 1.0f / 60.0f; const float32 inv60 = 1.0f / 60.0f;
const float32 inv120 = 1.0f / 120.0f; const float32 inv120 = 1.0f / 120.0f;
b3Vec3 diag(0.0f, 0.0f, 0.0f); const float32 ks[10] = { inv6, inv24, inv24, inv24, inv60, inv60, inv60, inv120, inv120, inv120 };
b3Vec3 off_diag(0.0f, 0.0f, 0.0f); float32 is[10] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
for (u32 i = 0; i < m_hull->faceCount; ++i) for (u32 i = 0; i < m_hull->faceCount; ++i)
{ {
@ -61,68 +73,83 @@ void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
const b3HalfEdge* edge = m_hull->GetEdge(begin->next); const b3HalfEdge* edge = m_hull->GetEdge(begin->next);
do do
{ {
const b3HalfEdge* next = m_hull->GetEdge(edge->next);
u32 i1 = begin->origin; u32 i1 = begin->origin;
u32 i2 = edge->origin; u32 i2 = edge->origin;
const b3HalfEdge* next = m_hull->GetEdge(edge->next);
u32 i3 = next->origin; u32 i3 = next->origin;
b3Vec3 p2 = m_hull->vertices[i1]; b3Vec3 p1 = m_hull->GetVertex(i1);
b3Vec3 p3 = m_hull->vertices[i2]; b3Vec3 p2 = m_hull->GetVertex(i2);
b3Vec3 p4 = m_hull->vertices[i3]; b3Vec3 p3 = m_hull->GetVertex(i3);
b3Vec3 e1 = p2; b3Vec3 e1 = p2 - p1;
b3Vec3 e2 = p3; b3Vec3 e2 = p3 - p1;
b3Vec3 e3 = p4;
float32 D = b3Det(e1, e2, e3); b3Vec3 d = b3Cross(e1, e2);
float32 tetraVolume = inv6 * D; float32 f1x, f1y, f1z;
volume += tetraVolume; float32 f2x, f2y, f2z;
float32 f3x, f3y, f3z;
// Volume weighted centroid float32 g0x, g0y, g0z;
center += tetraVolume * inv4 * (e1 + e2 + e3); float32 g1x, g1y, g1z;
float32 g2x, g2y, g2z;
// Volume weighted inertia tensor b3Subexpressions(p1.x, p2.x, p3.x, f1x, f2x, f3x, g0x, g1x, g2x);
// https://github.com/melax/sandbox b3Subexpressions(p1.y, p2.y, p3.y, f1y, f2y, f3y, g0y, g1y, g2y);
for (u32 j = 0; j < 3; ++j) b3Subexpressions(p1.z, p2.z, p3.z, f1z, f2z, f3z, g0z, g1z, g2z);
{
u32 j1 = (j + 1) % 3;
u32 j2 = (j + 2) % 3;
diag[j] += inv60 * D * is[0] += d.x * f1x;
(e1[j] * e2[j] + e2[j] * e3[j] + e3[j] * e1[j] +
e1[j] * e1[j] + e2[j] * e2[j] + e3[j] * e3[j]);
off_diag[j] += inv120 * D * is[1] += d.x * f2x;
(e1[j1] * e2[j2] + e2[j1] * e3[j2] + e3[j1] * e1[j2] + is[2] += d.y * f2y;
e1[j1] * e3[j2] + e2[j1] * e1[j2] + e3[j1] * e2[j2] + is[3] += d.z * f2z;
e1[j1] * e1[j2] * 2.0f + e2[j1] * e2[j2] * 2.0f + e3[j1] * e3[j2] * 2.0f);
} is[4] += d.x * f3x;
is[5] += d.y * f3y;
is[6] += d.z * f3z;
is[7] += d.x * (p1.y * g0x + p2.y * g1x + p3.y * g2x);
is[8] += d.y * (p1.z * g0y + p2.z * g1y + p3.z * g2y);
is[9] += d.z * (p1.x * g0z + p2.x * g1z + p3.x * g2z);
edge = next; edge = next;
} while (m_hull->GetEdge(edge->next) != begin); } while (m_hull->GetEdge(edge->next) != begin);
} }
// Total mass // Apply constants
massData->mass = density * volume; for (u32 i = 0; i < 10; ++i)
{
is[i] *= ks[i];
}
// Center of mass // Volume
B3_ASSERT(volume > B3_EPSILON); float32 V = is[0];
float32 inv_volume = 1.0f / volume; B3_ASSERT(V > B3_EPSILON);
center *= inv_volume;
diag *= inv_volume; // Center of volume
off_diag *= inv_volume; b3Vec3 c(is[1], is[2], is[3]);
c /= V;
I.x.Set(diag.y + diag.z, -off_diag.z, -off_diag.y); // Inertia about the local origin
I.y.Set(-off_diag.z, diag.x + diag.z, -off_diag.x); b3Mat33 I;
I.z.Set(-off_diag.y, -off_diag.x, diag.x + diag.y); I.x.x = is[5] + is[6];
I.x.y = -is[7];
I.x.z = -is[9];
// Center of mass I.y.x = I.x.y;
massData->center = center; I.y.y = is[4] + is[6];
I.y.z = -is[8];
// Inertia tensor relative to the local origin I.z.x = I.x.z;
massData->I = massData->mass * I; I.z.y = I.y.z;
I.z.z = is[4] + is[5];
// Apply density
data->center = c;
data->mass = density * V;
data->I = density * I;
} }
void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const