store tension action force inside spring, make spring output force and derivative for abstraction, cleanup

examples/testbed/tests/tension_mapping.h

@@ -108,6 +108,27 @@ public:
 		m_cloth.Step(dt);
 		m_cloth.Apply();
 
+		b3StackArray<b3Vec3, 256> tension;
+		tension.Resize(m_cloth.GetParticleCount());
+		for (u32 i = 0; i < tension.Count(); ++i)
+		{
+			tension[i].SetZero();
+		}
+
+		for (u32 i = 0; i < m_cloth.GetSpringCount(); ++i)
+		{
+			b3Spring* s = m_cloth.GetSpring(i);
+
+			b3Particle* p1 = s->p1;
+			b3Particle* p2 = s->p2;
+
+			u32 i1 = m_cloth.GetParticleIndex(p1);
+			u32 i2 = m_cloth.GetParticleIndex(p2);
+
+			tension[i1] += s->tension;
+			tension[i2] -= s->tension;
+		}
+
 		for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)
 		{
 			b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i;
@@ -124,13 +145,13 @@ public:
 			b3Draw_draw->DrawSegment(v2, v3, b3Color_black);
 			b3Draw_draw->DrawSegment(v3, v1, b3Color_black);
 
-			b3Vec3 f1 = p1->tension;
+			b3Vec3 f1 = tension[t->v1];
 			float32 L1 = b3Length(f1);
 
-			b3Vec3 f2 = p2->tension;
+			b3Vec3 f2 = tension[t->v2];
 			float32 L2 = b3Length(f2);
 
-			b3Vec3 f3 = p3->tension;
+			b3Vec3 f3 = tension[t->v3];
 			float32 L3 = b3Length(f3);
 
 			float32 L = (L1 + L2 + L3) / 3.0f;

include/bounce/dynamics/cloth/cloth.h

@@ -77,21 +77,18 @@ enum b3ParticleType
 // Read-only particle 
 struct b3Particle
 {
-	// Particle type
+	// Type
 	b3ParticleType type;
 
-	// Mass position
+	// Position
 	b3Vec3 position;
 
-	// Mass velocity
+	// Velocity
 	b3Vec3 velocity;
 	
-	// Mass force
+	// Applied external force
 	b3Vec3 force;
 	
-	// Mass tension force for visualization
-	b3Vec3 tension;
-
 	// Mass
 	float32 mass;
 	
@@ -123,6 +120,8 @@ enum b3SpringType
 	e_bendSpring,
 };
 
+struct b3ClothSolverData;
+
 // Read-only spring
 struct b3Spring
 {
@@ -143,6 +142,17 @@ struct b3Spring
 	
 	// Damping stiffness
 	float32 kd;
+
+	// Solver temp
+
+	// Action tensile force (f_i entry)
+	b3Vec3 tension;
+
+	// Jacobian (J_ii entry)
+	b3Mat33 Jx, Jv;
+
+	// Initialize solver data.
+	void Initialize(const b3ClothSolverData* data);
 };
 
 // Read-only contact
@@ -182,6 +192,10 @@ public:
 	// Return the particle at a given index in this cloth.
 	b3Particle* GetParticle(u32 i) const;
 	
+	// Convenience function.
+	// Return the index of a given particle.
+	u32 GetParticleIndex(const b3Particle* p) const;
+
 	// Set the type of a given particle.
 	void SetType(b3Particle* p, b3ParticleType type);
 
@@ -194,6 +208,12 @@ public:
 	// Apply a force to a given particle.
 	void ApplyForce(b3Particle* p, const b3Vec3& force);
 
+	// Return the number of springs in this cloth.
+	u32 GetSpringCount() const;
+
+	// Return the spring at a given index in this cloth.
+	b3Spring* GetSpring(u32 i) const;
+	
 	// Return the kinetic (or dynamic) energy in this system.
 	float32 GetEnergy() const;
 
@@ -273,6 +293,11 @@ inline b3Particle* b3Cloth::GetParticle(u32 i) const
 	return m_particles + i;
 }
 
+inline u32 b3Cloth::GetParticleIndex(const b3Particle* p) const
+{
+	return u32(p - m_particles);
+}
+
 inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type)
 {
 	if (p->type == type)
@@ -319,6 +344,17 @@ inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force)
 	p->force += force;
 }
 
