Keep particles and triangles in a tree

include/bounce/cloth/cloth.h

@@ -19,11 +19,11 @@
 #ifndef B3_CLOTH_H
 #define B3_CLOTH_H
 
-#include <bounce/common/math/transform.h>
 #include <bounce/common/template/list.h>
 #include <bounce/common/memory/stack_allocator.h>
 #include <bounce/common/memory/block_pool.h>
-#include <bounce/collision/trees/dynamic_tree.h>
+#include <bounce/common/math/transform.h>
+#include <bounce/cloth/cloth_contact_manager.h>
 
 class b3World;
 class b3Shape;
@@ -42,6 +42,8 @@ class b3RayCastListener;
 struct b3RayCastInput;
 struct b3RayCastOutput;
 
+struct b3ClothAABBProxy;
+
 struct b3ClothRayCastSingleOutput
 {
 	u32 triangle;
@@ -140,16 +142,23 @@ public:
 	void Draw() const;
 private:
 	friend class b3Particle;
+	friend class b3ClothContactManager;
 
 	// Compute mass of each particle.
 	void ComputeMass();
 
-	// Update contacts
-	void UpdateContacts();
+	// Update particle-body contacts
+	void UpdateParticleBodyContacts();
 
 	// Solve
 	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 
+	// Synchronize triangle AABB.
+	void SynchronizeTriangle(u32 triangleIndex);
+
+	// Time-step
+	float32 m_dt;
+
 	// Stack allocator
 	b3StackAllocator m_stackAllocator;
 
@@ -165,6 +174,9 @@ private:
 	// Vertex particles
 	b3Particle** m_vertexParticles;
 
+	// Triangle proxies
+	b3ClothAABBProxy* m_triangleProxies;
+
 	// Cloth density
 	float32 m_density;
 
@@ -174,11 +186,11 @@ private:
 	// List of particles
 	b3List2<b3Particle> m_particleList;
 
-	// Particle tree
-	b3DynamicTree m_particleTree;
-
 	// List of forces
 	b3List2<b3Force> m_forceList;
+
+	// Contact manager
+	b3ClothContactManager m_contactManager;
 };
 
 inline void b3Cloth::SetGravity(const b3Vec3& gravity)

include/bounce/cloth/cloth_contact_manager.h

@@ -0,0 +1,81 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CLOTH_CONTACT_MANAGER_H
+#define B3_CLOTH_CONTACT_MANAGER_H
+
+#include <bounce/common/memory/block_pool.h>
+#include <bounce/common/template/list.h>
+#include <bounce/collision/broad_phase.h>
+
+class b3Cloth;
+class b3Particle;
+struct b3ClothMeshTriangle;
+struct b3ClothAABBProxy;
+
+// Contact between particle and a triangle
+class b3ParticleTriangleContact
+{
+public:
+	b3Particle* m_p1;
+	
+	b3ClothMeshTriangle* m_triangle;
+	b3ClothAABBProxy* m_triangleProxy;
+	b3Particle* m_p2;
+	b3Particle* m_p3;
+	b3Particle* m_p4;
+
+	bool m_front;
+	
+	bool m_active;
+
+	float32 m_normalImpulse;
+
+	b3ParticleTriangleContact* m_prev;
+	b3ParticleTriangleContact* m_next;
+};
+
+// Contact delegator for b3Cloth.
+class b3ClothContactManager
+{
+public:
+	b3ClothContactManager();
+
+	// The broad-phase callback.
+	void AddPair(void* data1, void* data2);
+
+	void FindNewContacts();
+
+	void UpdateContacts();
+	
+	b3ParticleTriangleContact* Create();
+	
+	void Destroy(b3ParticleTriangleContact* c);
+
+	void Update(b3ParticleTriangleContact* c);
+
+	b3Cloth* m_cloth;
+
+	b3BlockPool m_particleTriangleContactBlocks;
+	
+	b3BroadPhase m_broadPhase;
+
+	b3List2<b3ParticleTriangleContact> m_particleTriangleContactList;
+};
+
+#endif

include/bounce/cloth/cloth_contact_solver.h

@@ -28,6 +28,7 @@ class b3Particle;
 class b3Body;
 
 struct b3ParticleBodyContact;
+class b3ParticleTriangleContact;
 
 struct b3DenseVec3;
 
@@ -80,6 +81,44 @@ struct b3ClothSolverBodyContactPositionConstraint
 	b3Vec3 localPointB;
 };
 
+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;
+};
+
 struct b3ClothContactSolverDef
 {
 	b3StackAllocator* allocator;
@@ -89,6 +128,9 @@ struct b3ClothContactSolverDef
 	
 	u32 bodyContactCount;
 	b3ParticleBodyContact** bodyContacts;
+
+	u32 triangleContactCount;
+	b3ParticleTriangleContact** triangleContacts;
 };
 
 inline float32 b3MixFriction(float32 u1, float32 u2)
