add tension mapping test

examples/testbed/framework/debug_draw.h

@@ -91,6 +91,8 @@ public:
 
 	void DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const b3Vec3& p2, const b3Vec3& p3, const b3Color& color);
 
+	void DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const b3Color& color1, const b3Vec3& p2, const b3Color& color2, const b3Vec3& p3, const b3Color& color3);
+	
 	void DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color);
 	
 	void DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 count, const b3Color& color);

examples/testbed/framework/debug_draw_2.cpp

@@ -1140,6 +1140,16 @@ void DebugDraw::DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const
 	DrawTriangle(p2, p3, p3, edgeColor);
 }
 
+void DebugDraw::DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const b3Color& color1, const b3Vec3& p2, const b3Color& color2, const b3Vec3& p3, const b3Color& color3)
+{
+	m_triangles->Vertex(p1, color1, normal);
+	m_triangles->Vertex(p2, color2, normal);
+	m_triangles->Vertex(p3, color3, normal);
+
+	b3Color edgeColor(0.0f, 0.0f, 0.0f, 1.0f);
+	DrawTriangle(p2, p3, p3, edgeColor);
+}
+
 void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color)
 {
 	b3Vec3 p1 = vertices[count - 1];

examples/testbed/tests/tension_mapping.h

@@ -0,0 +1,162 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* 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 TENSION_MAPPING_H
+#define TENSION_MAPPING_H
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+// Hot/Cold color map
+// See http://paulbourke.net/miscellaneous/colourspace/
+static inline b3Color Color(float32 x, float32 a, float32 b)
+{
+	x = b3Clamp(x, a, b);
+
+	float32 d = b - a;
+
+	b3Color c(1.0f, 1.0f, 1.0f); 
+
+	if (x < a + 0.25f * d) 
+	{
+		c.r = 0.0f;
+		c.g = 4.0f * (x - a) / d;
+		return c;
+	}
+	
+	if (x < a + 0.5f * d) 
+	{
+		c.r = 0.0f;
+		c.b = 1.0f + 4.0f * (a + 0.25f * d - x) / d;
+		return c;
+	}
+
+	if (x < a + 0.75f * d) 
+	{
+		c.r = 4.0f * (x - a - 0.5f * d) / d;
+		c.b = 0.0f;
+		return c;
+	}
+
+	c.g = 1.0f + 4.0f * (a + 0.75f * d - x) / d;
+	c.b = 0.0f;
+	return c;
+}
+
+class TensionMapping : public SpringClothTest
+{
+public:
+	TensionMapping()
+	{
+		b3SpringClothDef def;
+		def.allocator = &m_clothAllocator;
+		def.mesh = &m_clothMesh;
+		def.density = 0.2f;
+		def.ks = 100000.0f;
+		def.gravity.Set(0.0f, -10.0f, 0.0f);
+
+		m_cloth.Initialize(def);
+
+		b3AABB3 aabb;
+		aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
+		aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
+
+		for (u32 i = 0; i < def.mesh->vertexCount; ++i)
+		{
+			if (aabb.Contains(def.mesh->vertices[i]))
+			{
+				m_cloth.SetType(i, b3MassType::e_staticMass);
+			}
+		}
+	}
+
+	void Step()
+	{
+		float32 dt = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f;
+
+		if (g_settings.pause)
+		{
+			if (g_settings.singleStep)
+			{
+				g_settings.singleStep = false;
+			}
+			else
+			{
+				dt = 0.0f;
+			}
+		}
+
+		m_cloth.Step(dt);
+		m_cloth.Apply();
+		
+		b3StackArray<b3Vec3, 100> T;
+		m_cloth.GetTension(T);
+
+		for (u32 i = 0; i < m_clothMesh.triangleCount; ++i)
+		{
+			b3Triangle* t = m_clothMesh.triangles + i;
+
+			const float32 kMaxT = 100000.0f;
+
+			b3Vec3 f1 = T[t->v1];
+			float32 L1 = b3Length(f1);
+			b3Color c1 = Color(L1, 0.0f, kMaxT);
+
+			b3Vec3 f2 = T[t->v2];
+			float32 L2 = b3Length(f2);
+			b3Color c2 = Color(L2, 0.0f, kMaxT);
+
+			b3Vec3 f3 = T[t->v3];
+			float32 L3 = b3Length(f3);
+			b3Color c3 = Color(L3, 0.0f, kMaxT);
+
+			b3Vec3 v1 = m_clothMesh.vertices[t->v1];
+			b3Vec3 v2 = m_clothMesh.vertices[t->v2];
+			b3Vec3 v3 = m_clothMesh.vertices[t->v3];
+
+			b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
+			n1.Normalize();
+
+			b3Vec3 n2 = -n1;
+
+			g_debugDraw->DrawSolidTriangle(n1, v1, c1, v2, c2, v3, c3);
+			g_debugDraw->DrawSolidTriangle(n2, v1, c1, v3, c3, v2, c2);
+		}
+
+		b3SpringClothStep step = m_cloth.GetStep();
+
+		char text[256];
+		sprintf(text, "Iterations = %u", step.iterations);
+		g_debugDraw->DrawString(text, b3Color_white);
+
+		if (m_clothDragger.IsSelected() == true)
+		{
+			g_debugDraw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new TensionMapping();
+	}
+
+	b3GridMesh<10, 10> m_clothMesh;
+};
+
+#endif

include/bounce/dynamics/cloth/spring_cloth.h

@@ -20,6 +20,7 @@
 #define B3_SPRING_CLOTH_H
 
 #include <bounce/common/math/mat33.h>
+#include <bounce/common/template/array.h>
 
 #define B3_CLOTH_SHAPE_CAPACITY 32
 
@@ -163,6 +164,10 @@ public:
 	// Return the kinetic (or dynamic) energy in this system.
 	float32 GetEnergy() const;
 	
+	// Return the tension forces (due to springs) of each point mass.
+	// Units are kg * m / s^2
+	void GetTension(b3Array<b3Vec3>& tensions) const;
+
 	// Add a shape to the list of shapes in this cloth. 
 	// The cloth will be able to respond to collisions with each shape in the list of shapes.
 	void AddShape(b3Shape* shape);

src/bounce/dynamics/cloth/spring_cloth.cpp

@@ -297,6 +297,70 @@ void b3SpringCloth::AddShape(b3Shape* shape)
 	m_shapes[m_shapeCount++] = shape;
 }
 
+void b3SpringCloth::GetTension(b3Array<b3Vec3>& T) const
+{
+	B3_ASSERT(T.Count() == 0);
+
+	T.Resize(m_massCount);
+
+	for (u32 i = 0; i < T.Count(); ++i)
+	{
+		T[i].SetZero();
+	}
+
+	// T = F - mg
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		b3Spring* S = m_springs + i;
+
+		b3SpringType type = S->type;
+		u32 i1 = S->i1;
+		u32 i2 = S->i2;
+		float32 L0 = S->L0;
+		float32 ks = S->ks;
+		float32 kd = S->kd;
+
+		b3Vec3 x1 = m_x[i1];
+		b3Vec3 v1 = m_v[i1];
+
+		b3Vec3 x2 = m_x[i2];
+		b3Vec3 v2 = m_v[i2];
+
+		// Strech
+		b3Vec3 dx = x1 - x2;
+		float32 L = b3Length(dx);
+
+		float32 s = 1.0f;
+		if (L > 0.0f)
+		{
+			s -= L0 / L;
+		}
+
+		b3Vec3 sf1 = -ks * s * dx;
+		b3Vec3 sf2 = -sf1;
+
+		T[i1] += sf1;
+		T[i2] += sf2;
+
+		// Damping
+		b3Vec3 dv = v1 - v2;
+
+		b3Vec3 df1 = -kd * dv;
+		b3Vec3 df2 = -df1;
+
+		T[i1] += df1;
+		T[i2] += df2;
+	}
+
+	for (u32 i = 0; i < T.Count(); ++i)
+	{
+		if (m_types[i] == b3MassType::e_staticMass)
+		{
+			T[i].SetZero();
+		}
+	}
+}
+
 static B3_FORCE_INLINE void b3MakeTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n)
 {
 	t1 = dv - b3Dot(dv, n) * n;