+inline u32 b3Cloth::GetSpringCount() const
+{
+	return m_springCount;
+}
+
+inline b3Spring* b3Cloth::GetSpring(u32 i) const
+{
+	B3_ASSERT(i < m_springCount);
+	return m_springs + i;
+}
+
 inline float32 b3Cloth::GetEnergy() const
 {
 	float32 E = 0.0f;

include/bounce/dynamics/cloth/cloth_solver.h

@@ -41,6 +41,15 @@ struct b3ClothSolverDef
 	u32 contactCapacity;
 };
 
+struct b3ClothSolverData
+{
+	b3Vec3* x;
+	b3Vec3* v;
+	b3Vec3* f;
+	float32 dt;
+	float32 invdt;
+};
+
 class b3ClothSolver
 {
 public:
@@ -53,9 +62,6 @@ public:
 
 	void Solve(float32 dt, const b3Vec3& gravity);
 private:
-	// Compute forces.
-	void Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity);
-	
 	// Compute A and b in Ax = b
 	void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
 
@@ -71,8 +77,6 @@ private:
 
 	b3StackAllocator* m_allocator;
 
-	float32 m_h;
-	
 	u32 m_particleCapacity;
 	u32 m_particleCount;
 	b3Particle** m_particles;
@@ -80,12 +84,12 @@ private:
 	u32 m_springCapacity;
 	u32 m_springCount;
 	b3Spring** m_springs;
-	b3Mat33* m_Jx;
-	b3Mat33* m_Jv;
 
 	u32 m_contactCapacity;
 	u32 m_contactCount;
 	b3ParticleContact** m_contacts;
+
+	b3ClothSolverData m_solverData;
 };
 
 #endif

src/bounce/dynamics/cloth/cloth.cpp

@@ -31,6 +31,58 @@
 
 #define B3_CLOTH_FRICTION 0
 
+// b3Spring
+void b3Spring::Initialize(const b3ClothSolverData* data)
+{
+	u32 i1 = p1->solverId;
+	u32 i2 = p2->solverId;
+
+	b3Vec3 x1 = data->x[i1];
+	b3Vec3 v1 = data->v[i1];
+
+	b3Vec3 x2 = data->x[i2];
+	b3Vec3 v2 = data->v[i2];
+
+	const b3Mat33 I = b3Mat33_identity;
+
+	b3Vec3 dx = x1 - x2;
+
+	if (b3Dot(dx, dx) >= L0 * L0)
+	{
+		// Tension
+		float32 L = b3Length(dx);
+		b3Vec3 n = dx / L;
+
+		b3Vec3 sf1 = -ks * (L - L0) * n;
+		b3Vec3 sf2 = -sf1;
+
+		tension = sf1;
+
+		data->f[i1] += sf1;
+		data->f[i2] += sf2;
+
+		// Jacobian
+		Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
+	}
+	else
+	{
+		tension.SetZero();
+		Jx.SetZero();
+	}
+
+	// Damping
+	b3Vec3 dv = v1 - v2;
+
+	b3Vec3 df1 = -kd * dv;
+	b3Vec3 df2 = -df1;
+
+	data->f[i1] += df1;
+	data->f[i2] += df2;
+
+	Jv = -kd * I;
+}
+
+// b3Cloth
 b3Cloth::b3Cloth()
 {
 	m_gravity.SetZero();
@@ -204,7 +256,6 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 
 		p->translation.SetZero();
 		p->x.SetZero();
-		p->tension.SetZero();
 
 		b3ParticleContact* c = m_contacts + i;
 		c->n_active = false;
@@ -254,6 +305,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 		s->L0 = b3Distance(p1->position, p2->position);
 		s->ks = def.ks;
 		s->kd = def.kd;
+		s->tension.SetZero();
 	}
 
 #if B3_CLOTH_BENDING
@@ -276,6 +328,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 		s->L0 = b3Distance(p1->position, p2->position);
 		s->ks = def.kb;
 		s->kd = def.kd;
+		s->tension.SetZero();
 	}
 
 	m_allocator.Free(sharedEdges);
