Added per triangle stretching force and damping. This gives more realistics results. Also updated the tests.

Saying goodbye to mass-spring system!

examples/testbed/tests/cloth_self_collision.h

@@ -34,7 +34,7 @@ public:
 		b3ClothDef def;
 		def.mesh = &m_clothMesh;
 		def.density = 1.0f;
-		def.structural = 100000.0f;
+		def.streching = 100000.0f;
 		def.thickness = 0.2f;
 		def.friction = 0.3f;
 

examples/testbed/tests/pinned_cloth.h

@@ -28,8 +28,7 @@ public:
 		b3ClothDef def;
 		def.mesh = &m_clothMesh;
 		def.density = 0.2f;
-		def.structural = 100000.0f;
+		def.streching = 100000.0f;
-		def.damping = 0.0f;
 
 		m_cloth = new b3Cloth(def);
 

examples/testbed/tests/table_cloth.h

@@ -34,8 +34,7 @@ public:
 		b3ClothDef def;
 		def.mesh = &m_clothMesh;
 		def.density = 0.2f;
-		def.structural = 10000.0f;
+		def.streching = 10000.0f;
-		def.damping = 0.0f;
 		def.thickness = 0.2f;
 		def.friction = 0.1f;
 

examples/testbed/tests/tension_mapping.h

@@ -64,7 +64,8 @@ public:
 		b3ClothDef def;
 		def.mesh = &m_clothMesh;
 		def.density = 0.2f;
-		def.structural = 10000.0f;
+		def.streching = 10000.0f;
+		def.damping = 100.0f;
 
 		m_cloth = new b3Cloth(def);
 
@@ -110,27 +111,25 @@ public:
 
 		for (b3Force* f = m_cloth->GetForceList().m_head; f; f = f->GetNext())
 		{
-			if (f->GetType() == e_springForce)
+			if (f->GetType() == e_strechForce)
 			{
-				b3SpringForce* s = (b3SpringForce*)f;
+				b3StrechForce* s = (b3StrechForce*)f;
+				
+				b3ClothTriangle* triangle = s->GetTriangle();
+				u32 triangleIndex = triangle->GetTriangle();
+				b3ClothMeshTriangle* mesh_triangle = m_clothMesh.triangles + triangleIndex;
+				
+				u32 v1 = mesh_triangle->v1;
+				u32 v2 = mesh_triangle->v2;
+				u32 v3 = mesh_triangle->v3;
 
-				u32 v1 = s->GetParticle1()->GetVertex();
+				b3Vec3 f1 = s->GetActionForce1();
-				u32 v2 = s->GetParticle2()->GetVertex();
+				b3Vec3 f2 = s->GetActionForce2();
+				b3Vec3 f3 = s->GetActionForce3();
 
-				if (v1 != ~0)
+				tension[v1] += f1;
-				{
+				tension[v2] += f2;
-					tension[v1] += s->GetActionForce();
+				tension[v3] += f3;
-				}
-
-				if (v2 != ~0)
-				{
-					tension[v2] -= s->GetActionForce();
-				}
-
-				b3Vec3 p1 = s->GetParticle1()->GetPosition();
-				b3Vec3 p2 = s->GetParticle2()->GetPosition();
-
-				g_draw->DrawSegment(p1, p2, b3Color_black);
 			}
 		}
 
@@ -142,6 +141,8 @@ public:
 			b3Vec3 v2 = m_cloth->GetParticle(t->v2)->GetPosition();
 			b3Vec3 v3 = m_cloth->GetParticle(t->v3)->GetPosition();
 			
+			g_draw->DrawTriangle(v1, v2, v3, b3Color_black);
+
 			b3Vec3 c = (v1 + v2 + v3) / 3.0f;
 
 			float32 s = 0.9f;
@@ -161,7 +162,7 @@ public:
 
 			float32 L = (L1 + L2 + L3) / 3.0f;
 
