bugfix, particle friction, test code

include/bounce/dynamics/cloth/cloth.h

@@ -30,6 +30,7 @@ class b3Particle;
 class b3Force;
 class b3BodyContact;
 class b3ParticleContact;
+class b3TriangleContact;
 
 struct b3ParticleDef;
 struct b3ForceDef;
@@ -151,6 +152,9 @@ private:
 	// Update particle contacts. 
 	void UpdateParticleContacts();
 	
+	// Update triangle contacts. 
+	void UpdateTriangleContacts();
+	
 	// Solve
 	void Solve(float32 dt, const b3Vec3& gravity);
 
@@ -169,6 +173,9 @@ private:
 	// Pool of particle contacts
 	b3BlockPool m_particleContactBlocks;
 	
+	// Pool of triangle contacts
+	b3BlockPool m_triangleContactBlocks;
+	
 	// List of particles
 	b3List2<b3Particle> m_particleList;
 	
@@ -178,6 +185,9 @@ private:
 	// List of particle contacts
 	b3List2<b3ParticleContact> m_particleContactList;
 	
+	// List of triangle contacts
+	b3List2<b3TriangleContact> m_triangleContactList;
+	
 	// The parent world of this cloth.
 	b3World* m_world;
 	

include/bounce/dynamics/cloth/cloth_solver.h

@@ -29,6 +29,7 @@ class b3Body;
 class b3Force;
 class b3BodyContact;
 class b3ParticleContact;
+class b3TriangleContact;
 
 struct b3DenseVec3;
 struct b3DiagMat33;
@@ -41,6 +42,7 @@ struct b3ClothSolverDef
 	u32 forceCapacity;
 	u32 bodyContactCapacity;
 	u32 particleContactCapacity;
+	u32 triangleContactCapacity;
 };
 
 struct b3ClothSolverData
@@ -149,6 +151,44 @@ struct b3ClothSolverParticleContactPositionConstraint
 	float32 radiusB;
 };
 
+struct b3ClothSolverTriangleContactVelocityConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+
+	u32 indexB;
+	float32 invMassB;
+	u32 indexC;
+	float32 invMassC;
+	u32 indexD;
+	float32 invMassD;
+
+	b3Vec3 JA;
+	b3Vec3 JB;
+	b3Vec3 JC;
+	b3Vec3 JD;
+	
+	float32 normalMass;
+	float32 normalImpulse;
+};
+
+struct b3ClothSolverTriangleContactPositionConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	float32 radiusA;
+
+	u32 indexB;
+	float32 invMassB;
+	u32 indexC;
+	float32 invMassC;
+	u32 indexD;
+	float32 invMassD;
+	float32 triangleRadius;
+	
+	bool front;
+};
+
 class b3ClothSolver
 {
 public:
@@ -159,6 +199,7 @@ public:
 	void Add(b3Force* f);
 	void Add(b3BodyContact* c);
 	void Add(b3ParticleContact* c);
+	void Add(b3TriangleContact* c);
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
@@ -177,6 +218,9 @@ private:
 	// 
 	void InitializeParticleContactConstraints();
 	
+	// 
+	void InitializeTriangleContactConstraints();
+	
 	//
 	void WarmStart();
 
@@ -186,6 +230,9 @@ private:
 	// 
 	void SolveParticleContactVelocityConstraints();
 	
+	// 
+	void SolveTriangleContactVelocityConstraints();
+	
 	// 
 	void StoreImpulses();
 
@@ -195,6 +242,9 @@ private:
 	// 
 	bool SolveParticleContactPositionConstraints();
 	
+	// 
+	bool SolveTriangleContactPositionConstraints();
+	
 	b3StackAllocator* m_allocator;
 
 	u32 m_particleCapacity;
@@ -221,6 +271,12 @@ private:
 	b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
 	b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
 
+	u32 m_triangleContactCapacity;
+	u32 m_triangleContactCount;
+	b3TriangleContact** m_triangleContacts;
+	b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
+	b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
+	
 	b3ClothSolverData m_solverData;
 };
 

include/bounce/dynamics/cloth/particle.h

@@ -48,6 +48,7 @@ struct b3ParticleDef
 		velocity.SetZero();
 		force.SetZero();
 		radius = 0.0f;
+		friction = 0.0f;
 		userData = nullptr;
 	}
 
@@ -56,21 +57,10 @@ struct b3ParticleDef
 	b3Vec3 velocity;
 	b3Vec3 force;
 	float32 radius;
+	float32 friction;
 	void* userData;
 };
 
-//
-class b3FrictionForce : public b3Force
-{
-public:
-	b3FrictionForce() { }
-	~b3FrictionForce() { }
-
-	void Apply(const b3ClothSolverData* data);
-
-	b3Particle* m_p;
-};
-
 // A contact between a particle and a solid
 class b3BodyContact
 {
@@ -134,6 +124,27 @@ struct b3ParticleContactWorldPoint
 	float32 separation;
 };
 
+// A contact between a particle and a triangle
+class b3TriangleContact
+{
+public:
+	b3TriangleContact() { }
+	~b3TriangleContact() { }
+
+	b3Particle* p1;	
+	b3Particle* p2;
+	b3Particle* p3;
+	b3Particle* p4;
+
+	bool front;
+	
+	// Contact constraint
+	float32 normalImpulse;
+
+	b3TriangleContact* m_prev;
+	b3TriangleContact* m_next;
+};
+
 // A cloth particle.
 class b3Particle
 {
@@ -170,6 +181,12 @@ public:
 	// Get the particle radius.
 	float32 GetRadius() const;
 
+	// Set the particle coefficient of friction.
+	void SetFriction(float32 friction);
+	
+	// Get the particle coefficient of friction.
+	float32 GetFriction() const;
+	
 	// Apply a force.
 	void ApplyForce(const b3Vec3& force);
 
@@ -214,6 +231,9 @@ private:
 	// Radius
 	float32 m_radius;
 
+	// Coefficient of friction
+	float32 m_friction;
+	
 	// User data. 
 	void* m_userData;
 
@@ -291,6 +311,16 @@ inline float32 b3Particle::GetRadius() const
 	return m_radius;
 }
 
+inline void b3Particle::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b3Particle::GetFriction() const
+{
+	return m_friction;
+}
+
 inline void b3Particle::ApplyForce(const b3Vec3& force)
 {
 	if (m_type != e_dynamicParticle)