@@ -298,6 +351,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 		s->L0 = 0.0f;
 		s->ks = def.ks;
 		s->kd = def.kd;
+		s->tension.SetZero();
 	}
 
 	B3_ASSERT(m_springCount <= springCapacity);
@@ -557,12 +611,6 @@ void b3Cloth::Solve(float32 dt)
 {
 	B3_PROFILE("Solve");
 
-	// Clear tension
-	for (u32 i = 0; i < m_particleCount; ++i)
-	{
-		m_particles[i].tension.SetZero();
-	}
-
 	// Solve
 	b3ClothSolverDef solverDef;
 	solverDef.stack = &m_allocator;

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -44,8 +44,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 	m_springCapacity = def.springCapacity;
 	m_springCount = 0;
 	m_springs = (b3Spring**)m_allocator->Allocate(m_springCapacity * sizeof(b3Spring*));;
-	m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
-	m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
 
 	m_contactCapacity = def.contactCapacity;
 	m_contactCount = 0;
@@ -55,8 +53,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 b3ClothSolver::~b3ClothSolver()
 {
 	m_allocator->Free(m_contacts);
-	m_allocator->Free(m_Jv);
-	m_allocator->Free(m_Jx);
 	m_allocator->Free(m_springs);
 	m_allocator->Free(m_particles);
 }
@@ -94,65 +90,46 @@ static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator
 	allocator->Free(matrix.values);
 }
 
-static B3_FORCE_INLINE void b3CopyPosition(b3DenseVec3& v, b3Particle** const particles, u32 count)
-{
-	for (u32 i = 0; i < count; ++i)
-	{
-		v[i] = particles[i]->position;
-	}
-}
-
-static B3_FORCE_INLINE void b3CopyVelocity(b3DenseVec3& v, b3Particle** const particles, u32 count)
-{
-	for (u32 i = 0; i < count; ++i)
-	{
-		v[i] = particles[i]->velocity;
-	}
-}
-
-static B3_FORCE_INLINE void b3CopyForce(b3DenseVec3& v, b3Particle** const particles, u32 count)
-{
-	for (u32 i = 0; i < count; ++i)
-	{
-		v[i] = particles[i]->force;
-	}
-}
-
-static B3_FORCE_INLINE void b3CopyTranslation(b3DenseVec3& v, b3Particle** const particles, u32 count)
-{
-	for (u32 i = 0; i < count; ++i)
-	{
-		v[i] = particles[i]->translation;
-	}
-}
-
-static B3_FORCE_INLINE void b3CopyGuess(b3DenseVec3& v, b3Particle** const particles, u32 count)
-{
-	for (u32 i = 0; i < count; ++i)
-	{
-		v[i] = particles[i]->x;
-	}
-}
-
 void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 {
 	B3_PROFILE("Integrate");
 
-	m_h = dt;
-
 	b3DenseVec3 sx(m_particleCount);
-	b3CopyPosition(sx, m_particles, m_particleCount);
-
 	b3DenseVec3 sv(m_particleCount);
-	b3CopyVelocity(sv, m_particles, m_particleCount);
-
 	b3DenseVec3 sf(m_particleCount);
-	b3CopyForce(sf, m_particles, m_particleCount);
-
 	b3DenseVec3 sy(m_particleCount);
-	b3CopyTranslation(sy, m_particles, m_particleCount);
+	b3DenseVec3 sx0(m_particleCount);
 
-	Compute_f(sf, sx, sv, gravity);
+	m_solverData.x = sx.v;
+	m_solverData.v = sv.v;
+	m_solverData.f = sf.v;
+	m_solverData.dt = dt;
+	m_solverData.invdt = 1.0f / dt;
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+
+		sx[i] = p->position;
+		sv[i] = p->velocity;
+		sf[i] = p->force;
+
+		// Apply weight
+		if (p->type == e_dynamicParticle)
+		{
+			sf[i] += p->mass * gravity;
+		}
+
+		sy[i] = p->translation;
+		sx0[i] = p->x;
+	}
+
+	// Initialize spring forces
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		b3Spring* s = m_springs[i];
+		s->Initialize(&m_solverData);
+	}
 
 	// Solve Ax = b, where
 	// A = M - h * dfdv - h * h * dfdx