-			const float32 kMaxT = 100000.0f;
+			const float32 kMaxT = 10000.0f;
 			b3Color color = Color(L, 0.0f, kMaxT);
 			
 			b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);

include/bounce/bounce.h

@@ -68,6 +68,7 @@
 #include <bounce/cloth/cloth.h>
 #include <bounce/cloth/particle.h>
 #include <bounce/cloth/cloth_triangle.h>
+#include <bounce/cloth/strech_force.h>
 #include <bounce/cloth/spring_force.h>
 
 #include <bounce/cloth/garment/sewing_pattern.h>

include/bounce/cloth/cloth.h

@@ -57,7 +57,8 @@ struct b3ClothDef
 	{
 		mesh = nullptr;
 		density = 0.0f;
-		structural = 0.0f;
+		streching = 0.0f;
+		sewing = 0.0f;
 		bending = 0.0f;
 		damping = 0.0f;
 		thickness = 0.0f;
@@ -70,12 +71,15 @@ struct b3ClothDef
 	// Cloth density in kg/m^2
 	float32 density;
 
-	// Structural stiffness
+	// Streching stiffness
-	float32 structural;
+	float32 streching;
 
 	// Bending stiffness
 	float32 bending;
 
+	// Sewing stiffness
+	float32 sewing;
+	
 	// Damping stiffness
 	float32 damping;
 
@@ -152,6 +156,8 @@ public:
 private:
 	friend class b3Particle;
 	friend class b3ClothTriangle;
+	friend class b3StrechForce;
+	friend class b3SpringForce;
 	friend class b3ClothContactManager;
 
 	// Compute mass of each particle.

include/bounce/cloth/cloth_triangle.h

@@ -42,6 +42,7 @@ public:
 private:
 	friend class b3Cloth;
 	friend class b3Particle;
+	friend class b3StrechForce;
 	friend class b3ClothContactManager;
 	friend class b3ParticleTriangleContact;
 	friend class b3ClothSolver;
@@ -70,6 +71,14 @@ private:
 
 	// Broadphase ID
 	u32 m_broadPhaseId;
+
+	// Alpha
+	float32 m_alpha;
+
+	// Strech matrix
+	float32 m_du1, m_dv1;
+	float32 m_du2, m_dv2;
+	float32 m_inv_det;
 };
 
 inline u32 b3ClothTriangle::GetTriangle() const

include/bounce/cloth/force.h

@@ -29,6 +29,7 @@ class b3Particle;
 // Force types
 enum b3ForceType
 {
+	e_strechForce,
 	e_springForce,
 };
 

include/bounce/cloth/particle.h

@@ -120,6 +120,7 @@ private:
 	friend class b3ParticleTriangleContact;
 	friend class b3ClothContactSolver;
 	friend class b3Force;
+	friend class b3StrechForce;
 	friend class b3SpringForce;
 
 	b3Particle(const b3ParticleDef& def, b3Cloth* cloth);

include/bounce/cloth/strech_force.h

