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

move capsule inertia to the body frame

Browse Source 
With this bugfix capsule motions are realistic.
This commit is contained in:
Irlan 2018-04-19 03:21:50 -03:00
parent efae0fc202
commit 6d76caad1d
1 changed files with 59 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
src/bounce/dynamics/shapes/capsule_shape.cpp
View File

@ -44,91 +44,87 @@ void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const
b3Vec3 B = m_centers[1]; b3Vec3 B = m_centers[1];
b3Vec3 d = B - A; b3Vec3 d = B - A;
float32 h = b3Length(d); float32 h = b3Length(d);
B3_ASSERT(h > B3_LINEAR_SLOP);
float32 h2 = h * h; float32 h2 = h * h;
float32 r = m_radius; float32 r = m_radius;
float32 r2 = r * r; float32 r2 = r * r;
float32 r3 = r2 * r; float32 r3 = r2 * r;
// Cylinder inertia about the capsule center of mass //
b3MassData Ic_Cylinder; b3Vec3 center = 0.5f * (A + B);
float32 mass = 0.0f;
b3Mat33 I; I.SetZero();
b3Mat33 rotation;
rotation.y = (1.0f / h) * d;
rotation.x = b3Perp(rotation.y);
rotation.z = b3Cross(rotation.y, rotation.x);
// Cylinder
{ {
// Cylinder mass // Mass
float32 volume = B3_PI * r2 * h; float32 cylinderVolume = B3_PI * r2 * h;
float32 mass = density * volume; float32 cylinderMass = density * cylinderVolume;
// Cylinder inertia about the center of mass (same as capsule center of mass) // Inertia about the center of mass
float32 x = (1.0f / 12.0f) * mass * (3.0f * r2 + h2); float32 Ixx = (1.0f / 12.0f) * cylinderMass * (3.0f * r2 + h2);
float32 y = 0.5f * mass * r2; float32 Iyy = 0.5f * cylinderMass * r2;
float32 z = x; // Izz = Ixx
b3Mat33 cylinderI = b3Diagonal(Ixx, Iyy, Ixx);
Ic_Cylinder.center = 0.5f * (A + B); // Align the inertia with the body frame
Ic_Cylinder.mass = mass; cylinderI = b3RotateToFrame(cylinderI, rotation);
Ic_Cylinder.I = b3Diagonal(x, y, z);
// Shift the inertia to the body origin
cylinderI += cylinderMass * b3Steiner(center);
// Contribute
mass += cylinderMass;
I += cylinderI;
} }
// Hemisphere inertia about the capsule center of mass // Hemispheres
b3MassData Ic_Hemisphere;
{ {
// Hemisphere volume and mass // Mass
float32 volume = (2.0f / 3.0f) * B3_PI * r3; float32 hemiVolume = (2.0f / 3.0f) * B3_PI * r3;
float32 mass = density * volume; float32 hemiMass = density * hemiVolume;
// I = Ic + m * d^2
// Ic = I - m * d^2
// Hemisphere inertia about the origin // Hemisphere inertia about the origin
float32 Io = (2.0f / 5.0f) * mass * r2; float32 Io = (2.0f / 5.0f) * hemiMass * r2;
// Hemisphere center of mass relative to origin // Hemisphere center of mass relative to the origin
float32 d1 = (3.0f / 8.0f) * r; float32 coy = (3.0f / 8.0f) * r;
// Hemisphere inertia about the hemisphere center of mass // Hemisphere inertia about the hemisphere/capsule center of mass
float32 Ic = Io - (mass * d1 * d1); float32 Iyy = Io - hemiMass * coy * coy;
// Hemisphere center of mass relative to the capsule center of mass // Hemisphere center of mass relative to the capsule center of mass
float32 d2 = d1 + 0.5f * h; float32 ccy = coy + 0.5f * h;
// Hemisphere inertia about the capsule center of mass // Hemisphere inertia about the capsule the center of mass
float32 x = Ic + (mass * d2 * d2); float32 Ixx = Io + hemiMass * ccy * ccy;
float32 y = Io;
float32 z = x;
Ic_Hemisphere.center.Set(0.0f, d2, 0.0f); // Izz = Ixx
Ic_Hemisphere.mass = mass; b3Mat33 hemiI = b3Diagonal(Ixx, Iyy, Ixx);
Ic_Hemisphere.I = b3Diagonal(x, y, z);
// Align the inertia with the body frame
hemiI = b3RotateToFrame(hemiI, rotation);
// Shift the inertia to the body origin
hemiI += hemiMass * b3Steiner(center);
// Contribute twice
mass += 2.0f * hemiMass;
I += 2.0f * hemiI;
} }
// Capsule inertia about the capsule center of mass // Centroid, total mass, inertia at the origin
b3MassData Ic_Capsule; massData->center = center;
{ massData->mass = mass;
// Capsule center of mass massData->I = I;
Ic_Capsule.center = 0.5f * (A + B);
// Inertia about the capsule center of mass
// taking two hemispheres into account
Ic_Capsule.mass = Ic_Cylinder.mass + 2.0f * Ic_Hemisphere.mass;
Ic_Capsule.I = Ic_Cylinder.I + 2.0f * Ic_Hemisphere.I;
}
// Capsule inertia about the reference frame of the cylinder
// Center of mass doesn't change
B3_ASSERT(h > B3_LINEAR_SLOP);
b3Mat33 R;
R.SetIdentity();
if (h > B3_LINEAR_SLOP)
{
R.y = (1.0f / h) * d;
R.x = b3Perp(R.y);
R.z = b3Cross(R.y, R.x);
}
b3Mat33 Ic = b3RotateToFrame(Ic_Capsule.I, R);
// Inertia about the center of mass
massData->center = Ic_Capsule.center;
massData->mass = Ic_Capsule.mass;
massData->I = Ic;
} }
void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const