@@ -174,24 +151,19 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	b3DenseVec3 z(m_particleCount);
 	Compute_z(z);
 
-	// x0
-	b3DenseVec3 x0(m_particleCount);
-	b3CopyGuess(x0, m_particles, m_particleCount);
-
 	// x
 	b3DenseVec3 x(m_particleCount);
 
 	// Solve Ax = b
 	u32 iterations = 0;
-	Solve(x, iterations, A, b, S, z, x0);
+	Solve(x, iterations, A, b, S, z, sx0);
-
 	b3_clothSolverIterations = iterations;
 
 	// f = A * x - b
 	b3DenseVec3 f = A * x - b;
 
 	// Update state
-	float32 h = m_h;
+	float32 h = dt;
 
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
@@ -221,15 +193,15 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		sx[i] = x1;
 	}
 
-	// Write x to the solution cache for improving convergence
+	// Cache x to improve convergence
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
 		m_particles[i]->x = x[i];
 	}
 
 	// Store the extra contact constraint forces that should have been 
-	// supplied in order to enforce the contact constraints exactly
+	// supplied to enforce the contact constraints exactly.
-	// These forces can be used in contact constraint logic
+	// These forces can be used in contact constraint logic.
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3ParticleContact* c = m_contacts[i];
@@ -257,15 +229,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 
 #define B3_INDEX(i, j, size) (i + j * size)
 
-//
-static void b3SetZero(b3Vec3* out, u32 size)
-{
-	for (u32 i = 0; i < size; ++i)
-	{
-		out[i].SetZero();
-	}
-}
-
 //
 static void b3SetZero(b3Mat33* out, u32 size)
 {
@@ -275,19 +238,18 @@ static void b3SetZero(b3Mat33* out, u32 size)
 	}
 }
 
-// 
+// dfdx * v
-static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
+static void b3Mul_Jx(b3DenseVec3& out, b3Spring** springs, u32 springCount, const b3DenseVec3& v)
-	const b3Mat33* J_ii, b3Spring** const springs, u32 springCount)
 {
-	b3SetZero(out, massCount);
+	out.SetZero();
 
 	for (u32 i = 0; i < springCount; ++i)
 	{
-		const b3Spring* S = springs[i];
+		b3Spring* s = springs[i];
-		u32 i1 = S->p1->solverId;
+		u32 i1 = s->p1->solverId;
-		u32 i2 = S->p2->solverId;
+		u32 i2 = s->p2->solverId;
 
-		b3Mat33 J_11 = J_ii[i];
+		b3Mat33 J_11 = s->Jx;
 		b3Mat33 J_12 = -J_11;
 		b3Mat33 J_21 = J_12;
 		b3Mat33 J_22 = J_11;
@@ -297,87 +259,6 @@ static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
 	}
 }
 
