ngs bugfix: update inertia after solving each constraint

include/bounce/dynamics/contacts/contact_solver.h

@@ -46,13 +46,13 @@ struct b3ContactPositionConstraint
 {
 	u32 indexA;
 	float32 invMassA;
-	b3Mat33 invIA;
+	b3Mat33 localInvIA;
 	float32 radiusA;
 	b3Vec3 localCenterA;
 	u32 indexB;
 	b3Vec3 localCenterB;
 	float32 invMassB;
-	b3Mat33 invIB;
+	b3Mat33 localInvIB;
 	float32 radiusB;
 	b3PositionConstraintManifold* manifolds;
 	u32 manifoldCount;
@@ -118,6 +118,7 @@ struct b3ContactSolverDef
 {
 	b3Position* positions;
 	b3Velocity* velocities;
+	b3Mat33* invInertias;
 	b3Contact** contacts;
 	u32 count;
 	b3StackAllocator* allocator;
@@ -140,6 +141,7 @@ public:
 protected:
 	b3Position* m_positions;
 	b3Velocity* m_velocities;
+	b3Mat33* m_inertias;
 	b3Contact** m_contacts;
 	b3ContactPositionConstraint* m_positionConstraints;
 	b3ContactVelocityConstraint* m_velocityConstraints;

include/bounce/dynamics/island.h

@@ -19,7 +19,7 @@
 #ifndef B3_ISLAND_H
 #define B3_ISLAND_H
 
-#include <bounce/common/math/vec3.h>
+#include <bounce/common/math/mat33.h>
 
 class b3StackAllocator;
 class b3Contact;
@@ -67,6 +67,7 @@ private :
 	
 	b3Position* m_positions;
 	b3Velocity* m_velocities;
+	b3Mat33* m_invInertias;
 };
 
 #endif

include/bounce/dynamics/joints/cone_joint.h

@@ -110,9 +110,13 @@ private:
 	float32 m_mB;
 	b3Mat33 m_iA;
 	b3Mat33 m_iB;
+	
+	b3Mat33 m_localInvIA;
+	b3Mat33 m_localInvIB;
+	
 	b3Vec3 m_localCenterA;
 	b3Vec3 m_localCenterB;
-
+	
 	// Point-to-point
 	b3Vec3 m_rA;
 	b3Vec3 m_rB;

include/bounce/dynamics/joints/joint_solver.h

@@ -39,6 +39,7 @@ struct b3JointSolverDef
 	b3Joint** joints;
 	b3Position* positions;
 	b3Velocity* velocities;
+	b3Mat33* invInertias;
 };
 
 class b3JointSolver 

include/bounce/dynamics/joints/revolute_joint.h

@@ -171,6 +171,8 @@ private:
 	b3Mat33 m_iB;
 	b3Vec3 m_localCenterA;
 	b3Vec3 m_localCenterB;
+	b3Mat33 m_localInvIA;
+	b3Mat33 m_localInvIB;
 
 	// Hinge motor
 	b3Vec3 m_motor_J1; // 1x3 (row)

include/bounce/dynamics/joints/sphere_joint.h

@@ -76,7 +76,10 @@ private:
 	float32 m_mB;
 	b3Mat33 m_iA;
 	b3Mat33 m_iB;
-
+	
+	b3Mat33 m_localInvIA;
+	b3Mat33 m_localInvIB;
+	
 	b3Vec3 m_localCenterA;
 	b3Vec3 m_localCenterB;
 	b3Vec3 m_rA;

include/bounce/dynamics/joints/spring_joint.h

@@ -123,6 +123,8 @@ private:
 	b3Mat33 m_iB;
 	b3Vec3 m_localCenterA;
 	b3Vec3 m_localCenterB;
+	b3Mat33 m_localInvIA;
+	b3Mat33 m_localInvIB;
 
 	float32 m_bias;
 	float32 m_gamma;

include/bounce/dynamics/time_step.h

@@ -39,6 +39,7 @@ struct b3SolverData
 {
 	b3Position* positions;
 	b3Velocity* velocities;
+	b3Mat33* invInertias;
 	float32 dt;
 	float32 invdt;
 };
@@ -87,6 +88,18 @@ inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Mat33& rotation)
 	return rotation * inertia * b3Transpose(rotation);
 }
 
+// Compute the inertia matrix of a body measured in 
+// inertial frame (variable over time) given the 
+// inertia matrix in body-fixed frame (constant) 
+// and a rotation matrix representing the orientation 
+// of the body frame relative to the inertial frame.
+inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Quat& rotation)
+{
+	b3Mat33 R = b3QuatMat33(rotation);
+	
+	return R * inertia * b3Transpose(R);
+}
+
 // Compute the time derivative of an orientation given
 // the angular velocity of the rotating frame represented by the orientation.
 inline b3Quat b3Derivative(const b3Quat& orientation, const b3Vec3& velocity)