@@ -0,0 +1,126 @@
+/*
+* 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_STRECH_FORCE_H
+#define B3_STRECH_FORCE_H
+
+#include <bounce/cloth/force.h>
+
+class b3ClothTriangle;
+
+struct b3StrechForceDef : public b3ForceDef
+{
+	b3StrechForceDef()
+	{
+		type = e_strechForce;
+	}
+
+	// Triangle
+	b3ClothTriangle* triangle;
+
+	// Streching stiffness
+	float32 streching;
+
+	// Damping stiffness
+	float32 damping;
+	
+	// Desired strechiness in u direction
+	float32 bu;
+
+	// Desired strechiness in v direction
+	float32 bv;
+};
+
+// Strech force acting on a cloth triangle.
+class b3StrechForce : public b3Force
+{
+public:
+	bool HasParticle(const b3Particle* particle) const;
+
+	b3ClothTriangle* GetTriangle() const;
+
+	float32 GetStrechingStiffness() const;
+
+	float32 GetDampingStiffness() const;
+	
+	b3Vec3 GetActionForce1() const;
+
+	b3Vec3 GetActionForce2() const;
+	
+	b3Vec3 GetActionForce3() const;
+private:
+	friend class b3Force;
+	friend class b3Cloth;
+
+	b3StrechForce(const b3StrechForceDef* def);
+	~b3StrechForce();
+
+	void Apply(const b3ClothForceSolverData* data);
+
+	// Solver shared
+
+	// Triangle
+	b3ClothTriangle* m_triangle;
+
+	// Streching stiffness
+	float32 m_ks;
+
+	// Damping stiffness
+	float32 m_kd;
+	
+	// bu
+	float32 m_bu;
+
+	// bv
+	float32 m_bv;
+
+	// Action forces
+	b3Vec3 m_f1, m_f2, m_f3;
+};
+
+inline b3ClothTriangle* b3StrechForce::GetTriangle() const
+{
+	return m_triangle;
+}
+
+inline float32 b3StrechForce::GetStrechingStiffness() const
+{
+	return m_ks;
+}
+
+inline float32 b3StrechForce::GetDampingStiffness() const
+{
+	return m_kd;
+}
+
+inline b3Vec3 b3StrechForce::GetActionForce1() const
+{
+	return m_f1;
+}
+
+inline b3Vec3 b3StrechForce::GetActionForce2() const
+{
+	return m_f2;
+}
+
+inline b3Vec3 b3StrechForce::GetActionForce3() const
+{
+	return m_f3;
+}
+
+#endif

src/bounce/cloth/cloth.cpp

@@ -22,6 +22,7 @@
 #include <bounce/cloth/cloth_triangle.h>
 #include <bounce/cloth/force.h>
 #include <bounce/cloth/spring_force.h>
+#include <bounce/cloth/strech_force.h>
 #include <bounce/cloth/cloth_solver.h>
 #include <bounce/common/draw.h>
 
@@ -30,57 +31,6 @@ static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
 	return i + 1 < 3 ? i + 1 : 0;
 }
 
-struct b3UniqueEdge
-{
-	u32 v1, v2;
-};
-
-static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m)
-{
-	u32 uniqueCount = 0;
-
-	for (u32 i = 0; i < m->triangleCount; ++i)
-	{
-		b3ClothMeshTriangle* t1 = m->triangles + i;
-		u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
-
-		for (u32 j1 = 0; j1 < 3; ++j1)
-		{
-			u32 t1v1 = i1s[j1];
-			u32 t1v2 = i1s[b3NextIndex(j1)];
-
-			bool unique = true;
-
-			for (u32 j = 0; j < uniqueCount; ++j)
-			{
-				b3UniqueEdge* ue = uniqueEdges + j;
-
-				if (ue->v1 == t1v1 && ue->v2 == t1v2)
-				{
-					unique = false;
-					break;
-				}
-
-				if (ue->v2 == t1v1 && ue->v1 == t1v2)
-				{
-					unique = false;
-					break;
-				}
-			}
-
-			if (unique)
-			{
-				b3UniqueEdge ue;
-				ue.v1 = t1v1;
-				ue.v2 = t1v2;
-				uniqueEdges[uniqueCount++] = ue;
-			}
-		}
-	}
-
-	return uniqueCount;
-}
-
 struct b3SharedEdge
 {
 	u32 v1, v2;
@@ -187,17 +137,69 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 		triangle->m_friction = def.friction;
 		triangle->m_triangle = i;
 
-		b3Vec3 v1 = m_mesh->vertices[meshTriangle->v1];
+		b3Vec3 A = m_mesh->vertices[meshTriangle->v1];
-		b3Vec3 v2 = m_mesh->vertices[meshTriangle->v2];
+		b3Vec3 B = m_mesh->vertices[meshTriangle->v2];
-		b3Vec3 v3 = m_mesh->vertices[meshTriangle->v3];
+		b3Vec3 C = m_mesh->vertices[meshTriangle->v3];
 
 		b3AABB3 aabb;
-		aabb.Set(v1, v2, v3);
+		aabb.Set(A, B, C);
 		aabb.Extend(triangle->m_radius);
 
 		triangle->m_aabbProxy.type = e_triangleProxy;
 		triangle->m_aabbProxy.owner = triangle;
 		triangle->m_broadPhaseId = m_contactManager.m_broadPhase.CreateProxy(aabb, &triangle->m_aabbProxy);
+
+		// uv coordinates for triangle
+		
+		// v1
+		b3Vec2 uv1;
+		uv1.SetZero();
+
+		// v2
+		b3Vec3 AB = B - A;
+		
+		b3Vec2 uv2;
+		uv2.x = b3Length(AB);
+		uv2.y = 0.0f;
+
+		// v3
+		b3Vec3 n_AB = b3Normalize(AB);
+		b3Vec3 AC = C - A;
+		float32 len_AC = b3Length(AC);
+
+		b3Vec2 uv3;
+		uv3.x = b3Dot(n_AB, AC);
+		uv3.y = b3Sqrt(len_AC * len_AC - uv3.x * uv3.x);
+
+		// Strech matrix
+		float32 du1 = uv2.x - uv1.x;
+		float32 dv1 = uv2.y - uv1.y;
+		float32 du2 = uv3.x - uv1.x;
+		float32 dv2 = uv3.y - uv1.y;
+		
+		triangle->m_du1 = du1;
+		triangle->m_dv1 = dv1;
+		triangle->m_du2 = du2;
+		triangle->m_dv2 = dv2;
+
+		float32 det = du1 * dv2 - du2 * dv1;
+		B3_ASSERT(det != 0.0f);
+		triangle->m_inv_det = 1.0f / det;
+
+		// Triangle area
+		b3Vec3 v1(du1, dv1, 0.0f);
+		b3Vec3 v2(du2, dv2, 0.0f);
+		triangle->m_alpha = 0.5f * b3Length(b3Cross(v1, v2));
+
+		// Create strech force
+		b3StrechForceDef sfdef;
+		sfdef.triangle = triangle;
+		sfdef.streching = def.streching;
+		sfdef.damping = def.damping;
+		sfdef.bu = 1.0f;
+		sfdef.bv = 1.0f;
+
+		CreateForce(sfdef);
 	}
 
 	// Initialize forces
@@ -206,28 +208,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 	// Worst-case edge memory
 	u32 edgeCount = 3 * m->triangleCount;
 
-	b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)allocator->Allocate(edgeCount * sizeof(b3UniqueEdge));
-	u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m);
-
 	b3SharedEdge* sharedEdges = (b3SharedEdge*)allocator->Allocate(edgeCount * sizeof(b3SharedEdge));
 	u32 sharedCount = b3FindSharedEdges(sharedEdges, m);
 
-	for (u32 i = 0; i < uniqueCount; ++i)
-	{
-		b3UniqueEdge* e = uniqueEdges + i;
-
-		b3Particle* p1 = m_particles[e->v1];
-		b3Particle* p2 = m_particles[e->v2];
-
-		b3SpringForceDef fd;
-		fd.Initialize(p1, p2, def.structural, def.damping);
-
-		if (def.structural > 0.0f)
-		{
-			CreateForce(fd);
-		}
-	}
-
 	// Bending
 	for (u32 i = 0; i < sharedCount; ++i)
 	{
@@ -248,7 +231,6 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 	}
 	
 	allocator->Free(sharedEdges);
-	allocator->Free(uniqueEdges);
 
 	// Sewing
 	for (u32 i = 0; i < m->sewingLineCount; ++i)
@@ -259,9 +241,9 @@ b3Cloth::b3Cloth(const b3ClothDef& def) :
 		b3Particle* p2 = m_particles[line->v2];
 
 		b3SpringForceDef fd;
-		fd.Initialize(p1, p2, def.structural, def.damping);
+		fd.Initialize(p1, p2, def.sewing, def.damping);
 
-		if (def.structural > 0.0f)
+		if (def.sewing > 0.0f)
 		{
 			CreateForce(fd);
 		}
@@ -661,6 +643,8 @@ void b3Cloth::Draw() const
 		b3Vec3 v2 = p2->m_position;
 		b3Vec3 v3 = p3->m_position;
 
+		b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black);
+
 		b3Vec3 c = (v1 + v2 + v3) / 3.0f;
 
 		float32 s = 0.9f;

src/bounce/cloth/cloth_solver.cpp

@@ -90,11 +90,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
 
 		forceSolver.Solve(dt, gravity);
 	}
-
+	
 	// Copy particle state to state buffer
 	b3Vec3* positions = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
 	b3Vec3* velocities = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
-
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
 		positions[i] = m_particles[i]->m_position;
@@ -147,8 +146,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterati
 			positions[i] += h * velocities[i];
 		}
 
-		// Solve position constraints
 		{
+			// Solve position constraints
 			bool positionSolved = false;
 			for (u32 i = 0; i < positionIterations; ++i)
 			{

src/bounce/cloth/force.cpp

@@ -17,6 +17,7 @@
 */
 
 #include <bounce/cloth/force.h>