-void b3ClothSolver::Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity)
-{
-	// Zero force
-	f.SetZero();
-
-	// Apply weight
-	for (u32 i = 0; i < m_particleCount; ++i)
-	{
-		b3Particle* p = m_particles[i];
-
-		if (p->type == e_dynamicParticle)
-		{
-			f[i] += p->mass * gravity;
-		}
-	}
-
-	// Apply tension, damping
-	// Store the Jacobians
-
-	// Zero Jacobians
-	b3SetZero(m_Jx, m_springCount);
-	b3SetZero(m_Jv, m_springCount);
-
-	for (u32 i = 0; i < m_springCount; ++i)
-	{
-		b3Spring* S = m_springs[i];
-		b3SpringType type = S->type;
-		b3Particle* p1 = S->p1;
-		b3Particle* p2 = S->p2;
-		float32 L0 = S->L0;
-		float32 ks = S->ks;
-		float32 kd = S->kd;
-
-		u32 i1 = p1->solverId;
-		u32 i2 = p2->solverId;
-
-		b3Vec3 x1 = x[i1];
-		b3Vec3 v1 = v[i1];
-
-		b3Vec3 x2 = x[i2];
-		b3Vec3 v2 = v[i2];
-
-		const b3Mat33 I = b3Mat33_identity;
-
-		b3Vec3 dx = x1 - x2;
-
-		if (b3Dot(dx, dx) >= L0 * L0)
-		{
-			// Tension
-			float32 L = b3Length(dx);
-			b3Vec3 n = dx / L;
-
-			b3Vec3 sf1 = -ks * (L - L0) * n;
-			b3Vec3 sf2 = -sf1;
-
-			f[i1] += sf1;
-			f[i2] += sf2;
-
-			p1->tension += sf1;
-			p2->tension += sf2;
-
-			b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
-
-			m_Jx[i] = Jx11;
-		}
-
-		// Damping
-		b3Vec3 dv = v1 - v2;
-
-		b3Vec3 df1 = -kd * dv;
-		b3Vec3 df2 = -df1;
-
-		f[i1] += df1;
-		f[i2] += df2;
-
-		b3Mat33 Jv11 = -kd * I;
-
-		m_Jv[i] = Jv11;
-	}
-}
-
 static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
 {
 	bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
@@ -389,6 +270,8 @@ static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
 
 void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
 {
+	float32 h = m_solverData.dt;
+
 	// Compute dfdx, dfdv
 	b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
 	b3SetZero(dfdx, m_particleCount * m_particleCount);
@@ -398,11 +281,11 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 
 	for (u32 i = 0; i < m_springCount; ++i)
 	{
-		const b3Spring* S = m_springs[i];
+		b3Spring* s = m_springs[i];
-		u32 i1 = S->p1->solverId;
+		u32 i1 = s->p1->solverId;
-		u32 i2 = S->p2->solverId;
+		u32 i2 = s->p2->solverId;
 
-		b3Mat33 Jx11 = m_Jx[i];
+		b3Mat33 Jx11 = s->Jx;
 		b3Mat33 Jx12 = -Jx11;
 		b3Mat33 Jx21 = Jx12;
 		b3Mat33 Jx22 = Jx11;
@@ -412,7 +295,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 		dfdx[B3_INDEX(i2, i1, m_particleCount)] += Jx21;
 		dfdx[B3_INDEX(i2, i2, m_particleCount)] += Jx22;
 
-		b3Mat33 Jv11 = m_Jv[i];
+		b3Mat33 Jv11 = s->Jv;
 		b3Mat33 Jv12 = -Jv11;
 		b3Mat33 Jv21 = Jv12;
 		b3Mat33 Jv22 = Jv11;
@@ -424,6 +307,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 	}
 
 	// Compute A
+	
 	// A = M - h * dfdv - h * h * dfdx
 
 	// A = 0
@@ -441,7 +325,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 	{
 		for (u32 j = 0; j < m_particleCount; ++j)
 		{
-			A[B3_INDEX(i, j, m_particleCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_particleCount)]);
+			A[B3_INDEX(i, j, m_particleCount)] += (-h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-h * h * dfdx[B3_INDEX(i, j, m_particleCount)]);
 		}
 	}
 
@@ -482,23 +366,17 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
 	m_allocator->Free(dfdx);
 
 	// Compute b
-	// b = h * (f0 + h * Jx_v + Jx_y)
 
 	// Jx_v = dfdx * v
-	b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
+	b3DenseVec3 Jx_v(m_particleCount);
-	b3Mul_Jacobian(Jx_v, v.v, m_particleCount, m_Jx, m_springs, m_springCount);
+	b3Mul_Jx(Jx_v, m_springs, m_springCount, v);
 
 	// Jx_y = dfdx * y
-	b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
+	b3DenseVec3 Jx_y(m_particleCount);
-	b3Mul_Jacobian(Jx_y, y.v, m_particleCount, m_Jx, m_springs, m_springCount);
+	b3Mul_Jx(Jx_y, m_springs, m_springCount, y);
 
-	for (u32 i = 0; i < m_particleCount; ++i)
+	// b = h * (f0 + h * Jx_v + Jx_y)
-	{
+	b = h * (f + h * Jx_v + Jx_y);
-		b[i] = m_h * (f[i] + m_h * Jx_v[i] + Jx_y[i]);
-	}
-
-	m_allocator->Free(Jx_y);
-	m_allocator->Free(Jx_v);
 }
 
 void b3ClothSolver::Compute_z(b3DenseVec3& z)