bugfix, particle vs. particle collision

include/bounce/dynamics/cloth/cloth.h

@@ -28,6 +28,8 @@ class b3Shape;
 
 class b3Particle;
 class b3Force;
+class b3BodyContact;
+class b3ParticleContact;
 
 struct b3ParticleDef;
 struct b3ForceDef;
@@ -143,10 +145,12 @@ private:
 	// Compute mass of each particle.
 	void ComputeMass();
 
-	// Update contacts. 
-	// This is where some contacts might be initiated or terminated.
-	void UpdateContacts();
+	// Update body contacts. 
+	void UpdateBodyContacts();
 
+	// Update particle contacts. 
+	void UpdateParticleContacts();
+	
 	// Solve
 	void Solve(float32 dt, const b3Vec3& gravity);
 
@@ -162,12 +166,18 @@ private:
 	// Pool of particles
 	b3BlockPool m_particleBlocks;
 
+	// Pool of particle contacts
+	b3BlockPool m_particleContactBlocks;
+	
 	// List of particles
 	b3List2<b3Particle> m_particleList;
 	
 	// List of forces
 	b3List2<b3Force> m_forceList;
 
+	// List of particle contacts
+	b3List2<b3ParticleContact> m_particleContactList;
+	
 	// The parent world of this cloth.
 	b3World* m_world;
 	

include/bounce/dynamics/cloth/cloth_solver.h

@@ -28,6 +28,7 @@ class b3Particle;
 class b3Body;
 class b3Force;
 class b3BodyContact;
+class b3ParticleContact;
 
 struct b3DenseVec3;
 struct b3DiagMat33;
@@ -38,7 +39,8 @@ struct b3ClothSolverDef
 	b3StackAllocator* stack;
 	u32 particleCapacity;
 	u32 forceCapacity;
-	u32 contactCapacity;
+	u32 bodyContactCapacity;
+	u32 particleContactCapacity;
 };
 
 struct b3ClothSolverData
@@ -113,6 +115,40 @@ struct b3ClothSolverContactPositionConstraint
 	b3Vec3 localPointB;
 };
 
+struct b3ClothSolverParticleContactVelocityConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+
+	u32 indexB;
+	float32 invMassB;
+
+	float32 friction;
+
+	b3Vec3 point;
+
+	b3Vec3 normal;
+	float32 normalMass;
+	float32 normalImpulse;
+	float32 velocityBias;
+
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Mat22 tangentMass;
+	b3Vec2 tangentImpulse;
+};
+
+struct b3ClothSolverParticleContactPositionConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	float32 radiusA;
+
+	u32 indexB;
+	float32 invMassB;
+	float32 radiusB;
+};
+
 class b3ClothSolver
 {
 public:
@@ -122,6 +158,7 @@ public:
 	void Add(b3Particle* p);
 	void Add(b3Force* f);
 	void Add(b3BodyContact* c);
+	void Add(b3ParticleContact* c);
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
@@ -135,19 +172,28 @@ private:
 	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
 
 	// 
-	void InitializeConstraints();
+	void InitializeBodyContactConstraints();
+	
+	// 
+	void InitializeParticleContactConstraints();
 	
 	//
 	void WarmStart();
 
 	// 
-	void SolveVelocityConstraints();
+	void SolveBodyContactVelocityConstraints();
 
+	// 
+	void SolveParticleContactVelocityConstraints();
+	
 	// 
 	void StoreImpulses();
 
 	// 
-	bool SolvePositionConstraints();
+	bool SolveBodyContactPositionConstraints();
+	
+	// 
+	bool SolveParticleContactPositionConstraints();
 	
 	b3StackAllocator* m_allocator;
 
@@ -163,12 +209,17 @@ private:
 	u32 m_constraintCount;
 	b3AccelerationConstraint* m_constraints;
 
-	u32 m_contactCapacity;
-	u32 m_contactCount;
-	b3BodyContact** m_contacts;
+	u32 m_bodyContactCapacity;
+	u32 m_bodyContactCount;
+	b3BodyContact** m_bodyContacts;
+	b3ClothSolverContactVelocityConstraint* m_bodyVelocityConstraints;
+	b3ClothSolverContactPositionConstraint* m_bodyPositionConstraints;
 
-	b3ClothSolverContactVelocityConstraint* m_velocityConstraints;
-	b3ClothSolverContactPositionConstraint* m_positionConstraints;
+	u32 m_particleContactCapacity;
+	u32 m_particleContactCount;
+	b3ParticleContact** m_particleContacts;
+	b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints;
+	b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints;
 
 	b3ClothSolverData m_solverData;
 };

include/bounce/dynamics/cloth/particle.h

@@ -104,6 +104,36 @@ struct b3BodyContactWorldPoint
 	float32 separation;
 };
 
+// A contact between two particles
+class b3ParticleContact
+{
+public:
+	b3ParticleContact() { }
+	~b3ParticleContact() { }
+
+	b3Particle* p1;
+	b3Particle* p2;
+
+	// Contact constraint
+	float32 normalImpulse;
+
+	// Friction constraint
+	b3Vec3 t1, t2;
+	b3Vec2 tangentImpulse;
+
+	b3ParticleContact* m_prev;
+	b3ParticleContact* m_next;
+};
+
+struct b3ParticleContactWorldPoint
+{
+	void Initialize(const b3ParticleContact* c);
+
+	b3Vec3 point;
+	b3Vec3 normal;
+	float32 separation;
+};
+
 // A cloth particle.
 class b3Particle
 {
@@ -134,7 +164,10 @@ public:
 	// Get the particle mass.
 	float32 GetMass() const;
 
-	// Get the particle radius;
+	// Set the particle radius.
+	void SetRadius(float32 radius);
+	
+	// Get the particle radius.
 	float32 GetRadius() const;
 
 	// Apply a force.
@@ -248,6 +281,11 @@ inline float32 b3Particle::GetMass() const
 	return m_mass;
 }
 
+inline void b3Particle::SetRadius(float32 radius)
+{
+	m_radius = radius;
+}
+
 inline float32 b3Particle::GetRadius() const
 {
 	return m_radius;

include/bounce/dynamics/shapes/shape.h

@@ -40,6 +40,7 @@ enum b3ShapeType
 
 struct b3TestSphereOutput
 {
+	b3Vec3 point;
 	float32 separation;
 	b3Vec3 normal;
 };