@@ -103,14 +145,18 @@ public:
 	~b3ClothContactSolver();
 
 	void InitializeBodyContactConstraints();
+	void InitializeTriangleContactConstraints();
 
-	void WarmStart();
+	void WarmStartBodyContactConstraints();
+	void WarmStartTriangleContactConstraints();
 
 	void SolveBodyContactVelocityConstraints();
+	void SolveTriangleContactVelocityConstraints();
 
 	void StoreImpulses();
 
 	bool SolveBodyContactPositionConstraints();
+	bool SolveTriangleContactPositionConstraints();
 protected:
 	b3StackAllocator* m_allocator;
 
@@ -121,6 +167,11 @@ protected:
 	b3ParticleBodyContact** m_bodyContacts;
 	b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
 	b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints;
+
+	u32 m_triangleContactCount;
+	b3ParticleTriangleContact** m_triangleContacts;
+	b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints;
+	b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints;
 };
 
 #endif

include/bounce/cloth/cloth_solver.h

@@ -34,6 +34,7 @@ struct b3SparseSymMat33;
 struct b3SparseSymMat33View;
 
 struct b3ParticleBodyContact;
+class b3ParticleTriangleContact;
 
 struct b3ClothSolverDef
 {
@@ -41,6 +42,7 @@ struct b3ClothSolverDef
 	u32 particleCapacity;
 	u32 forceCapacity;
 	u32 bodyContactCapacity;
+	u32 triangleContactCapacity;
 };
 
 struct b3ClothSolverData
@@ -75,6 +77,7 @@ public:
 	void Add(b3Particle* p);
 	void Add(b3Force* f);
 	void Add(b3ParticleBodyContact* c);
+	void Add(b3ParticleTriangleContact* c);
 
 	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
 private:
@@ -107,6 +110,10 @@ private:
 	u32 m_bodyContactCount;
 	b3ParticleBodyContact** m_bodyContacts;
 
+	u32 m_triangleContactCapacity;
+	u32 m_triangleContactCount;
+	b3ParticleTriangleContact** m_triangleContacts;
+	
 	b3ClothSolverData m_solverData;
 };
 

include/bounce/cloth/particle.h

@@ -44,6 +44,7 @@ struct b3ParticleDef
 	b3ParticleDef()
 	{
 		type = e_staticParticle;
+		mass = 0.0f;
 		position.SetZero();
 		velocity.SetZero();
 		force.SetZero();
@@ -53,6 +54,7 @@ struct b3ParticleDef
 	}
 
 	b3ParticleType type;
+	float32 mass;
 	b3Vec3 position;
 	b3Vec3 velocity;
 	b3Vec3 force;
@@ -89,6 +91,20 @@ struct b3ParticleBodyContactWorldPoint
 	float32 separation;
 };
 
+enum b3ClothAABBProxyType
+{
+	e_particleProxy,
+	e_triangleProxy
+};
+
+struct b3ClothAABBProxy
+{
+	b3ClothAABBProxyType type;
+	void* data;
+	u32 index;
+	u32 broadPhaseId;
+};
+
 // A cloth particle.
 class b3Particle
 {
@@ -143,18 +159,23 @@ private:
 	friend class b3List2<b3Particle>;
 	friend class b3Cloth;
 	friend class b3ClothSolver;
+	friend class b3ClothContactManager;
 	friend class b3ClothContactSolver;
 	friend class b3Force;
 	friend class b3SpringForce;
-	friend class b3BendForce;
-	friend class b3FrictionForce;
 
 	b3Particle(const b3ParticleDef& def, b3Cloth* cloth);
 	~b3Particle();
 
-	// Synchronize AABB
+	// Synchronize particle AABB
 	void Synchronize();
 
+	// Synchronize triangles AABB
+	void SynchronizeTriangles();
+
+	// Destroy contacts.
+	void DestroyContacts();
+
 	// Type
 	b3ParticleType m_type;
 
@@ -202,8 +223,8 @@ private:
 	// Contact
 	b3ParticleBodyContact m_bodyContact;
 
-	// Particle tree identifier
-	u32 m_treeId;
+	// AABB Proxy
+	b3ClothAABBProxy m_aabbProxy;
 
 	// 
 	b3Particle* m_prev;
@@ -228,6 +249,7 @@ inline void b3Particle::SetPosition(const b3Vec3& position)
 	m_translation.SetZero();
 
 	Synchronize();
+	SynchronizeTriangles();
 }
 
 inline const b3Vec3& b3Particle::GetPosition() const