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.