solve forces then solve an lcp, decouple contact constraints

include/bounce/dynamics/body.h

@@ -197,9 +197,15 @@ public:
 	// Get the mass of the body. Typically in kg/m^3.
 	float32 GetMass() const;
 
-	// Get the rotational inertia of the body about the center of mass. Typically in kg/m^3.
+	// Get the inverse mass of the body. Typically in kg/m^3.
+	float32 GetInverseMass() const;
+	
+	// Get the rotational inertia of the body about the local center of mass. Typically in kg/m^3.
 	const b3Mat33& GetInertia() const;
 
+	// Get the inverse of the rotational inertia of the body about the world center of mass. Typically in kg/m^3.
+	const b3Mat33& GetWorldInverseInertia() const;
+	
 	// Get this body mass data. 
 	// However, the mass data returned by this function contains the mass of the body, 
 	// the body local center of mass, and the rotational inertia about the body local center of mass.
@@ -609,6 +615,16 @@ inline float32 b3Body::GetMass() const
 	return m_mass;
 }
 
+inline float32 b3Body::GetInverseMass() const
+{
+	return m_invMass;
+}
+
+inline const b3Mat33& b3Body::GetWorldInverseInertia() const
+{
+	return m_worldInvI;
+}
+
 inline const b3Mat33& b3Body::GetInertia() const
 {
 	return m_I;

include/bounce/dynamics/cloth/cloth_solver.h

@@ -19,12 +19,13 @@
 #ifndef B3_CLOTH_SOLVER_H
 #define B3_CLOTH_SOLVER_H
 
-#include <bounce/common/math/vec3.h>
+#include <bounce/common/math/mat22.h>
 #include <bounce/common/math/mat33.h>
 
 class b3StackAllocator;
 
 class b3Particle;
+class b3Body;
 class b3Force;
 class b3BodyContact;
 
@@ -63,6 +64,36 @@ struct b3AccelerationConstraint
 	void Apply(const b3ClothSolverData* data);
 };
 
+struct b3ClothContactVelocityConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+
+	b3Body* bodyB;
+	float32 invMassB;
+	b3Mat33 invIB;
+	
+	float32 friction;
+
+	b3Vec3 point;
+	b3Vec3 rA;
+	b3Vec3 rB;
+	
+	b3Vec3 normal;
+	float32 normalMass;
+	float32 normalImpulse;
+	float32 velocityBias;
+
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Mat22 tangentMass;
+	b3Vec2 tangentImpulse;	
+	
+	float32 motorMass;
+	float32 motorImpulse;
+};
+
 class b3ClothSolver
 {
 public:
@@ -84,6 +115,18 @@ private:
 	// Solve Ax = b.
 	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
 
+	// 
+	void InitializeVelocityConstraints();
+	
+	//
+	void WarmStart();
+
+	// 
+	void SolveVelocityConstraints();
+
+	// 
+	void StoreImpulses();
+
 	b3StackAllocator* m_allocator;
 
 	u32 m_particleCapacity;
@@ -102,6 +145,8 @@ private:
 	u32 m_constraintCount;
 	b3AccelerationConstraint* m_constraints;
 
+	b3ClothContactVelocityConstraint* m_velocityConstraints;
+
 	b3ClothSolverData m_solverData;
 };
 

include/bounce/dynamics/cloth/particle.h

@@ -22,6 +22,7 @@
 #include <bounce/common/math/transform.h>
 #include <bounce/common/template/list.h>
 #include <bounce/dynamics/cloth/force.h>
+#include <bounce/common/math/vec2.h>
 
 class b3Shape;
 class b3Cloth;
@@ -81,19 +82,19 @@ public:
 	b3Shape* s2;
 
 	// Contact constraint
-	bool n_active;
 	b3Vec3 p;
 	b3Vec3 n;
-	float32 Fn;
+	float32 normalImpulse;
 
 	// Friction constraint
-	bool t1_active, t2_active;
 	b3Vec3 t1, t2;
-	float32 Ft1, Ft2;
+	b3Vec2 tangentImpulse;
+	
+	// Motor constraint
+	float32 motorImpulse;
 
-	// Friction force
-	bool f_active;
-	b3FrictionForce f;
+	// 
+	bool active;
 };
 
 // A cloth particle.