Hi, I've been mulling over a few rigid body papers the last week or so, and getting my head around the concepts and the crazy notation. Now I finally understand (or I think I do) the inertia tensor and how torque influences angular velocity I have written a few simple classes, a kind-of abstract rigid body class and a cube and sphere class that derive from it. I'm also using the glm library - it has a few handy functions for working out inertia tensors for non-uniform cubes, spheres etc.

The parent class has these properties:


float m_mass;
vec3 m_position;
vec3 m_linearMomentum;
vec3 m_angularMomentum;
mat3x3 m_orientation;
mat3x3 m_inertiaTensor;
mat3x3 m_inverseInertiaTensor;
vec3 m_force;
vec3 m_torque;


and the important functions are as follows:


void RigidBody::Update(float dt)
{

m_linearMomentum += m_force * dt;
m_position += m_linearMomentum/m_mass * dt;

//angular update
m_angularMomentum += m_torque * dt;
mat3x3 R(m_orientation);
mat3x3 RT = glm::transpose(R);
mat3x3 I = R * m_inverseInertiaTensor * RT;
vec3 angularVelocity = I._inverse() * m_angularMomentum;

float angle = sqrt(glm::dot(angularVelocity,angularVelocity)) * dt;

R = (mat3x3)glm::rotate(glm::mat4x4(R),angle,glm::normalize(angularVelocity));

m_orientation = R;
}

//takes a force and a contact point relative in local space,
//so if the force is gravity it should act on the center of mass (0,0,0) for these
//simple simulations

void RigidBody::ApplyForce(vec3& force,vec3& contactPoint)
{
//force is dP/dt
//so...
m_force += force;

//torque is force * distance to r so...
m_torque += glm::cross(contactPoint-m_position,force);

//use a numeric threshold - if the new values of force and/or
//torque are under the threshold in any direction set that direction's
//total to 0.0f

float threshold = 0.0001f;
for(int direction = 0; direction < 3; direction++)
{
if(m_force[direction] < threshold) m_force[direction] = 0.0f;
if(m_torque[direction] < threshold) m_torque[direction] = 0.0f;
}

}


I'm 100% sure that the above code is the culprit, but I am not sure what exactly I'm doing wrong.

The derived class are so simple I'll just post their constructors:


CubeRigidBody(float mass,vec3& position,mat3x3& orientation,vec3& dimensions):m_Dimensions(dimensions),
RigidBody(mass,position,orientation)
{
m_inertiaTensor = (glm::boxInertia3(mass,dimensions));
m_inverseInertiaTensor = m_inertiaTensor._inverse();
};
SphereRigidBody(float mass,vec3& position,mat3x3& orientation,float radius):
RigidBody(mass,position,orientation), m_Radius(radius)
{
m_inertiaTensor = (glm::sphereInertia3(mass,radius));
m_inverseInertiaTensor = m_inertiaTensor._inverse();
};


Anyway, when I apply a force to a cube in a test, when getting the orientation matrix after an update many elements of the matrix are NANs or -NANs, so I can't render using the orientation matrix. Position update seems ok though.

Any ideas? Thanks in advance, and I'll be back.

I don't know if this has anything to do with your problem, but you should change this:

if(m_force[direction] < threshold) m_force[direction] = 0.0f;
if(m_torque[direction] < threshold) m_torque[direction] = 0.0f;

to:

if(abs(m_force[direction]) < threshold) m_force[direction] = 0.0f;
if(abs(m_torque[direction]) < threshold) m_torque[direction] = 0.0f;

because the components of the force can take minus values..

Ahhh yes! Thanks, I had actually noticed and changed that since but thank you!

I think you got one inverse too much in there. You should transform the actual inertia tensor with your rotation matrix and not the inverse.

edit: If the option is available you should also not calculate the inverse of the transformed inertia tensor. For speed and stability it is better to not explicitly calculate the inverse when solving a linear system of equations.