+#include <bounce/cloth/strech_force.h>
 #include <bounce/cloth/spring_force.h>
 
 b3Force* b3Force::Create(const b3ForceDef* def)
@@ -24,6 +25,12 @@ b3Force* b3Force::Create(const b3ForceDef* def)
 	b3Force* force = NULL;
 	switch (def->type)
 	{
+	case e_strechForce:
+	{
+		void* block = b3Alloc(sizeof(b3StrechForce));
+		force = new (block) b3StrechForce((b3StrechForceDef*)def);
+		break;
+	}
 	case e_springForce:
 	{
 		void* block = b3Alloc(sizeof(b3SpringForce));
@@ -46,6 +53,13 @@ void b3Force::Destroy(b3Force* force)
 	b3ForceType type = force->GetType();
 	switch (type)
 	{
+	case e_strechForce:
+	{
+		b3StrechForce* o = (b3StrechForce*)force;
+		o->~b3StrechForce();
+		b3Free(force);
+		break;
+	}
 	case e_springForce:
 	{
 		b3SpringForce* o = (b3SpringForce*)force;

src/bounce/cloth/strech_force.cpp

@@ -0,0 +1,247 @@
+/*
+* 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.
+*/
+
+#include <bounce/cloth/strech_force.h>
+#include <bounce/cloth/cloth_triangle.h>
+#include <bounce/cloth/particle.h>
+#include <bounce/cloth/cloth.h>
+#include <bounce/cloth/cloth_mesh.h>
+#include <bounce/cloth/cloth_force_solver.h>
+#include <bounce/sparse/dense_vec3.h>
+#include <bounce/sparse/sparse_sym_mat33.h>
+
+b3StrechForce::b3StrechForce(const b3StrechForceDef* def)
+{
+	m_type = e_strechForce;
+	m_triangle = def->triangle;
+	m_ks = def->streching;
+	m_kd = def->damping;
+	m_bu = def->bu;
+	m_bv = def->bv;
+	m_f1.SetZero();
+	m_f2.SetZero();
+	m_f3.SetZero();
+}
+
+b3StrechForce::~b3StrechForce()
+{
+
+}
+
+bool b3StrechForce::HasParticle(const b3Particle* particle) const
+{
+	b3Cloth* cloth = m_triangle->m_cloth;
+	u32 triangleIndex = m_triangle->m_triangle;
+	b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex;
+
+	b3Particle* p1 = cloth->m_particles[triangle->v1];
+	b3Particle* p2 = cloth->m_particles[triangle->v2];
+	b3Particle* p3 = cloth->m_particles[triangle->v3];
+
+	return p1 == particle || p2 == particle || p3 == particle;
+}
+
+void b3StrechForce::Apply(const b3ClothForceSolverData* data)
+{
+	b3Cloth* cloth = m_triangle->m_cloth;
+	u32 triangleIndex = m_triangle->m_triangle;
+	b3ClothMeshTriangle* triangle = cloth->m_mesh->triangles + triangleIndex;
+
+	float32 alpha = m_triangle->m_alpha;
+	float32 du1 = m_triangle->m_du1;
+	float32 dv1 = m_triangle->m_dv1;
+	float32 du2 = m_triangle->m_du2;
+	float32 dv2 = m_triangle->m_dv2;
+	float32 inv_det = m_triangle->m_inv_det;
+
+	b3Particle* p1 = cloth->m_particles[triangle->v1];
+	b3Particle* p2 = cloth->m_particles[triangle->v2];
+	b3Particle* p3 = cloth->m_particles[triangle->v3];
+
+	u32 i1 = p1->m_solverId;
+	u32 i2 = p2->m_solverId;
+	u32 i3 = p3->m_solverId;
+
+	b3DenseVec3& x = *data->x;
+	b3DenseVec3& v = *data->v;
+	b3DenseVec3& f = *data->f;
+	b3SparseSymMat33& dfdx = *data->dfdx;
+	b3SparseSymMat33& dfdv = *data->dfdv;
+
+	b3Vec3 x1 = x[i1];
+	b3Vec3 x2 = x[i2];
+	b3Vec3 x3 = x[i3];
+
+	b3Vec3 v1 = v[i1];
+	b3Vec3 v2 = v[i2];
+	b3Vec3 v3 = v[i3];
+	
+	b3Mat33 I; I.SetIdentity();
+
+	b3Vec3 dx1 = x2 - x1;
+	b3Vec3 dx2 = x3 - x1;
+
+	b3Vec3 wu = inv_det * (dv2 * dx1 - dv1 * dx2);
+	float32 len_wu = b3Length(wu);
+	if (len_wu == 0.0f) 
+	{ 
+		return; 
+	}
+	B3_ASSERT(len_wu > 0.0f);
+	float32 inv_len_wu = 1.0f / len_wu;
+	b3Vec3 n_wu = inv_len_wu * wu;
+
+	b3Vec3 wv = inv_det * (-du2 * dx1 + du1 * dx2);
+	float32 len_wv = b3Length(wv);
+	if (len_wv == 0.0f)
+	{
+		return;
+	}
+	B3_ASSERT(len_wv > 0.0f);
+	float32 inv_len_wv = 1.0f / len_wv;
+	b3Vec3 n_wv = inv_len_wv * wv;
+
+	b3Vec3 dwudx;
+	dwudx[0] = inv_det * (dv1 - dv2);
+	dwudx[1] = inv_det * dv2;
+	dwudx[2] = -inv_det * dv1;
+
+	b3Vec3 dwvdx;
+	dwvdx[0] = inv_det * (du2 - du1);
+	dwvdx[1] = -inv_det * du2;
+	dwvdx[2] = inv_det * du1;
+
+	float32 Cu = alpha * (len_wu - m_bu);
+	float32 Cv = alpha * (len_wv - m_bv);
+
+	// This is a small trick to ensure PD-ness of the linear system 
+	Cu = b3Max(Cu, 0.0f);
+	Cv = b3Max(Cv, 0.0f);
+
+	if (Cu == 0.0f && Cv == 0.0f)
+	{
+		return;
+	}
+
+	// Jacobian
+	b3Vec3 dCudx[3];
+	b3Vec3 dCvdx[3];
+	for (u32 i = 0; i < 3; ++i)
+	{
+		dCudx[i] = alpha * dwudx[i] * n_wu;
+		dCvdx[i] = alpha * dwvdx[i] * n_wv;
+	}
+
+	m_f1.SetZero();
+	m_f2.SetZero();
+	m_f3.SetZero();
+
+	if (m_ks > 0.0f)
+	{
+		// Force
+		b3Vec3 fs[3];
+		for (u32 i = 0; i < 3; ++i)
+		{
+			fs[i] = -m_ks * (Cu * dCudx[i] + Cv * dCvdx[i]);
+		}
+
+		m_f1 += fs[0];
+		m_f2 += fs[1];
+		m_f3 += fs[2];
+
+		// Jacobian
+		b3Mat33 J[3][3];
+		for (u32 i = 0; i < 3; ++i)
+		{
+			for (u32 j = 0; j < 3; ++j)
+			{
+				b3Mat33 d2Cuxij = (alpha * inv_len_wu * dwudx[i] * dwudx[j]) * (I - b3Outer(n_wu, n_wu));
+
+				b3Mat33 d2Cvxij = (alpha * inv_len_wv * dwvdx[i] * dwvdx[j]) * (I - b3Outer(n_wv, n_wv));
+
+				b3Mat33 Jij = -m_ks * (b3Outer(dCudx[i], dCudx[j]) + b3Outer(dCvdx[i], dCvdx[j]) + Cu * d2Cuxij + Cv * d2Cvxij);
+
+				J[i][j] = Jij;
+			}
+		}
+
+		dfdx(i1, i1) += J[0][0];
+		dfdx(i1, i2) += J[0][1];
+		dfdx(i1, i3) += J[0][2];
+
+		//dfdx(i2, i1) += J[1][0];
+		dfdx(i2, i2) += J[1][1];
+		dfdx(i2, i3) += J[1][2];
+
+		//dfdx(i3, i1) += J[2][0];
+		//dfdx(i3, i2) += J[2][1];
+		dfdx(i3, i3) += J[2][2];
+	}
+
+	if (m_kd > 0.0f)
+	{
+		float32 dCudt = 0.0f;
+		float32 dCvdt = 0.0f;
+
+		b3Vec3 vs[3] = { v1, v2, v3 };
+		for (u32 i = 0; i < 3; ++i) 
+		{
+			dCudt += b3Dot(dCudx[i], vs[i]);
+			dCvdt += b3Dot(dCvdx[i], vs[i]);
+		}
+
+		// Force
+		b3Vec3 fs[3];
+		for (u32 i = 0; i < 3; ++i)
+		{
+			fs[i] = -m_kd * (dCudt * dCudx[i] + dCvdt * dCvdx[i]);
+		}
+
+		m_f1 += fs[0];
+		m_f2 += fs[1];
+		m_f3 += fs[2];
+
+		// Jacobian
+		b3Mat33 J[3][3];
+		for (u32 i = 0; i < 3; ++i)
+		{
+			for (u32 j = 0; j < 3; ++j)
+			{
+				b3Mat33 Jij = -m_kd * (b3Outer(dCudx[i], dCudx[j]) + b3Outer(dCvdx[i], dCvdx[j]));
+
+				J[i][j] = Jij;
+			}
+		}
+
+		dfdv(i1, i1) += J[0][0];
+		dfdv(i1, i2) += J[0][1];
+		dfdv(i1, i3) += J[0][2];
+
+		//dfdv(i2, i1) += J[1][0];
+		dfdv(i2, i2) += J[1][1];
+		dfdv(i2, i3) += J[1][2];
+
+		//dfdv(i3, i1) += J[2][0];
+		//dfdv(i3, i2) += J[2][1];
+		dfdv(i3, i3) += J[2][2];
+	}
+
+	f[i1] += m_f1;
+	f[i2] += m_f2;
+	f[i3] += m_f3;
+}