Upgrade bounce

bat/premake_vs2019.bat

@@ -0,0 +1,2 @@
+cd ..\
+premake5 solution_vs2019

doc/doxyfile

@@ -58,7 +58,7 @@ PROJECT_LOGO           =
 # entered, it will be relative to the location where doxygen was started. If
 # left blank the current directory will be used.
 
-OUTPUT_DIRECTORY       = doc/api
+OUTPUT_DIRECTORY       = api
 
 # If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
 # directories (in 2 levels) under the output directory of each output format and

examples/hello_world/main.cpp

@@ -45,7 +45,7 @@ int main(int argc, char** argv)
 	world->SetGravity(gravity);
 	
 	// The fixed time step size.
-	const float32 timeStep = 1.0f / 60.0f;
+	const scalar timeStep = 1.0f / 60.0f;
 	
 	// Number of iterations for the velocity constraint solver.
 	const u32 velocityIterations = 8;
@@ -59,14 +59,8 @@ int main(int argc, char** argv)
 
 	// Create a box positioned at the world origin and 
 	// aligned with the world frame.
-	b3BoxHull groundBox;
-	
-	// Set the ground box dimensions using a linear scale transform.
-	b3Transform scale;
-	scale.position.SetZero();
-	scale.rotation = b3Diagonal(10.0f, 1.0f, 10.0f);
-	groundBox.SetTransform(scale);
-	
+	b3BoxHull groundBox(10.0f, 1.0f, 10.0f);
+
 	// Create the box physics wrapper.
 	b3HullShape groundShape;
 	groundShape.m_hull = &groundBox;
@@ -115,7 +109,7 @@ int main(int argc, char** argv)
 		
 		// Decode the axis and angle of rotation about it from the quaternion.
 		b3Vec3 axis;
-		float32 angle;
+		scalar angle;
 		orientation.GetAxisAngle(&axis, &angle);
 
 		// Visualize the body state in this frame.

examples/testbed/framework/body_dragger.cpp

@@ -35,12 +35,23 @@ bool b3BodyDragger::StartDragging()
 {
 	B3_ASSERT(IsDragging() == false);
 
+	class RayCastFilter : public b3RayCastFilter
+	{
+	public:
+		bool ShouldRayCast(b3Shape* shape)
+		{
+			return true;
+		}
+	};
+
+	RayCastFilter filter;
+
 	b3RayCastSingleOutput out;
-	if (m_world->RayCastSingle(&out, m_ray->A(), m_ray->B()) == false)
+	if (m_world->RayCastSingle(&out, &filter, m_ray->A(), m_ray->B()) == false)
 	{
 		return false;
 	}
-
+	
 	m_x = out.fraction;
 	m_shape = out.shape;
 
@@ -54,7 +65,7 @@ bool b3BodyDragger::StartDragging()
 	jd.bodyA = groundBody;
 	jd.bodyB = body;
 	jd.target = out.point;
-	jd.maxForce = 2000.0f * body->GetMass();
+	jd.maxForce = 1000.0f * body->GetMass();
 
 	m_mouseJoint = (b3MouseJoint*)m_world->CreateJoint(jd);
 
@@ -79,10 +90,10 @@ void b3BodyDragger::StopDragging()
 	m_shape = nullptr;
 }
 
-b3Body* b3BodyDragger::GetBody() const
+b3Shape* b3BodyDragger::GetShape() const
 {
 	B3_ASSERT(IsDragging() == true);
-	return m_shape->GetBody();
+	return m_shape;
 }
 
 b3Vec3 b3BodyDragger::GetPointA() const

examples/testbed/framework/body_dragger.h

@@ -24,6 +24,7 @@
 #include <bounce/dynamics/shapes/shape.h>
 #include <bounce/dynamics/body.h>
 #include <bounce/dynamics/world.h>
+#include <bounce/dynamics/world_listeners.h>
 #include <bounce/dynamics/joints/mouse_joint.h>
 
 // A body shape dragger.
@@ -43,14 +44,14 @@ public:
 
 	b3Ray3* GetRay() const;
 
-	b3Body* GetBody() const;
+	b3Shape* GetShape() const;
 
 	b3Vec3 GetPointA() const;
 
 	b3Vec3 GetPointB() const;
 private:
 	b3Ray3 * m_ray;
-	float32 m_x;
+	scalar m_x;
 
 	b3World* m_world;
 	

examples/testbed/framework/cloth_dragger.cpp

@@ -23,9 +23,9 @@ b3ClothDragger::b3ClothDragger(b3Ray3* ray, b3Cloth* cloth)
 	m_staticDrag = true;
 	m_ray = ray;
 	m_cloth = cloth;
-	m_triangle = nullptr;
-	m_km = 10000.0f;
-	m_kd = 0.0f;
+	m_isDragging = false;
+	m_km = 100000.0f;
+	m_kd = 1000.0f;
 }
 
 b3ClothDragger::~b3ClothDragger()
@@ -43,22 +43,20 @@ bool b3ClothDragger::StartDragging()
 		return false;
 	}
 
-	m_mesh = m_cloth->GetMesh();
-	m_triangleIndex = rayOut.triangle;
-	m_triangle = m_mesh->triangles + m_triangleIndex;
+	m_isDragging = true;
 	m_x = rayOut.fraction;
 
-	b3Particle* p1 = m_cloth->GetParticle(m_triangle->v1);
-	b3Particle* p2 = m_cloth->GetParticle(m_triangle->v2);
-	b3Particle* p3 = m_cloth->GetParticle(m_triangle->v3);
+	m_p1 = rayOut.triangle->GetParticle1();
+	m_p2 = rayOut.triangle->GetParticle2();
+	m_p3 = rayOut.triangle->GetParticle3();
 
-	b3Vec3 v1 = p1->GetPosition();
-	b3Vec3 v2 = p2->GetPosition();
-	b3Vec3 v3 = p3->GetPosition();
+	b3Vec3 v1 = m_p1->GetPosition();
+	b3Vec3 v2 = m_p2->GetPosition();
+	b3Vec3 v3 = m_p3->GetPosition();
 
 	b3Vec3 B = GetPointB();
 
-	float32 wABC[4];
+	scalar wABC[4];
 	b3BarycentricCoordinates(wABC, v1, v2, v3, B);
 
 	if (wABC[3] > B3_EPSILON)
@@ -73,33 +71,34 @@ bool b3ClothDragger::StartDragging()
 
 	if (m_staticDrag)
 	{
-		m_t1 = p1->GetType();
-		p1->SetType(e_staticParticle);
+		m_t1 = m_p1->GetType();
+		m_p1->SetType(e_staticClothParticle);
 
-		m_t2 = p2->GetType();
-		p2->SetType(e_staticParticle);
+		m_t2 = m_p2->GetType();
+		m_p2->SetType(e_staticClothParticle);
 
-		m_t3 = p3->GetType();
-		p3->SetType(e_staticParticle);
+		m_t3 = m_p3->GetType();
+		m_p3->SetType(e_staticClothParticle);
 	}
 	else
 	{
-		b3ParticleDef pd;
-		pd.type = e_staticParticle;
+		b3ClothParticleDef pd;
+		pd.type = e_staticClothParticle;
 		pd.position = GetPointA();
 
 		m_particle = m_cloth->CreateParticle(pd);
 
-		b3ClothTriangle* triangle = m_cloth->GetTriangle(m_triangleIndex);
-
 		b3MouseForceDef def;
-		def.particle = m_particle;
-		def.triangle = triangle;
+		def.p1 = m_particle;
+		def.p2 = m_p1;
+		def.p3 = m_p2;
+		def.p4 = m_p3;
 		def.w2 = m_u;
 		def.w3 = m_v;
-		def.w4 = (1.0f - m_u - m_v);
+		def.w4 = 1.0f - m_u - m_v;
 		def.mouse = m_km;
 		def.damping = m_kd;
+		def.restLength = 0.0f;
 
 		m_mf = (b3MouseForce*)m_cloth->CreateForce(def);
 	}
@@ -111,24 +110,23 @@ void b3ClothDragger::Drag()
 {
 	B3_ASSERT(IsDragging() == true);
 
-	b3Vec3 A = GetPointA();
 	b3Vec3 B = GetPointB();
 
-	b3Vec3 dx = B - A;
-
 	if (m_staticDrag)
 	{
-		b3Particle* p1 = m_cloth->GetParticle(m_triangle->v1);
-		p1->ApplyTranslation(dx);
+		b3Vec3 A = GetPointA();
 
-		b3Particle* p2 = m_cloth->GetParticle(m_triangle->v2);
-		p2->ApplyTranslation(dx);
+		b3Vec3 dx = B - A;
 
-		b3Particle* p3 = m_cloth->GetParticle(m_triangle->v3);
-		p3->ApplyTranslation(dx);
+		m_p1->ApplyTranslation(dx);
+		m_p2->ApplyTranslation(dx);
+		m_p3->ApplyTranslation(dx);
 	}
 	else
 	{
+		//b3Vec3 A = m_particle->GetPosition();
+		//b3Vec3 dx = B - A;		
+		//m_particle->ApplyTranslation(dx);
 		m_particle->SetPosition(B);
 	}
 }
@@ -154,9 +152,9 @@ void b3ClothDragger::StopDragging()
 
 	if (m_staticDrag)
 	{
-		m_cloth->GetParticle(m_triangle->v1)->SetType(m_t1);
-		m_cloth->GetParticle(m_triangle->v2)->SetType(m_t2);
-		m_cloth->GetParticle(m_triangle->v3)->SetType(m_t3);
+		m_p1->SetType(m_t1);
+		m_p2->SetType(m_t2);
+		m_p3->SetType(m_t3);
 	}
 	else
 	{
@@ -164,18 +162,18 @@ void b3ClothDragger::StopDragging()
 		m_cloth->DestroyParticle(m_particle);
 	}
 
-	m_triangle = nullptr;
+	m_isDragging = false;
 }
 
 b3Vec3 b3ClothDragger::GetPointA() const
 {
 	B3_ASSERT(IsDragging() == true);
 
-	b3Vec3 A = m_cloth->GetParticle(m_triangle->v1)->GetPosition();
-	b3Vec3 B = m_cloth->GetParticle(m_triangle->v2)->GetPosition();
-	b3Vec3 C = m_cloth->GetParticle(m_triangle->v3)->GetPosition();
+	b3Vec3 v1 = m_p1->GetPosition() + m_p1->GetTranslation();
+	b3Vec3 v2 = m_p2->GetPosition() + m_p2->GetTranslation();
+	b3Vec3 v3 = m_p3->GetPosition() + m_p3->GetTranslation();
 
-	return m_u * A + m_v * B + (1.0f - m_u - m_v) * C;
+	return m_u * v1 + m_v * v2 + (1.0f - m_u - m_v) * v3;
 }
 
 b3Vec3 b3ClothDragger::GetPointB() const

examples/testbed/framework/cloth_dragger.h

@@ -21,9 +21,8 @@
 
 #include <bounce/common/geometry.h>
 #include <bounce/cloth/cloth.h>
-#include <bounce/cloth/cloth_mesh.h>
-#include <bounce/cloth/particle.h>
-#include <bounce/cloth/cloth_triangle.h>
+#include <bounce/cloth/cloth_particle.h>
+#include <bounce/cloth/shapes/cloth_triangle_shape.h>
 #include <bounce/cloth/forces/mouse_force.h>
 
 // A cloth triangle dragger.
@@ -37,13 +36,13 @@ public:
 
 	bool GetStaticDrag() const;
 
-	void SetMouseStiffness(float32 k);
+	void SetMouseStiffness(scalar k);
 
-	float32 GetMouseStiffness();
+	scalar GetMouseStiffness();
 
-	void SetMouseDamping(float32 k);
+	void SetMouseDamping(scalar k);
 
-	float32 GetMouseDamping();
+	scalar GetMouseDamping();
 	
 	bool IsDragging() const;
 
@@ -58,21 +57,23 @@ public:
 	b3Vec3 GetPointB() const;
 private:
 	b3Ray3* m_ray;
-	float32 m_x;
+	scalar m_x;
 
 	b3Cloth* m_cloth;
-	const b3ClothMesh* m_mesh;
-	u32 m_triangleIndex;
-	b3ClothMeshTriangle* m_triangle;
-	float32 m_u, m_v;
+	
+	bool m_isDragging;
+	b3ClothParticle* m_p1;
+	b3ClothParticle* m_p2;
+	b3ClothParticle* m_p3;
+	scalar m_u, m_v;
 
-	float32 m_km;
-	float32 m_kd;
-	b3Particle* m_particle;
+	scalar m_km;
+	scalar m_kd;
+	b3ClothParticle* m_particle;
 	b3MouseForce* m_mf;
 
 	bool m_staticDrag;
-	b3ParticleType m_t1, m_t2, m_t3;
+	b3ClothParticleType m_t1, m_t2, m_t3;
 };
 
 inline bool b3ClothDragger::GetStaticDrag() const
@@ -80,29 +81,29 @@ inline bool b3ClothDragger::GetStaticDrag() const
 	return m_staticDrag;
 }
 
-inline void b3ClothDragger::SetMouseStiffness(float32 k)
+inline void b3ClothDragger::SetMouseStiffness(scalar k)
 {
 	m_km = k;
 }
 
-inline float32 b3ClothDragger::GetMouseStiffness()
+inline scalar b3ClothDragger::GetMouseStiffness()
 {
 	return m_km;
 }
 
-inline void b3ClothDragger::SetMouseDamping(float32 k)
+inline void b3ClothDragger::SetMouseDamping(scalar k)
 {
 	m_kd = k;
 }
 
-inline float32 b3ClothDragger::GetMouseDamping()
+inline scalar b3ClothDragger::GetMouseDamping()
 {
 	return m_kd;
 }
 
 inline bool b3ClothDragger::IsDragging() const
 {
-	return m_triangle != nullptr;
+	return m_isDragging;
 }
 
 #endif

examples/testbed/framework/draw.cpp

@@ -50,17 +50,17 @@ Camera::Camera()
 
 b3Mat44 Camera::BuildProjectionMatrix() const
 {
-	float32 w = m_width, h = m_height;
+	scalar w = m_width, h = m_height;
 	
-	float32 t = tan(0.5f * m_fovy);
-	float32 ratio = w / h;
-	float32 sx = 1.0f / (ratio * t);
-	float32 sy = 1.0f / t;
+	scalar t = tan(0.5f * m_fovy);
+	scalar ratio = w / h;
+	scalar sx = 1.0f / (ratio * t);
+	scalar sy = 1.0f / t;
 	
-	float32 inv_range = 1.0f / (m_zNear - m_zFar);
-	float32 sz = inv_range * (m_zNear + m_zFar);
+	scalar inv_range = 1.0f / (m_zNear - m_zFar);
+	scalar sz = inv_range * (m_zNear + m_zFar);
 	
-	float32 tz = inv_range * m_zNear * m_zFar;
+	scalar tz = inv_range * m_zNear * m_zFar;
 
 	b3Mat44 m;
 	m.x = b3Vec4(sx, 0.0f, 0.0f, 0.0f);
@@ -73,8 +73,8 @@ b3Mat44 Camera::BuildProjectionMatrix() const
 b3Transform Camera::BuildWorldTransform() const
 {
 	b3Transform xf;
-	xf.rotation = b3QuatMat33(m_q);
-	xf.position = (m_zoom * xf.rotation.z) - m_center;
+	xf.rotation = m_q;
+	xf.translation = (m_zoom * m_q.GetZAxis()) - m_center;
 	return xf;
 }
 
@@ -87,8 +87,8 @@ b3Mat44 Camera::BuildWorldMatrix() const
 b3Transform Camera::BuildViewTransform() const
 {
 	b3Transform xf;
-	xf.rotation = b3QuatMat33(m_q);
-	xf.position = (m_zoom * xf.rotation.z) - m_center;
+	xf.rotation = m_q;
+	xf.translation = (m_zoom * m_q.GetZAxis()) - m_center;
 	return b3Inverse(xf);
 }
 
@@ -100,7 +100,7 @@ b3Mat44 Camera::BuildViewMatrix() const
 
 b3Vec2 Camera::ConvertWorldToScreen(const b3Vec3& pw3) const
 {
-	float32 w = m_width, h = m_height;
+	scalar w = m_width, h = m_height;
 	b3Mat44 P = BuildProjectionMatrix();
 	b3Mat44 V = BuildViewMatrix();
 
@@ -109,11 +109,11 @@ b3Vec2 Camera::ConvertWorldToScreen(const b3Vec3& pw3) const
 	b3Vec4 pp = P * V * pw;
 
 	b3Vec3 pn(pp.x, pp.y, pp.z);
-	float32 inv_w = pp.w != 0.0f ? 1.0f / pp.w : 1.0f;
+	scalar inv_w = pp.w != 0.0f ? 1.0f / pp.w : 1.0f;
 	pn *= inv_w;
 
-	float32 u = 0.5f * (pn.x + 1.0f);
-	float32 v = 0.5f * (pn.y + 1.0f);
+	scalar u = 0.5f * (pn.x + 1.0f);
+	scalar v = 0.5f * (pn.y + 1.0f);
 
 	b3Vec2 ps;
 	ps.x = u * w;
@@ -123,10 +123,10 @@ b3Vec2 Camera::ConvertWorldToScreen(const b3Vec3& pw3) const
 
 b3Ray3 Camera::ConvertScreenToWorld(const b3Vec2& ps) const
 {
-	float32 w = m_width, h = m_height;
+	scalar w = m_width, h = m_height;
 	
-	float32 t = tan(0.5f * m_fovy);
-	float32 ratio = w / h;
+	scalar t = tan(0.5f * m_fovy);
+	scalar ratio = w / h;
 	
 	b3Vec3 vv;
 	vv.x = 2.0f * ratio * ps.x / w - ratio;
@@ -135,12 +135,12 @@ b3Ray3 Camera::ConvertScreenToWorld(const b3Vec2& ps) const
 
 	b3Transform xf = BuildWorldTransform();
 	
-	b3Vec3 vw = xf.rotation * vv;
+	b3Vec3 vw = b3Mul(xf.rotation, vv);
 	vw.Normalize();
 
 	b3Ray3 rw;
 	rw.direction = vw;
-	rw.origin = xf.position;
+	rw.origin = xf.translation;
 	rw.fraction = m_zFar;
 	return rw;
 }
@@ -194,7 +194,7 @@ void Draw::EnableDrawTriangles(bool flag)
 	g_glDrawTriangles = flag;
 }
 
-void Draw::DrawPoint(const b3Vec3& p, float32 size, const b3Color& color)
+void Draw::DrawPoint(const b3Vec3& p, scalar size, const b3Color& color)
 {
 	m_points->Vertex(p, size, color);
 }
@@ -249,14 +249,15 @@ void Draw::DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 co
 	}
 }
 
-void Draw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
+void Draw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color)
 {
 	b3Vec3 n1, n3;
 	b3ComputeBasis(normal, n1, n3);
 
 	u32 kEdgeCount = 20;
-	float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
-	b3Quat q(normal, kAngleInc);
+	scalar kAngleInc = 2.0f * B3_PI / scalar(kEdgeCount);
+	b3Quat q;
+	q.SetAxisAngle(normal, kAngleInc);
 
 	b3Vec3 p1 = center + radius * n1;
 	for (u32 i = 0; i < kEdgeCount; ++i)
@@ -272,7 +273,7 @@ void Draw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius
 	}
 }
 
-void Draw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
+void Draw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color)
 {
 	b3Color fillColor(color.r, color.g, color.b, color.a);
 	b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f);
@@ -281,8 +282,10 @@ void Draw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 r
 	b3ComputeBasis(normal, n1, n3);
 
 	const u32 kEdgeCount = 20;
-	const float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
-	b3Quat q(normal, kAngleInc);
+	const scalar kAngleInc = 2.0f * B3_PI / scalar(kEdgeCount);
+	
+	b3Quat q;
+	q.SetAxisAngle(normal, kAngleInc);
 
 	b3Vec3 p1 = center + radius * n1;
 	for (u32 i = 0; i < kEdgeCount; ++i)
@@ -299,32 +302,32 @@ void Draw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 r
 	}
 }
 
-void Draw::DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color)
+void Draw::DrawSphere(const b3Vec3& center, scalar radius, const b3Color& color)
 {
 	b3Transform xf;
 	xf.rotation.SetIdentity();
-	xf.position = center;
+	xf.translation = center;
 
 	m_wire->DrawSphere(radius, color, xf);
 }
 
-void Draw::DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Mat33& rotation, const b3Color& color)
+void Draw::DrawSolidSphere(const b3Vec3& center, scalar radius, const b3Quat& rotation, const b3Color& color)
 {
 	b3Transform xf;
 	xf.rotation = rotation;
-	xf.position = center;
+	xf.translation = center;
 
 	m_solid->DrawSphere(radius, color, xf);
 }
 
-void Draw::DrawCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const b3Color& color)
+void Draw::DrawCapsule(const b3Vec3& c1, const b3Vec3& c2, scalar radius, const b3Color& color)
 {
-	float32 height = b3Length(c1 - c2);
+	scalar height = b3Length(c1 - c2);
 
 	{
 		b3Transform xf;
 		xf.rotation.SetIdentity();
-		xf.position = c1;
+		xf.translation = c1;
 		m_wire->DrawSphere(radius, color, xf);
 	}
 
@@ -335,20 +338,20 @@ void Draw::DrawCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const
 		{
 			b3Transform xf;
 			xf.rotation.SetIdentity();
-			xf.position = c2;
+			xf.translation = c2;
 			m_wire->DrawSphere(radius, color, xf);
 		}
 	}
 }
 
-void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const b3Mat33& rotation, const b3Color& c)
+void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, scalar radius, const b3Quat& rotation, const b3Color& c)
 {
-	float32 height = b3Length(c1 - c2);
+	scalar height = b3Length(c1 - c2);
 
 	{
 		b3Transform xf;
 		xf.rotation = rotation;
-		xf.position = c1;
+		xf.translation = c1;
 		m_solid->DrawSphere(radius, c, xf);
 	}
 
@@ -359,9 +362,11 @@ void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius,
 			R.y = (1.0f / height) * (c1 - c2);
 			b3ComputeBasis(R.y, R.z, R.x);
 
+			b3Quat Q = b3Mat33Quat(R);
+
 			b3Transform xf;
-			xf.position = 0.5f * (c1 + c2);
-			xf.rotation = R;
+			xf.translation = 0.5f * (c1 + c2);
+			xf.rotation = Q;
 
 			m_solid->DrawCylinder(radius, height, c, xf);
 		}
@@ -369,7 +374,7 @@ void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius,
 		{
 			b3Transform xf;
 			xf.rotation = rotation;
-			xf.position = c2;
+			xf.translation = c2;
 			m_solid->DrawSphere(radius, c, xf);
 		}
 	}
@@ -377,24 +382,24 @@ void Draw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius,
 
 void Draw::DrawTransform(const b3Transform& xf)
 {
-	float32 lenght = 1.0f;
+	scalar lenght = 1.0f;
 
-	b3Vec3 position = xf.position;
-	b3Mat33 rotation = xf.rotation;
+	b3Vec3 translation = xf.translation;
+	b3Mat33 rotation = b3QuatMat33(xf.rotation);
 
-	b3Vec3 A = position + lenght * rotation.x;
-	b3Vec3 B = position + lenght * rotation.y;
-	b3Vec3 C = position + lenght * rotation.z;
+	b3Vec3 A = translation + lenght * rotation.x;
+	b3Vec3 B = translation + lenght * rotation.y;
+	b3Vec3 C = translation + lenght * rotation.z;
 
-	DrawSegment(position, A, b3Color_red);
-	DrawSegment(position, B, b3Color_green);
-	DrawSegment(position, C, b3Color_blue);
+	DrawSegment(translation, A, b3Color_red);
+	DrawSegment(translation, B, b3Color_green);
+	DrawSegment(translation, C, b3Color_blue);
 }
 
-void Draw::DrawAABB(const b3AABB3& aabb, const b3Color& color)
+void Draw::DrawAABB(const b3AABB& aabb, const b3Color& color)
 {
-	b3Vec3 lower = aabb.m_lower;
-	b3Vec3 upper = aabb.m_upper;
+	b3Vec3 lower = aabb.lowerBound;
+	b3Vec3 upper = aabb.upperBound;
 
 	b3Vec3 vs[8];
 
@@ -425,12 +430,12 @@ void Draw::DrawAABB(const b3AABB3& aabb, const b3Color& color)
 	DrawSegment(vs[1], vs[7], color);
 }
 
-void Draw::DrawPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
+void Draw::DrawPlane(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color)
 {
 	b3Vec3 n1, n2;
 	b3ComputeBasis(normal, n1, n2);
 
-	float32 scale = 2.0f * radius;
+	scalar scale = 2.0f * radius;
 	
 	// v1__v4
 	// |    |
@@ -448,14 +453,14 @@ void Draw::DrawPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius,
 	DrawSegment(center, center + normal, color);
 }
 
-void Draw::DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
+void Draw::DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color)
 {
 	b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f);
 
 	b3Vec3 n1, n2;
 	b3ComputeBasis(normal, n1, n2);
 
-	float32 scale = 2.0f * radius;
+	scalar scale = 2.0f * radius;
 
 	b3Vec3 v1 = center - scale * n1 - scale * n2;
 	b3Vec3 v2 = center + scale * n1 - scale * n2;

examples/testbed/framework/draw.h

@@ -42,13 +42,13 @@ public:
 	b3Vec2 ConvertWorldToScreen(const b3Vec3& pw) const;
 	b3Ray3 ConvertScreenToWorld(const b3Vec2& ps) const;
 
-	float32 m_zoom;
+	scalar m_zoom;
 	b3Vec3 m_center;
 	b3Quat m_q;
-	float32 m_width, m_height;
-	float32 m_fovy;
-	float32 m_zNear;
-	float32 m_zFar;
+	scalar m_width, m_height;
+	scalar m_fovy;
+	scalar m_zNear;
+	scalar m_zFar;
 };
 
 class Draw : public b3Draw
@@ -67,7 +67,7 @@ public:
 	
 	void EnableDrawTriangles(bool flag);
 
-	void DrawPoint(const b3Vec3& p, float32 size, const b3Color& color);
+	void DrawPoint(const b3Vec3& p, scalar size, const b3Color& color);
 
 	void DrawSegment(const b3Vec3& p1, const b3Vec3& p2, const b3Color& color);
 	
@@ -79,23 +79,23 @@ public:
 	
 	void DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 count, const b3Color& color);
 
-	void DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
+	void DrawCircle(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color);
 
-	void DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
+	void DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color);
 
-	void DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color);
+	void DrawSphere(const b3Vec3& center, scalar radius, const b3Color& color);
 	
-	void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Mat33& rotation, const b3Color& color);
+	void DrawSolidSphere(const b3Vec3& center, scalar radius, const b3Quat& rotation, const b3Color& color);
 
-	void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color);
+	void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, scalar radius, const b3Color& color);
 
-	void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Mat33& rotation, const b3Color& color);
+	void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, scalar radius, const b3Quat& rotation, const b3Color& color);
 
-	void DrawPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
+	void DrawPlane(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color);
 
-	void DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
+	void DrawSolidPlane(const b3Vec3& normal, const b3Vec3& center, scalar radius, const b3Color& color);
 
-	void DrawAABB(const b3AABB3& aabb, const b3Color& color);
+	void DrawAABB(const b3AABB& aabb, const b3Color& color);
 
 	void DrawTransform(const b3Transform& xf);
 

examples/testbed/framework/draw_gl2.h

@@ -166,7 +166,7 @@ struct DrawPoints
 		glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(b3Color), m_colors, GL_DYNAMIC_DRAW);
 
 		glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[2]);
-		glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(float32), m_sizes, GL_DYNAMIC_DRAW);
+		glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(scalar), m_sizes, GL_DYNAMIC_DRAW);
 
 		AssertGL();
 
@@ -181,7 +181,7 @@ struct DrawPoints
 		glDeleteBuffers(3, m_vboIds);
 	}
 
-	void Vertex(const b3Vec3& v, float32 size, const b3Color& color)
+	void Vertex(const b3Vec3& v, scalar size, const b3Color& color)
 	{
 		if (m_count == e_vertexCapacity)
 		{
@@ -226,7 +226,7 @@ struct DrawPoints
 		glVertexAttribPointer(m_colorAttribute, 4, GL_FLOAT, GL_FALSE, 0, BUFFER_OFFSET(0));
 
 		glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[2]);
-		glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(float32), m_sizes);
+		glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(scalar), m_sizes);
 		glEnableVertexAttribArray(m_sizeAttribute);
 		glVertexAttribPointer(m_sizeAttribute, 1, GL_FLOAT, GL_FALSE, 0, BUFFER_OFFSET(0));
 
@@ -255,7 +255,7 @@ struct DrawPoints
 
 	b3Vec3 m_vertices[e_vertexCapacity];
 	b3Color m_colors[e_vertexCapacity];
-	float32 m_sizes[e_vertexCapacity];
+	scalar m_sizes[e_vertexCapacity];
 	u32 m_count;
 
 	GLuint m_programId;
@@ -365,7 +365,7 @@ struct DrawLines
 
 		glDisableVertexAttribArray(m_colorAttribute);
 		
-		glEnableVertexAttribArray(m_vertexAttribute);
+		glDisableVertexAttribArray(m_vertexAttribute);
 
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
 		glUseProgram(0);
@@ -608,7 +608,7 @@ struct DrawWire
 		glDeleteProgram(m_programId);
 	}
 
-	void DrawSphere(float32 radius, const b3Color& c, const b3Transform& xf)
+	void DrawSphere(scalar radius, const b3Color& c, const b3Transform& xf)
 	{
 		if (!g_glDrawLines)
 		{
@@ -777,7 +777,7 @@ struct DrawSolid
 	{
 	}
 
-	void DrawCylinder(float32 radius, float32 height, const b3Color& c, const b3Transform& xf)
+	void DrawCylinder(scalar radius, scalar height, const b3Color& c, const b3Transform& xf)
 	{
 		if (!g_glDrawTriangles)
 		{
@@ -820,7 +820,7 @@ struct DrawSolid
 		glUseProgram(0);
 	}
 
-	void DrawSphere(float32 radius, const b3Color& c, const b3Transform& xf)
+	void DrawSphere(scalar radius, const b3Color& c, const b3Transform& xf)
 	{
 		if (!g_glDrawTriangles)
 		{
@@ -874,4 +874,4 @@ struct DrawSolid
 	DrawSolidCylinder m_cylinder;
 };
 
-#endif
+#endif

examples/testbed/framework/draw_gl4.h

@@ -178,7 +178,7 @@ struct DrawPoints
 
 		glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[2]);
 		glVertexAttribPointer(m_sizeAttribute, 1, GL_FLOAT, GL_FALSE, 0, BUFFER_OFFSET(0));
-		glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(float32), m_sizes, GL_DYNAMIC_DRAW);
+		glBufferData(GL_ARRAY_BUFFER, e_vertexCapacity * sizeof(scalar), m_sizes, GL_DYNAMIC_DRAW);
 
 		AssertGL();
 
@@ -195,7 +195,7 @@ struct DrawPoints
 		glDeleteBuffers(3, m_vboIds);
 	}
 
-	void Vertex(const b3Vec3& v, float32 size, const b3Color& color)
+	void Vertex(const b3Vec3& v, scalar size, const b3Color& color)
 	{
 		if (m_count == e_vertexCapacity)
 		{
@@ -238,7 +238,7 @@ struct DrawPoints
 		glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(b3Color), m_colors);
 
 		glBindBuffer(GL_ARRAY_BUFFER, m_vboIds[2]);
-		glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(float32), m_sizes);
+		glBufferSubData(GL_ARRAY_BUFFER, 0, m_count * sizeof(scalar), m_sizes);
 
 		glEnable(GL_PROGRAM_POINT_SIZE);
 		glDrawArrays(GL_POINTS, 0, m_count);
@@ -260,7 +260,7 @@ struct DrawPoints
 
 	b3Vec3 m_vertices[e_vertexCapacity];
 	b3Color m_colors[e_vertexCapacity];
-	float32 m_sizes[e_vertexCapacity];
+	scalar m_sizes[e_vertexCapacity];
 	u32 m_count;
 
 	GLuint m_programId;
@@ -625,7 +625,7 @@ struct DrawWire
 		glDeleteProgram(m_programId);
 	}
 
-	void DrawSphere(float32 radius, const b3Color& c, const b3Transform& xf)
+	void DrawSphere(scalar radius, const b3Color& c, const b3Transform& xf)
 	{
 		if (!g_glDrawLines)
 		{
@@ -792,7 +792,7 @@ struct DrawSolid
 	{
 	}
 
-	void DrawCylinder(float32 radius, float32 height, const b3Color& c, const b3Transform& xf)
+	void DrawCylinder(scalar radius, scalar height, const b3Color& c, const b3Transform& xf)
 	{
 		if (!g_glDrawTriangles)
 		{
@@ -831,7 +831,7 @@ struct DrawSolid
 		glUseProgram(0);
 	}
 
-	void DrawSphere(float32 radius, const b3Color& c, const b3Transform& xf)
+	void DrawSphere(scalar radius, const b3Color& c, const b3Transform& xf)
 	{
 		if (!g_glDrawTriangles)
 		{

examples/testbed/framework/json_profiler.cpp

@@ -77,7 +77,7 @@ void JsonProfiler::EndEvents()
 	m_file = nullptr;
 }
 
-void JsonProfiler::BeginEvent(const char* name, float64 t)
+void JsonProfiler::BeginEvent(const char* name, scalar64 t)
 {
 	if (!m_writer)
 	{
@@ -86,7 +86,7 @@ void JsonProfiler::BeginEvent(const char* name, float64 t)
 
 	const char* phase = "B";
 
-	float64 scale = 1000.0;
+	scalar64 scale = 1000.0;
 
 	m_writer->StartObject();
 	m_writer->STRING("pid");  m_writer->Int(0);
@@ -99,7 +99,7 @@ void JsonProfiler::BeginEvent(const char* name, float64 t)
 	m_writer->EndObject();
 }
 
-void JsonProfiler::EndEvent(const char* name, float64 t)
+void JsonProfiler::EndEvent(const char* name, scalar64 t)
 {
 	if (!m_writer)
 	{
@@ -108,7 +108,7 @@ void JsonProfiler::EndEvent(const char* name, float64 t)
 
 	const char* phase = "E";
 
-	float64 scale = 1000.0;
+	scalar64 scale = 1000.0;
 
 	m_writer->StartObject();
 	m_writer->STRING("pid");  m_writer->Int(0);

examples/testbed/framework/json_profiler.h

@@ -43,9 +43,9 @@ public:
 
 	void EndEvents();
 
-	void BeginEvent(const char* name, float64 time);
+	void BeginEvent(const char* name, scalar64 time);
 
-	void EndEvent(const char* name, float64 time);
+	void EndEvent(const char* name, scalar64 time);
 private:
 	FILE* m_file;
 	FileWriteStream* m_stream;

examples/testbed/framework/main.cpp

@@ -24,6 +24,8 @@
 	// error
 #endif
 
+#include <stdio.h>
+
 #include <glfw/glfw3.h>
 
 #include <testbed/framework/model.h>
@@ -103,14 +105,12 @@ static void Run()
 
 	while (glfwWindowShouldClose(g_window) == 0)
 	{
+		g_frameAllocator->Reset();
+
 		g_profiler->Begin();
-		
-		g_profilerSt->Begin();
 
 		g_profiler->BeginScope("Frame");
 
-		g_profilerSt->BeginScope("Frame");
-
 		g_view->BeginInterface();
 
 		if (g_model->IsPaused())
@@ -126,22 +126,13 @@ static void Run()
 
 		g_model->Update();
 
-		g_profilerSt->EndScope();
-		
 		g_profiler->EndScope();
 		
-		if (g_settings->drawProfileTree)
+		if (g_settings->drawProfiler)
 		{
-			g_view->InterfaceProfileTree();
+			g_view->InterfaceProfiler();
 		}
 
-		if (g_settings->drawProfileTreeStats)
-		{
-			g_view->InterfaceProfileTreeStats();
-		}
-
-		g_profilerSt->End();
-
 #if PROFILE_JSON == 1
 		g_model->UpdateJson();
 #endif
@@ -160,7 +151,7 @@ int main(int argc, char** args)
 #if defined(_WIN32)
 	// Report memory leaks
 	_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF | _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG));
-	//_CrtSetBreakAlloc(0);
+	//_CrtSetBreakAlloc();
 #endif
 
 	if (glfwInit() == 0)
@@ -186,7 +177,7 @@ int main(int argc, char** args)
 	glfwSwapInterval(1);
 
 	glfwMakeContextCurrent(g_window);
-
+	
 	if (gladLoadGL() == 0)
 	{
 		fprintf(stderr, "Failed to load OpenGL extensions\n");

examples/testbed/framework/model.cpp

@@ -20,14 +20,17 @@
 #include <testbed/framework/view_model.h>
 #include <testbed/framework/test.h>
 
+b3FrameAllocator* g_frameAllocator = nullptr;
+b3Profiler* g_profiler = nullptr;
+
 Model::Model()
 {
 	m_viewModel = nullptr;
 	g_draw = &m_draw;
 	g_camera = &m_camera;
 	g_profiler = &m_profiler;
-	g_profilerSt = &m_profilerSt;
-	
+	g_frameAllocator = &m_frame;
+
 #if (PROFILE_JSON == 1)
 	g_jsonProfiler = &m_jsonProfiler;
 #endif
@@ -41,7 +44,7 @@ Model::Model()
 	glEnable(GL_BLEND);
 	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 
-	glClearColor(0.3f, 0.3f, 0.3f, 1.0f);
+	glClearColor(0.2f, 0.2f, 0.2f, 1.0f);
 	glClearDepth(1.0f);
 
 	Action_ResetCamera();
@@ -56,7 +59,7 @@ Model::~Model()
 	g_draw = nullptr;
 	g_camera = nullptr;
 	g_profiler = nullptr;
-	g_profilerSt = nullptr;
+	g_frameAllocator = nullptr;
 
 #if (PROFILE_JSON == 1)
 	g_jsonProfiler = nullptr;
@@ -114,11 +117,11 @@ void Model::Update()
 
 	if (m_setTest)
 	{
+		Action_ResetCamera();
 		delete m_test;
 		m_test = g_tests[g_settings->testID].create();
 		m_setTest = false;
 		m_pause = true;
-		Action_ResetCamera();
 	}
 	
 	if (g_settings->drawGrid)
@@ -129,9 +132,9 @@ void Model::Update()
 		b3Vec3 vs[h * w];
 
 		b3Vec3 t;
-		t.x = -0.5f * float32(w) + 0.5f;
+		t.x = -0.5f * scalar(w) + 0.5f;
 		t.y = 0.0f;
-		t.z = -0.5f * float32(h) + 0.5f;
+		t.z = -0.5f * scalar(h) + 0.5f;
 
 		for (u32 i = 0; i < h; ++i)
 		{
@@ -140,9 +143,9 @@ void Model::Update()
 				u32 iv = i * w + j;
 
 				b3Vec3 v;
-				v.x = float32(j);
+				v.x = scalar(j);
 				v.y = 0.0f;
-				v.z = float32(i);
+				v.z = scalar(i);
 
 				v += t;
 				
@@ -150,57 +153,55 @@ void Model::Update()
 			}
 		}
 
-		b3Color color(0.2f, 0.2f, 0.2f, 1.0f);
+		b3Color borderColor(0.0f, 0.0f, 0.0f, 1.0f);
+		b3Color centerColor(0.8f, 0.8f, 0.8f, 1.0f);
+		b3Color color(0.4f, 0.4f, 0.4f, 1.0f);
 
-		// Left-Right Lines
-		u32 hv1 = (h - 1) / 2 * w + 0;
-		u32 hv2 = (h - 1) / 2 * w + (w - 1);
-		{
-			b3Vec3 v1 = vs[hv1];
-			b3Vec3 v2 = vs[hv2];
-
-			b3Draw_draw->DrawSegment(v1, v2, b3Color_black);
-		}
-		
+		// Left to right lines
 		for (u32 i = 0; i < h; ++i)
 		{
-			if (i == hv1)
-			{
-				continue;
-			}
-
 			u32 iv1 = i * w + 0;
 			u32 iv2 = i * w + (w - 1);
 
 			b3Vec3 v1 = vs[iv1];
 			b3Vec3 v2 = vs[iv2];
 
-			b3Draw_draw->DrawSegment(v1, v2, color);
-		}
-
-		// Up-Bottom Lines
-		u32 wv1 = 0 * w + (w - 1) / 2;
-		u32 wv2 = (h - 1) * w + (w - 1) / 2;
-		{
-			b3Vec3 v1 = vs[wv1];
-			b3Vec3 v2 = vs[wv2];
-
-			b3Draw_draw->DrawSegment(v1, v2, b3Color_black);
-		}
-
-		for (u32 j = 0; j < w; ++j)
-		{
-			if (j == wv1)
+			if (i == 0 || i == (h - 1))
 			{
+				b3Draw_draw->DrawSegment(v1, v2, borderColor);
 				continue;
 			}
 
+			if (i == (h - 1) / 2)
+			{
+				b3Draw_draw->DrawSegment(v1, v2, centerColor);
+				continue;
+			}
+
+			b3Draw_draw->DrawSegment(v1, v2, color);
+		}
+
+		// Up to bottom lines
+		for (u32 j = 0; j < w; ++j)
+		{
 			u32 iv1 = 0 * w + j;
 			u32 iv2 = (h - 1) * w + j;
 
 			b3Vec3 v1 = vs[iv1];
 			b3Vec3 v2 = vs[iv2];
 
+			if (j == 0 || j == (w - 1))
+			{
+				b3Draw_draw->DrawSegment(v1, v2, borderColor);
+				continue;
+			}
+
+			if (j == (w - 1) / 2)
+			{
+				b3Draw_draw->DrawSegment(v1, v2, centerColor);
+				continue;
+			}
+
 			b3Draw_draw->DrawSegment(v1, v2, color);
 		}
 	}
@@ -233,15 +234,17 @@ void Model::Update()
 
 #if (PROFILE_JSON == 1)
 
-static inline void RecurseEvents(ProfilerNode* node)
+static inline void RecurseEvents(b3ProfilerNode* node)
 {
 	g_jsonProfiler->BeginEvent(node->name, node->t0);
 	
 	g_jsonProfiler->EndEvent(node->name, node->t1);
 
-	for (u32 i = 0; i < node->children.Count(); ++i)
+	b3ProfilerNode* child = node->head;
+	while (child)
 	{
-		RecurseEvents(node->children[i]);
+		RecurseEvents(child);
+		child = child->next;
 	}
 }
 
@@ -249,7 +252,7 @@ void Model::UpdateJson()
 {
 	m_jsonProfiler.BeginEvents();
 
-	ProfilerNode* root = m_profiler.GetRoot();
+	b3ProfilerNode* root = m_profiler.GetRoot();
 
 	if (root)
 	{

examples/testbed/framework/model.h

@@ -20,8 +20,11 @@
 #define MODEL_H
 
 #include <testbed/framework/draw.h>
-#include <testbed/framework/profiler.h>
-#include <testbed/framework/profiler_st.h>
+#include <bounce/common/profiler.h>
+
+extern b3FrameAllocator* g_frameAllocator;
+
+extern b3Profiler* g_profiler;
 
 // Set to 1 to write profile events into a .json file. Set to 0 otherwise.
 #define PROFILE_JSON 0
@@ -53,12 +56,12 @@ public:
 	void Command_Release_Mouse_Left(const b3Vec2& ps);
 	void Command_Move_Cursor(const b3Vec2& ps);
 
-	void Command_ResizeCamera(float32 w, float32 h);
-	void Command_RotateCameraX(float32 angle);
-	void Command_RotateCameraY(float32 angle);
-	void Command_TranslateCameraX(float32 d);
-	void Command_TranslateCameraY(float32 d);
-	void Command_ZoomCamera(float32 d);
+	void Command_ResizeCamera(scalar w, scalar h);
+	void Command_RotateCameraX(scalar angle);
+	void Command_RotateCameraY(scalar angle);
+	void Command_TranslateCameraX(scalar d);
+	void Command_TranslateCameraY(scalar d);
+	void Command_ZoomCamera(scalar d);
 
 	void Update();
 
@@ -74,8 +77,8 @@ private:
 
 	Draw m_draw;
 	Camera m_camera;
-	Profiler m_profiler;
-	ProfilerSt m_profilerSt;
+	b3FrameAllocator m_frame;
+	b3Profiler m_profiler;
 
 #if (PROFILE_JSON == 1)
 	JsonProfiler m_jsonProfiler;
@@ -115,13 +118,13 @@ inline void Model::Action_ResetCamera()
 	m_camera.m_zoom = 50.0f;
 }
 
-inline void Model::Command_ResizeCamera(float32 w, float32 h)
+inline void Model::Command_ResizeCamera(scalar w, scalar h)
 {
 	m_camera.m_width = w;
 	m_camera.m_height = h;
 }
 
-inline void Model::Command_RotateCameraX(float32 angle)
+inline void Model::Command_RotateCameraX(scalar angle)
 {
 	b3Quat d = b3QuatRotationX(angle);
 
@@ -129,7 +132,7 @@ inline void Model::Command_RotateCameraX(float32 angle)
 	m_camera.m_q.Normalize();
 }
 
-inline void Model::Command_RotateCameraY(float32 angle)
+inline void Model::Command_RotateCameraY(scalar angle)
 {
 	b3Quat d = b3QuatRotationY(angle);
 
@@ -137,21 +140,21 @@ inline void Model::Command_RotateCameraY(float32 angle)
 	m_camera.m_q.Normalize();
 }
 
-inline void Model::Command_TranslateCameraX(float32 d)
+inline void Model::Command_TranslateCameraX(scalar d)
 {
 	b3Transform transform = m_camera.BuildWorldTransform();
 
-	m_camera.m_center += d * transform.rotation.x;
+	m_camera.m_center += d * transform.rotation.GetXAxis();
 }
 
-inline void Model::Command_TranslateCameraY(float32 d)
+inline void Model::Command_TranslateCameraY(scalar d)
 {
 	b3Transform transform = m_camera.BuildWorldTransform();
 
-	m_camera.m_center += d * transform.rotation.y;
+	m_camera.m_center += d * transform.rotation.GetYAxis();
 }
 
-inline void Model::Command_ZoomCamera(float32 d)
+inline void Model::Command_ZoomCamera(scalar d)
 {
 	m_camera.m_zoom += d;
 }

examples/testbed/framework/profiler.cpp

@@ -1,110 +0,0 @@
-/*
-* 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 <testbed/framework/profiler.h>
-
-Profiler* g_profiler = nullptr;
-
-Profiler::Profiler() : m_pool(sizeof(ProfilerNode))
-{
-	m_root = nullptr;
-	m_top = nullptr;
-}
-
-Profiler::~Profiler()
-{
-	assert(m_root == nullptr);
-	assert(m_top == nullptr);
-}
-
-ProfilerNode* Profiler::CreateNode()
-{
-	void* block = m_pool.Allocate();
-	ProfilerNode* n = new (block) ProfilerNode();
-	return n;
-}
-
-void Profiler::DestroyNode(ProfilerNode* node)
-{
-	node->~ProfilerNode();
-	m_pool.Free(node);
-}
-
-void Profiler::BeginScope(const char* name)
-{
-	m_time.Update();
-
-	ProfilerNode* n = CreateNode();
-	n->name = name;
-	n->t0 = m_time.GetCurrentMilis();
-	n->t1 = 0.0;
-	n->parent = m_top;
-
-	if (m_root == nullptr)
-	{
-		m_root = n;
-		m_top = n;
-		return;
-	}
-
-	if (m_top)
-	{
-		m_top->children.PushBack(n);
-	}
-
-	m_top = n;
-}
-
-void Profiler::EndScope()
-{
-	m_time.Update();
-	
-	assert(m_top != nullptr);
-	m_top->t1 = m_time.GetCurrentMilis();
-	assert(m_top->t1 > m_top->t0);
-
-	m_top = m_top->parent;
-}
-
-void Profiler::Begin()
-{
-	// If this assert is hit then it means Profiler::End hasn't been called.
-	assert(m_root == nullptr);
-	assert(m_top == nullptr);
-}
-
-void Profiler::RecurseDestroyNode(ProfilerNode* node)
-{
-	for (u32 i = 0; i < node->children.Count(); ++i)
-	{
-		RecurseDestroyNode(node->children[i]);
-	}
-
-	DestroyNode(node);
-}
-
-void Profiler::End()
-{
-	assert(m_top == nullptr);
-
-	if (m_root)
-	{
-		RecurseDestroyNode(m_root);
-		m_root = nullptr;
-	}
-}

examples/testbed/framework/profiler.h

@@ -1,73 +0,0 @@
-/*
-* 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 PROFILER_H
-#define PROFILER_H
-
-#include <bounce/common/math/math.h>
-#include <bounce/common/memory/block_pool.h>
-#include <bounce/common/template/array.h>
-#include <bounce/common/time.h>
-
-// Profiler node
-struct ProfilerNode
-{
-	const char* name;
-	float64 t0;
-	float64 t1;
-	ProfilerNode* parent;
-	b3StackArray<ProfilerNode*, 32> children;
-};
-
-// A single-threaded profiler.
-class Profiler
-{
-public:
-	Profiler();
-	
-	~Profiler();
-
-	// Must be called before profiling. 
-	void Begin();
-
-	// Must be called after profiling.
-	void End();
-
-	// Begin a new scope.
-	void BeginScope(const char* name);
-	
-	// End the top scope.
-	void EndScope();
-
-	// Get the root profiler node.
-	ProfilerNode* GetRoot() { return m_root; }
-private:
-	ProfilerNode* CreateNode();
-	void DestroyNode(ProfilerNode* node);
-	
-	void RecurseDestroyNode(ProfilerNode* node);
-
-	b3BlockPool m_pool; // pool of nodes
-	b3Time m_time; // timer
-	ProfilerNode* m_root; // tree root node
-	ProfilerNode* m_top; // top node
-};
-
-extern Profiler* g_profiler;
-
-#endif

examples/testbed/framework/profiler_st.cpp

@@ -1,197 +0,0 @@
-/*
-* 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 <testbed/framework/profiler_st.h>
-
-ProfilerSt* g_profilerSt = nullptr;
-
-ProfilerSt::ProfilerSt() : m_pool(sizeof(ProfilerStNode))
-{
-	m_root = nullptr;
-	m_top = nullptr;
-}
-
-ProfilerSt::~ProfilerSt()
-{
-	assert(m_root == nullptr);
-	assert(m_top == nullptr);
-}
-
-ProfilerStNodeStat* ProfilerSt::FindStat(const char* name)
-{
-	for (u32 i = 0; i < m_stats.Count(); ++i)
-	{
-		if (m_stats[i].name == name)
-		{
-			return &m_stats[i];
-		}
-	}
-
-	return nullptr;
-}
-
-ProfilerStNodeStat* ProfilerSt::CreateStat()
-{
-	m_stats.PushBack(ProfilerStNodeStat());
-	return &m_stats.Back();
-}
-
-ProfilerStNode* ProfilerSt::CreateNode()
-{
-	void* block = m_pool.Allocate();
-	return new (block) ProfilerStNode();
-}
-
-void ProfilerSt::DestroyNode(ProfilerStNode* node)
-{
-	node->~ProfilerStNode();
-	m_pool.Free(node);
-}
-
-void ProfilerSt::RecurseDestroyNode(ProfilerStNode* node)
-{
-	for (u32 i = 0; i < node->children.Count(); ++i)
-	{
-		return RecurseDestroyNode(node->children[i]);
-	}
-
-	DestroyNode(node);
-}
-
-static ProfilerStNode* RecurseFindNode(ProfilerStNode* node, const char* name)
-{
-	if (node->name == name)
-	{
-		return node;
-	}
-
-	ProfilerStNode* result = nullptr;
-	for (u32 i = 0; result == nullptr && i < node->children.Count(); ++i)
-	{
-		result = RecurseFindNode(node->children[i], name);
-	}
-
-	return result;
-}
-
-ProfilerStNode* ProfilerSt::FindNode(const char* name)
-{
-	if (m_top)
-	{
-		return RecurseFindNode(m_top, name);
-	}
-
-	if (m_root)
-	{
-		return RecurseFindNode(m_root, name);
-	}
-
-	return nullptr;
-}
-
-void ProfilerSt::BeginScope(const char* name)
-{
-	ProfilerStNode* fn = FindNode(name);
-
-	if (fn)
-	{
-		m_time.Update();
-		fn->t0 = m_time.GetCurrentMilis();
-		++fn->callCount;
-		m_top = fn;
-		return;
-	}
-
-	m_time.Update();
-
-	ProfilerStNode* n = CreateNode();
-	n->name = name;
-	n->t0 = m_time.GetCurrentMilis();
-	n->elapsed = 0.0f;
-	n->callCount = 1;
-	n->parent = m_top;
-	n->stat = nullptr;
-
-	if (m_root == nullptr)
-	{
-		m_root = n;
-		m_top = n;
-
-		return;
-	}
-
-	if (m_top)
-	{
-		m_top->children.PushBack(n);
-	}
-
-	m_top = n;
-}
-
-void ProfilerSt::EndScope()
-{
-	assert(m_top != nullptr);
-
-	m_time.Update();
-
-	m_top->t1 = m_time.GetCurrentMilis();
-	
-	float64 elapsedTime = m_top->t1 - m_top->t0;
-
-	m_top->elapsed += elapsedTime;
-	
-	ProfilerStNodeStat* stat = FindStat(m_top->name);
-
-	if (stat == nullptr)
-	{
-		stat = CreateStat();
-		stat->name = m_top->name;
-		stat->minElapsed = elapsedTime;
-		stat->maxElapsed = elapsedTime;	
-	}
-	else
-	{
-		stat->minElapsed = b3Min(stat->minElapsed, elapsedTime);
-		stat->maxElapsed = b3Max(stat->maxElapsed, elapsedTime);
-	}
-
-	if (m_top->stat == nullptr)
-	{
-		m_top->stat = stat;
-	}
-
-	assert(m_top->stat == stat);
-
-	m_top = m_top->parent;
-}
-
-void ProfilerSt::Begin()
-{
-	assert(m_top == nullptr);
-}
-
-void ProfilerSt::End()
-{
-	assert(m_top == nullptr);
-	
-	if (m_root)
-	{
-		RecurseDestroyNode(m_root);
-		m_root = nullptr;
-	}
-}

examples/testbed/framework/profiler_st.h

@@ -1,94 +0,0 @@
-/*
-* 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 PROFILER_ST_H
-#define PROFILER_ST_H
-
-#include <bounce/common/math/math.h>
-#include <bounce/common/memory/block_pool.h>
-#include <bounce/common/template/array.h>
-#include <bounce/common/time.h>
-
-// Profiler tree node statistics
-struct ProfilerStNodeStat
-{
-	const char* name;
-	float64 minElapsed;
-	float64 maxElapsed;
-};
-
-// A profiler tree node
-struct ProfilerStNode
-{
-	const char* name; 
-	float64 t0;
-	float64 t1; 
-	
-	float64 elapsed;
-	u32 callCount;
-	
-	ProfilerStNode* parent;
-	b3StackArray<ProfilerStNode*, 32> children;
-
-	ProfilerStNodeStat* stat;
-};
-
-// A profiler tree 
-class ProfilerSt
-{
-public:
-	ProfilerSt();
-	
-	~ProfilerSt();
-
-	// Must be called before profiling. 
-	void Begin();
-
-	// Must be called after profiling.
-	void End();
-
-	// Begin a new scope.
-	void BeginScope(const char* name);
-
-	// End the top scope.
-	void EndScope();
-
-	ProfilerStNode* GetRoot() { return m_root; }
-private:
-	ProfilerStNode* CreateNode();
-	void DestroyNode(ProfilerStNode* node);
-	
-	void RecurseDestroyNode(ProfilerStNode* node);
-
-	ProfilerStNode* FindNode(const char* name);
-
-	ProfilerStNodeStat* CreateStat();
-
-	ProfilerStNodeStat* FindStat(const char* name);
-
-	b3BlockPool m_pool; // pool of nodes
-	b3Time m_time; // timer
-	ProfilerStNode* m_root; // tree root node
-	ProfilerStNode* m_top; // top node
-
-	b3StackArray<ProfilerStNodeStat, 256> m_stats; // node statistics
-};
-
-extern ProfilerSt* g_profilerSt;
-
-#endif

examples/testbed/framework/softbody_dragger.cpp

@@ -22,7 +22,7 @@ b3SoftBodyDragger::b3SoftBodyDragger(b3Ray3* ray, b3SoftBody* body)
 {
 	m_ray = ray;
 	m_body = body;
-	m_tetrahedron = nullptr;
+	m_isDragging = false;
 }
 
 b3SoftBodyDragger::~b3SoftBodyDragger()
@@ -40,41 +40,45 @@ bool b3SoftBodyDragger::StartDragging()
 		return false;
 	}
 
-	m_mesh = m_body->GetMesh();
-	m_tetrahedron = m_mesh->tetrahedrons + rayOut.tetrahedron;
-	m_v1 = m_tetrahedron->v1;
-	m_v2 = m_tetrahedron->v2;
-	m_v3 = m_tetrahedron->v3;
-	m_v4 = m_tetrahedron->v4;
+	m_isDragging = true;
 	m_x = rayOut.fraction;
 
-	b3SoftBodyNode* n1 = m_body->GetVertexNode(m_v1);
-	b3SoftBodyNode* n2 = m_body->GetVertexNode(m_v2);
-	b3SoftBodyNode* n3 = m_body->GetVertexNode(m_v3);
-	b3SoftBodyNode* n4 = m_body->GetVertexNode(m_v4);
+	const b3SoftBodyMesh* mesh = m_body->GetMesh();
+	const b3SoftBodyMeshTriangle* triangle = mesh->triangles + rayOut.triangle;
 
-	b3Vec3 v1 = n1->GetPosition();
-	b3Vec3 v2 = n2->GetPosition();
-	b3Vec3 v3 = n3->GetPosition();
-	b3Vec3 v4 = n4->GetPosition();
+	m_n1 = m_body->GetNode(triangle->v1);
+	m_n2 = m_body->GetNode(triangle->v2);
+	m_n3 = m_body->GetNode(triangle->v3);
+
+	b3Vec3 v1 = m_n1->GetPosition();
+	b3Vec3 v2 = m_n2->GetPosition();
+	b3Vec3 v3 = m_n3->GetPosition();
 
 	b3Vec3 B = GetPointB();
 
-	float32 wABCD[5];
-	b3BarycentricCoordinates(wABCD, v1, v2, v3, v4, B);
+	scalar wABC[4];
+	b3BarycentricCoordinates(wABC, v1, v2, v3, B);
 
-	if (wABCD[4] > B3_EPSILON)
+	if (wABC[3] > B3_EPSILON)
 	{
-		m_tu = wABCD[0] / wABCD[4];
-		m_tv = wABCD[1] / wABCD[4];
-		m_tw = wABCD[2] / wABCD[4];
-		m_tx = wABCD[3] / wABCD[4];
+		m_tu = wABC[0] / wABC[3];
+		m_tv = wABC[1] / wABC[3];
+		m_tw = wABC[2] / wABC[3];
 	}
 	else
 	{
-		m_tu = m_tv = m_tw = m_tx = 0.0f;
+		m_tu = m_tv = m_tw = 0.0f;
 	}
 
+	m_t1 = m_n1->GetType();
+	m_n1->SetType(e_staticSoftBodyNode);
+
+	m_t2 = m_n2->GetType();
+	m_n2->SetType(e_staticSoftBodyNode);
+
+	m_t3 = m_n3->GetType();
+	m_n3->SetType(e_staticSoftBodyNode);
+
 	return true;
 }
 
@@ -87,38 +91,31 @@ void b3SoftBodyDragger::Drag()
 
 	b3Vec3 dx = B - A;
 
-	const float32 k = 100.0f;
-
-	b3Vec3 f = k * dx;
-
-	b3Vec3 f1 = m_tu * f;
-	b3Vec3 f2 = m_tv * f;
-	b3Vec3 f3 = m_tw * f;
-	b3Vec3 f4 = m_tx * f;
-
-	m_body->GetVertexNode(m_v1)->ApplyForce(f1);
-	m_body->GetVertexNode(m_v2)->ApplyForce(f2);
-	m_body->GetVertexNode(m_v3)->ApplyForce(f3);
-	m_body->GetVertexNode(m_v4)->ApplyForce(f4);
+	m_n1->ApplyTranslation(dx);
+	m_n2->ApplyTranslation(dx);
+	m_n3->ApplyTranslation(dx);
 }
 
 void b3SoftBodyDragger::StopDragging()
 {
 	B3_ASSERT(IsDragging() == true);
 	
-	m_tetrahedron = nullptr;
+	m_n1->SetType(m_t1);
+	m_n2->SetType(m_t2);
+	m_n3->SetType(m_t3);
+	
+	m_isDragging = false;
 }
 
 b3Vec3 b3SoftBodyDragger::GetPointA() const
 {
 	B3_ASSERT(IsDragging() == true);
 
-	b3Vec3 A = m_body->GetVertexNode(m_v1)->GetPosition();
-	b3Vec3 B = m_body->GetVertexNode(m_v2)->GetPosition();
-	b3Vec3 C = m_body->GetVertexNode(m_v3)->GetPosition();
-	b3Vec3 D = m_body->GetVertexNode(m_v4)->GetPosition();
+	b3Vec3 A = m_n1->GetPosition() + m_n1->GetTranslation();
+	b3Vec3 B = m_n2->GetPosition() + m_n2->GetTranslation();
+	b3Vec3 C = m_n3->GetPosition() + m_n3->GetTranslation();
 
-	return m_tu * A + m_tv * B + m_tw * C + m_tx * D;
+	return m_tu * A + m_tv * B + m_tw * C;
 }
 
 b3Vec3 b3SoftBodyDragger::GetPointB() const

examples/testbed/framework/softbody_dragger.h

@@ -44,18 +44,18 @@ public:
 	b3Vec3 GetPointB() const;
 private:
 	b3Ray3* m_ray;
-	float32 m_x;
-
 	b3SoftBody* m_body;
-	const b3SoftBodyMesh* m_mesh;
-	const b3SoftBodyMeshTetrahedron* m_tetrahedron;
-	u32 m_v1, m_v2, m_v3, m_v4;
-	float32 m_tu, m_tv, m_tw, m_tx;
+
+	bool m_isDragging;
+	scalar m_x;
+	scalar m_tu, m_tv, m_tw;
+	b3SoftBodyNode * m_n1, * m_n2, * m_n3;
+	b3SoftBodyNodeType m_t1, m_t2, m_t3;
 };
 
 inline bool b3SoftBodyDragger::IsDragging() const
 {
-	return m_tetrahedron != nullptr;
+	return m_isDragging;
 }
 
 #endif

examples/testbed/framework/test.cpp

@@ -17,8 +17,10 @@
 */
 
 #include <testbed/framework/test.h>
-#include <testbed/framework/profiler.h>
-#include <testbed/framework/profiler_st.h>
+#include <bounce/common/profiler.h>
+
+extern b3FrameAllocator* g_frameAllocator;
+extern b3Profiler* g_profiler;
 
 extern u32 b3_allocCalls, b3_maxAllocCalls;
 extern u32 b3_convexCalls, b3_convexCacheHits;
@@ -28,19 +30,18 @@ extern bool b3_convexCache;
 void b3BeginProfileScope(const char* name)
 {
 	g_profiler->BeginScope(name);
-	g_profilerSt->BeginScope(name);
 }
 
 void b3EndProfileScope()
 {
 	g_profiler->EndScope();
-	g_profilerSt->EndScope();
 }
 
 Test::Test() : 
 	m_bodyDragger(&m_ray, &m_world)
 {
 	b3Draw_draw = g_draw;
+	b3FrameAllocator_sparseAllocator = g_frameAllocator;
 	b3_convexCache = g_testSettings->convexCache;
 
 	m_world.SetContactListener(this);
@@ -49,13 +50,16 @@ Test::Test() :
 	m_ray.direction.Set(0.0f, 0.0f, -1.0f);
 	m_ray.fraction = g_camera->m_zFar;
 
-	m_groundHull.Set(50.0f, 1.0f, 50.0f);
+	m_groundHull.SetExtents(50.0f, 1.0f, 50.0f);
+	
 	m_groundMesh.BuildTree();
+	m_groundMesh.BuildAdjacency();
 }
 
 Test::~Test()
 {
 	b3Draw_draw = nullptr;
+	b3FrameAllocator_sparseAllocator = nullptr;
 }
 
 void Test::Step()
@@ -63,7 +67,7 @@ void Test::Step()
 	b3_convexCache = g_testSettings->convexCache;
 
 	// Step
-	float32 dt = g_testSettings->inv_hertz;
+	scalar dt = g_testSettings->inv_hertz;
 
 	m_world.SetSleeping(g_testSettings->sleep);
 	m_world.SetWarmStart(g_testSettings->warmStart);
@@ -97,19 +101,19 @@ void Test::Step()
 		g_draw->DrawString(b3Color_white, "Joints %d", m_world.GetJointList().m_count);
 		g_draw->DrawString(b3Color_white, "Contacts %d", m_world.GetContactList().m_count);
 
-		float32 avgGjkIters = 0.0f;
+		scalar avgGjkIters = 0.0f;
 		if (b3_gjkCalls > 0)
 		{
-			avgGjkIters = float32(b3_gjkIters) / float32(b3_gjkCalls);
+			avgGjkIters = scalar(b3_gjkIters) / scalar(b3_gjkCalls);
 		}
 
 		g_draw->DrawString(b3Color_white, "GJK Calls %d", b3_gjkCalls);
 		g_draw->DrawString(b3Color_white, "GJK Iterations %d (%d) (%f)", b3_gjkIters, b3_gjkMaxIters, avgGjkIters);
 
-		float32 convexCacheHitRatio = 0.0f;
+		scalar convexCacheHitRatio = 0.0f;
 		if (b3_convexCalls > 0)
 		{
-			convexCacheHitRatio = float32(b3_convexCacheHits) / float32(b3_convexCalls);
+			convexCacheHitRatio = scalar(b3_convexCacheHits) / scalar(b3_convexCalls);
 		}
 
 		g_draw->DrawString(b3Color_white, "Convex Calls %d", b3_convexCalls);

examples/testbed/framework/test.h

@@ -24,15 +24,16 @@
 
 #include <testbed/framework/body_dragger.h>
 #include <testbed/framework/cloth_dragger.h>
+#include <testbed/framework/softbody_dragger.h>
 
 #include <testbed/framework/draw.h>
 #include <testbed/framework/view_model.h>
 
-inline float32 RandomFloat(float32 a, float32 b)
+inline float RandomFloat(scalar a, scalar b)
 {
-	float32 x = float32(rand()) / float32(RAND_MAX);
-	float32 diff = b - a;
-	float32 r = x * diff;
+	float x = float(rand()) / float(RAND_MAX);
+	float diff = b - a;
+	float r = x * diff;
 	return a + r;
 }
 

examples/testbed/framework/test_entries.cpp

@@ -20,12 +20,13 @@
 #include <testbed/tests/convex_hull.h>
 #include <testbed/tests/cluster.h>
 #include <testbed/tests/distance_test.h>
-#include <testbed/tests/shape_cast.h>
+#include <testbed/tests/linear_time_of_impact.h>
+#include <testbed/tests/time_of_impact.h>
+#include <testbed/tests/aabb_time_of_impact.h>
 #include <testbed/tests/collide_test.h>
 #include <testbed/tests/capsule_collision.h>
 #include <testbed/tests/hull_collision.h>
 #include <testbed/tests/deep_capsule.h>
-#include <testbed/tests/degenerate_capsule.h>
 #include <testbed/tests/box_face_contact.h>
 #include <testbed/tests/box_edge_contact.h>
 #include <testbed/tests/linear_motion.h>
@@ -35,14 +36,18 @@
 #include <testbed/tests/capsule_spin.h>
 #include <testbed/tests/quadric_shapes.h>
 #include <testbed/tests/compound_body.h>
-#include <testbed/tests/spring.h>
+#include <testbed/tests/spring_test.h>
+#include <testbed/tests/motor_test.h>
 #include <testbed/tests/weld_test.h>
 #include <testbed/tests/cone_test.h>
-#include <testbed/tests/hinge_motor.h>
+#include <testbed/tests/revolute_test.h>
+#include <testbed/tests/prismatic_test.h>
+#include <testbed/tests/wheel_test.h>
 #include <testbed/tests/hinge_chain.h>
 #include <testbed/tests/newton_cradle.h>
 #include <testbed/tests/ragdoll.h>
 #include <testbed/tests/mesh_contact_test.h>
+#include <testbed/tests/triangle_contact_test.h>
 #include <testbed/tests/hull_contact_test.h>
 #include <testbed/tests/sphere_stack.h>
 #include <testbed/tests/capsule_stack.h>
@@ -53,46 +58,61 @@
 #include <testbed/tests/pyramid.h>
 #include <testbed/tests/pyramids.h>
 #include <testbed/tests/ray_cast.h>
+#include <testbed/tests/convex_cast.h>
 #include <testbed/tests/sensor_test.h>
 #include <testbed/tests/body_types.h>
 #include <testbed/tests/varying_friction.h>
 #include <testbed/tests/varying_restitution.h>
 #include <testbed/tests/tumbler.h>
 #include <testbed/tests/multiple_pendulum.h>
+#include <testbed/tests/conveyor_belt.h>
 #include <testbed/tests/table_cloth.h>
+#include <testbed/tests/cloth_sdf.h>
 #include <testbed/tests/pinned_cloth.h>
 #include <testbed/tests/particle_types.h>
 #include <testbed/tests/tension_mapping.h>
 #include <testbed/tests/cloth_self_collision.h>
+#include <testbed/tests/cape.h>
+#include <testbed/tests/cloth_tearing.h>
+#include <testbed/tests/cloth_element_test.h>
 #include <testbed/tests/rope_test.h>
 #include <testbed/tests/beam.h>
+#include <testbed/tests/sheet.h>
+#include <testbed/tests/node_types.h>
 #include <testbed/tests/pinned_softbody.h>
+#include <testbed/tests/softbody_anchor.h>
 #include <testbed/tests/smash_softbody.h>
+#include <testbed/tests/tetgen_softbody.h>
 
 TestEntry g_tests[] =
 {
 	{ "Convex Hull", &ConvexHull::Create },
 	{ "Cluster", &Cluster::Create },
 	{ "Distance", &Distance::Create },
-	{ "Shape Cast", &ShapeCast::Create },
+	{ "Linear Time of Impact", &LinearTimeOfImpact::Create },
+	{ "Time of Impact", &TimeOfImpact::Create },
+	{ "AABB Time of Impact", &AABBTimeOfImpact::Create },
 	{ "Capsule Collision", &CapsuleCollision::Create },
 	{ "Hull Collision", &HullCollision::Create },
 	{ "Deep Capsule", &DeepCapsule::Create },
-	{ "Degenerate Capsule", &DegenerateCapsule::Create },
 	{ "Box Face Contact", &BoxFaceContact::Create },
 	{ "Box Edge Contact", &BoxEdgeContact::Create },
 	{ "Capsule Spin", &CapsuleSpin::Create },
 	{ "Hull Contact Test", &HullContactTest::Create },
+	{ "Triangle Contact Test", &TriangleContactTest::Create },
 	{ "Mesh Contact Test", &MeshContactTest::Create },
 	{ "Linear Motion", &LinearMotion::Create },
 	{ "Angular Motion", &AngularMotion::Create },
 	{ "Gyroscopic Motion", &GyroMotion::Create },
 	{ "Compound Body", &CompoundBody::Create },
 	{ "Quadric Shapes", &QuadricShapes::Create },
-	{ "Springs", &Spring::Create },
+	{ "Spring Test", &SpringTest::Create },
+	{ "Prismatic Test", &PrismaticTest::Create },
+	{ "Wheel Test", &WheelTest::Create },
 	{ "Weld Test", &WeldTest::Create },
 	{ "Cone Test", &ConeTest::Create },
-	{ "Hinge Motor", &HingeMotor::Create },
+	{ "Motor Test", &MotorTest::Create },
+	{ "Revolute Test", &RevoluteTest::Create },
 	{ "Hinge Chain", &HingeChain::Create },
 	{ "Ragdoll", &Ragdoll::Create },
 	{ "Newton's Cradle", &NewtonCradle::Create },
@@ -105,6 +125,7 @@ TestEntry g_tests[] =
 	{ "Box Pyramid", &Pyramid::Create },
 	{ "Box Pyramid Rows", &Pyramids::Create },
 	{ "Ray Cast", &RayCast::Create },
+	{ "Convex Cast", &ConvexCast::Create },
 	{ "Sensor Test", &SensorTest::Create },
 	{ "Body Types", &BodyTypes::Create },
 	{ "Varying Friction", &VaryingFriction::Create },
@@ -112,14 +133,23 @@ TestEntry g_tests[] =
 	{ "Tumbler", &Tumbler::Create },
 	{ "Initial Overlap", &InitialOverlap::Create },
 	{ "Multiple Pendulum", &MultiplePendulum::Create },
+	{ "Conveyor Belt", &ConveyorBelt::Create },
 	{ "Table Cloth", &TableCloth::Create },
+	{ "Cloth SDF", &ClothSDF::Create },
 	{ "Pinned Cloth", &PinnedCloth::Create },
 	{ "Particle Types", &ParticleTypes::Create },
 	{ "Tension Mapping", &TensionMapping::Create },
 	{ "Cloth Self-Collision", &ClothSelfCollision::Create },
+	{ "Cloth Tearing", &ClothTearing::Create },
+	{ "Cloth Element Test", &ClothElementTest::Create },
+	{ "Cape", &Cape::Create },
 	{ "Beam", &Beam::Create },
+	{ "Sheet", &Sheet::Create },
+	{ "Node Types", &NodeTypes::Create },
 	{ "Pinned Soft Body", &PinnedSoftBody::Create },
+	{ "Soft Body Anchor", &SoftBodyAnchor::Create },
 	{ "Smash Soft Body", &SmashSoftBody::Create },
+	{ "TetGen Soft Body", &TetGenSoftBody::Create },
 	{ "Rope", &Rope::Create },
 	{ NULL, NULL }
 };

examples/testbed/framework/view.cpp

@@ -19,14 +19,18 @@
 #include <testbed/framework/view.h>
 #include <testbed/framework/view_model.h>
 #include <testbed/framework/test.h>
-#include <testbed/framework/profiler.h>
-#include <testbed/framework/profiler_st.h>
+#include <bounce/common/profiler.h>
+
+extern b3Profiler* g_profiler;
 
 #include <imgui/imgui.h>
+
+#include <imgui/imgui_impl_glfw.h>
+
 #if defined (U_OPENGL_2)
-#include <imgui/imgui_impl_glfw_gl2.h>
+#include <imgui/imgui_impl_opengl2.h>
 #elif defined (U_OPENGL_4)
-#include <imgui/imgui_impl_glfw_gl3.h>
+#include <imgui/imgui_impl_opengl3.h>
 #else
 #endif
 
@@ -39,16 +43,15 @@ static inline bool GetTestName(void* userData, int idx, const char** name)
 	return true;
 }
 
-static inline bool ImGui_GLFW_GL_Init(GLFWwindow* w, bool install_callbacks)
+static inline bool ImGui_OpenGL_Init()
 {
-
 #if defined(U_OPENGL_2)
 
-	return ImGui_ImplGlfwGL2_Init(w, install_callbacks);
+	return ImGui_ImplOpenGL2_Init();
 
 #elif defined(U_OPENGL_4)
 
-	return ImGui_ImplGlfwGL3_Init(w, install_callbacks);
+	return ImGui_ImplOpenGL3_Init();
 
 #else
 
@@ -56,16 +59,16 @@ static inline bool ImGui_GLFW_GL_Init(GLFWwindow* w, bool install_callbacks)
 	return false;
 }
 
-static inline void ImGui_GLFW_GL_Shutdown()
+static inline void ImGui_OpenGL_Shutdown()
 {
 
 #if defined(U_OPENGL_2)
 
-	ImGui_ImplGlfwGL2_Shutdown();
+	ImGui_ImplOpenGL2_Shutdown();
 
 #elif defined(U_OPENGL_4)
 
-	ImGui_ImplGlfwGL3_Shutdown();
+	ImGui_ImplOpenGL3_Shutdown();
 
 #else
 
@@ -75,16 +78,16 @@ static inline void ImGui_GLFW_GL_Shutdown()
 
 }
 
-static inline void ImGui_GLFW_GL_NewFrame()
+static inline void ImGui_OpenGL_NewFrame()
 {
 
 #if defined(U_OPENGL_2)
 
-	ImGui_ImplGlfwGL2_NewFrame();
+	ImGui_ImplOpenGL2_NewFrame();
 
 #elif defined(U_OPENGL_4)
 
-	ImGui_ImplGlfwGL3_NewFrame();
+	ImGui_ImplOpenGL3_NewFrame();
 
 #else
 
@@ -94,16 +97,16 @@ static inline void ImGui_GLFW_GL_NewFrame()
 
 }
 
-static inline void ImGui_GLFW_GL_RenderDrawData(ImDrawData* draw_data)
+static inline void ImGui_OpenGL_RenderDrawData(ImDrawData* draw_data)
 {
 
 #if defined(U_OPENGL_2)
 
-	ImGui_ImplGlfwGL2_RenderDrawData(draw_data);
+	ImGui_ImplOpenGL2_RenderDrawData(draw_data);
 
 #elif defined(U_OPENGL_4)
 
-	ImGui_ImplGlfwGL3_RenderDrawData(draw_data);
+	ImGui_ImplOpenGL3_RenderDrawData(draw_data);
 
 #else
 
@@ -124,9 +127,9 @@ View::View(GLFWwindow* window)
 	ImGuiIO& io = ImGui::GetIO();
 
 	io.IniFilename = NULL;
-	io.Fonts[0].AddFontDefault();
 
-	ImGui_GLFW_GL_Init(m_window, false);
+	ImGui_ImplGlfw_InitForOpenGL(m_window, false);
+	ImGui_OpenGL_Init();
 
 	ImGui::StyleColorsDark();
 
@@ -136,7 +139,8 @@ View::View(GLFWwindow* window)
 View::~View()
 {
 	// Destroy UI
-	ImGui_GLFW_GL_Shutdown();
+	ImGui_OpenGL_Shutdown();
+	ImGui_ImplGlfw_Shutdown();
 
 	ImGui::DestroyContext();
 }
@@ -145,7 +149,7 @@ b3Vec2 View::GetCursorPosition() const
 {
 	double x, y;
 	glfwGetCursorPos(m_window, &x, &y);
-	return b3Vec2(float32(x), float32(y));
+	return b3Vec2(scalar(x), scalar(y));
 }
 
 void View::Event_SetWindowSize(int w, int h)
@@ -186,7 +190,9 @@ void View::Event_Scroll(float dx, float dy)
 
 void View::BeginInterface()
 {
-	ImGui_GLFW_GL_NewFrame();
+	ImGui_OpenGL_NewFrame();
+	ImGui_ImplGlfw_NewFrame();
+	ImGui::NewFrame();
 
 	ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f);
 }
@@ -219,9 +225,8 @@ void View::Interface()
 
 		if (ImGui::BeginMenu("View"))
 		{
-			ImGui::MenuItem("Profile Tree", "", &settings.drawProfileTree);
-			ImGui::MenuItem("Profile Tree Statistics", "", &settings.drawProfileTreeStats);
 			ImGui::MenuItem("Statistics", "", &settings.drawStats);
+			ImGui::MenuItem("Profiler", "", &settings.drawProfiler);
 
 			ImGui::Separator();
 
@@ -403,54 +408,7 @@ void View::Interface()
 	ImGui::End();
 }
 
-static void TreeNode(ProfilerNode* node, u32& index)
-{
-	ImGui::PushID(index);
-	++index;
-
-	if (ImGui::TreeNode(node->name))
-	{
-		float64 elapsedTime = node->t1 - node->t0;
-		ImGui::Text("%.4f [ms]", elapsedTime);
-
-		for (u32 i = 0; i < node->children.Count(); ++i)
-		{
-			TreeNode(node->children[i], index);
-		}
-		ImGui::TreePop();
-	}	
-	
-	ImGui::PopID();
-}
-
-void View::InterfaceProfileTree()
-{
-	ImGui::Begin("Overlay", NULL, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
-	ImVec2 ws = ImGui::GetWindowSize();
-	ImVec2 wp = ImGui::GetWindowPos();
-	ImGui::End();
-
-	ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f);
-	
-	ImGui::SetNextWindowBgAlpha(0.0f);
-	ImGui::SetNextWindowPos(ImVec2(0.0f, wp.y + ws.y));
-	ImGui::SetNextWindowSize(ImVec2(g_camera->m_width - 250.0f, 0.0f));
-
-	ImGui::Begin("Profile Tree", NULL, ImGuiWindowFlags_AlwaysAutoResize);
-
-	ProfilerNode* root = g_profiler->GetRoot();
-	if (root)
-	{
-		u32 index = 0;
-		TreeNode(root, index);
-	}
-	
-	ImGui::End();
-	
-	ImGui::PopStyleVar();
-}
-
-static void TreeNode(ProfilerStNode* node, u32& index)
+static void TreeNode(b3ProfilerNode* node, u32& index)
 {
 	ImGui::PushID(index);
 	++index;
@@ -459,9 +417,11 @@ static void TreeNode(ProfilerStNode* node, u32& index)
 	{
 		ImGui::Text("%.4f (min = %.4f) (max = %.4f) (calls = %d) [ms]", node->elapsed, node->stat->minElapsed, node->stat->maxElapsed, node->callCount);
 
-		for (u32 i = 0; i < node->children.Count(); ++i)
+		b3ProfilerNode* n = node->head;
+		while(n)
 		{
-			TreeNode(node->children[i], index);
+			TreeNode(n, index);
+			n = n->next;
 		}
 		ImGui::TreePop();
 	}
@@ -469,7 +429,7 @@ static void TreeNode(ProfilerStNode* node, u32& index)
 	ImGui::PopID();
 }
 
-void View::InterfaceProfileTreeStats()
+void View::InterfaceProfiler()
 {
 	ImGui::Begin("Overlay", NULL, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
 	ImVec2 wp = ImGui::GetWindowPos();
@@ -478,25 +438,15 @@ void View::InterfaceProfileTreeStats()
 
 	wp.y = wp.y + ws.y;
 
-	if (g_settings->drawProfileTree)
-	{
-		ImGui::Begin("Profile Tree", NULL, ImGuiWindowFlags_AlwaysAutoResize);
-		ImVec2 ptwp = ImGui::GetWindowPos();
-		ImVec2 ptws = ImGui::GetWindowSize();
-		ImGui::End();
-
-		wp.y = ptwp.y + ptws.y;
-	}
-
 	ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f);
 
 	ImGui::SetNextWindowBgAlpha(0.0f);
 	ImGui::SetNextWindowPos(ImVec2(0.0f, wp.y));
 	ImGui::SetNextWindowSize(ImVec2(g_camera->m_width - 250.0f, 0.0f));
 
-	ImGui::Begin("Profile Tree Statistics", NULL, ImGuiWindowFlags_AlwaysAutoResize);
+	ImGui::Begin("Profiler", NULL, ImGuiWindowFlags_AlwaysAutoResize);
 
-	ProfilerStNode* root = g_profilerSt->GetRoot();
+	b3ProfilerNode* root = g_profiler->GetRoot();
 	if (root)
 	{
 		u32 index = 0;
@@ -514,5 +464,5 @@ void View::EndInterface()
 
 	ImGui::Render();
 
-	ImGui_GLFW_GL_RenderDrawData(ImGui::GetDrawData());
+	ImGui_OpenGL_RenderDrawData(ImGui::GetDrawData());
 }

examples/testbed/framework/view.h

@@ -41,8 +41,7 @@ public:
 
 	void BeginInterface();
 	void Interface();
-	void InterfaceProfileTree();
-	void InterfaceProfileTreeStats();
+	void InterfaceProfiler();
 	void EndInterface();
 private:
 	friend class ViewModel;

examples/testbed/framework/view_model.cpp

@@ -84,7 +84,7 @@ void ViewModel::Action_ResetCamera()
 
 void ViewModel::Event_SetWindowSize(int w, int h)
 {
-	m_model->Command_ResizeCamera(float32(w), float32(h));
+	m_model->Command_ResizeCamera(scalar(w), scalar(h));
 }
 
 void ViewModel::Event_Press_Key(int button)
@@ -148,8 +148,7 @@ void ViewModel::Event_Move_Cursor(float x, float y)
 
 	b3Vec2 dp = ps - m_view->m_ps0;
 
-	float32 ndx = b3Clamp(dp.x, -1.0f, 1.0f);
-	float32 ndy = b3Clamp(dp.y, -1.0f, 1.0f);
+	b3Vec2 n = b3Normalize(dp);
 
 	bool shiftDown = glfwGetKey(m_view->m_window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS;
 	bool leftDown = glfwGetMouseButton(m_view->m_window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS;
@@ -159,8 +158,8 @@ void ViewModel::Event_Move_Cursor(float x, float y)
 	{
 		if (leftDown)
 		{
-			float32 ax = -0.005f * B3_PI * ndx;
-			float32 ay = -0.005f * B3_PI * ndy;
+			scalar ax = -0.005f * B3_PI * n.x;
+			scalar ay = -0.005f * B3_PI * n.y;
 
 			m_model->Command_RotateCameraY(ax);
 			m_model->Command_RotateCameraX(ay);
@@ -168,8 +167,8 @@ void ViewModel::Event_Move_Cursor(float x, float y)
 
 		if (rightDown)
 		{
-			float32 tx = 0.2f * ndx;
-			float32 ty = -0.2f * ndy;
+			scalar tx = 0.2f * n.x;
+			scalar ty = -0.2f * n.y;
 
 			m_model->Command_TranslateCameraX(tx);
 			m_model->Command_TranslateCameraY(ty);
@@ -177,16 +176,18 @@ void ViewModel::Event_Move_Cursor(float x, float y)
 	}
 	else
 	{
-		m_model->Command_Move_Cursor(m_view->GetCursorPosition());
+		m_model->Command_Move_Cursor(ps);
 	}
 }
 
 void ViewModel::Event_Scroll(float dx, float dy)
 {
+	b3Vec2 n(dx, dy);
+	n.Normalize();
+
 	bool shiftDown = glfwGetKey(m_view->m_window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS;
 	if (shiftDown)
 	{
-		float32 ny = b3Clamp(dy, -1.0f, 1.0f);
-		m_model->Command_ZoomCamera(1.0f * ny);
+		m_model->Command_ZoomCamera(1.0f * n.y);
 	}
 }

examples/testbed/framework/view_model.h

@@ -32,9 +32,8 @@ struct Settings
 		drawLines = true;
 		drawTriangles = true;
 		drawGrid = true;
-		drawProfileTree = false;
-		drawProfileTreeStats = false;
 		drawStats = false;
+		drawProfiler = false;
 	}
 
 	int testID;
@@ -43,9 +42,8 @@ struct Settings
 	bool drawLines;
 	bool drawTriangles;
 	bool drawGrid;
-	bool drawProfileTree;
-	bool drawProfileTreeStats;
 	bool drawStats;
+	bool drawProfiler;
 };
 
 //

examples/testbed/tests/aabb_time_of_impact.h

@@ -0,0 +1,178 @@
+/*
+* 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 AABB_TIME_OF_IMPACT_H
+#define AABB_TIME_OF_IMPACT_H
+
+class AABBTimeOfImpact : public Test
+{
+public:
+	AABBTimeOfImpact()
+	{
+		b3Vec3 cA = b3Vec3_zero;
+		b3Vec3 eA(1.0f, 1.0f, 1.0f);
+		m_aabbA.Set(cA, eA);
+
+		b3Vec3 cB(-2.0f, -2.0f, 0.0f);
+		b3Vec3 eB(1.0f, 1.0f, 1.0f);
+		m_aabbB.Set(cB, eB);
+
+		m_dB.Set(1.0f, 1.0f, 0.0f);
+
+		m_time = 0.0f;
+	}
+
+	void Step()
+	{
+		g_draw->DrawString(b3Color_white, "Arrows - Translate AABB");
+
+		g_draw->DrawAABB(m_aabbA, b3Color_white);
+		g_draw->DrawAABB(m_aabbB, b3Color_white);
+
+		b3Vec3 cA = m_aabbA.GetCenter();
+		b3Vec3 cB = m_aabbB.GetCenter();
+
+		b3Vec3 eA = m_aabbA.GetExtents();
+		b3Vec3 eB = m_aabbB.GetExtents();
+
+		b3Vec3 dA = b3Vec3_zero;
+		b3Vec3 dB = m_dB;
+
+		g_draw->DrawSegment(cA, cA + dA, b3Color_white);
+		g_draw->DrawSegment(cB, cB + dB, b3Color_white);
+
+		{
+			b3Vec3 cBt = cB + m_time * dB;
+
+			b3AABB B;
+			B.Set(cBt, eB);
+
+			g_draw->DrawAABB(B, b3Color_red);
+		}
+
+		b3TOIOutput out = b3TimeOfImpact(m_aabbA, dA, m_aabbB, dB);
+
+		b3TOIOutput::State state = out.state;
+		scalar t = out.t;
+
+		if (state == b3TOIOutput::e_touching)
+		{
+			b3Vec3 cAt = cA + t * dA;
+			b3Vec3 cBt = cB + t * dB;
+
+			b3AABB A;
+			A.Set(cAt, eA);
+
+			b3AABB B;
+			B.Set(cBt, eB);
+
+			g_draw->DrawAABB(A, b3Color_black);
+			g_draw->DrawAABB(B, b3Color_black);
+		}
+
+		if (state == b3TOIOutput::e_failed)
+		{
+			g_draw->DrawString(b3Color_white, "State = Failed");
+		}
+		else if (state == b3TOIOutput::e_overlapped)
+		{
+			g_draw->DrawString(b3Color_white, "State = Overlapped");
+		}
+		else if (state == b3TOIOutput::e_separated)
+		{
+			g_draw->DrawString(b3Color_white, "State = Separated!");
+		}
+		else if (state == b3TOIOutput::e_touching)
+		{
+			g_draw->DrawString(b3Color_white, "State = Touching!");
+		}
+	}
+
+	void KeyDown(int key)
+	{
+		const scalar dt = 0.01f;
+		const scalar d = 0.1f;
+
+		if (key == GLFW_KEY_F)
+		{
+			m_time += dt;
+			if (m_time > 1.0f)
+			{
+				m_time = 0.0f;
+			}
+		}
+
+		if (key == GLFW_KEY_B)
+		{
+			m_time -= dt;
+			if (m_time < 0.0f)
+			{
+				m_time = 1.0f;
+			}
+		}
+
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_aabbB.lowerBound.x -= d;
+			m_aabbB.upperBound.x -= d;
+		}
+
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_aabbB.lowerBound.x += d;
+			m_aabbB.upperBound.x += d;
+		}
+
+		if (key == GLFW_KEY_UP)
+		{
+			m_aabbB.lowerBound.y += d;
+			m_aabbB.upperBound.y += d;
+		}
+
+		if (key == GLFW_KEY_DOWN)
+		{
+			m_aabbB.lowerBound.y -= d;
+			m_aabbB.upperBound.y -= d;
+		}
+		
+		if (key == GLFW_KEY_W)
+		{
+			m_aabbB.lowerBound.z += d;
+			m_aabbB.upperBound.z += d;
+		}
+
+		if (key == GLFW_KEY_S)
+		{
+			m_aabbB.lowerBound.z -= d;
+			m_aabbB.upperBound.z -= d;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new AABBTimeOfImpact();
+	}
+
+	scalar m_time;
+
+	b3AABB m_aabbA;
+	b3AABB m_aabbB;
+	b3Vec3 m_dB;
+};
+
+#endif

examples/testbed/tests/angular_motion.h

@@ -32,8 +32,8 @@ public:
 		b3Body* body = m_world.CreateBody(bd);
 
 		b3CapsuleShape shape;
-		shape.m_centers[0].Set(0.0f, 0.0f, -1.0f);
-		shape.m_centers[1].Set(0.0f, 0.0f, 1.0f);
+		shape.m_vertex1.Set(0.0f, 0.0f, -1.0f);
+		shape.m_vertex2.Set(0.0f, 0.0f, 1.0f);
 		shape.m_radius = 1.0f;
 
 		b3ShapeDef sdef;

examples/testbed/tests/beam.h

@@ -19,33 +19,34 @@
 #ifndef BEAM_H
 #define BEAM_H
 
-#include <testbed/framework/softbody_dragger.h>
-
 class Beam : public Test
 {
 public:
+	enum
+	{
+		e_w = 5,
+		e_h = 2,
+		e_d = 2
+	};
+
 	Beam()
 	{
+		m_E0 = 1000.0f;
+		m_E = m_E0;
+
 		// Create soft body
 		b3SoftBodyDef def;
 		def.mesh = &m_mesh;
 		def.density = 0.2f;
-		def.E = 1000.0f;
+		def.E = m_E0;
 		def.nu = 0.33f;
+		def.radius = 0.2f;
+		def.friction = 0.6f;
 
 		m_body = new b3SoftBody(def);
 
 		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
 		m_body->SetGravity(gravity);
-		m_body->SetWorld(&m_world);
-
-		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
-		{
-			b3SoftBodyNode* n = m_body->GetVertexNode(i);
-
-			n->SetRadius(0.05f);
-			n->SetFriction(0.2f);
-		}
 
 		// Create body
 		{
@@ -55,7 +56,7 @@ public:
 
 			b3Body* b = m_world.CreateBody(bd);
 
-			m_wallHull.Set(1.0f, 5.0f, 5.0f);
+			m_wallHull.SetExtents(1.0f, 5.0f, 5.0f);
 
 			b3HullShape wallShape;
 			wallShape.m_hull = &m_wallHull;
@@ -64,18 +65,20 @@ public:
 			sd.shape = &wallShape;
 
 			b3Shape* wall = b->CreateShape(sd);
+
+			b3SoftBodyWorldShapeDef ssd;
+			ssd.shape = wall;
+
+			m_body->CreateWorldShape(ssd);
 		}
 
-		b3AABB3 aabb;
-		aabb.m_lower.Set(-3.0f, -5.0f, -5.0f);
-		aabb.m_upper.Set(-2.0f, 5.0f, 5.0f);
-
-		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		for (u32 i = 0; i < e_h + 1; ++i)
 		{
-			b3SoftBodyNode* n = m_body->GetVertexNode(i);
-			b3Vec3 p = n->GetPosition();
-			if (aabb.Contains(p))
+			for (u32 k = 0; k < e_d + 1; ++k)
 			{
+				u32 v = m_mesh.GetVertex(i, 0, k);
+
+				b3SoftBodyNode* n = m_body->GetNode(v);
 				n->SetType(e_staticSoftBodyNode);
 			}
 		}
@@ -107,9 +110,9 @@ public:
 			b3Vec3 pA = m_bodyDragger->GetPointA();
 			b3Vec3 pB = m_bodyDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
@@ -117,8 +120,10 @@ public:
 		extern u32 b3_softBodySolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
 
-		float32 E = m_body->GetEnergy();
+		scalar E = m_body->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
+		
+		g_draw->DrawString(b3Color_white, "Up/Down - Young Modulus (%f)", m_E);
 	}
 
 	void MouseMove(const b3Ray3& pw)
@@ -146,13 +151,37 @@ public:
 		}
 	}
 
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_UP)
+		{
+			m_E = b3Clamp(m_E + scalar(10), scalar(0), m_E0);
+			for (u32 i = 0; i < m_mesh.tetrahedronCount; ++i)
+			{
+				m_body->GetElement(i)->SetE(m_E);
+			}
+		}
+		
+		if (button == GLFW_KEY_DOWN)
+		{
+			m_E = b3Clamp(m_E - scalar(10), scalar(10), m_E0);
+			for (u32 i = 0; i < m_mesh.tetrahedronCount; ++i)
+			{
+				m_body->GetElement(i)->SetE(m_E);
+			}
+		}
+	}
+
 	static Test* Create()
 	{
 		return new Beam();
 	}
 
-	b3BlockSoftBodyMesh<5, 2, 2> m_mesh;
+	b3BlockSoftBodyMesh<e_w, e_h, e_d> m_mesh;
 	
+	scalar m_E0;
+	scalar m_E;
+
 	b3SoftBody* m_body;
 	b3SoftBodyDragger* m_bodyDragger;
 	

examples/testbed/tests/body_types.h

@@ -47,8 +47,8 @@ public:
 			m_body = m_world.CreateBody(bd);
 
 			b3CapsuleShape cap;
-			cap.m_centers[0].Set(0.0f, 2.0f, 0.0f);
-			cap.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cap.m_vertex1.Set(0.0f, 2.0f, 0.0f);
+			cap.m_vertex2.Set(0.0f, -2.0f, 0.0f);
 			cap.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -94,7 +94,7 @@ public:
 			bd.type = b3BodyType::e_dynamicBody;
 			bd.position.Set(RandomFloat(-20.0f, 20.0f), RandomFloat(10.0f, 20.0f), RandomFloat(-20.0f, 20.0f));
 
-			b3Vec3 n = m_body->GetTransform().position - bd.position;
+			b3Vec3 n = m_body->GetTransform().translation - bd.position;
 			n.Normalize();
 
 			bd.linearVelocity = 100.0f * n;
@@ -122,7 +122,7 @@ public:
 				
 				p.x -= 1.0f;
 
-				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+				m_body->SetTransform(p, q);
 			}
 
 			if (button == GLFW_KEY_RIGHT)
@@ -132,7 +132,7 @@ public:
 
 				p.x += 1.0f;
 
-				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+				m_body->SetTransform(p, q);
 			}
 
 			if (button == GLFW_KEY_UP)
@@ -142,7 +142,7 @@ public:
 
 				p.z += 1.0f;
 
-				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+				m_body->SetTransform(p, q);
 			}
 
 			if (button == GLFW_KEY_DOWN)
@@ -152,7 +152,7 @@ public:
 
 				p.z -= 1.0f;
 
-				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+				m_body->SetTransform(p, q);
 			}
 		}
 

examples/testbed/tests/box_edge_contact.h

@@ -24,27 +24,19 @@ class BoxEdgeContact : public Collide
 public:
 	BoxEdgeContact()
 	{
-		b3Transform xf;
-		xf.position.SetZero();
-		xf.rotation = b3Diagonal(1.0f, 2.0f, 1.0f);
-		m_box.SetTransform(xf);
+		m_box.SetExtents(1.0f, 2.0f, 1.0f);
 
 		m_sA.m_hull = &m_box;
-		m_sB.m_hull = &m_box;
-		
-		m_xfA.position.Set(1.500000, 1.000000, 0.000000);
-		m_xfA.rotation.x.Set(0.707107, 0.000000, -0.707107);
-		m_xfA.rotation.y.Set(0.000000, 1.000000, 0.000000);
-		m_xfA.rotation.z.Set(0.707107, 0.000000, 0.707107);
+		m_xfA.translation.Set(1.500000, 1.000000, 0.000000);
+		m_xfA.rotation.SetIdentity();
+		m_shapeA = &m_sA;
 
-		m_xfB.position.Set(-1.300000, 0.000000, 0.000000);
-		m_xfB.rotation.x.Set(0.809017, 0.266849, -0.523721);
-		m_xfB.rotation.y.Set(0.000000, 0.891007, 0.453991);
-		m_xfB.rotation.z.Set(0.587785, -0.367286, 0.720840);
+		m_sB.m_hull = &m_box;
+		m_xfB.translation.Set(-1.29999995, 1.34999979, 0.000000000);
+		m_xfB.rotation.Set(0.810514629, 0.342624813, 0.334119707, 0.337692767);
+		m_shapeB = &m_sB;
 
 		m_cache.count = 0;
-		m_shapeA = &m_sA;
-		m_shapeB = &m_sB;
 	}
 	
 	static Test* Create()

examples/testbed/tests/box_face_contact.h

@@ -24,28 +24,24 @@ class BoxFaceContact : public Collide
 public:
 	BoxFaceContact()
 	{
-		b3Transform m;
-		m.rotation = b3Diagonal(1.0f, 2.0f, 1.0f);
-		m.position.Set(0.0f, 2.0f, 0.0f);
-		m_box1.SetTransform(m);
+		m_boxA.SetExtents(1.0f, 2.0f, 1.0f);
+		b3Vec3 translation(0.0f, 2.0f, 0.0f);
+		m_boxA.Translate(translation);
 
-		m.rotation = b3Diagonal(1.0f, 1.0f, 1.0f);
-		m.position.Set(0.0f, 0.0f, 0.0f);
-		m_box2.SetTransform(m);
-
-		m_xfA.SetIdentity();
-		m_xfA.position.SetZero();
-		m_xfA.rotation.SetIdentity();
-		m_sA.m_hull = &m_box1;
+		m_sA.m_hull = &m_boxA;
 		
-		m_xfB.SetIdentity();
-		m_xfB.position.Set(0.0f, 0.0f, 0.0f);
-		m_xfB.rotation.SetIdentity();
-		m_sB.m_hull = &m_box2;
+		m_xfA.SetIdentity();
 
-		m_cache.count = 0;
 		m_shapeA = &m_sA;
+		
+		m_boxB.SetExtents(1.0f, 1.0f, 1.0f);
+
+		m_xfB.SetIdentity();
+		m_sB.m_hull = &m_boxB;
+
 		m_shapeB = &m_sB;
+		
+		m_cache.count = 0;
 	}
 
 	static Test* Create()
@@ -53,8 +49,8 @@ public:
 		return new BoxFaceContact();
 	}
 
-	b3BoxHull m_box1;
-	b3BoxHull m_box2;
+	b3BoxHull m_boxA;
+	b3BoxHull m_boxB;
 	b3HullShape m_sA;
 	b3HullShape m_sB;
 };

examples/testbed/tests/box_stack.h

@@ -24,17 +24,15 @@ class BoxStack : public Test
 public:
 	enum
 	{
-		e_rowCount = 1,
-		e_columnCount = 5,
-		e_depthCount = 1
+		e_h = 5,
+		e_w = 1,
+		e_d = 1
 	};
 
 	BoxStack()
 	{
 		{
 			b3BodyDef bdef;
-			bdef.type = b3BodyType::e_staticBody;
-
 			b3Body* body = m_world.CreateBody(bdef);
 
 			b3HullShape hs;
@@ -47,30 +45,53 @@ public:
 			body->CreateShape(sdef);
 		}
 
-		b3Vec3 boxScale(1.0f, 1.0f, 1.0f);
+		b3Vec3 e(1.0f, 1.0f, 1.0f);
 
-		b3Vec3 stackOrigin(0.0f, 4.05f, 0.0f);
+		m_boxHull.SetExtents(e.x, e.y, e.z);
 
-		for (u32 i = 0; i < e_rowCount; ++i)
+		b3Vec3 separation;
+		separation.x = 1.0f;
+		separation.y = 1.0f;
+		separation.z = 1.0f;
+
+		b3Vec3 scale;
+		scale.x = 2.0f * e.x + separation.x;
+		scale.y = 2.0f * e.y + separation.y;
+		scale.z = 2.0f * e.z + separation.z;
+
+		b3Vec3 size;
+		size.x = 2.0f * e.x + scale.x * scalar(e_w - 1);
+		size.y = 2.0f * e.y + scale.y * scalar(e_h - 1);
+		size.z = 2.0f * e.z + scale.z * scalar(e_d - 1);
+
+		b3Vec3 translation;
+		translation.x = e.x - 0.5f * size.x;
+		translation.y = e.y - 0.5f * size.y;
+		translation.z = e.z - 0.5f * size.z;
+
+		translation.y += 9.0f;
+
+		for (u32 i = 0; i < e_h; ++i)
 		{
-			for (u32 j = 0; j < e_columnCount; ++j)
+			for (u32 j = 0; j < e_w; ++j)
 			{
-				for (u32 k = 0; k < e_depthCount; ++k)
+				for (u32 k = 0; k < e_d; ++k)
 				{
 					b3BodyDef bdef;
-					bdef.type = b3BodyType::e_dynamicBody;
-					bdef.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.5f * B3_PI);
+					bdef.type = e_dynamicBody;
 
-					bdef.position.x = float32(i) * boxScale.x;
-					bdef.position.y = 2.5f * float32(j) * boxScale.y;
-					bdef.position.z = float32(k) * boxScale.z;
+					bdef.position.Set(scalar(j), scalar(i), scalar(k));
 
-					bdef.position += stackOrigin;
+					bdef.position.x *= scale.x;
+					bdef.position.y *= scale.y;
+					bdef.position.z *= scale.z;
+
+					bdef.position += translation;
 
 					b3Body* body = m_world.CreateBody(bdef);
 
 					b3HullShape hs;
-					hs.m_hull = &b3BoxHull_identity;
+					hs.m_hull = &m_boxHull;
 
 					b3ShapeDef sdef;
 					sdef.density = 0.1f;
@@ -78,15 +99,30 @@ public:
 					sdef.shape = &hs;
 
 					body->CreateShape(sdef);
+
+					u32 bodyIndex = GetBodyIndex(i, j, k);
+
+					m_bodies[bodyIndex] = body;
 				}
 			}
 		}
 	}
 
+	u32 GetBodyIndex(u32 i, u32 j, u32 k)
+	{
+		B3_ASSERT(i < e_h);
+		B3_ASSERT(j < e_w);
+		B3_ASSERT(k < e_d);
+		return k + e_d * (j + e_w * i);
+	}
+
 	static Test* Create()
 	{
 		return new BoxStack();
 	}
+
+	b3BoxHull m_boxHull;
+	b3Body* m_bodies[e_h * e_w * e_d];
 };
 
 #endif

examples/testbed/tests/cape.h

@@ -0,0 +1,215 @@
+/*
+* 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 CAPE_H
+#define CAPE_H
+
+class Cape : public Test
+{
+public:
+	enum
+	{
+		e_w = 5,
+		e_h = 10
+	};
+
+	Cape()
+	{
+		// Translate the cloth mesh
+		for (u32 i = 0; i < m_clothMesh.vertexCount; ++i)
+		{
+			m_clothMesh.vertices[i].y += 5.0f;
+			m_clothMesh.vertices[i].z -= 6.0f;
+		}
+
+		// Create cloth
+		b3ClothDef def;
+		def.mesh = &m_clothMesh;
+		def.density = 0.2f;
+		def.streching = 100000.0f;
+
+		m_cloth = new b3Cloth(def);
+
+		m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
+
+		// Freeze some particles
+		for (u32 j = 0; j < e_w + 1; ++j)
+		{
+			u32 vj = m_clothMesh.GetVertex(e_h, j);
+
+			b3ClothParticle* p = m_cloth->GetParticle(vj);
+			p->SetType(e_kinematicClothParticle);
+		}
+
+		m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
+
+		{
+			// Create body
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_kinematicBody;
+
+			m_body = m_world.CreateBody(bdef);
+
+			static b3BoxHull box(1.0f, 5.0f, 1.0f);
+			
+			b3HullShape hs;
+			hs.m_hull = &box;
+			hs.m_radius = 0.25f;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.1f;
+			sdef.friction = 0.3f;
+			sdef.shape = &hs;
+
+			m_body->CreateShape(sdef);
+		}
+
+		// Store cloth vertices in body space
+		for (u32 j = 0; j < e_w + 1; ++j)
+		{
+			u32 vj = m_clothMesh.GetVertex(e_h, j);
+
+			b3ClothParticle* p = m_cloth->GetParticle(vj);
+			b3Vec3 position = p->GetPosition();
+			
+			m_localPoints[j] = m_body->GetLocalPoint(position);
+		}
+	}
+
+	~Cape()
+	{
+		delete m_clothDragger;
+		delete m_cloth;
+	}
+	
+	void KeyDown(int button)
+	{
+		b3Vec3 v = m_body->GetLinearVelocity();
+		
+		if (button == GLFW_KEY_LEFT)
+		{
+			v.x -= 5.0f;
+		}
+
+		if (button == GLFW_KEY_RIGHT)
+		{
+			v.x += 5.0f;
+		}
+
+		if (button == GLFW_KEY_UP)
+		{
+			v.z -= 5.0f;
+		}
+
+		if (button == GLFW_KEY_DOWN)
+		{
+			v.z += 5.0f;
+		}
+		
+		m_body->SetLinearVelocity(v);
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		scalar inv_h = g_testSettings->hertz;
+
+		for (u32 j = 0; j < e_w + 1; ++j)
+		{
+			u32 vj = m_clothMesh.GetVertex(e_h, j);
+
+			b3ClothParticle* p = m_cloth->GetParticle(vj);
+			b3Vec3 x0 = p->GetPosition();
+
+			b3Vec3 x = m_body->GetWorldPoint(m_localPoints[j]);
+
+			// Apply finite difference method
+			b3Vec3 v = inv_h * (x - x0);
+
+			p->SetVelocity(v);
+		}
+
+		m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_cloth->Draw();
+
+		if (m_clothDragger->IsDragging())
+		{
+			b3Vec3 pA = m_clothDragger->GetPointA();
+			b3Vec3 pB = m_clothDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_clothSolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
+
+		scalar E = m_cloth->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+		
+		g_draw->DrawString(b3Color_white, "Arrows - Apply Velocity");
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->Drag();
+		}
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_clothDragger->IsDragging() == false)
+		{
+			m_clothDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Cape();
+	}
+
+	b3GridClothMesh<e_w, e_h> m_clothMesh;
+	b3Cloth* m_cloth;
+	b3ClothDragger* m_clothDragger;
+	b3Body* m_body;
+	b3Vec3 m_localPoints[e_w + 1];
+};
+
+#endif

examples/testbed/tests/capsule_collision.h

@@ -24,25 +24,21 @@ class CapsuleCollision : public Collide
 public:
 	CapsuleCollision()
 	{
-		m_xfA.SetIdentity();
-		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
-		//m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.25f * B3_PI));
-		m_xfA.rotation.SetIdentity();
-		m_sA.m_centers[0].Set(0.0f, -5.0f, 0.0f);
-		m_sA.m_centers[1].Set(0.0f, 5.0f, 0.0f);
+		m_sA.m_vertex1.Set(0.0f, -5.0f, 0.0f);
+		m_sA.m_vertex2.Set(0.0f, 5.0f, 0.0f);
 		m_sA.m_radius = 1.0f;
 
-		m_xfB.SetIdentity();
-		m_xfB.position.Set(0.f, 0.0f, 0.0f);
-		//m_xfB.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.251f * B3_PI));
-		m_xfB.rotation.SetIdentity();
-		m_sB.m_centers[0].Set(0.0f, -1.0f, 0.0f);
-		m_sB.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+		m_xfA.SetIdentity();
+		
+		m_sB.m_vertex1.Set(0.0f, -1.0f, 0.0f);
+		m_sB.m_vertex2.Set(0.0f, 1.0f, 0.0f);
 		m_sB.m_radius = 1.0f;
 
-		m_cache.count = 0;
+		m_xfB.SetIdentity();
+
 		m_shapeA = &m_sA;
 		m_shapeB = &m_sB;
+		m_cache.count = 0;
 	}
 
 	static Test* Create()

examples/testbed/tests/capsule_spin.h

@@ -40,16 +40,15 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
+			
 			bdef.position.Set(0.0f, 10.0f, 0.0f);
-			bdef.orientation.Set(b3Vec3(0.0f, 0.0f, -1.0f), 1.5f * B3_PI);
-			bdef.linearVelocity.Set(0.005f, -10.0f, 0.005f);
-			bdef.angularVelocity.Set(2000.0f * B3_PI, 2000.0f * B3_PI, 10000.0f * B3_PI);
+			bdef.angularVelocity.Set(0.5f * B3_PI, 10.0f * B3_PI, 0.0f);
 			
 			b3Body* body = m_world.CreateBody(bdef);
 	
 			b3CapsuleShape capsule;
-			capsule.m_centers[0].Set(0.0f, 4.0f, 0.0f);
-			capsule.m_centers[1].Set(0.0f, -4.0f, 0.0f);
+			capsule.m_vertex1.Set(0.0f, 4.0f, 0.0f);
+			capsule.m_vertex2.Set(0.0f, -4.0f, 0.0f);
 			capsule.m_radius = 0.5f;
 
 			b3ShapeDef sd;

examples/testbed/tests/capsule_stack.h

@@ -24,104 +24,110 @@ class CapsuleStack : public Test
 public:
 	enum
 	{
-		e_rowCount = 1,
-		e_columnCount = 5,
-		e_depthCount = 1
+		e_h = 5,
+		e_w = 1,
+		e_d = 1
 	};
 
 	CapsuleStack()
 	{
 		{
-			b3BodyDef bd;
-			bd.type = b3BodyType::e_staticBody;
-			
-			b3Body* body = m_world.CreateBody(bd);
+			b3BodyDef bdef;
+			b3Body* body = m_world.CreateBody(bdef);
 
 			b3HullShape hs;
 			hs.m_hull = &m_groundHull;
 
-			b3ShapeDef sd;
-			sd.shape = &hs;
-			
-			body->CreateShape(sd);
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 1.0f;
+
+			body->CreateShape(sdef);
 		}
-		
-		float32 height = 3.0f;
-		float32 radius = 1.0f;
-		float32 separation = 0.0f;
+
+		scalar rx = 1.0f;
+		scalar r = 1.0f;
 
 		b3CapsuleShape capsule;
-		capsule.m_centers[0].Set(0.0f, -0.5f * height, 0.0f);
-		capsule.m_centers[1].Set(0.0f, 0.5f * height, 0.0f);
-		capsule.m_radius = radius;
+		capsule.m_vertex1.Set(-rx, 0.0f, 0.0f);
+		capsule.m_vertex2.Set(rx, 0.0f, 0.0f);
+		capsule.m_radius = r;
 
-		b3ShapeDef sdef;
-		sdef.shape = &capsule;
-		sdef.density = 0.1f;
-		sdef.friction = 0.4f;
+		b3Vec3 e;
+		e.x = rx + r;
+		e.y = r;
+		e.z = r;
 
-		const u32 c = e_rowCount * e_columnCount * e_depthCount;
-		b3Body* bs[c];
-		u32 n = 0;
+		b3Vec3 separation;
+		separation.x = 1.0f;
+		separation.y = 1.0f;
+		separation.z = 1.0f;
 
-		b3AABB3 aabb;
-		aabb.m_lower.Set(0.0f, 0.0f, 0.0f);
-		aabb.m_upper.Set(0.0f, 0.0f, 0.0f);
+		b3Vec3 scale;
+		scale.x = 2.0f * e.x + separation.x;
+		scale.y = 2.0f * e.y + separation.y;
+		scale.z = 2.0f * e.z + separation.z;
 
-		for (u32 i = 0; i < e_rowCount; ++i)
+		b3Vec3 size;
+		size.x = 2.0f * e.x + scale.x * scalar(e_w - 1);
+		size.y = 2.0f * e.y + scale.y * scalar(e_h - 1);
+		size.z = 2.0f * e.z + scale.z * scalar(e_d - 1);
+
+		b3Vec3 translation;
+		translation.x = e.x - 0.5f * size.x;
+		translation.y = e.y - 0.5f * size.y;
+		translation.z = e.z - 0.5f * size.z;
+
+		translation.y += 9.0f;
+
+		for (u32 i = 0; i < e_h; ++i)
 		{
-			for (u32 j = 0; j < e_columnCount; ++j)
+			for (u32 j = 0; j < e_w; ++j)
 			{
-				for (u32 k = 0; k < e_depthCount; ++k)
+				for (u32 k = 0; k < e_d; ++k)
 				{
 					b3BodyDef bdef;
-					bdef.type = b3BodyType::e_dynamicBody;
-					
-					bdef.position.x = (2.0f + separation) * float32(i) * (0.5f * height + radius);
-					bdef.position.y = 2.0f + (2.0f + separation) * float32(j) * radius;
-					bdef.position.z = (2.0f + separation) * float32(k) * radius;
-					
-					bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
-					
+					bdef.type = e_dynamicBody;
+
+					bdef.position.Set(scalar(j), scalar(i), scalar(k));
+
+					bdef.position.x *= scale.x;
+					bdef.position.y *= scale.y;
+					bdef.position.z *= scale.z;
+
+					bdef.position += translation;
+
 					b3Body* body = m_world.CreateBody(bdef);
-					bs[n++] = body;
 
-					b3Shape* shape = body->CreateShape(sdef);
+					b3ShapeDef sdef;
+					sdef.density = 0.1f;
+					sdef.friction = 0.3f;
+					sdef.shape = &capsule;
 
-					b3AABB3 aabb2;
-					shape->ComputeAABB(&aabb2, body->GetTransform());
+					body->CreateShape(sdef);
 
-					aabb = b3Combine(aabb, aabb2);
+					u32 bodyIndex = GetBodyIndex(i, j, k);
+
+					m_bodies[bodyIndex] = body;
 				}
 			}
 		}
+	}
 
-		b3Vec3 center = aabb.Centroid();
-		
-		for (u32 i = 0; i < n; ++i)
-		{
-			b3Body* b = bs[i];
-			const b3Vec3& p = b->GetSweep().worldCenter;
-			const b3Quat& q = b->GetSweep().orientation;
-
-			// centralize
-			b3Vec3 position = p - center;
-			
-			// move up
-			position.y += 5.0f + 0.5f * aabb.Height() + radius;
-
-			// maintain orientation
-			b3Vec3 axis;
-			float32 angle;
-			q.GetAxisAngle(&axis, &angle);
-			b->SetTransform(position, axis, angle);
-		}
+	u32 GetBodyIndex(u32 i, u32 j, u32 k)
+	{
+		B3_ASSERT(i < e_h);
+		B3_ASSERT(j < e_w);
+		B3_ASSERT(k < e_d);
+		return k + e_d * (j + e_w * i);
 	}
 
 	static Test* Create()
 	{
 		return new CapsuleStack();
 	}
+
+	b3Body* m_bodies[e_h * e_w * e_d];
 };
 
-#endif
+#endif

examples/testbed/tests/cloth_element_test.h

@@ -0,0 +1,181 @@
+/*
+* 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 CLOTH_ELEMENT_TEST_H
+#define CLOTH_ELEMENT_TEST_H
+
+class ClothElementTest : public Test
+{
+public:
+	enum
+	{
+		e_w = 10,
+		e_h = 10
+	};
+
+	ClothElementTest()
+	{
+		g_camera->m_zoom = 20.0f;
+
+		b3GridClothMesh<e_w, e_h> m;
+
+		b3ClothParticle** particles = (b3ClothParticle * *)b3Alloc(m.vertexCount * sizeof(b3ClothParticle*));
+		for (u32 i = 0; i < m.vertexCount; ++i)
+		{
+			b3ClothParticleDef pd;
+			pd.type = e_dynamicClothParticle;
+			pd.position = m.vertices[i];
+
+			b3ClothParticle* p = m_cloth.CreateParticle(pd);
+			particles[i] = p;
+
+			b3ClothSphereShapeDef sd;
+			sd.p = p;
+			sd.radius = 0.2f;
+			sd.friction = 0.4f;
+
+			m_cloth.CreateSphereShape(sd);
+		}
+
+		for (u32 i = 0; i < m.triangleCount; ++i)
+		{
+			u32 v1 = m.triangles[i].v1;
+			u32 v2 = m.triangles[i].v2;
+			u32 v3 = m.triangles[i].v3;
+
+			b3Vec3 x1 = m.vertices[v1];
+			b3Vec3 x2 = m.vertices[v2];
+			b3Vec3 x3 = m.vertices[v3];
+
+			b3ClothParticle* p1 = particles[v1];
+			b3ClothParticle* p2 = particles[v2];
+			b3ClothParticle* p3 = particles[v3];
+
+			b3ClothTriangleShapeDef tsd;
+			tsd.p1 = p1;
+			tsd.p2 = p2;
+			tsd.p3 = p3;
+			tsd.v1 = x1;
+			tsd.v2 = x2;
+			tsd.v3 = x3;
+			tsd.density = 0.1f;
+
+			m_cloth.CreateTriangleShape(tsd);
+
+			b3ElementForceDef fd;
+			fd.p1 = p1;
+			fd.p2 = p2;
+			fd.p3 = p3;
+			fd.E_x = 500.0f;
+			fd.E_y = 500.0f;
+			fd.E_s = 500.0f;
+			fd.nu_xy = 0.3f;
+			fd.nu_yx = 0.3f;
+			fd.v1 = x1;
+			fd.v2 = x2;
+			fd.v3 = x3;
+
+			m_cloth.CreateForce(fd);
+		}
+
+		for (u32 i = 0; i < e_w + 1; ++i)
+		{
+			u32 vertex = m.GetVertex(0, i);
+			particles[vertex]->SetType(e_staticClothParticle);
+		}
+
+		b3Free(particles);
+
+		m_cloth.SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
+
+		m_clothDragger = new b3ClothDragger(&m_ray, &m_cloth);
+		m_clothDragger->SetStaticDrag(false);
+	}
+
+	~ClothElementTest()
+	{
+		delete m_clothDragger;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		m_cloth.Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_cloth.Draw();
+
+		if (m_clothDragger->IsDragging())
+		{
+			b3Vec3 pA = m_clothDragger->GetPointA();
+			b3Vec3 pB = m_clothDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_clothSolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
+
+		scalar E = m_cloth.GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->Drag();
+		}
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_clothDragger->IsDragging() == false)
+		{
+			m_clothDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new ClothElementTest();
+	}
+
+	b3Cloth m_cloth;
+	b3ClothDragger* m_clothDragger;
+};
+
+#endif

examples/testbed/tests/cloth_sdf.h

@@ -0,0 +1,268 @@
+/* 
+* 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 CLOTH_SDF_H
+#define CLOTH_SDF_H
+
+#define TINYOBJLOADER_IMPLEMENTATION 
+#include <tinyobjloader/tiny_obj_loader.h>
+
+struct SDFMesh
+{
+	u32 vertexCount;
+	b3Vec3* vertices;
+	u32 indexCount;
+	u32* indices;
+
+	SDFMesh()
+	{
+		vertexCount = 0;
+		vertices = nullptr;
+		indexCount = 0;
+		indices = nullptr;
+	}
+
+	~SDFMesh()
+	{
+		free(vertices);
+		free(indices);
+	}
+
+	bool Load(const char* filename)
+	{
+		tinyobj::attrib_t attributes;
+		std::vector<tinyobj::shape_t> shapes;
+		std::vector<tinyobj::material_t> materials;
+
+		std::string warning;
+		std::string error;
+		bool ok = tinyobj::LoadObj(&attributes, &shapes, &materials, &warning, &error, filename);
+		if (!ok)
+		{
+			return false;
+		}
+		
+		assert(vertexCount == 0);
+		vertexCount = attributes.vertices.size() / 3;
+		vertices = (b3Vec3*)malloc(vertexCount * sizeof(b3Vec3));
+		for (size_t i = 0; i < attributes.vertices.size() / 3; ++i)
+		{
+			tinyobj::real_t x = attributes.vertices[3 * i + 0];
+			tinyobj::real_t y = attributes.vertices[3 * i + 1];
+			tinyobj::real_t z = attributes.vertices[3 * i + 2];
+
+			b3Vec3 v(x, y, z);
+
+			vertices[i] = v;
+		}
+
+		assert(indexCount == 0);
+		for (size_t s = 0; s < shapes.size(); s++)
+		{
+			tinyobj::shape_t& shape = shapes[s];
+			for (size_t f = 0; f < shapes[s].mesh.num_face_vertices.size(); f++)
+			{
+				indexCount += 3;
+			}
+		}
+		indices = (u32*)malloc(indexCount * sizeof(u32));
+
+		indexCount = 0;
+		for (size_t s = 0; s < shapes.size(); s++) 
+		{
+			tinyobj::shape_t& shape = shapes[s];
+
+			size_t index_offset = 0;
+			for (size_t f = 0; f < shapes[s].mesh.num_face_vertices.size(); f++) 
+			{
+				unsigned char fv = shapes[s].mesh.num_face_vertices[f];
+
+				for (size_t v = 0; v < 3; v++) 
+				{
+					tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v];
+					
+					size_t vi = idx.vertex_index;
+
+					indices[indexCount++] = vi;
+				}
+				
+				index_offset += fv;
+			}
+		}
+		
+		return true;
+	}
+
+	void Draw(const b3Transform& xf, const b3Vec3& scale, const b3Color& color) const
+	{
+		for (u32 i = 0; i < indexCount / 3; ++i)
+		{
+			u32 i1 = indices[3 * i + 0];
+			u32 i2 = indices[3 * i + 1];
+			u32 i3 = indices[3 * i + 2];
+
+			b3Vec3 v1 = xf * b3MulCW(scale, vertices[i1]);
+			b3Vec3 v2 = xf * b3MulCW(scale, vertices[i2]);
+			b3Vec3 v3 = xf * b3MulCW(scale, vertices[i3]);
+
+			b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
+			n.Normalize();
+
+			g_draw->DrawSolidTriangle(n, v1, v2, v3, color);
+		}
+	}
+};
+
+class ClothSDF : public Test
+{
+public:
+	ClothSDF()
+	{
+		// Translate the cloth mesh
+		for (u32 i = 0; i < m_clothMesh.vertexCount; ++i)
+		{
+			m_clothMesh.vertices[i].y += 5.0f;
+		}
+
+		// Create cloth
+		b3ClothDef def;
+		def.mesh = &m_clothMesh;
+		def.density = 0.2f;
+		def.streching = 10000.0f;
+		def.strechDamping = 100.0f;
+		def.thickness = 0.2f;
+		def.friction = 0.2f;
+
+		m_cloth = new b3Cloth(def);
+
+		m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
+
+		{
+			bool ok = m_sdfMesh.Load("data/teapot.obj");
+			assert(ok);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_staticBody;
+
+			b3Body* b = m_world.CreateBody(bd);
+
+			bool ok = m_sdf.Load("data/teapot.cdf");
+			assert(ok);
+
+			b3SDFShape sdfShape;
+			sdfShape.m_sdf = &m_sdf;
+			sdfShape.m_radius = 0.2f;
+
+			b3ShapeDef sd;
+			sd.shape = &sdfShape;
+			sd.friction = 1.0f;
+			
+			m_sdfShape = (b3SDFShape*)b->CreateShape(sd);
+
+			b3ClothWorldShapeDef csd;
+			csd.shape = m_sdfShape;
+
+			m_cloth->CreateWorldShape(csd);
+		}
+
+		m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
+	}
+
+	~ClothSDF()
+	{
+		delete m_clothDragger;
+		delete m_cloth;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_cloth->Draw();
+
+		if (m_clothDragger->IsDragging())
+		{
+			b3Vec3 pA = m_clothDragger->GetPointA();
+			b3Vec3 pB = m_clothDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		b3Body* sdfBody = m_sdfShape->GetBody();
+		m_sdfMesh.Draw(sdfBody->GetTransform(), m_sdfShape->m_scale, b3Color_white);
+
+		extern u32 b3_clothSolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
+
+		scalar E = m_cloth->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->Drag();
+		}
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_clothDragger->IsDragging() == false)
+		{
+			m_clothDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new ClothSDF();
+	}
+
+	b3GridClothMesh<10, 10> m_clothMesh;
+	b3Cloth* m_cloth;
+	b3ClothDragger* m_clothDragger;
+
+	SDFMesh m_sdfMesh;
+	b3SDF m_sdf;
+	b3SDFShape* m_sdfShape;
+};
+
+#endif

examples/testbed/tests/cloth_self_collision.h

@@ -22,15 +22,82 @@
 class ClothSelfCollision : public Test
 {
 public:
+	enum
+	{
+		e_w1 = 5,
+		e_h1 = 5,
+		e_w2 = 5,
+		e_h2 = 5
+	};
+
 	ClothSelfCollision()
 	{
-		// Translate the mesh
-		for (u32 i = 0; i < m_clothMesh.vertexCount; ++i)
+		b3GridClothMesh<e_w1, e_h1> mesh1;
+		b3Quat qX = b3QuatRotationX(0.5f * B3_PI);
+		for (u32 i = 0; i < mesh1.vertexCount; ++i)
 		{
-			m_clothMesh.vertices[i].y += 5.0f;
+			mesh1.vertices[i] = b3Mul(qX, mesh1.vertices[i]);
+			mesh1.vertices[i].y += 5.0f;
 		}
 
-		// Create cloth
+		b3GridClothMesh<e_w2, e_h2> mesh2;
+		b3Quat qY = b3QuatRotationY(0.5f * B3_PI);
+		for (u32 i = 0; i < mesh2.vertexCount; ++i)
+		{
+			mesh2.vertices[i] = b3Mul(qY * qX, mesh2.vertices[i]);
+			mesh2.vertices[i].y += 12.0f;
+		}
+
+		// Merge the meshes
+		m_clothMesh.vertexCount = mesh1.vertexCount + mesh2.vertexCount;
+		m_clothMesh.vertices = (b3Vec3*)b3Alloc(m_clothMesh.vertexCount * sizeof(b3Vec3));
+		
+		u32* newVertices1 = (u32*)b3Alloc(mesh1.vertexCount * sizeof(u32));
+		u32 vertexIndex = 0;
+		for (u32 i = 0; i < mesh1.vertexCount; ++i)
+		{
+			newVertices1[i] = vertexIndex;
+			m_clothMesh.vertices[vertexIndex++] = mesh1.vertices[i];
+		}
+
+		u32* newVertices2 = (u32*)b3Alloc(mesh2.vertexCount * sizeof(u32));
+		for (u32 i = 0; i < mesh2.vertexCount; ++i)
+		{
+			newVertices2[i] = vertexIndex;
+			m_clothMesh.vertices[vertexIndex++] = mesh2.vertices[i];
+		}
+		
+		m_clothMesh.triangleCount = mesh1.triangleCount + mesh2.triangleCount;
+		m_clothMesh.triangles = (b3ClothMeshTriangle*)b3Alloc(m_clothMesh.triangleCount * sizeof(b3ClothMeshTriangle));
+		u32 triangleIndex = 0;
+		for (u32 i = 0; i < mesh1.triangleCount; ++i)
+		{
+			m_clothMesh.triangles[triangleIndex].v1 = newVertices1[mesh1.triangles[i].v1];
+			m_clothMesh.triangles[triangleIndex].v2 = newVertices1[mesh1.triangles[i].v2];
+			m_clothMesh.triangles[triangleIndex].v3 = newVertices1[mesh1.triangles[i].v3];
+			++triangleIndex;
+		}
+
+		for (u32 i = 0; i < mesh2.triangleCount; ++i)
+		{
+			m_clothMesh.triangles[triangleIndex].v1 = newVertices2[mesh2.triangles[i].v1];
+			m_clothMesh.triangles[triangleIndex].v2 = newVertices2[mesh2.triangles[i].v2];
+			m_clothMesh.triangles[triangleIndex].v3 = newVertices2[mesh2.triangles[i].v3];
+			++triangleIndex;
+		}
+
+		m_clothMesh.meshCount = 1;
+		m_clothMesh.meshes = (b3ClothMeshMesh*)b3Alloc(sizeof(b3ClothMeshMesh));
+		m_clothMesh.meshes->startTriangle = 0;
+		m_clothMesh.meshes->triangleCount = m_clothMesh.triangleCount;
+		m_clothMesh.meshes->startVertex = 0;
+		m_clothMesh.meshes->vertexCount = m_clothMesh.vertexCount;
+
+		m_clothMesh.shearingLineCount = 0;
+		m_clothMesh.bendingLineCount = 0;
+		m_clothMesh.sewingLineCount = 0;
+
+		// Create the cloth
 		b3ClothDef def;
 		def.mesh = &m_clothMesh;
 		def.density = 1.0f;
@@ -41,24 +108,37 @@ public:
 		m_cloth = new b3Cloth(def);
 
 		m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
-		m_cloth->SetWorld(&m_world);
+		m_cloth->EnableSelfCollision(true);
+
+		for (u32 i = 0; i < mesh1.vertexCount; ++i)
+		{
+			u32 newVertex = newVertices1[i];
+
+			m_cloth->GetParticle(newVertex)->SetType(e_staticClothParticle);
+		}
+
+		b3Free(newVertices1);
+		b3Free(newVertices2);
 
 		{
 			b3BodyDef bd;
-			bd.type = e_staticBody;
 
 			b3Body* b = m_world.CreateBody(bd);
 
-			b3CapsuleShape capsuleShape;
-			capsuleShape.m_centers[0].Set(0.0f, 0.0f, -5.0f);
-			capsuleShape.m_centers[1].Set(0.0f, 0.0f, 5.0f);
-			capsuleShape.m_radius = 1.0f;;
+			b3HullShape hullShape;
+			hullShape.m_hull = &m_groundHull;
+			hullShape.m_radius = 0.0f;;
 
 			b3ShapeDef sd;
-			sd.shape = &capsuleShape;
+			sd.shape = &hullShape;
 			sd.friction = 1.0f;
 
-			b->CreateShape(sd);
+			b3Shape* s = b->CreateShape(sd);
+
+			b3ClothWorldShapeDef csd;
+			csd.shape = s;
+
+			m_cloth->CreateWorldShape(csd);
 		}
 
 		m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
@@ -66,6 +146,10 @@ public:
 
 	~ClothSelfCollision()
 	{
+		b3Free(m_clothMesh.vertices);
+		b3Free(m_clothMesh.triangles);
+		b3Free(m_clothMesh.meshes);
+
 		delete m_cloth;
 		delete m_clothDragger;
 	}
@@ -83,17 +167,27 @@ public:
 			b3Vec3 pA = m_clothDragger->GetPointA();
 			b3Vec3 pB = m_clothDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
 
+		g_draw->DrawString(b3Color_white, "S - Turn on/off self collision");
+		if (m_cloth->IsSelfCollisionEnabled())
+		{
+			g_draw->DrawString(b3Color_white, "Self collision enabled");
+		}
+		else
+		{
+			g_draw->DrawString(b3Color_white, "Self collision disabled");
+		}
+
 		extern u32 b3_clothSolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
 
-		float32 E = m_cloth->GetEnergy();
+		scalar E = m_cloth->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
 	}
 
@@ -127,12 +221,20 @@ public:
 		}
 	}
 
+	void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_S)
+		{
+			m_cloth->EnableSelfCollision(!m_cloth->IsSelfCollisionEnabled());
+		}
+	}
+
 	static Test* Create()
 	{
 		return new ClothSelfCollision();
 	}
 
-	b3GridClothMesh<10, 10> m_clothMesh;
+	b3ClothMesh m_clothMesh;
 	b3Cloth* m_cloth;
 	b3ClothDragger* m_clothDragger; 
 };

examples/testbed/tests/cloth_tearing.h

@@ -0,0 +1,571 @@
+/*
+* 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 CLOTH_TEARING_H
+#define CLOTH_TEARING_H
+
+class ClothTearing : public Test
+{
+public:
+	enum
+	{
+		e_w = 10,
+		e_h = 10
+	};
+
+	ClothTearing()
+	{
+		g_camera->m_zoom = 20.0f;
+
+		b3GridClothMesh<e_w, e_h> m;
+
+		b3ClothParticle** particles = (b3ClothParticle * *)b3Alloc(m.vertexCount * sizeof(b3ClothParticle*));
+		for (u32 i = 0; i < m.vertexCount; ++i)
+		{
+			b3ClothParticleDef pd;
+			pd.type = e_dynamicClothParticle;
+			pd.position = m.vertices[i];
+
+			b3ClothParticle* p = m_cloth.CreateParticle(pd);
+			particles[i] = p;
+
+			b3ClothSphereShapeDef sd;
+			sd.p = p;
+			sd.radius = 0.2f;
+			sd.friction = 0.4f;
+
+			m_cloth.CreateSphereShape(sd);
+		}
+
+		for (u32 i = 0; i < m.triangleCount; ++i)
+		{
+			u32 v1 = m.triangles[i].v1;
+			u32 v2 = m.triangles[i].v2;
+			u32 v3 = m.triangles[i].v3;
+
+			b3ClothParticle* p1 = particles[v1];
+			b3ClothParticle* p2 = particles[v2];
+			b3ClothParticle* p3 = particles[v3];
+
+			b3ClothTriangleShapeDef tsd;
+			tsd.p1 = p1;
+			tsd.p2 = p2;
+			tsd.p3 = p3;
+			tsd.v1 = m.vertices[v1];
+			tsd.v2 = m.vertices[v2];
+			tsd.v3 = m.vertices[v3];
+			tsd.density = 0.1f;
+
+			m_cloth.CreateTriangleShape(tsd);
+
+			{
+				b3SpringForceDef sfd;
+				sfd.Initialize(p1, p2, 1000.0f, 10.0f);
+
+				CreateSpringForce(sfd);
+			}
+
+			{
+				b3SpringForceDef sfd;
+				sfd.Initialize(p2, p3, 1000.0f, 10.0f);
+
+				CreateSpringForce(sfd);
+			}
+
+			{
+				b3SpringForceDef sfd;
+				sfd.Initialize(p3, p1, 1000.0f, 10.0f);
+
+				CreateSpringForce(sfd);
+			}
+		}
+
+		for (u32 i = 0; i < e_w + 1; ++i)
+		{
+			u32 vertex = m.GetVertex(0, i);
+			particles[vertex]->SetType(e_staticClothParticle);
+		}
+
+		b3Free(particles);
+
+		m_cloth.SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
+
+		m_clothDragger = new b3ClothDragger(&m_ray, &m_cloth);
+		m_clothDragger->SetStaticDrag(false);
+	}
+
+	~ClothTearing()
+	{
+		delete m_clothDragger;
+	}
+
+	b3SpringForce* FindSpringForce(b3ClothParticle* p1, b3ClothParticle* p2)
+	{
+		for (b3Force* f = m_cloth.GetForceList().m_head; f; f = f->GetNext())
+		{
+			if (f->GetType() != e_springForce)
+			{
+				continue;
+			}
+
+			b3SpringForce* sf = (b3SpringForce*)f;
+
+			b3ClothParticle* sp1 = sf->GetParticle1();
+			b3ClothParticle* sp2 = sf->GetParticle2();
+
+			if (sp1 == p1 && sp2 == p2)
+			{
+				return sf;
+			}
+
+			if (sp1 == p2 && sp2 == p1)
+			{
+				return sf;
+			}
+		}
+
+		return nullptr;
+	}
+
+	b3SpringForce* CreateSpringForce(const b3SpringForceDef& def)
+	{
+		b3SpringForce* sf = FindSpringForce(def.p1, def.p2);
+		if (sf != nullptr)
+		{
+			return sf;
+		}
+
+		return (b3SpringForce*)m_cloth.CreateForce(def);
+	}
+
+	void DrawSpringForces()
+	{
+		for (b3Force* f = m_cloth.GetForceList().m_head; f; f = f->GetNext())
+		{
+			if (f->GetType() != e_springForce)
+			{
+				continue;
+			}
+
+			b3SpringForce* s = (b3SpringForce*)f;
+
+			b3ClothParticle* p1 = s->GetParticle1();
+			b3ClothParticle* p2 = s->GetParticle2();
+
+			g_draw->DrawSegment(p1->GetPosition(), p2->GetPosition(), b3Color_black);
+		}
+	}
+
+	void Partition(b3ClothParticle* p, const b3Plane& plane,
+		b3Array<b3ClothTriangleShape*>& above,
+		b3Array<b3ClothTriangleShape*>& below)
+	{
+		for (b3ClothTriangleShape* t = m_cloth.GetTriangleShapeList().m_head; t; t = t->GetNext())
+		{
+			b3ClothParticle* p1 = t->GetParticle1();
+			b3ClothParticle* p2 = t->GetParticle2();
+			b3ClothParticle* p3 = t->GetParticle3();
+
+			if (p1 != p && p2 != p && p3 != p)
+			{
+				continue;
+			}
+
+			b3Vec3 x1 = p1->GetPosition();
+			b3Vec3 x2 = p2->GetPosition();
+			b3Vec3 x3 = p3->GetPosition();
+
+			b3Vec3 center = (x1 + x2 + x3) / 3.0f;
+
+			scalar distance = b3Distance(center, plane);
+			if (distance > 0.0f)
+			{
+				above.PushBack(t);
+			}
+			else
+			{
+				below.PushBack(t);
+			}
+		}
+	}
+
+	bool HasSpring(const b3Array<b3ClothTriangleShape*>& triangles,
+		b3ClothParticle* pOld, b3ClothParticle* pOther)
+	{
+		for (u32 i = 0; i < triangles.Count(); ++i)
+		{
+			b3ClothTriangleShape* triangle = triangles[i];
+
+			b3ClothParticle* tp1 = triangle->GetParticle1();
+			b3ClothParticle* tp2 = triangle->GetParticle2();
+			b3ClothParticle* tp3 = triangle->GetParticle3();
+
+			// 1, 2
+			if (tp1 == pOld && tp2 == pOther)
+			{
+				return true;
+			}
+
+			// 2, 1
+			if (tp2 == pOld && tp1 == pOther)
+			{
+				return true;
+			}
+
+			// 2, 3
+			if (tp2 == pOld && tp3 == pOther)
+			{
+				return true;
+			}
+
+			// 3, 2
+			if (tp3 == pOld && tp2 == pOther)
+			{
+				return true;
+			}
+
+			// 3, 1
+			if (tp3 == pOld && tp1 == pOther)
+			{
+				return true;
+			}
+
+			// 1, 3
+			if (tp1 == pOld && tp3 == pOther)
+			{
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	bool SplitParticle(b3ClothParticle* pOld, const b3Plane& plane)
+	{
+		// Collect triangles.
+		b3StackArray<b3ClothTriangleShape*, 32> trianglesAbove, trianglesBelow;
+		Partition(pOld, plane, trianglesAbove, trianglesBelow);
+
+		// There must be at least one triangle on each side of the plane.
+		if (trianglesAbove.Count() == 0 || trianglesBelow.Count() == 0)
+		{
+			return false;
+		}
+
+		b3ClothParticleDef pdNew;
+		pdNew.type = pOld->GetType();
+		pdNew.position = pOld->GetPosition() - 0.2f * plane.normal;
+
+		b3ClothParticle* pNew = m_cloth.CreateParticle(pdNew);
+
+		b3ClothSphereShapeDef ssdNew;
+		ssdNew.p = pNew;
+		ssdNew.radius = 0.2f;
+		ssdNew.friction = 0.4f;
+		
+		m_cloth.CreateSphereShape(ssdNew);
+
+		for (u32 i = 0; i < trianglesBelow.Count(); ++i)
+		{
+			b3ClothTriangleShape* triangle = trianglesBelow[i];
+
+			b3ClothParticle* p1 = triangle->GetParticle1();
+			b3ClothParticle* p2 = triangle->GetParticle2();
+			b3ClothParticle* p3 = triangle->GetParticle3();
+
+			m_cloth.DestroyTriangleShape(triangle);
+
+			if (p1 == pOld)
+			{
+				b3ClothTriangleShapeDef tdNew;
+				tdNew.p1 = pNew;
+				tdNew.p2 = p2;
+				tdNew.p3 = p3;
+				tdNew.v1 = pNew->GetPosition();
+				tdNew.v2 = p2->GetPosition();
+				tdNew.v3 = p3->GetPosition();
+
+				m_cloth.CreateTriangleShape(tdNew);
+
+				b3SpringForce* sf1 = FindSpringForce(p1, p2);
+				if (sf1)
+				{
+					b3SpringForceDef sNew;
+					sNew.p1 = pNew;
+					sNew.p2 = p2;
+					sNew.restLength = sf1->GetRestLenght();
+					sNew.structural = sf1->GetStructuralStiffness();
+					sNew.damping = sf1->GetDampingStiffness();
+
+					m_cloth.CreateForce(sNew);
+
+					if (HasSpring(trianglesAbove, p1, p2) == false)
+					{
+						m_cloth.DestroyForce(sf1);
+					}
+				}
+
+				b3SpringForce* sf2 = FindSpringForce(p3, p1);
+				if (sf2)
+				{
+					b3SpringForceDef sNew;
+					sNew.p1 = p3;
+					sNew.p2 = pNew;
+					sNew.restLength = sf2->GetRestLenght();
+					sNew.structural = sf2->GetStructuralStiffness();
+					sNew.damping = sf2->GetDampingStiffness();
+					
+					m_cloth.CreateForce(sNew);
+					
+					if (HasSpring(trianglesAbove, p3, p1) == false)
+					{
+						m_cloth.DestroyForce(sf2);
+					}
+				}
+			}
+			
+			if (p2 == pOld)
+			{
+				b3ClothTriangleShapeDef tdNew;
+				tdNew.p1 = p1;
+				tdNew.p2 = pNew;
+				tdNew.p3 = p3;
+				tdNew.v1 = p1->GetPosition();
+				tdNew.v2 = pNew->GetPosition();
+				tdNew.v3 = p3->GetPosition();
+
+				m_cloth.CreateTriangleShape(tdNew);
+
+				b3SpringForce* sf1 = FindSpringForce(p1, p2);
+				if (sf1)
+				{
+					b3SpringForceDef sNew;
+					sNew.p1 = p1;
+					sNew.p2 = pNew;
+					sNew.restLength = sf1->GetRestLenght();
+					sNew.structural = sf1->GetStructuralStiffness();
+					sNew.damping = sf1->GetDampingStiffness();
+
+					m_cloth.CreateForce(sNew);
+					
+					if (HasSpring(trianglesAbove, p1, p2) == false)
+					{
+						m_cloth.DestroyForce(sf1);
+					}
+				}
+
+				b3SpringForce* sf2 = FindSpringForce(p2, p3);
+				if (sf2)
+				{
+					b3SpringForceDef sNew;
+					sNew.p1 = pNew;
+					sNew.p2 = p3;
+					sNew.restLength = sf2->GetRestLenght();
+					sNew.structural = sf2->GetStructuralStiffness();
+					sNew.damping = sf2->GetDampingStiffness();
+					
+					m_cloth.CreateForce(sNew);
+					
+					if (HasSpring(trianglesAbove, p2, p3) == false)
+					{
+						m_cloth.DestroyForce(sf2);
+					}
+				}
+			}
+			
+			if (p3 == pOld)
+			{
+				b3ClothTriangleShapeDef tdNew;
+				tdNew.p1 = p1;
+				tdNew.p2 = p2;
+				tdNew.p3 = pNew;
+				tdNew.v1 = p1->GetPosition();
+				tdNew.v2 = p2->GetPosition();
+				tdNew.v3 = pNew->GetPosition();
+
+				m_cloth.CreateTriangleShape(tdNew);
+				
+				b3SpringForce* sf1 = FindSpringForce(p2, p3);
+				if (sf1)
+				{
+					b3SpringForceDef sNew;
+					sNew.p1 = p2;
+					sNew.p2 = pNew;
+					sNew.restLength = sf1->GetRestLenght();
+					sNew.structural = sf1->GetStructuralStiffness();
+					sNew.damping = sf1->GetDampingStiffness();
+					
+					m_cloth.CreateForce(sNew);
+					
+					if (HasSpring(trianglesAbove, p2, p3) == false)
+					{
+						m_cloth.DestroyForce(sf1);
+					}
+				}
+
+				b3SpringForce* sf2 = FindSpringForce(p3, p1);
+				if (sf2)
+				{
+					b3SpringForceDef sNew;
+					sNew.p1 = pNew;
+					sNew.p2 = p1;
+					sNew.restLength = sf2->GetRestLenght();
+					sNew.structural = sf2->GetStructuralStiffness();
+					sNew.damping = sf2->GetDampingStiffness();
+					
+					m_cloth.CreateForce(sNew);
+					
+					if (HasSpring(trianglesAbove, p3, p1) == false)
+					{
+						m_cloth.DestroyForce(sf2);
+					}
+				}
+			}
+		}
+
+		return true;
+	}
+
+	bool Tear()
+	{
+		b3Force* f = m_cloth.GetForceList().m_head;
+		while (f)
+		{
+			if (f->GetType() != e_springForce)
+			{
+				f = f->GetNext();
+				continue;
+			}
+
+			b3SpringForce* s = (b3SpringForce*)f;
+			f = f->GetNext();
+
+			b3Vec3 tension = s->GetActionForce();
+
+			const scalar kMaxTension = 1000.0f;
+
+			if (b3LengthSquared(tension) <= kMaxTension * kMaxTension)
+			{
+				continue;
+			}
+
+			b3ClothParticle* p1 = s->GetParticle1();
+			b3ClothParticle* p2 = s->GetParticle2();
+
+			b3Vec3 x1 = p1->GetPosition();
+			b3Vec3 x2 = p2->GetPosition();
+
+			if (p1->GetType() == e_dynamicClothParticle)
+			{
+				b3Vec3 n = b3Normalize(x2 - x1);
+				b3Plane plane(n, x1);
+
+				bool wasSplit = SplitParticle(p1, plane);
+				if (wasSplit)
+				{
+					return true;
+				}
+			}
+
+			if (p2->GetType() == e_dynamicClothParticle)
+			{
+				b3Vec3 n = b3Normalize(x1 - x2);
+				b3Plane plane(n, x2);
+
+				bool wasSplit = SplitParticle(p2, plane);
+				if (wasSplit)
+				{
+					return true;
+				}
+			}
+		}
+
+		return false;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		m_cloth.Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		while (Tear());
+
+		m_cloth.Draw();
+
+		DrawSpringForces();
+
+		if (m_clothDragger->IsDragging())
+		{
+			b3Vec3 pA = m_clothDragger->GetPointA();
+			b3Vec3 pB = m_clothDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_clothSolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
+
+		scalar E = m_cloth.GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->Drag();
+		}
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_clothDragger->IsDragging() == false)
+		{
+			m_clothDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_clothDragger->IsDragging() == true)
+		{
+			m_clothDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new ClothTearing();
+	}
+
+	b3Cloth m_cloth;
+	b3ClothDragger* m_clothDragger;
+};
+
+#endif

examples/testbed/tests/cluster.h

@@ -38,9 +38,9 @@ public:
 	{
 		for (u32 i = 0; i < e_count; ++i)
 		{
-			float32 x = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 y = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 z = 3.0f * RandomFloat(-1.0f, 1.0f);
+			scalar x = 3.0f * RandomFloat(-1.0f, 1.0f);
+			scalar y = 3.0f * RandomFloat(-1.0f, 1.0f);
+			scalar z = 3.0f * RandomFloat(-1.0f, 1.0f);
 
 			b3Vec3 p(x, y, z);
 			m_points[i] = p;
@@ -48,9 +48,9 @@ public:
 
 		for (u32 i = 0; i < B3_MAX_MANIFOLDS; ++i)
 		{
-			float32 r = RandomFloat(0.0f, 1.0f);
-			float32 g = RandomFloat(0.0f, 1.0f);
-			float32 b = RandomFloat(0.0f, 1.0f);
+			scalar r = RandomFloat(0.0f, 1.0f);
+			scalar g = RandomFloat(0.0f, 1.0f);
+			scalar b = RandomFloat(0.0f, 1.0f);
 
 			b3Color c(r, g, b);
 			m_colors[i] = c;

examples/testbed/tests/collide_test.h

@@ -68,46 +68,43 @@ public:
 	{
 		if (key == GLFW_KEY_LEFT)
 		{
-			m_xfB.position.x -= 0.05f;
+			m_xfB.translation.x -= 0.05f;
 		}
 
 		if (key == GLFW_KEY_RIGHT)
 		{
-			m_xfB.position.x += 0.05f;
+			m_xfB.translation.x += 0.05f;
 		}
 		
 		if (key == GLFW_KEY_UP)
 		{
-			m_xfB.position.y += 0.05f;
+			m_xfB.translation.y += 0.05f;
 		}
 		
 		if (key == GLFW_KEY_DOWN)
 		{
-			m_xfB.position.y -= 0.05f;
+			m_xfB.translation.y -= 0.05f;
 		}
 
 		if (key == GLFW_KEY_X)
 		{
-			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3QuatMat33(qx);
+			b3Quat qx = b3QuatRotationX(0.05f * B3_PI);
 
-			m_xfB.rotation = m_xfB.rotation * xfx;
+			m_xfB.rotation = m_xfB.rotation * qx;
 		}
 
 		if (key == GLFW_KEY_Y)
 		{
-			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3QuatMat33(qy);
+			b3Quat qy = b3QuatRotationY(0.05f * B3_PI);
 
-			m_xfB.rotation = m_xfB.rotation * xfy;
+			m_xfB.rotation = m_xfB.rotation * qy;
 		}
 
 		if (key == GLFW_KEY_Z)
 		{
-			b3Quat qy(b3Vec3(0.0f, 0.0f, 1.0f), 0.05f * B3_PI);
-			b3Mat33 xfz = b3QuatMat33(qy);
+			b3Quat qz = b3QuatRotationZ(0.05f * B3_PI);
 
-			m_xfB.rotation = m_xfB.rotation * xfz;
+			m_xfB.rotation = m_xfB.rotation * qz;
 		}
 	}
 

examples/testbed/tests/compound_body.h

@@ -38,38 +38,33 @@ public:
 		}
 
 		{
-			b3Transform xf;
-			xf.SetIdentity();
-			xf.position.Set(-5.0f, 10.0f, 0.0f);
-			m_box1.SetTransform(xf);
+			m_box1.SetExtents(1.0f, 1.0f, 1.0f);
+			b3Vec3 translation(-5.0f, 10.0f, 0.0f);
+			m_box1.Translate(translation);
 		}
 		
 		{
-			b3Transform xf;
-			xf.SetIdentity();
-			xf.position.Set(5.0f, 10.0f, 0.0f);
-			m_box2.SetTransform(xf);
+			b3Vec3 translation(5.0f, 10.0f, 0.0f);
+			m_box2.SetExtents(1.0f, 1.0f, 1.0f);
+			m_box2.Translate(translation);
 		}
 		
 		{
-			b3Transform xf;
-			xf.SetIdentity();
-			xf.position.Set(0.0f, 2.0f, 0.0f);
-			m_box3.SetTransform(xf);
+			b3Vec3 translation(0.0f, 2.0f, 0.0f);
+			m_box3.SetExtents(1.0f, 1.0f, 1.0f);
+			m_box3.Translate(translation);
 		}
 		 
 		{
-			b3Transform xf;
-			xf.SetIdentity();
-			xf.position.Set(0.0f, 6.0f, 0.0f);
-			m_box4.SetTransform(xf);
+			b3Vec3 translation(0.0f, 6.0f, 0.0f);
+			m_box4.SetExtents(1.0f, 1.0f, 1.0f);
+			m_box4.Translate(translation);
 		}
 		
 		{
-			b3Transform xf;
-			xf.SetIdentity();
-			xf.position.Set(0.0f, 10.0f, 0.0f);
-			m_box5.SetTransform(xf);
+			b3Vec3 translation(0.0f, 10.0f, 0.0f);
+			m_box5.SetExtents(1.0f, 1.0f, 1.0f);
+			m_box5.Translate(translation);
 		}
 		
 		{

examples/testbed/tests/cone_test.h

@@ -42,8 +42,8 @@ public:
 			head = m_world.CreateBody(bd);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 0.15f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -0.15f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 0.15f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -0.15f, 0.0f);
 			cs.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -55,19 +55,22 @@ public:
 		{
 			b3Vec3 anchor(0.0f, 0.0f, 0.0f);
 			b3Vec3 axis(0.0f, 1.0f, 0.0f);
-			float32 coneAngle = 0.5f * B3_PI;
+			scalar coneAngle = 0.5f * B3_PI;
 
 			b3ConeJointDef cd;
 			cd.Initialize(ref, head, axis, anchor, coneAngle);
-			cd.enableLimit = true;
+			cd.enableConeLimit = true;
 			
+			cd.enableTwistLimit = true;
+			cd.lowerAngle = 0.0f;
+			cd.upperAngle = B3_PI;
+
 			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
 		}
 		
 		// Invalidate the orientation
-		b3Vec3 axis(1.0f, 0.0f, 0.0f);
-		float32 angle = B3_PI;
-		head->SetTransform(head->GetPosition(), axis, angle);
+		b3Quat q = b3QuatRotationX(B3_PI);
+		head->SetTransform(head->GetPosition(), q);
 	}
 
 	static Test* Create()

examples/testbed/tests/convex_cast.h

@@ -0,0 +1,282 @@
+/*
+* 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 CONVEX_CAST_H
+#define CONVEX_CAST_H
+
+class ConvexCast : public Test
+{
+public:
+	ConvexCast()
+	{
+		{
+			b3BodyDef bdef;
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(0.0f, 2.0f, 0.0f);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			m_shape = body->CreateShape(sdef);
+		}
+		
+		{
+			m_grid.BuildTree();
+			m_grid.BuildAdjacency();
+
+			b3BodyDef bdef;
+			bdef.position.Set(-10.0f, 5.0f, -2.0f);
+			bdef.orientation = b3QuatRotationZ(0.25f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3MeshShape hs;
+			hs.m_mesh = &m_grid;
+			hs.m_scale.Set(-1.0f, -1.0f, -2.0f);
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(0.0f, 2.0f, 10.0f);
+			bdef.orientation = b3QuatRotationY(0.25f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(-10.0f, 6.0f, -10.0f);
+			bdef.orientation = b3QuatRotationY(0.25f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			static b3BoxHull boxHull(2.0f, 4.0f, 0.5f);
+
+			b3HullShape hs;
+			hs.m_hull = &boxHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(10.0f, 2.0f, 0.0f);
+			bdef.orientation = b3QuatRotationY(0.20f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(-10.0f, 2.0f, 14.0f);
+			bdef.orientation = b3QuatRotationY(0.05f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(-14.0f, 2.0f, 5.0f);
+			bdef.orientation = b3QuatRotationY(-0.05f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(20.0f, 2.0f, 5.0f);
+			bdef.orientation = b3QuatRotationY(-0.05f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(12.0f, 2.0f, 5.0f);
+			bdef.orientation = b3QuatRotationY(-0.35f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3SphereShape hs;
+			hs.m_center.SetZero();
+			hs.m_radius = 2.5f;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(0.0f, 1.0f, -12.0f);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3CapsuleShape hs;
+			hs.m_vertex1.Set(0.0f, 1.0f, 0.0f);
+			hs.m_vertex2.Set(0.0f, -1.0f, 0.0f);
+			hs.m_radius = 3.0f;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		m_d.Set(-50.0f, 2.0f, 0.0f);
+	}
+
+	void CastConvex() const
+	{
+		class ConvexCastFilter : public b3ConvexCastFilter
+		{
+		public:
+			bool ShouldConvexCast(b3Shape* shape)
+			{
+				return true;
+			}
+		};
+
+		ConvexCastFilter filter;
+		
+		b3Body* body = m_shape->GetBody();
+		b3Transform xf = body->GetTransform();
+
+		m_world.DrawSolidShape(xf, m_shape, b3Color_red);
+
+		b3Vec3 p1 = xf.translation;
+
+		b3Vec3 p2 = p1 + m_d;
+		
+		b3ConvexCastSingleOutput out;
+		if (m_world.ConvexCastSingle(&out, &filter, m_shape, m_d))
+		{
+			g_draw->DrawPoint(out.point, 4.0f, b3Color_red);
+			g_draw->DrawSegment(out.point, out.point + out.normal, b3Color_white);
+			
+			b3Transform xft;
+			xft.rotation = xf.rotation;
+			xft.translation = xf.translation + out.fraction * m_d;
+
+			m_world.DrawShape(xft, m_shape, b3Color_red);
+
+			g_draw->DrawSegment(p1, xft.translation, b3Color_green);
+		}
+		else
+		{
+			g_draw->DrawSegment(p1, p2, b3Color_green);
+			
+			b3Transform xf1;
+			xf1.rotation = xf.rotation;
+			xf1.translation = xf.translation + m_d;
+			
+			m_world.DrawShape(xf1, m_shape, b3Color_red);
+		}
+	}
+
+	void Step()
+	{
+		scalar dt = g_testSettings->inv_hertz;
+		
+		b3Quat q = b3QuatRotationY(0.05f * B3_PI * dt);
+
+		m_d = b3Mul(q, m_d);
+		
+		CastConvex();
+
+		Test::Step();
+	}
+
+	static Test* Create()
+	{
+		return new ConvexCast();
+	}
+
+	b3GridMesh<5, 5> m_grid;
+
+	b3Shape* m_shape;
+	b3Vec3 m_d;
+};
+
+#endif

examples/testbed/tests/convex_hull.h

@@ -66,9 +66,9 @@ public:
 		m_count = 0;
 		for (u32 i = 0; i < e_count; ++i)
 		{
-			float32 x = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 y = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 z = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float x = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float y = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float z = 3.0f * RandomFloat(-1.0f, 1.0f);
 			
 			// Clamp to force coplanarities.
 			// This will stress the convex hull creation code.
@@ -135,7 +135,7 @@ public:
 				edge = m_hull.GetEdge(edge->next);
 			} while (edge != begin);
 
-			c /= float32(vn);
+			c /= scalar(vn);
 			g_draw->DrawSegment(c, c + n, b3Color_white);
 		}
 

examples/testbed/tests/conveyor_belt.h

@@ -0,0 +1,119 @@
+/*
+* 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 CONVEYOR_BELT_H
+#define CONVEYOR_BELT_H
+
+class ConveyorBelt : public Test
+{
+public:
+
+	ConveyorBelt()
+	{
+		{
+			// Ground
+			b3BodyDef bd;
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			body->CreateShape(sd);
+		}
+
+		{
+			// Platform
+			b3BodyDef bd;
+			bd.position.Set(0.0f, 5.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			m_platformHull.SetExtents(10.0f, 0.5f, 2.0f);
+
+			b3HullShape hs;
+			hs.m_hull = &m_platformHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			sd.friction = 0.8f;
+			m_platform = body->CreateShape(sd);
+		}
+
+		// Boxes
+		m_boxHull.SetExtents(0.5f, 0.5f, 0.5f);
+		for (u32 i = 0; i < 5; ++i)
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(2.0f * i, 7.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			sd.density = 0.2f;
+			
+			body->CreateShape(sd);
+		}
+	}
+
+	void PreSolve(b3Contact* contact)
+	{
+		Test::PreSolve(contact);
+
+		b3Shape* shapeA = contact->GetShapeA();
+		b3Shape* shapeB = contact->GetShapeB();
+
+		if (shapeA == m_platform)
+		{
+			for (u32 i = 0; i < contact->GetManifoldCount(); ++i)
+			{
+				b3Manifold* manifold = contact->GetManifold(i);
+
+				manifold->motorSpeed = 0.25f * B3_PI;
+				manifold->tangentSpeed2 = -2.0f;
+			}
+		}
+
+		if (shapeB == m_platform)
+		{
+			for (u32 i = 0; i < contact->GetManifoldCount(); ++i)
+			{
+				b3Manifold* manifold = contact->GetManifold(i);
+
+				manifold->motorSpeed = -0.25f * B3_PI;
+				manifold->tangentSpeed2 = 2.0f;
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new ConveyorBelt();
+	}
+
+	b3BoxHull m_platformHull;
+	b3Shape* m_platform;
+	b3BoxHull m_boxHull;
+};
+
+#endif

examples/testbed/tests/deep_capsule.h

@@ -24,23 +24,17 @@ class DeepCapsule : public Collide
 public:
 	DeepCapsule()
 	{
-		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
-		m_xfA.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
-		
-		m_sA.m_centers[0].Set(1.0f, -1.0f, 0.0f);
-		m_sA.m_centers[1].Set(0.0f, 1.0f, 0.0f);
-		m_sA.m_radius = 2.0f;
+		m_box.SetExtents(4.0f, 1.0f, 4.0f);
+		m_sA.m_hull = &m_box;
 
-		m_xfB.position.Set(0.f, 0.0f, 0.0f);
-		m_xfB.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
+		m_xfA.SetIdentity();
 
-		b3Transform xf;
-		xf.SetIdentity();
-		xf.rotation = b3Diagonal(4.0f, 1.0f, 4.0f);
+		m_sB.m_vertex1.Set(1.0f, -1.0f, 0.0f);
+		m_sB.m_vertex2.Set(0.0f, 1.0f, 0.0f);
+		m_sB.m_radius = 2.0f;
 
-		m_box.SetTransform(xf);
-
-		m_sB.m_hull = &m_box;
+		m_xfB.translation.Set(0.0f, 0.0f, 0.0f);
+		m_xfB.rotation = b3QuatRotationZ(0.55f * B3_PI);
 
 		m_shapeA = &m_sA;
 		m_shapeB = &m_sB;
@@ -52,8 +46,8 @@ public:
 		return new DeepCapsule();
 	}
 
-	b3CapsuleShape m_sA;
-	b3HullShape m_sB;
+	b3HullShape m_sA;
+	b3CapsuleShape m_sB;
 	b3BoxHull m_box;
 };
 

examples/testbed/tests/distance_test.h

@@ -24,15 +24,11 @@ class Distance : public Test
 public:
 	Distance()
 	{	
-		m_xfA.SetIdentity();
-		m_xfA.position.Set(-5.0f, 0.0f, 0.0f);
+		m_xfA.translation.Set(-5.0f, 0.0f, 0.0f);
 		m_xfA.rotation.SetIdentity();
-		m_shapeA.m_centers[0].Set(0.0f, -2.0f, 0.0f);
-		m_shapeA.m_centers[1].Set(0.0f, 2.0f, 0.0f);
-		m_shapeA.m_radius = 1.0f;
+		m_shapeA.m_hull = &b3BoxHull_identity;
 
-		m_xfB.SetIdentity();
-		m_xfB.position.Set(5.0f, 0.0f, 0.0f);
+		m_xfB.translation.Set(5.0f, 0.0f, 0.0f);
 		m_xfB.rotation.SetIdentity();
 		m_shapeB.m_hull = &b3BoxHull_identity;
 
@@ -79,46 +75,43 @@ public:
 	{
 		if (key == GLFW_KEY_LEFT)
 		{
-			m_xfB.position.x -= 0.05f;
+			m_xfB.translation.x -= 0.05f;
 		}
 		
 		if (key == GLFW_KEY_RIGHT)
 		{
-			m_xfB.position.x += 0.05f;
+			m_xfB.translation.x += 0.05f;
 		}
 		
 		if (key == GLFW_KEY_UP)
 		{
-			m_xfB.position.y += 0.05f;
+			m_xfB.translation.y += 0.05f;
 		}
 
 		if (key == GLFW_KEY_DOWN)
 		{
-			m_xfB.position.y -= 0.05f;
+			m_xfB.translation.y -= 0.05f;
 		}
 
 		if (key == GLFW_KEY_X)
 		{
-			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3QuatMat33(qx);
+			b3Quat qx = b3QuatRotationX(0.05f * B3_PI);
 
-			m_xfB.rotation = m_xfB.rotation * xfx;
+			m_xfB.rotation = m_xfB.rotation * qx;
 		}
 		
 		if (key == GLFW_KEY_Y)
 		{
-			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3QuatMat33(qy);
+			b3Quat qy = b3QuatRotationY(0.05f * B3_PI);
 
-			m_xfB.rotation = m_xfB.rotation * xfy;
+			m_xfB.rotation = m_xfB.rotation * qy;
 		}
 
 		if (key == GLFW_KEY_Z)
 		{
-			b3Quat qy(b3Vec3(0.0f, 0.0f, 1.0f), 0.05f * B3_PI);
-			b3Mat33 xfz = b3QuatMat33(qy);
+			b3Quat qz = b3QuatRotationZ(0.05f * B3_PI);
 
-			m_xfB.rotation = m_xfB.rotation * xfz;
+			m_xfB.rotation = m_xfB.rotation * qz;
 		}
 	}
 
@@ -127,7 +120,7 @@ public:
 		return new Distance();
 	}
 
-	b3CapsuleShape m_shapeA;
+	b3HullShape m_shapeA;
 	b3Transform m_xfA;
 	b3ShapeGJKProxy m_proxyA;
 

examples/testbed/tests/gyro_motion.h

@@ -40,14 +40,14 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
-			bdef.orientation.Set(b3Vec3(1.0f, 0.0f, 0.0f), 0.5f * B3_PI);
+			bdef.orientation.SetAxisAngle(b3Vec3(1.0f, 0.0f, 0.0f), 0.5f * B3_PI);
 			bdef.position.Set(0.0f, 10.0f, 0.0f);
 			bdef.angularVelocity.Set(0.0f, 0.0f, 4.0f * B3_PI);
 
 			b3Body* body = m_world.CreateBody(bdef);
 
 			{
-				m_rotorBox.Set(1.0f, 0.5f, 7.0f);
+				m_rotorBox.SetExtents(1.0f, 0.5f, 7.0f);
 
 				b3HullShape hull;
 				hull.m_hull = &m_rotorBox;
@@ -60,7 +60,7 @@ public:
 			}
 
 			{
-				m_cylinderHull.SetAsCylinder(0.95f, 4.0f);
+				m_cylinderHull.SetExtents(0.95f, 4.0f);
 
 				b3HullShape hull;
 				hull.m_hull = &m_cylinderHull;
@@ -86,7 +86,7 @@ public:
 	}
 
 	b3BoxHull m_rotorBox;
-	b3QHull m_cylinderHull;
+	b3CylinderHull m_cylinderHull;
 };
 
 #endif

examples/testbed/tests/hinge_chain.h

@@ -24,16 +24,10 @@ class HingeChain : public Test
 public:
 	HingeChain()
 	{
-		static b3BoxHull doorHull;
-		{
-			b3Transform xf;
-			xf.position.SetZero();
-			xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f);
-			doorHull.SetTransform(xf);
-		}
+		static b3BoxHull box(2.0f, 4.0f, 0.5f);
 
-		float32 x = -50.0f;
-		float32 y = 0.0f;
+		scalar x = -50.0f;
+		scalar y = 0.0f;
 
 		b3Body* lastHinge;
 		{
@@ -42,7 +36,7 @@ public:
 			lastHinge = m_world.CreateBody(bd);
 
 			b3HullShape hull;
-			hull.m_hull = &doorHull;
+			hull.m_hull = &box;
 
 			b3ShapeDef sdef;
 			sdef.shape = &hull;
@@ -60,7 +54,7 @@ public:
 			b3Body* hinge = m_world.CreateBody(bd);
 
 			b3HullShape hull;
-			hull.m_hull = &doorHull;
+			hull.m_hull = &box;
 			
 			b3ShapeDef sdef;
 			sdef.shape = &hull;

examples/testbed/tests/hull_collision.h

@@ -29,7 +29,7 @@ public:
 
 	HullCollision()
 	{
-		m_xfA.position.Set(0.0f, 1.5f, 0.0f);
+		m_xfA.translation.Set(0.0f, 1.5f, 0.0f);
 		m_xfA.rotation.SetIdentity();
 
 		m_xfB.SetIdentity();
@@ -53,9 +53,9 @@ public:
 	{
 		for (u32 i = 0; i < e_count; ++i)
 		{
-			float32 x = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 y = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 z = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float x = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float y = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float z = 3.0f * RandomFloat(-1.0f, 1.0f);
 
 			x = b3Clamp(x, -2.5f, 2.5f);
 			y = b3Clamp(y, -2.5f, 2.5f);
@@ -68,9 +68,9 @@ public:
 
 		for (u32 i = 0; i < e_count; ++i)
 		{
-			float32 x = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 y = 3.0f * RandomFloat(-1.0f, 1.0f);
-			float32 z = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float x = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float y = 3.0f * RandomFloat(-1.0f, 1.0f);
+			float z = 3.0f * RandomFloat(-1.0f, 1.0f);
 
 			x = b3Clamp(x, -2.5f, 2.5f);
 			y = b3Clamp(y, -2.5f, 2.5f);
@@ -98,7 +98,7 @@ public:
 		sB.m_hull = &hull2;
 		m_shapeB = &sB;
 
-		g_draw->DrawString(b3Color_white, "G - Generate a random convex hull pair");
+		g_draw->DrawString(b3Color_white, "G - Generate random convex hulls");
 		
 		Collide::Step();
 	}

examples/testbed/tests/hull_contact_test.h

@@ -53,9 +53,9 @@ public:
 			{
 				// Clamp to force coplanarities.
 				// This will stress the generation code.
-				float32 x = 3.0f * RandomFloat(-1.0f, 1.0f);
-				float32 y = 3.0f * RandomFloat(-1.0f, 1.0f);
-				float32 z = 3.0f * RandomFloat(-1.0f, 1.0f);
+				float x = 3.0f * RandomFloat(-1.0f, 1.0f);
+				float y = 3.0f * RandomFloat(-1.0f, 1.0f);
+				float z = 3.0f * RandomFloat(-1.0f, 1.0f);
 
 				x = b3Clamp(x, -1.5f, 1.5f);
 				y = b3Clamp(y, -1.5f, 1.5f);

examples/testbed/tests/initial_overlap.h

@@ -38,14 +38,7 @@ public:
 		}
 
 		b3Vec3 boxScale(1.0f, 0.5f, 2.0f);
-
-		static b3BoxHull boxHull;
-
-		b3Transform m;
-		m.rotation = b3Diagonal(boxScale.x, boxScale.y, boxScale.z);
-		m.position.SetZero();
-
-		boxHull.SetTransform(m);
+		static b3BoxHull boxHull(boxScale.x, boxScale.y, boxScale.z);
 
 		{
 			b3BodyDef bd;
@@ -70,8 +63,8 @@ public:
 			bd.type = e_dynamicBody;
 			bd.position.Set(0.0f, 1.5f, 0.0f);
 
-			b3Quat q_y(b3Vec3(0.0f, 1.0f, 0.0f), 0.4f * B3_PI);
-			b3Quat q_z(b3Vec3(0.0f, 0.0f, 1.0f), 0.04f * B3_PI);
+			b3Quat q_y = b3QuatRotationY(0.4f * B3_PI);
+			b3Quat q_z = b3QuatRotationZ(0.04f * B3_PI);
 			b3Quat q = q_z * q_y;
 
 			bd.orientation = q;

examples/testbed/tests/jenga.h

@@ -45,15 +45,9 @@ public:
 
 		b3Vec3 boxScale(1.0f, 0.5f, 3.0f);
 
-		static b3BoxHull boxHull;
+		static b3BoxHull boxHull(boxScale.x, boxScale.y, boxScale.z);
 
-		b3Transform m;
-		m.rotation = b3Diagonal(boxScale.x, boxScale.y, boxScale.z);
-		m.position.SetZero();
-
-		boxHull.SetTransform(m);
-
-		float32 y = 2.0f;
+		scalar y = 2.0f;
 
 		for (u32 i = 0; i < e_layerCount / 2; ++i)
 		{
@@ -62,7 +56,7 @@ public:
 				b3BodyDef bd;
 				bd.type = b3BodyType::e_dynamicBody;
 
-				bd.position.x = 2.0f * float32(j) * boxScale.x;
+				bd.position.x = 2.0f * scalar(j) * boxScale.x;
 				bd.position.y = y;
 				bd.position.z = 0.0f;
 				
@@ -86,11 +80,11 @@ public:
 				b3BodyDef bd;
 				bd.type = b3BodyType::e_dynamicBody;
 
-				bd.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.5f * B3_PI);
+				bd.orientation = b3QuatRotationY(0.5f * B3_PI);
 
 				bd.position.x = 2.0f * boxScale.x;
 				bd.position.y = y;
-				bd.position.z = -2.0f * boxScale.x + 2.0f * float32(j) * boxScale.x;
+				bd.position.z = -2.0f * boxScale.x + 2.0f * scalar(j) * boxScale.x;
 				
 				b3Body* body = m_world.CreateBody(bd);
 

examples/testbed/tests/linear_motion.h

@@ -34,8 +34,8 @@ public:
 		m_body = m_world.CreateBody(bdef);
 
 		b3CapsuleShape shape;
-		shape.m_centers[0].Set(0.0f, 1.0f, 0.0f);
-		shape.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+		shape.m_vertex1.Set(0.0f, 1.0f, 0.0f);
+		shape.m_vertex2.Set(0.0f, -1.0f, 0.0f);
 		shape.m_radius = 1.0f;
 
 		b3ShapeDef sdef;
@@ -55,16 +55,11 @@ public:
 	{
 		Test::Step();
 
-		b3Vec3 q(0.0f, 0.0f, 0.0f);
-		b3Vec3 p = m_body->GetTransform().position;
-		if (b3Distance(p, q) > 50.0f)
+		b3Vec3 p(0.0f, 0.0f, 0.0f);
+		if (b3Distance(m_body->GetPosition(), p) > 50.0f)
 		{
-			b3Quat quat = m_body->GetSweep().orientation;
-
-			b3Vec3 axis;
-			float32 angle;
-			quat.GetAxisAngle(&axis, &angle);
-			m_body->SetTransform(q, axis, angle);
+			b3Quat q = m_body->GetOrientation();
+			m_body->SetTransform(p, q);
 		}
 	}
 

examples/testbed/tests/linear_time_of_impact.h

@@ -0,0 +1,171 @@
+/*
+* 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 LINEAR_TIME_OF_IMPACT_H
+#define LINEAR_TIME_OF_IMPACT_H
+
+class LinearTimeOfImpact : public Test
+{
+public:
+	LinearTimeOfImpact()
+	{
+		m_shapeA.m_hull = &b3BoxHull_identity;
+		m_shapeA.m_radius = 0.0f;
+
+		m_shapeB.m_hull = &b3BoxHull_identity;
+		m_shapeB.m_radius = 0.0f;
+
+		m_xfA.translation.Set(0.0f, 0.0f, 0.0f);
+		m_xfA.rotation.SetIdentity();
+
+		m_xfB.translation.Set(5.0f, 1.0f, 0.0f);
+		m_xfB.rotation.SetIdentity();
+		
+		m_proxyA.Set(&m_shapeA, 0);
+		m_proxyB.Set(&m_shapeB, 0);
+	}
+
+	void Step()
+	{
+		b3Vec3 dA = b3Vec3_zero;
+		b3Vec3 dB(-10.0f, 0.0f, 0.0f);
+		
+		b3TOIOutput out = b3TimeOfImpact(m_xfA, m_proxyA, dA, m_xfB, m_proxyB, dB);
+
+		b3TOIOutput::State state = out.state;
+		scalar t = out.t;
+		u32 iterations = out.iterations;
+
+		if (state == b3TOIOutput::e_touching)
+		{
+			b3Transform xfA;
+			xfA.rotation = m_xfA.rotation;
+			xfA.translation = m_xfA.translation + t * dA;
+
+			b3Transform xfB;
+			xfB.rotation = m_xfB.rotation;
+			xfB.translation = m_xfB.translation + t * dB;
+
+			b3GJKOutput query = b3GJK(xfA, m_proxyA, xfB, m_proxyB, false);
+			
+			b3Vec3 p1 = query.point1;
+			b3Vec3 p2 = query.point2;
+			b3Vec3 n1 = b3Normalize(p2 - p1);
+			b3Vec3 p = 0.5f * (p1 + p2);
+
+			g_draw->DrawPoint(p, 4.0f, b3Color_green);
+			g_draw->DrawSegment(p1, p1 + n1, b3Color_green);
+
+			m_world.DrawShape(xfA, &m_shapeA, b3Color_black);
+			m_world.DrawShape(xfB, &m_shapeB, b3Color_black);
+		}
+		
+		if (state == b3TOIOutput::e_failed)
+		{
+			g_draw->DrawString(b3Color_white, "State = Failed");
+		}
+		else if (state == b3TOIOutput::e_overlapped)
+		{
+			g_draw->DrawString(b3Color_white, "State = Overlapped");
+		}
+		else if (state == b3TOIOutput::e_separated)
+		{
+			g_draw->DrawString(b3Color_white, "State = Separated!");
+		}
+		else if (state == b3TOIOutput::e_touching)
+		{
+			g_draw->DrawString(b3Color_white, "State = Touching!");
+		}
+
+		g_draw->DrawString(b3Color_white, "Iterations = %d", out.iterations);
+
+		g_draw->DrawString(b3Color_white, "Left/Right/Up/Down Arrow - Translate shape");
+		g_draw->DrawString(b3Color_white, "X/Y/Z - Rotate shape");
+
+		g_draw->DrawTransform(m_xfA);
+		g_draw->DrawTransform(m_xfB);
+
+		m_world.DrawShape(m_xfA, &m_shapeA, b3Color_black);
+		m_world.DrawShape(m_xfB, &m_shapeB, b3Color_black);
+
+		m_world.DrawSolidShape(m_xfA, &m_shapeA, b3Color_white);
+		m_world.DrawSolidShape(m_xfB, &m_shapeB, b3Color_white);
+
+		g_draw->DrawSegment(m_xfA.translation, m_xfA.translation + dA, b3Color_white);
+		g_draw->DrawSegment(m_xfB.translation, m_xfB.translation + dB, b3Color_white);
+	}
+
+	void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_xfB.translation.x -= 0.105f;
+		}
+
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_xfB.translation.x += 0.105f;
+		}
+
+		if (key == GLFW_KEY_UP)
+		{
+			m_xfB.translation.y += 0.105f;
+		}
+
+		if (key == GLFW_KEY_DOWN)
+		{
+			m_xfB.translation.y -= 0.105f;
+		}
+
+		if (key == GLFW_KEY_X)
+		{
+			b3Quat qx = b3QuatRotationX(0.05f * B3_PI);
+
+			m_xfB.rotation = m_xfB.rotation * qx;
+		}
+
+		if (key == GLFW_KEY_Y)
+		{
+			b3Quat qy = b3QuatRotationY(0.05f * B3_PI);
+
+			m_xfB.rotation = m_xfB.rotation * qy;
+		}
+
+		if (key == GLFW_KEY_Z)
+		{
+			b3Quat qz = b3QuatRotationZ(0.05f * B3_PI);
+
+			m_xfB.rotation = m_xfB.rotation * qz;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new LinearTimeOfImpact();
+	}
+
+	b3HullShape m_shapeA;
+	b3Transform m_xfA;
+	b3ShapeGJKProxy m_proxyA;
+
+	b3HullShape m_shapeB;
+	b3Transform m_xfB;
+	b3ShapeGJKProxy m_proxyB;
+};
+
+#endif

examples/testbed/tests/mesh_contact_test.h

@@ -25,6 +25,7 @@ public:
 	MeshContactTest()
 	{
 		m_gridMesh.BuildTree();
+		m_gridMesh.BuildAdjacency();
 
 		// Transform grid into a terrain
 		for (u32 i = 0; i < m_terrainMesh.vertexCount; ++i)
@@ -33,18 +34,22 @@ public:
 		}
 
 		m_terrainMesh.BuildTree();
+		m_terrainMesh.BuildAdjacency();
 
 		{
 			b3BodyDef bd;
-			m_ground = m_world.CreateBody(bd);
+			b3Body* groundBody = m_world.CreateBody(bd);
 
 			b3MeshShape ms;
 			ms.m_mesh = &m_gridMesh;
+			ms.m_scale.Set(2.0f, 1.0f, 2.0f);
 
 			b3ShapeDef sd;
 			sd.shape = &ms;
 
-			m_ground->CreateShape(sd);
+			m_groundShape = (b3MeshShape*)groundBody->CreateShape(sd);
+			
+			m_selection = m_groundShape->m_mesh->triangleCount / 2;
 		}
 
 		{
@@ -52,8 +57,8 @@ public:
 			bd.type = b3BodyType::e_dynamicBody;
 			bd.position.Set(0.0f, 5.0f, 0.0f);
 
-			m_body = m_world.CreateBody(bd);
-			
+			b3Body* body = m_world.CreateBody(bd);
+
 			{
 				b3SphereShape sphere;
 				sphere.m_center.SetZero();
@@ -64,25 +69,43 @@ public:
 				sd.density = 1.0f;
 				sd.friction = 0.5f;
 
-				m_body->CreateShape(sd);
+				m_bodyShape = body->CreateShape(sd);
 			}
 		}
+
+		m_drawEdgeTypes = true;
 	}
 
 	void KeyDown(int key)
 	{
+		u32 minSelection = 0;
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_selection = m_selection == minSelection ? minSelection : m_selection - 1;
+		}
+
+		u32 maxSelection = m_groundShape->m_mesh->triangleCount - 1;
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_selection = m_selection == maxSelection ? maxSelection : m_selection + 1;
+		}
+
+		if (key == GLFW_KEY_E)
+		{
+			m_drawEdgeTypes = !m_drawEdgeTypes;
+		}
+
 		if (key == GLFW_KEY_S || key == GLFW_KEY_C || key == GLFW_KEY_H)
 		{
-			if (m_body)
-			{
-				m_world.DestroyBody(m_body);
-			}
+			b3Body* body = m_bodyShape->GetBody();
+
+			m_world.DestroyBody(body);
 
 			b3BodyDef bd;
 			bd.type = b3BodyType::e_dynamicBody;
 			bd.position.Set(0.0f, 5.0f, 0.0f);
 
-			m_body = m_world.CreateBody(bd);
+			body = m_world.CreateBody(bd);
 
 			if (key == GLFW_KEY_S)
 			{
@@ -95,14 +118,14 @@ public:
 				sd.density = 1.0f;
 				sd.friction = 0.5f;
 
-				m_body->CreateShape(sd);
+				m_bodyShape = body->CreateShape(sd);
 			}
 
 			if (key == GLFW_KEY_C)
 			{
 				b3CapsuleShape capsule;
-				capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
-				capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+				capsule.m_vertex1.Set(0.0f, -1.0f, 0.0f);
+				capsule.m_vertex2.Set(0.0f, 1.0f, 0.0f);
 				capsule.m_radius = 1.0f;
 
 				b3ShapeDef sd;
@@ -110,7 +133,7 @@ public:
 				sd.density = 1.0f;
 				sd.friction = 0.5f;
 
-				m_body->CreateShape(sd);
+				m_bodyShape = body->CreateShape(sd);
 			}
 
 			if (key == GLFW_KEY_H)
@@ -123,41 +146,43 @@ public:
 				sd.density = 1.0f;
 				sd.friction = 0.5f;
 
-				m_body->CreateShape(sd);
+				m_bodyShape = body->CreateShape(sd);
 			}
 		}
 
 		if (key == GLFW_KEY_G || key == GLFW_KEY_T)
 		{
-			if (m_ground)
-			{
-				m_world.DestroyBody(m_ground);
-			}
+			b3Body* groundBody = m_groundShape->GetBody();
+			m_world.DestroyBody(groundBody);
 
 			b3BodyDef bd;
-			m_ground = m_world.CreateBody(bd);
+			groundBody = m_world.CreateBody(bd);
 
 			if (key == GLFW_KEY_G)
 			{
 				b3MeshShape ms;
 				ms.m_mesh = &m_gridMesh;
+				ms.m_scale.Set(2.0f, 1.0f, 2.0f);
 
 				b3ShapeDef sd;
 				sd.shape = &ms;
 
-				m_ground->CreateShape(sd);
+				m_groundShape = (b3MeshShape*)groundBody->CreateShape(sd);
 			}
 
 			if (key == GLFW_KEY_T)
 			{
 				b3MeshShape ms;
 				ms.m_mesh = &m_terrainMesh;
+				ms.m_scale.Set(2.0f, 1.5f, 2.0f);
 
 				b3ShapeDef sd;
 				sd.shape = &ms;
 
-				m_ground->CreateShape(sd);
+				m_groundShape = (b3MeshShape*)groundBody->CreateShape(sd);
 			}
+			
+			m_selection = m_groundShape->m_mesh->triangleCount / 2;
 		}
 	}
 
@@ -165,6 +190,108 @@ public:
 	{
 		Test::Step();
 
+		const b3Mesh* mesh = m_groundShape->m_mesh;
+		b3Vec3 scale = m_groundShape->m_scale;
+		b3Body* body = m_groundShape->GetBody();
+		b3Transform xf = body->GetTransform();
+
+		{
+			const b3MeshTriangle* triangle = mesh->triangles + m_selection;
+			const b3MeshTriangleWings* triangleWings = mesh->triangleWings + m_selection;
+
+			for (u32 i = 0; i < 3; ++i)
+			{
+				u32 j = i + 1 < 3 ? i + 1 : 0;
+
+				u32 v1 = triangle->GetVertex(i);
+				u32 v2 = triangle->GetVertex(j);
+
+				b3Vec3 p1 = xf * b3MulCW(scale, mesh->vertices[v1]);
+				b3Vec3 p2 = xf * b3MulCW(scale, mesh->vertices[v2]);
+
+				b3Vec3 center = scalar(0.5) * (p1 + p2);
+				g_draw->DrawString(b3Color_white, center, "e%d", i);
+
+				u32 wingVertex = triangleWings->GetVertex(i);
+
+				if (wingVertex != B3_NULL_VERTEX)
+				{
+					b3Vec3 vertex = xf * b3MulCW(scale, mesh->vertices[wingVertex]);
+					g_draw->DrawString(b3Color_white, vertex, "u%d", i);
+				}
+			}
+		}
+
+		if (m_drawEdgeTypes)
+		{
+			b3Vec3 eyePoint(0.0f, 10.0f, 0.0f);
+
+			for (u32 i = 0; i < mesh->triangleCount; ++i)
+			{
+				b3MeshTriangle* triangle = mesh->triangles + i;
+				b3MeshTriangleWings* triangleWings = mesh->triangleWings + i;
+
+				b3Vec3 A = xf * b3MulCW(scale, mesh->vertices[triangle->v1]);
+				b3Vec3 B = xf * b3MulCW(scale, mesh->vertices[triangle->v2]);
+				b3Vec3 C = xf * b3MulCW(scale, mesh->vertices[triangle->v3]);
+
+				b3Vec3 N = b3Cross(B - A, C - A);
+				N.Normalize();
+
+				b3Plane plane(N, A);
+				if (b3Distance(eyePoint, plane) < 0.0f)
+				{
+					plane = b3Plane(-N, A);
+				}
+
+				for (u32 j = 0; j < 3; ++j)
+				{
+					u32 k = j + 1 < 3 ? j + 1 : 0;
+
+					u32 v1 = triangle->GetVertex(j);
+					u32 v2 = triangle->GetVertex(k);
+
+					u32 u = triangleWings->GetVertex(j);
+
+					b3Vec3 p1 = xf * b3MulCW(scale, mesh->vertices[v1]);
+					b3Vec3 p2 = xf * b3MulCW(scale, mesh->vertices[v2]);
+
+					b3Vec3 center = scalar(0.5) * (p1 + p2);
+
+					if (u == B3_NULL_VERTEX)
+					{
+						g_draw->DrawPoint(center, scalar(4), b3Color_white);
+						continue;
+					}
+
+					b3Vec3 wingVertex = xf * b3MulCW(scale, mesh->vertices[u]);
+
+					scalar d = b3Distance(wingVertex, plane);
+
+					const scalar kCoplanarTol = 0.005f;
+
+					if (d < -kCoplanarTol)
+					{
+						// Below <=> Convex
+						g_draw->DrawPoint(center, scalar(4), b3Color_green);
+					}
+					else if (d > kCoplanarTol)
+					{
+						// Above <=> Concave
+						g_draw->DrawPoint(center, scalar(4), b3Color_yellow);
+					}
+					else
+					{
+						// d > -e && d < e
+						// On <=> Coplanar
+						g_draw->DrawPoint(center, scalar(4), b3Color_red);
+					}
+				}
+			}
+		}
+
+		g_draw->DrawString(b3Color_white, "E - View Edge Types");
+		g_draw->DrawString(b3Color_white, "Arrows - Select Face Wings");
 		g_draw->DrawString(b3Color_white, "S - Sphere");
 		g_draw->DrawString(b3Color_white, "C - Capsule");
 		g_draw->DrawString(b3Color_white, "H - Hull");
@@ -177,11 +304,14 @@ public:
 		return new MeshContactTest();
 	}
 
+	bool m_drawEdgeTypes;
+	u32 m_selection;
+
 	b3GridMesh<25, 25> m_terrainMesh;
 	b3GridMesh<25, 25> m_gridMesh;
 
-	b3Body* m_ground;
-	b3Body* m_body;
+	b3MeshShape* m_groundShape;
+	b3Shape* m_bodyShape;
 };
 
 #endif

examples/testbed/tests/motor_test.h

@@ -0,0 +1,123 @@
+/*
+* 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 MOTOR_TEST_H
+#define MOTOR_TEST_H
+
+class MotorTest : public Test
+{
+public:
+
+	MotorTest()
+	{
+		b3Body* ground = nullptr;
+		{
+			// Ground
+			b3BodyDef bd;
+			ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		{
+			// Motorized body
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(0.0f, 8.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			m_boxHull.SetExtents(2.0f, 0.5f, 0.5f);
+
+			b3HullShape shape;
+			shape.m_hull = &m_boxHull;
+
+			b3ShapeDef sd;
+			sd.shape = &shape;
+			sd.friction = 0.3f;
+			sd.density = 2.0f;
+			body->CreateShape(sd);
+
+			b3MotorJointDef mjd;
+			mjd.Initialize(ground, body);
+			mjd.maxForce = 1000.0f;
+			mjd.maxTorque = 1000.0f;
+			m_joint = (b3MotorJoint*)m_world.CreateJoint(mjd);
+		}
+
+		m_play = false;
+		m_x = 0.0f;
+	}
+	
+	void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_S)
+		{
+			m_play = !m_play;
+		}
+	}
+
+	void Step()
+	{
+		if (m_play)
+		{
+			m_x += g_testSettings->inv_hertz;
+
+			if (m_x >= 2.0f * B3_PI)
+			{
+				m_x = 0.0f;
+			}
+		}
+		
+		b3Vec3 linearOffset;
+		linearOffset.x = 8.0f * sinf(2.0f * m_x);
+		linearOffset.y = 8.0f + sinf(m_x);
+		linearOffset.z = 0.0f;
+
+		m_joint->SetLinearOffset(linearOffset);
+
+		b3Quat angularOffset;
+		angularOffset.SetAxisAngle(b3Vec3_z, m_x);
+		angularOffset.Normalize();
+
+		m_joint->SetAngularOffset(angularOffset);
+
+		Test::Step();
+
+		g_draw->DrawPoint(linearOffset, 4.0f, b3Color(0.9f, 0.9f, 0.9f));
+
+		g_draw->DrawString(b3Color_white, "S - Play/Pause");
+	}
+
+	static Test* Create()
+	{
+		return new MotorTest();
+	}
+	
+	b3BoxHull m_boxHull;
+	b3MotorJoint* m_joint;
+	bool m_play;
+	scalar m_x;
+};
+
+#endif

examples/testbed/tests/multiple_pendulum.h

@@ -39,8 +39,8 @@ public:
 			bs[1] = m_world.CreateBody(bd);
 
 			b3CapsuleShape s;
-			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
-			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_vertex1.Set(0.5f, 0.0f, 0.0f);
+			s.m_vertex2.Set(-0.5f, 0.0f, 0.0f);
 			s.m_radius = 0.05f;
 
 			b3ShapeDef sd;
@@ -60,8 +60,8 @@ public:
 			bs[2] = m_world.CreateBody(bd);
 
 			b3CapsuleShape s;
-			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
-			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_vertex1.Set(0.5f, 0.0f, 0.0f);
+			s.m_vertex2.Set(-0.5f, 0.0f, 0.0f);
 			s.m_radius = 0.05f;
 
 			b3ShapeDef sd;
@@ -81,8 +81,8 @@ public:
 			bs[3] = m_world.CreateBody(bd);
 
 			b3CapsuleShape s;
-			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
-			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_vertex1.Set(0.5f, 0.0f, 0.0f);
+			s.m_vertex2.Set(-0.5f, 0.0f, 0.0f);
 			s.m_radius = 0.05f;
 
 			b3ShapeDef sd;
@@ -102,8 +102,8 @@ public:
 			bs[4] = m_world.CreateBody(bd);
 
 			b3CapsuleShape s;
-			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
-			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_vertex1.Set(0.5f, 0.0f, 0.0f);
+			s.m_vertex2.Set(-0.5f, 0.0f, 0.0f);
 			s.m_radius = 0.05f;
 
 			b3ShapeDef sd;
@@ -123,8 +123,8 @@ public:
 			bs[5] = m_world.CreateBody(bd);
 
 			b3CapsuleShape s;
-			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
-			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_vertex1.Set(0.5f, 0.0f, 0.0f);
+			s.m_vertex2.Set(-0.5f, 0.0f, 0.0f);
 			s.m_radius = 0.05f;
 
 			b3ShapeDef sd;

examples/testbed/tests/newton_cradle.h

@@ -38,15 +38,15 @@ public:
 		}
 
 		b3CapsuleShape edge;
-		edge.m_centers[0].Set(0.0f, -10.0f, 0.0f);
-		edge.m_centers[1].Set(0.0f, 10.0f, 0.0f);
+		edge.m_vertex1.Set(0.0f, -10.0f, 0.0f);
+		edge.m_vertex2.Set(0.0f, 10.0f, 0.0f);
 		edge.m_radius = 0.5f;
 
 		b3Body* frame1, *frame2;
 
 		{
 			b3BodyDef bd;
-			bd.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.orientation = b3QuatRotationZ(0.5f * B3_PI);
 			bd.position.Set(0.0f, 10.0f, -5.0f);
 
 			frame1 = m_world.CreateBody(bd);
@@ -59,7 +59,7 @@ public:
 
 		{
 			b3BodyDef bd;
-			bd.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.orientation = b3QuatRotationZ(0.5f * B3_PI);
 			bd.position.Set(0.0f, 10.0f, 5.0f);
 
 			frame2 = m_world.CreateBody(bd);

examples/testbed/tests/node_types.h

@@ -0,0 +1,222 @@
+/*
+* 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 NODE_TYPES_H
+#define NODE_TYPES_H
+
+class NodeTypes : public Test
+{
+public:
+	enum
+	{
+		e_w = 5,
+		e_h = 2,
+		e_d = 2
+	};
+
+	NodeTypes()
+	{
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.2f;
+		def.E = 1000.0f;
+		def.nu = 0.33f;
+		def.radius = 0.2f;
+		def.friction = 0.6f;
+
+		m_body = new b3SoftBody(def);
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+
+		for (u32 i = 0; i < e_h + 1; ++i)
+		{
+			for (u32 k = 0; k < e_d + 1; ++k)
+			{
+				u32 v = m_mesh.GetVertex(i, 0, k);
+
+				b3SoftBodyNode* n = m_body->GetNode(v);
+				n->SetType(e_staticSoftBodyNode);
+			}
+		}
+
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~NodeTypes()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		scalar E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+
+		g_draw->DrawString(b3Color_white, "S - Static");
+		g_draw->DrawString(b3Color_white, "D - Dynamic");
+		g_draw->DrawString(b3Color_white, "K - Kinematic");
+		g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position");
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	void SetBodyType(b3SoftBodyNodeType type)
+	{
+		for (u32 i = 0; i < e_h + 1; ++i)
+		{
+			for (u32 k = 0; k < e_d + 1; ++k)
+			{
+				u32 v = m_mesh.GetVertex(i, 0, k);
+
+				b3SoftBodyNode* n = m_body->GetNode(v);
+				n->SetType(type);
+			}
+		}
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_S)
+		{
+			SetBodyType(e_staticSoftBodyNode);
+		}
+
+		if (button == GLFW_KEY_K)
+		{
+			SetBodyType(e_kinematicSoftBodyNode);
+		}
+
+		if (button == GLFW_KEY_D)
+		{
+			SetBodyType(e_dynamicSoftBodyNode);
+		}
+
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			b3SoftBodyNode* n = m_body->GetNode(i);
+
+			b3Vec3 d;
+			d.SetZero();
+
+			if (button == GLFW_KEY_LEFT)
+			{
+				d.x = -1.0f;
+			}
+
+			if (button == GLFW_KEY_RIGHT)
+			{
+				d.x = 1.0f;
+			}
+
+			if (button == GLFW_KEY_UP)
+			{
+				d.y = 1.0f;
+			}
+
+			if (button == GLFW_KEY_DOWN)
+			{
+				d.y = -1.0f;
+			}
+
+			if (button == GLFW_KEY_LEFT ||
+				button == GLFW_KEY_RIGHT ||
+				button == GLFW_KEY_UP ||
+				button == GLFW_KEY_DOWN)
+			{
+				if (n->GetType() == e_staticSoftBodyNode)
+				{
+					n->ApplyTranslation(d);
+				}
+
+				if (n->GetType() == e_kinematicSoftBodyNode)
+				{
+					b3Vec3 v = n->GetVelocity();
+
+					v += 5.0f * d;
+
+					n->SetVelocity(v);
+				}
+
+				if (n->GetType() == e_dynamicSoftBodyNode)
+				{
+					b3Vec3 f = 100.0f * d;
+
+					n->ApplyForce(f);
+				}
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new NodeTypes();
+	}
+
+	b3BlockSoftBodyMesh<e_w, e_h, e_d> m_mesh;
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+};
+
+#endif

examples/testbed/tests/particle_types.h

@@ -36,10 +36,21 @@ public:
 		g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position");
 	}
 
-	void SetClothType(b3ParticleType type)
+	void SetClothType(b3ClothParticleType type)
 	{
-		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
+		for (u32 j = 0; j < e_w + 1; ++j)
 		{
+			u32 v = m_clothMesh.GetVertex(0, j);
+
+			b3ClothParticle* p = m_cloth->GetParticle(v);
+			p->SetType(type);
+		}
+
+		for (u32 j = 0; j < e_w + 1; ++j)
+		{
+			u32 v = m_clothMesh.GetVertex(e_h, j);
+
+			b3ClothParticle* p = m_cloth->GetParticle(v);
 			p->SetType(type);
 		}
 	}
@@ -48,20 +59,20 @@ public:
 	{
 		if (button == GLFW_KEY_S)
 		{
-			SetClothType(e_staticParticle);
+			SetClothType(e_staticClothParticle);
 		}
 
 		if (button == GLFW_KEY_K)
 		{
-			SetClothType(e_kinematicParticle);
+			SetClothType(e_kinematicClothParticle);
 		}
 
 		if (button == GLFW_KEY_D)
 		{
-			SetClothType(e_dynamicParticle);
+			SetClothType(e_dynamicClothParticle);
 		}
 
-		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
+		for (b3ClothParticle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
 		{
 			b3Vec3 d;
 			d.SetZero();
@@ -91,12 +102,12 @@ public:
 				button == GLFW_KEY_UP ||
 				button == GLFW_KEY_DOWN)
 			{
-				if (p->GetType() == e_staticParticle)
+				if (p->GetType() == e_staticClothParticle)
 				{
 					p->ApplyTranslation(d);
 				}
 
-				if (p->GetType() == e_kinematicParticle)
+				if (p->GetType() == e_kinematicClothParticle)
 				{
 					b3Vec3 v = p->GetVelocity();
 
@@ -105,7 +116,7 @@ public:
 					p->SetVelocity(v);
 				}
 
-				if (p->GetType() == e_dynamicParticle)
+				if (p->GetType() == e_dynamicClothParticle)
 				{
 					b3Vec3 f = 100.0f * d;
 

examples/testbed/tests/pinned_cloth.h

@@ -22,6 +22,12 @@
 class PinnedCloth : public Test
 {
 public:
+	enum
+	{
+		e_w = 10,
+		e_h = 10
+	};
+
 	PinnedCloth()
 	{
 		// Create cloth
@@ -33,30 +39,24 @@ public:
 		m_cloth = new b3Cloth(def);
 
 		m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
-		m_cloth->SetWorld(&m_world);
 
 		// Freeze some particles
-		b3AABB3 aabb1;
-		aabb1.m_lower.Set(-5.0f, -1.0f, -6.0f);
-		aabb1.m_upper.Set(5.0f, 1.0f, -4.0f);
-
-		b3AABB3 aabb2;
-		aabb2.m_lower.Set(-5.0f, -1.0f, 4.0f);
-		aabb2.m_upper.Set(5.0f, 1.0f, 6.0f);
-
-		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
+		for (u32 j = 0; j < e_w + 1; ++j)
 		{
-			if (aabb1.Contains(p->GetPosition()))
-			{
-				p->SetType(e_staticParticle);
-			}
+			u32 v = m_clothMesh.GetVertex(0, j);
 
-			if (aabb2.Contains(p->GetPosition()))
-			{
-				p->SetType(e_staticParticle);
-			}
+			b3ClothParticle* p = m_cloth->GetParticle(v);
+			p->SetType(e_staticClothParticle);
 		}
-		
+
+		for (u32 j = 0; j < e_w + 1; ++j)
+		{
+			u32 v = m_clothMesh.GetVertex(e_h, j);
+
+			b3ClothParticle* p = m_cloth->GetParticle(v);
+			p->SetType(e_staticClothParticle);
+		}
+
 		m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
 	}
 
@@ -79,9 +79,9 @@ public:
 			b3Vec3 pA = m_clothDragger->GetPointA();
 			b3Vec3 pB = m_clothDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
@@ -89,7 +89,7 @@ public:
 		extern u32 b3_clothSolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
 
-		float32 E = m_cloth->GetEnergy();
+		scalar E = m_cloth->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
 	}
 
@@ -128,7 +128,7 @@ public:
 		return new PinnedCloth();
 	}
 
-	b3GridClothMesh<10, 10> m_clothMesh;
+	b3GridClothMesh<e_w, e_h> m_clothMesh;
 	b3Cloth* m_cloth;
 	b3ClothDragger* m_clothDragger;
 };

examples/testbed/tests/pinned_softbody.h

@@ -19,14 +19,12 @@
 #ifndef PINNED_SOFTBODY_H
 #define PINNED_SOFTBODY_H
 
-#include <testbed/framework/softbody_dragger.h>
-
 class PinnedSoftBody : public Test
 {
 public:
 	PinnedSoftBody()
 	{
-		m_mesh.SetAsSphere(5.0f, 0);
+		m_mesh.SetAsSphere(4.0f, 0);
 
 		// Create soft body
 		b3SoftBodyDef def;
@@ -37,20 +35,15 @@ public:
 		def.c_yield = 0.1f;
 		def.c_creep = 0.5f;
 		def.c_max = 1.0f;
+		def.massDamping = 0.2f;
 
 		m_body = new b3SoftBody(def);
 
-		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
-		{
-			b3SoftBodyNode* n = m_body->GetVertexNode(i);
-			n->SetMassDamping(0.2f);
-		}
-
 		u32 pinIndex = ~0;
-		float32 pinDot = -B3_MAX_FLOAT;
+		scalar pinDot = -B3_MAX_SCALAR;
 		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
 		{
-			float32 dot = b3Dot(m_mesh.vertices[i], b3Vec3_y);
+			scalar dot = b3Dot(m_mesh.vertices[i], b3Vec3_y);
 			if (dot > pinDot)
 			{
 				pinDot = dot;
@@ -58,7 +51,7 @@ public:
 			}
 		}
 
-		b3SoftBodyNode* pinNode = m_body->GetVertexNode(pinIndex);
+		b3SoftBodyNode* pinNode = m_body->GetNode(pinIndex);
 		pinNode->SetType(e_staticSoftBodyNode);
 
 		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
@@ -91,9 +84,9 @@ public:
 			b3Vec3 pA = m_bodyDragger->GetPointA();
 			b3Vec3 pB = m_bodyDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
@@ -101,7 +94,7 @@ public:
 		extern u32 b3_softBodySolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
 
-		float32 E = m_body->GetEnergy();
+		scalar E = m_body->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
 	}
 

examples/testbed/tests/prismatic_test.h

@@ -0,0 +1,135 @@
+/*
+* 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 PRISMATIC_TEST_H
+#define PRISMATIC_TEST_H
+
+class PrismaticTest : public Test
+{
+public:
+	PrismaticTest()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape shape;
+			shape.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &shape;
+
+			ground->CreateShape(sd);
+		}
+
+		b3Body* bA, * bB;
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-5.0f, 5.0f, 0.0f);
+
+			bA = m_world.CreateBody(bd);
+
+			m_hullA.SetExtents(2.0f, 2.0f, 0.5f);
+
+			b3HullShape hull;
+			hull.m_hull = &m_hullA;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hull;
+			sdef.density = 1.0f;
+
+			bA->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(5.0f, 5.0f, 0.0f);
+
+			bB = m_world.CreateBody(bd);
+
+			m_hullB.SetExtents(2.0f, 2.0f, 0.5f);
+
+			b3HullShape hull;
+			hull.m_hull = &m_hullB;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hull;
+			sdef.density = 1.0f;
+
+			bB->CreateShape(sdef);
+		}
+
+		// Create prismatic joint
+		{
+			b3Vec3 anchor(0.0f, 5.0f, 0.0f);
+			b3Vec3 axis(1.0f, 0.0f, 0.0f);
+
+			b3PrismaticJointDef jd;
+			jd.Initialize(bA, bB, anchor, axis);
+			jd.motorSpeed = 10.0f;
+			jd.maxMotorForce = 10000.0f;
+			jd.enableMotor = true;
+			jd.lowerTranslation = 0.0f;
+			jd.upperTranslation = 10.0f;
+			jd.enableLimit = true;
+
+			m_joint = (b3PrismaticJoint*)m_world.CreateJoint(jd);
+		}
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		g_draw->DrawString(b3Color_white, "L - Enable Limit");
+		g_draw->DrawString(b3Color_white, "M - Enable Motor");
+		g_draw->DrawString(b3Color_white, "S - Flip Motor Speed");
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_L)
+		{
+			m_joint->EnableLimit(!m_joint->IsLimitEnabled());
+		}
+	
+		if (button == GLFW_KEY_M)
+		{
+			m_joint->EnableMotor(!m_joint->IsMotorEnabled());
+		}
+	
+		if (button == GLFW_KEY_S)
+		{
+			m_joint->SetMotorSpeed(-m_joint->GetMotorSpeed());
+		}
+	}
+
+	static Test* Create()
+	{
+		return new PrismaticTest();
+	}
+
+	b3BoxHull m_hullA;
+	b3BoxHull m_hullB;
+	b3PrismaticJoint* m_joint;
+};
+
+#endif

examples/testbed/tests/pyramid.h

@@ -47,9 +47,9 @@ public:
 
 		// shift to ground center
 		b3Vec3 translation;
-		translation.x = -0.5f * float32(e_count) * boxSize.x;
+		translation.x = -0.5f * scalar(e_count) * boxSize.x;
 		translation.y = 1.5f * boxSize.y;
-		translation.z = -0.5f * float32(e_count) * boxSize.z;
+		translation.z = -0.5f * scalar(e_count) * boxSize.z;
 
 		u32 count = e_count;
 		for (u32 i = 0; i < e_count; ++i)
@@ -61,9 +61,9 @@ public:
 				{
 					b3BodyDef bd;
 					bd.type = b3BodyType::e_dynamicBody;
-					bd.position.x = 1.05f * float32(j) * boxSize.x;
+					bd.position.x = 1.05f * scalar(j) * boxSize.x;
 					bd.position.y = 0.0f;
-					bd.position.z = 1.05f * float32(k) * boxSize.z;
+					bd.position.z = 1.05f * scalar(k) * boxSize.z;
 					bd.position += translation;
 
 					b3Body* body = m_world.CreateBody(bd);

examples/testbed/tests/pyramids.h

@@ -48,15 +48,15 @@ public:
 
 		// shift to ground center
 		b3Vec3 translation;
-		translation.x = -0.5f * float32(e_count - 1) * 4.0f * boxSize.x;
+		translation.x = -0.5f * scalar(e_count - 1) * 4.0f * boxSize.x;
 		translation.y = 1.5f * boxSize.y;
-		translation.z = -0.5f * float32(e_depthCount) * boxSize.z;
+		translation.z = -0.5f * scalar(e_depthCount) * boxSize.z;
 		
 		for (u32 i = 0; i < e_count; ++i)
 		{
 			// reset 
 			translation.y = 1.5f * boxSize.y;
-			translation.z = -0.5f * float32(e_depthCount) * boxSize.z;
+			translation.z = -0.5f * scalar(e_depthCount) * boxSize.z;
 
 			for (u32 j = 0; j < e_depthCount; ++j)
 			{
@@ -66,7 +66,7 @@ public:
 					bd.type = e_dynamicBody;
 					bd.position.x = 0.0f;
 					bd.position.y = 0.0f;
-					bd.position.z = 1.05f * float32(k) * boxSize.z;
+					bd.position.z = 1.05f * scalar(k) * boxSize.z;
 					bd.position += translation;
 
 					b3Body* body = m_world.CreateBody(bd);

examples/testbed/tests/quadric_shapes.h

@@ -64,8 +64,8 @@ public:
 			b3Body* body = m_world.CreateBody(bdef);
 
 			b3CapsuleShape capsule;
-			capsule.m_centers[0].Set(0.0f, 0.0f, -1.0f);
-			capsule.m_centers[1].Set(0.0f, 0.0f, 1.0f);
+			capsule.m_vertex1.Set(0.0f, 0.0f, -1.0f);
+			capsule.m_vertex2.Set(0.0f, 0.0f, 1.0f);
 			capsule.m_radius = 1.0f;
 
 			b3ShapeDef sdef;
@@ -83,7 +83,7 @@ public:
 
 			b3Body* body = m_world.CreateBody(bdef);
 
-			m_coneHull.SetAsCone();
+			m_coneHull.SetExtents(1.0f, 1.0f);
 
 			b3HullShape hull;
 			hull.m_hull = &m_coneHull;
@@ -103,7 +103,7 @@ public:
 
 			b3Body* body = m_world.CreateBody(bdef);
 
-			m_cylinderHull.SetAsCylinder();
+			m_cylinderHull.SetExtents(1.0f, 1.0f);
 
 			b3HullShape hull;
 			hull.m_hull = &m_cylinderHull;
@@ -126,8 +126,8 @@ public:
 		return new QuadricShapes();
 	}
 
-	b3QHull m_coneHull;
-	b3QHull m_cylinderHull;
+	b3ConeHull m_coneHull;
+	b3CylinderHull m_cylinderHull;
 };
 
 #endif

examples/testbed/tests/ragdoll.h

@@ -52,8 +52,8 @@ public:
 			hip->ApplyForceToCenter(b3Vec3(0.0f, 0.0f, -5000.0f), true);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 0.5f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -0.5f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 0.5f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -0.5f, 0.0f);
 			cs.m_radius = 1.0f;
 
 			b3ShapeDef sd;
@@ -70,8 +70,8 @@ public:
 			head = m_world.CreateBody(bd);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 0.15f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -0.15f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 0.15f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -0.15f, 0.0f);
 			cs.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -86,7 +86,7 @@ public:
 			cd.bodyA = hip;
 			cd.bodyB = head;
 			cd.collideLinked = false;
-			cd.enableLimit = true;
+			cd.enableConeLimit = true;
 			cd.Initialize(hip, head, b3Vec3(0.0f, 1.0f, 0.0f), b3Vec3(0.0f, 11.55f, 0.0f), 0.25f * B3_PI);
 			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
 		}
@@ -95,12 +95,12 @@ public:
 			b3BodyDef bd;
 			bd.type = e_dynamicBody;
 			bd.position.Set(-2.5f, 11.0f, 0.0f);
-			bd.orientation.Set(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.orientation = b3QuatRotationZ(0.5f * B3_PI);
 			lArm = m_world.CreateBody(bd);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 1.0f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 1.0f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -1.0f, 0.0f);
 			cs.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -116,7 +116,7 @@ public:
 			cd.bodyA = hip;
 			cd.bodyB = lArm;
 			cd.collideLinked = false;
-			cd.enableLimit = true;
+			cd.enableConeLimit = true;
 			cd.Initialize(hip, lArm, b3Vec3(-1.0f, 0.0f, 0.0f), b3Vec3(-1.0f, 11.0f, 0.0f), B3_PI);
 			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
 		}
@@ -125,12 +125,12 @@ public:
 			b3BodyDef bd;
 			bd.type = e_dynamicBody;
 			bd.position.Set(2.5f, 11.0f, 0.0f);
-			bd.orientation.Set(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.orientation.SetAxisAngle(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
 			rArm = m_world.CreateBody(bd);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 1.0f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 1.0f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -1.0f, 0.0f);
 			cs.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -146,7 +146,7 @@ public:
 			cd.bodyA = hip;
 			cd.bodyB = rArm;
 			cd.collideLinked = false;
-			cd.enableLimit = true;
+			cd.enableConeLimit = true;
 			cd.Initialize(hip, rArm, b3Vec3(1.0f, 0.0f, 0.0f), b3Vec3(1.0f, 11.0f, 0.0f), B3_PI);
 			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
 		}
@@ -158,8 +158,8 @@ public:
 			lLeg = m_world.CreateBody(bd);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 2.0f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 2.0f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -2.0f, 0.0f);
 			cs.m_radius = 0.45f;
 
 			b3ShapeDef sd;
@@ -175,7 +175,7 @@ public:
 			cd.bodyA = hip;
 			cd.bodyB = lLeg;
 			cd.collideLinked = false;
-			cd.enableLimit = true;
+			cd.enableConeLimit = true;
 			cd.Initialize(hip, lLeg, b3Vec3(0.0f, -1.0f, 0.0f), b3Vec3(-0.5f, 8.5f, 0.0f), 0.25f * B3_PI);
 			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
 		}
@@ -187,8 +187,8 @@ public:
 			rLeg = m_world.CreateBody(bd);
 
 			b3CapsuleShape cs;
-			cs.m_centers[0].Set(0.0f, 2.0f, 0.0f);
-			cs.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cs.m_vertex1.Set(0.0f, 2.0f, 0.0f);
+			cs.m_vertex2.Set(0.0f, -2.0f, 0.0f);
 			cs.m_radius = 0.45f;
 
 			b3ShapeDef sd;
@@ -204,7 +204,7 @@ public:
 			cd.bodyA = hip;
 			cd.bodyB = rLeg;
 			cd.collideLinked = false;
-			cd.enableLimit = true;
+			cd.enableConeLimit = true;
 			cd.Initialize(hip, rLeg, b3Vec3(0.0f, -1.0f, 0.0f), b3Vec3(0.5f, 8.5f, 0.0f), 0.25f * B3_PI);
 			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
 		}

examples/testbed/tests/ray_cast.h

@@ -159,8 +159,8 @@ public:
 			b3Body* body = m_world.CreateBody(bdef);
 
 			b3CapsuleShape hs;
-			hs.m_centers[0].Set(0.0f, 1.0f, 0.0f);
-			hs.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			hs.m_vertex1.Set(0.0f, 1.0f, 0.0f);
+			hs.m_vertex2.Set(0.0f, -1.0f, 0.0f);
 			hs.m_radius = 3.0f;
 
 			b3ShapeDef sdef;
@@ -175,11 +175,22 @@ public:
 		m_p12.Set(0.0f, 2.0f, 0.0f);
 		m_p22.Set(-50.0f, 2.0f, 0.0f);
 	}
-
+	
 	void CastRay(const b3Vec3 p1, const b3Vec3 p2) const
 	{
+		class RayCastFilter : public b3RayCastFilter
+		{
+		public:
+			bool ShouldRayCast(b3Shape* shape)
+			{
+				return true;
+			}
+		};
+
+		RayCastFilter filter;
+
 		b3RayCastSingleOutput out;
-		if (m_world.RayCastSingle(&out, p1, p2))
+		if (m_world.RayCastSingle(&out, &filter, p1, p2))
 		{
 			g_draw->DrawSegment(p1, out.point, b3Color_green);
 			
@@ -194,7 +205,7 @@ public:
 
 	void Step()
 	{
-		float32 dt = g_testSettings->inv_hertz;
+		scalar dt = g_testSettings->inv_hertz;
 		b3Quat dq = b3QuatRotationY(0.05f * B3_PI * dt);
 		
 		m_p1 = b3Mul(dq, m_p1);

examples/testbed/tests/revolute_test.h

@@ -16,13 +16,13 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef HINGE_MOTOR_H
-#define HINGE_MOTOR_H
+#ifndef REVOLUTE_TEST_H
+#define REVOLUTE_TEST_H
 
-class HingeMotor : public Test
+class RevoluteTest : public Test
 {
 public:
-	HingeMotor()
+	RevoluteTest()
 	{
 		{
 			b3BodyDef bd;
@@ -46,8 +46,8 @@ public:
 			hinge = m_world.CreateBody(bd);
 
 			b3CapsuleShape shape;
-			shape.m_centers[0].Set(0.0f, 0.0f, -4.0f);
-			shape.m_centers[1].Set(0.0f, 0.0f, 4.0f);
+			shape.m_vertex1.Set(0.0f, 0.0f, -4.0f);
+			shape.m_vertex2.Set(0.0f, 0.0f, 4.0f);
 			shape.m_radius = 0.5f;
 			
 			b3ShapeDef sd;
@@ -66,7 +66,7 @@ public:
 
 			door = m_world.CreateBody(bd);
 
-			m_doorBox.Set(1.0f, 0.5f, 4.0f);
+			m_doorBox.SetExtents(1.0f, 0.5f, 4.0f);
 
 			b3HullShape hull;
 			hull.m_hull = &m_doorBox;
@@ -78,23 +78,34 @@ public:
 			door->CreateShape(sdef);
 		}
 		
+
 		{
 			b3Vec3 axis(0.0f, 0.0f, 1.0f);
 			b3Vec3 anchor(0.0f, 7.0f, 0.0f);
 			
 			b3RevoluteJointDef jd;
-			jd.Initialize(hinge, door, axis, anchor, 0.0f, B3_PI);
+			jd.Initialize(hinge, door, axis, anchor, -0.25f * B3_PI, 0.5f * B3_PI);
+			jd.maxMotorTorque = 1000.0f;
+			jd.enableMotor = false;
+			jd.enableLimit = true;
 			jd.motorSpeed = B3_PI;
-			jd.maxMotorTorque = door->GetMass() * 10000.0f;
-			jd.enableMotor = true;
 
 			m_rj = (b3RevoluteJoint*)m_world.CreateJoint(jd);
 		}
 
 		// Invalidate the orientation
-		b3Vec3 axis(1.0f, 0.0f, 0.0f);
-		float32 angle = B3_PI;
-		door->SetTransform(door->GetPosition(), axis, angle);		
+		b3Quat q = b3QuatRotationX(B3_PI);
+		door->SetTransform(door->GetPosition(), q);		
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		g_draw->DrawString(b3Color_white, "M - Motor");
+		g_draw->DrawString(b3Color_white, "L - Limits");
+		g_draw->DrawString(b3Color_white, "S/K/D - Static/kinematic/dynamic body");
+
 	}
 
 	void KeyDown(int button)
@@ -127,7 +138,7 @@ public:
 
 	static Test* Create()
 	{
-		return new HingeMotor();
+		return new RevoluteTest();
 	}
 
 	b3BoxHull m_doorBox;

examples/testbed/tests/rope_test.h

@@ -30,7 +30,7 @@ public:
 	Rope()
 	{
 		b3Vec3 vs[e_count];
-		float32 ms[e_count];
+		scalar ms[e_count];
 		
 		vs[0].Set(0.0f, 0.0f, 0.0f);
 		ms[0] = 0.0f;
@@ -38,7 +38,7 @@ public:
 		for (u32 i = 1; i < e_count; ++i)
 		{
 			ms[i] = 1.0f;
-			vs[i].Set(float32(i), 0.0f, 0.0f);
+			vs[i].Set(scalar(i), 0.0f, 0.0f);
 		}
 
 		b3RopeDef rd;

examples/testbed/tests/sensor_test.h

@@ -63,8 +63,8 @@ public:
 			m_character = m_world.CreateBody(bd);
 
 			b3CapsuleShape cap;
-			cap.m_centers[0].Set(0.0f, 2.0f, 0.0f);
-			cap.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cap.m_vertex1.Set(0.0f, 2.0f, 0.0f);
+			cap.m_vertex2.Set(0.0f, -2.0f, 0.0f);
 			cap.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -129,7 +129,7 @@ public:
 			bd.type = b3BodyType::e_dynamicBody;
 			bd.position.Set(RandomFloat(-20.0f, 20.0f), RandomFloat(10.0f, 20.0f), RandomFloat(-20.0f, 20.0f));
 
-			b3Vec3 n = m_character->GetTransform().position - bd.position;
+			b3Vec3 n = m_character->GetTransform().translation - bd.position;
 			n.Normalize();
 
 			bd.linearVelocity = 60.0f * n;

examples/testbed/tests/shape_cast.h

@@ -1,121 +0,0 @@
-#ifndef SHAPE_CAST_H
-#define SHAPE_CAST_H
-
-class ShapeCast : public Test
-{
-public:
-	ShapeCast()
-	{
-		m_shapeA.m_hull = &b3BoxHull_identity;
-		m_shapeA.m_radius = 0.0f;
-
-		m_shapeB.m_hull = &b3BoxHull_identity;
-		m_shapeB.m_radius = 0.0f;
-
-		m_xfA.position.Set(-5.0f, 0.0f, 0.0f);
-		m_xfA.rotation.SetIdentity();
-
-		m_xfB.position.Set(10.0f, 0.0f, 0.0f);
-		m_xfB.rotation.SetIdentity();
-		
-		m_proxyA.Set(&m_shapeA, 0);
-		m_proxyB.Set(&m_shapeB, 0);
-	}
-
-	void Step()
-	{
-		g_draw->DrawString(b3Color_white, "Left/Right/Up/Down Arrow - Translate shape");
-		g_draw->DrawString(b3Color_white, "X/Y/Z - Rotate shape");
-
-		g_draw->DrawTransform(m_xfA);
-		g_draw->DrawTransform(m_xfB);
-
-		m_world.DrawShape(m_xfA, &m_shapeA, b3Color_black);
-		m_world.DrawShape(m_xfB, &m_shapeB, b3Color_black);
-
-		m_world.DrawSolidShape(m_xfA, &m_shapeA, b3Color_white);
-		m_world.DrawSolidShape(m_xfB, &m_shapeB, b3Color_white);
-
-		b3Vec3 translationB = -100.0f * b3Vec3_x;
-		g_draw->DrawSegment(m_xfB.position, m_xfB.position + translationB, b3Color_white);
-
-		b3GJKShapeCastOutput out;
-		bool hit = b3GJKShapeCast(&out, m_xfA, m_proxyA, m_xfB, m_proxyB, translationB);
-
-		g_draw->DrawString(b3Color_white, "Iterations = %d", out.iterations);
-
-		if (hit)
-		{
-			g_draw->DrawPoint(out.point, 4.0f, b3Color_green);
-			g_draw->DrawSegment(out.point, out.point + out.normal, b3Color_green);
-
-			b3Transform xfB;
-			xfB.rotation = m_xfB.rotation;
-			xfB.position = m_xfB.position + out.t * translationB;
-
-			m_world.DrawShape(xfB, &m_shapeB, b3Color_black);
-		}
-	}
-
-	void KeyDown(int key)
-	{
-		if (key == GLFW_KEY_LEFT)
-		{
-			m_xfB.position.x -= 0.105f;
-		}
-
-		if (key == GLFW_KEY_RIGHT)
-		{
-			m_xfB.position.x += 0.105f;
-		}
-
-		if (key == GLFW_KEY_UP)
-		{
-			m_xfB.position.y += 0.105f;
-		}
-
-		if (key == GLFW_KEY_DOWN)
-		{
-			m_xfB.position.y -= 0.105f;
-		}
-
-		if (key == GLFW_KEY_X)
-		{
-			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3QuatMat33(qx);
-
-			m_xfB.rotation = m_xfB.rotation * xfx;
-		}
-
-		if (key == GLFW_KEY_Y)
-		{
-			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3QuatMat33(qy);
-
-			m_xfB.rotation = m_xfB.rotation * xfy;
-		}
-
-		if (key == GLFW_KEY_Z)
-		{
-			b3Quat qy(b3Vec3(0.0f, 0.0f, 1.0f), 0.05f * B3_PI);
-			b3Mat33 xfz = b3QuatMat33(qy);
-
-			m_xfB.rotation = m_xfB.rotation * xfz;
-		}
-	}
-
-	static Test* Create()
-	{
-		return new ShapeCast();
-	}
-
-	b3HullShape m_shapeA;
-	b3Transform m_xfA;
-	b3ShapeGJKProxy m_proxyA;
-
-	b3HullShape m_shapeB;
-	b3Transform m_xfB;
-	b3ShapeGJKProxy m_proxyB;
-};
-
-#endif

examples/testbed/tests/shape_stack.h

@@ -26,7 +26,6 @@ public:
 	{
 		{
 			b3BodyDef bd;
-			bd.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.18f * B3_PI);
 			b3Body* ground = m_world.CreateBody(bd);
 
 			b3MeshShape ms;
@@ -39,7 +38,7 @@ public:
 		}
 
 
-		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		for (scalar y = 2.5f; y < 20.0f; y += 2.5f)
 		{
 			b3BodyDef bd;
 			bd.type = b3BodyType::e_dynamicBody;
@@ -59,18 +58,18 @@ public:
 			body->CreateShape(sdef);
 		}
 
-		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		for (scalar y = 2.5f; y < 20.0f; y += 2.5f)
 		{
 			b3BodyDef bd;
 			bd.type = b3BodyType::e_dynamicBody;
 			bd.position.Set(0.0f, y, 0.0f);
-			bd.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.orientation = b3QuatRotationZ(0.5f * B3_PI);
 
 			b3Body* body = m_world.CreateBody(bd);
 
 			b3CapsuleShape capsule;
-			capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
-			capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+			capsule.m_vertex1.Set(0.0f, -1.0f, 0.0f);
+			capsule.m_vertex2.Set(0.0f, 1.0f, 0.0f);
 			capsule.m_radius = 1.0f;
 
 			b3ShapeDef sd;
@@ -81,7 +80,7 @@ public:
 			body->CreateShape(sd);
 		}
 		
-		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		for (scalar y = 2.5f; y < 20.0f; y += 2.5f)
 		{
 			b3BodyDef bd;
 			bd.type = b3BodyType::e_dynamicBody;

examples/testbed/tests/sheet.h

@@ -0,0 +1,136 @@
+/*
+* 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 SHEET_H
+#define SHEET_H
+
+class Sheet : public Test
+{
+public:
+	enum
+	{
+		e_w = 10,
+		e_h = 1,
+		e_d = 10
+	};
+
+	Sheet()
+	{
+		// Downscale the block along the y axis
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			m_mesh.vertices[i].y *= 0.5f;
+		}
+
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.3f;
+		def.E = 200.0f;
+		def.nu = 0.3f;
+
+		m_body = new b3SoftBody(def);
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+
+		for (u32 j = 0; j < e_w + 1; ++j)
+		{
+				u32 v = m_mesh.GetVertex(0, j, 0);
+
+				b3SoftBodyNode* n = m_body->GetNode(v);
+				n->SetType(e_staticSoftBodyNode);
+		}
+
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~Sheet()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		scalar E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Sheet();
+	}
+
+	b3BlockSoftBodyMesh<e_w, e_h, e_d> m_mesh;
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+};
+
+#endif

examples/testbed/tests/sheet_stack.h

@@ -24,17 +24,15 @@ class SheetStack : public Test
 public:
 	enum
 	{
-		e_rowCount = 1,
-		e_columnCount = 10,
-		e_depthCount = 1
+		e_h = 5,
+		e_w = 1,
+		e_d = 1
 	};
 
 	SheetStack()
 	{
 		{
 			b3BodyDef bdef;
-			bdef.type = b3BodyType::e_staticBody;
-
 			b3Body* body = m_world.CreateBody(bdef);
 
 			b3HullShape hs;
@@ -44,49 +42,87 @@ public:
 			sdef.shape = &hs;
 			sdef.friction = 1.0f;
 
-			b3Shape* shape = body->CreateShape(sdef);
+			body->CreateShape(sdef);
 		}
-		
-		static b3Vec3 sheetExtents(4.05f, 2.0f * B3_LINEAR_SLOP, 4.05f);
-		static b3BoxHull sheetHull(sheetExtents.x, sheetExtents.y, sheetExtents.z);
 
-		b3Vec3 stackOrigin;
-		stackOrigin.Set(0.0f, 4.05f, 0.0f);
+		b3Vec3 e(4.0f, 2.0f * B3_LINEAR_SLOP, 4.0f);
 
-		for (u32 i = 0; i < e_rowCount; ++i)
+		m_boxHull.SetExtents(e.x, e.y, e.z);
+
+		b3Vec3 separation;
+		separation.x = 1.0f;
+		separation.y = 1.0f;
+		separation.z = 1.0f;
+
+		b3Vec3 scale;
+		scale.x = 2.0f * e.x + separation.x;
+		scale.y = 2.0f * e.y + separation.y;
+		scale.z = 2.0f * e.z + separation.z;
+
+		b3Vec3 size;
+		size.x = 2.0f * e.x + scale.x * scalar(e_w - 1);
+		size.y = 2.0f * e.y + scale.y * scalar(e_h - 1);
+		size.z = 2.0f * e.z + scale.z * scalar(e_d - 1);
+
+		b3Vec3 translation;
+		translation.x = e.x - 0.5f * size.x;
+		translation.y = e.y - 0.5f * size.y;
+		translation.z = e.z - 0.5f * size.z;
+
+		translation.y += 9.0f;
+
+		for (u32 i = 0; i < e_h; ++i)
 		{
-			for (u32 j = 0; j < e_columnCount; ++j)
+			for (u32 j = 0; j < e_w; ++j)
 			{
-				for (u32 k = 0; k < e_depthCount; ++k)
+				for (u32 k = 0; k < e_d; ++k)
 				{
 					b3BodyDef bdef;
-					bdef.type = b3BodyType::e_dynamicBody;
+					bdef.type = e_dynamicBody;
 
-					bdef.position.x = float32(i) * sheetExtents.x;
-					bdef.position.y = float32(j) * 50.0f * sheetExtents.y;
-					bdef.position.z = float32(k) * sheetExtents.z;
-					bdef.position += stackOrigin;
+					bdef.position.Set(scalar(j), scalar(i), scalar(k));
+
+					bdef.position.x *= scale.x;
+					bdef.position.y *= scale.y;
+					bdef.position.z *= scale.z;
+
+					bdef.position += translation;
 
 					b3Body* body = m_world.CreateBody(bdef);
 
 					b3HullShape hs;
-					hs.m_hull = &sheetHull;
+					hs.m_hull = &m_boxHull;
 
 					b3ShapeDef sdef;
+					sdef.density = 0.1f;
+					sdef.friction = 0.3f;
 					sdef.shape = &hs;
-					sdef.density = 0.5f;
-					sdef.friction = 0.2f;
-					
+
 					body->CreateShape(sdef);
+
+					u32 bodyIndex = GetBodyIndex(i, j, k);
+
+					m_bodies[bodyIndex] = body;
 				}
 			}
 		}
 	}
 
+	u32 GetBodyIndex(u32 i, u32 j, u32 k)
+	{
+		B3_ASSERT(i < e_h);
+		B3_ASSERT(j < e_w);
+		B3_ASSERT(k < e_d);
+		return k + e_d * (j + e_w * i);
+	}
+
 	static Test* Create()
 	{
 		return new SheetStack();
 	}
+
+	b3BoxHull m_boxHull;
+	b3Body* m_bodies[e_h * e_w * e_d];
 };
 
 #endif

examples/testbed/tests/smash_softbody.h

@@ -19,8 +19,6 @@
 #ifndef SMASH_SOFTBODY_H
 #define SMASH_SOFTBODY_H
 
-#include <testbed/framework/softbody_dragger.h>
-
 class SmashSoftBody : public Test
 {
 public:
@@ -42,20 +40,13 @@ public:
 		def.c_yield = 0.6f;
 		def.c_creep = 1.0f;
 		def.c_max = 1.0f;
+		def.radius = 0.05f;
+		def.friction = 0.2f;
 
 		m_body = new b3SoftBody(def);
 
 		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
 		m_body->SetGravity(gravity);
-		m_body->SetWorld(&m_world);
-
-		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
-		{
-			b3SoftBodyNode* n = m_body->GetVertexNode(i);
-
-			n->SetRadius(0.05f);
-			n->SetFriction(0.2f);
-		}
 
 		// Create ground
 		{
@@ -71,30 +62,14 @@ public:
 			sd.shape = &groundShape;
 			sd.friction = 0.3f;
 
-			b->CreateShape(sd);
+			b3Shape* s = b->CreateShape(sd);
+
+			b3SoftBodyWorldShapeDef ssd;
+			ssd.shape = s;
+
+			m_body->CreateWorldShape(ssd); 
 		}
 
-		// Create body
-		{
-			b3BodyDef bd;
-			bd.type = e_dynamicBody;
-			bd.position.y = 10.0f;
-
-			b3Body* b = m_world.CreateBody(bd);
-
-			static b3BoxHull boxHull(5.0f, 1.0f, 5.0f);
-
-			b3HullShape boxShape;
-			boxShape.m_hull = &boxHull;
-
-			b3ShapeDef sd;
-			sd.shape = &boxShape;
-			sd.density = 0.1f;
-			sd.friction = 0.3f;
-
-			b->CreateShape(sd);
-		}
-		
 		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
 	}
 
@@ -122,9 +97,9 @@ public:
 			b3Vec3 pA = m_bodyDragger->GetPointA();
 			b3Vec3 pB = m_bodyDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
@@ -132,7 +107,7 @@ public:
 		extern u32 b3_softBodySolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
 
-		float32 E = m_body->GetEnergy();
+		scalar E = m_body->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
 	}
 

examples/testbed/tests/softbody_anchor.h

@@ -0,0 +1,177 @@
+/*
+* 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 SOFTBODY_ANCHOR_H
+#define SOFTBODY_ANCHOR_H
+
+class SoftBodyAnchor : public Test
+{
+public:
+	SoftBodyAnchor()
+	{
+		m_mesh.SetAsSphere(4.0f, 0);
+
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.2f;
+		def.E = 1000.0f;
+		def.nu = 0.33f;
+		def.c_yield = 0.1f;
+		def.c_creep = 0.5f;
+		def.c_max = 1.0f;
+		def.massDamping = 0.2f;
+
+		m_body = new b3SoftBody(def);
+
+		u32 pinIndex = ~0;
+		scalar pinDot = -B3_MAX_SCALAR;
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			scalar dot = b3Dot(m_mesh.vertices[i], b3Vec3_y);
+			if (dot > pinDot)
+			{
+				pinDot = dot;
+				pinIndex = i;
+			}
+		}
+
+		b3SoftBodyNode* pinNode = m_body->GetNode(pinIndex);
+		pinNode->SetType(e_staticSoftBodyNode);
+
+		u32 anchorIndex = ~0;
+		scalar anchorDot = -B3_MAX_SCALAR;
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			scalar dot = b3Dot(m_mesh.vertices[i], -b3Vec3_y);
+			if (dot > anchorDot)
+			{
+				anchorDot = dot;
+				anchorIndex = i;
+			}
+		}
+		
+		b3SoftBodyNode* anchorNode = m_body->GetNode(anchorIndex);
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.y = -10.0f;
+			
+			b3Body* b = m_world.CreateBody(bd);
+
+			b3CapsuleShape cs;
+			cs.m_vertex1.Set(0.0f, -1.0f, 0.0f);
+			cs.m_vertex2.Set(0.0f, 1.0f, 0.0f);
+			cs.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 0.1f;
+
+			m_shape = b->CreateShape(sd);
+
+			// Create anchor
+			b3SoftBodyAnchorDef ad;
+			ad.Initialize(b, anchorNode, anchorNode->GetPosition());
+			
+			m_body->CreateAnchor(ad);
+		}
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~SoftBodyAnchor()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		scalar E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new SoftBodyAnchor();
+	}
+
+	b3QSoftBodyMesh m_mesh;
+
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+	b3Shape* m_shape;
+};
+
+#endif

examples/testbed/tests/sphere_stack.h

@@ -24,70 +24,103 @@ class SphereStack : public Test
 public:
 	enum
 	{
-		e_rowCount = 1,
-		e_columnCount = 5,
-		e_depthCount = 1
+		e_h = 5,
+		e_w = 1,
+		e_d = 1
 	};
 
 	SphereStack()
 	{
 		{
-			b3BodyDef bd;
-			bd.type = e_staticBody;
-			b3Body* ground = m_world.CreateBody(bd);
+			b3BodyDef bdef;
+			b3Body* body = m_world.CreateBody(bdef);
 
 			b3HullShape hs;
 			hs.m_hull = &m_groundHull;
-			
-			b3ShapeDef sd;
-			sd.shape = &hs;
-			sd.density = 0.0f;
-			sd.friction = 1.0f;
-			sd.restitution = 0.0f;
-			
-			b3Shape* groundShape = ground->CreateShape(sd);
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 1.0f;
+
+			body->CreateShape(sdef);
 		}
 
-		b3Vec3 stackOrigin;
-		stackOrigin.Set(0.0f, 5.0f, 0.0f);
-		float32 radius = 1.0f;
-		float32 diameter = 2.0f * radius;
+		scalar e = 1.0f;
 
-		for (u32 i = 0; i < e_rowCount; ++i)
+		b3SphereShape sphere;
+		sphere.m_center.SetZero();
+		sphere.m_radius = e;
+
+		b3Vec3 separation;
+		separation.x = 1.0f;
+		separation.y = 1.0f;
+		separation.z = 1.0f;
+
+		b3Vec3 scale;
+		scale.x = 2.0f * e + separation.x;
+		scale.y = 2.0f * e + separation.y;
+		scale.z = 2.0f * e + separation.z;
+
+		b3Vec3 size;
+		size.x = 2.0f * e + scale.x * scalar(e_w - 1);
+		size.y = 2.0f * e + scale.y * scalar(e_h - 1);
+		size.z = 2.0f * e + scale.z * scalar(e_d - 1);
+
+		b3Vec3 translation;
+		translation.x = e - 0.5f * size.x;
+		translation.y = e - 0.5f * size.y;
+		translation.z = e - 0.5f * size.z;
+
+		translation.y += 9.0f;
+
+		for (u32 i = 0; i < e_h; ++i)
 		{
-			for (u32 j = 0; j < e_columnCount; ++j)
+			for (u32 j = 0; j < e_w; ++j)
 			{
-				for (u32 k = 0; k < e_depthCount; ++k)
+				for (u32 k = 0; k < e_d; ++k)
 				{
 					b3BodyDef bdef;
-					bdef.type = b3BodyType::e_dynamicBody;
-					bdef.position.x = float32(i) * diameter;
-					bdef.position.y = float32(j) * diameter;
-					bdef.position.z = float32(k) * diameter;
-					bdef.position += stackOrigin;
-					bdef.linearVelocity.Set(0.0f, -50.0f, 0.0f);
+					bdef.type = e_dynamicBody;
+
+					bdef.position.Set(scalar(j), scalar(i), scalar(k));
+
+					bdef.position.x *= scale.x;
+					bdef.position.y *= scale.y;
+					bdef.position.z *= scale.z;
+
+					bdef.position += translation;
 
 					b3Body* body = m_world.CreateBody(bdef);
 
-					b3SphereShape sphere;
-					sphere.m_center.SetZero();
-					sphere.m_radius = radius;
-
 					b3ShapeDef sdef;
-					sdef.shape = &sphere;
-					sdef.density = 1.0f;
+					sdef.density = 0.1f;
 					sdef.friction = 0.3f;
+					sdef.shape = &sphere;
 
-					b3Shape* shape = body->CreateShape(sdef);
+					body->CreateShape(sdef);
+
+					u32 bodyIndex = GetBodyIndex(i, j, k);
+
+					m_bodies[bodyIndex] = body;
 				}
 			}
 		}
 	}
 
+	u32 GetBodyIndex(u32 i, u32 j, u32 k)
+	{
+		B3_ASSERT(i < e_h);
+		B3_ASSERT(j < e_w);
+		B3_ASSERT(k < e_d);
+		return k + e_d * (j + e_w * i);
+	}
+
 	static Test* Create()
 	{
 		return new SphereStack();
 	}
+
+	b3Body* m_bodies[e_h * e_w * e_d];
 };
 
-#endif
+#endif

examples/testbed/tests/spring_test.h

@@ -16,13 +16,13 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef SPRING_H
-#define SPRING_H
+#ifndef SPRING_TEST_H
+#define SPRING_TEST_H
 
-class Spring : public Test
+class SpringTest : public Test
 {
 public:
-	Spring()
+	SpringTest()
 	{
 		{
 			b3BodyDef bd;
@@ -33,18 +33,12 @@ public:
 
 			b3ShapeDef sd;
 			sd.shape = &hs;
-			
+
 			ground->CreateShape(sd);
 		}
-		
-		// Car frame shape
-		{
-			b3Transform xf;
-			xf.SetIdentity();
-			xf.rotation = b3Diagonal(2.0f, 0.5f, 5.0f);
 
-			m_frameHull.SetTransform(xf);
-		}
+		// Car frame shape
+		m_frameHull.SetExtents(2.0f, 0.5f, 5.0f);
 
 		b3HullShape box;
 		box.m_hull = &m_frameHull;
@@ -60,22 +54,22 @@ public:
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
 			bdef.position.Set(0.0f, 10.0f, 0.0f);
-			
+
 			frame = m_world.CreateBody(bdef);
 
 			b3ShapeDef sdef;
 			sdef.density = 0.1f;
 			sdef.friction = 0.3f;
 			sdef.shape = &box;
-			
+
 			frame->CreateShape(sdef);
 		}
-		
+
 		b3Body* wheelLF;
 		{
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
-			bdef.position.Set(-1.0f, 7.0f, -4.5f);
+			bdef.position.Set(-1.0f, 7.0f, 4.5f);
 			bdef.fixedRotationY = true;
 
 			wheelLF = m_world.CreateBody(bdef);
@@ -84,17 +78,17 @@ public:
 			sdef.shape = &sphere;
 			sdef.density = 0.1f;
 			sdef.friction = 1.0f;
-			
+
 			wheelLF->CreateShape(sdef);
 		}
 
 		{
 			b3SpringJointDef def;
-			def.Initialize(frame, wheelLF, b3Vec3(-1.0f, 9.0f, -4.5), b3Vec3(-1.0f, 9.0f, -4.5f));
+			def.Initialize(frame, wheelLF, b3Vec3(-1.0f, 9.0f, 4.5), b3Vec3(-1.0f, 9.0f, 4.5f));
 			def.collideLinked = true;
 			def.dampingRatio = 0.5f;
 			def.frequencyHz = 4.0f;
-			
+
 			m_world.CreateJoint(def);
 		}
 
@@ -102,7 +96,7 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
-			bdef.position.Set(1.0f, 7.0, -4.5f);
+			bdef.position.Set(1.0f, 7.0, 4.5f);
 			bdef.fixedRotationY = true;
 
 			wheelRF = m_world.CreateBody(bdef);
@@ -111,25 +105,25 @@ public:
 			sdef.density = 0.1f;
 			sdef.friction = 1.0f;
 			sdef.shape = &sphere;
-			
+
 			wheelRF->CreateShape(sdef);
 		}
-		
+
 		{
 			b3SpringJointDef def;
-			def.Initialize(frame, wheelRF, b3Vec3(1.0f, 9.0, -4.5), b3Vec3(1.0f, 9.0, -4.5f));
+			def.Initialize(frame, wheelRF, b3Vec3(1.0f, 9.0, 4.5), b3Vec3(1.0f, 9.0, 4.5f));
 			def.collideLinked = true;
 			def.dampingRatio = 0.5f;
 			def.frequencyHz = 4.0f;
 
 			m_world.CreateJoint(def);
 		}
-		
+
 		b3Body* wheelLB;
 		{
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
-			bdef.position.Set(-1.0f, 7.0f, 4.5f);
+			bdef.position.Set(-1.0f, 7.0f, -4.5f);
 			bdef.fixedRotationY = true;
 
 			wheelLB = m_world.CreateBody(bdef);
@@ -144,7 +138,7 @@ public:
 
 		{
 			b3SpringJointDef def;
-			def.Initialize(frame, wheelLB, b3Vec3(-1.0f, 9.0f, 4.5f), b3Vec3(-1.0f, 9.0f, 4.5f));
+			def.Initialize(frame, wheelLB, b3Vec3(-1.0f, 9.0f, -4.5f), b3Vec3(-1.0f, 9.0f, -4.5f));
 			def.collideLinked = true;
 			def.dampingRatio = 0.8f;
 			def.frequencyHz = 4.0f;
@@ -156,7 +150,7 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.type = e_dynamicBody;
-			bdef.position.Set(1.0f, 7.0f, 4.5f);
+			bdef.position.Set(1.0f, 7.0f, -4.5f);
 			bdef.fixedRotationY = true;
 
 			wheelRB = m_world.CreateBody(bdef);
@@ -171,7 +165,7 @@ public:
 
 		{
 			b3SpringJointDef def;
-			def.Initialize(frame, wheelRB, b3Vec3(1.0f, 9.0f, 4.5f), b3Vec3(1.0f, 9.0f, 4.5f));
+			def.Initialize(frame, wheelRB, b3Vec3(1.0f, 9.0f, -4.5f), b3Vec3(1.0f, 9.0f, -4.5f));
 			def.collideLinked = true;
 			def.frequencyHz = 4.0f;
 			def.dampingRatio = 0.8f;
@@ -182,7 +176,7 @@ public:
 
 	static Test* Create()
 	{
-		return new Spring();
+		return new SpringTest();
 	}
 
 	b3BoxHull m_frameHull;

examples/testbed/tests/table_cloth.h

@@ -22,6 +22,12 @@
 class TableCloth : public Test
 {
 public:
+	enum
+	{
+		e_w = 10,
+		e_h = 10
+	};
+	
 	TableCloth()
 	{
 		// Translate the mesh
@@ -35,15 +41,13 @@ public:
 		def.mesh = &m_clothMesh;
 		def.density = 0.2f;
 		def.streching = 10000.0f;
-		//def.shearing = 10000.0f;
-		def.damping = 100.0f;
+		def.strechDamping = 100.0f;
 		def.thickness = 0.2f;
 		def.friction = 0.1f;
 
 		m_cloth = new b3Cloth(def);
 
 		m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
-		m_cloth->SetWorld(&m_world);
 
 		{
 			b3BodyDef bd;
@@ -51,7 +55,7 @@ public:
 
 			b3Body* b = m_world.CreateBody(bd);
 
-			m_tableHull.SetAsCylinder(5.0f, 2.0f);
+			m_tableHull.SetExtents(5.0f, 2.0f);
 
 			b3HullShape tableShape;
 			tableShape.m_hull = &m_tableHull;
@@ -60,7 +64,12 @@ public:
 			sd.shape = &tableShape;
 			sd.friction = 1.0f;
 
-			b->CreateShape(sd);
+			b3Shape* shape = b->CreateShape(sd);
+
+			b3ClothWorldShapeDef csd;
+			csd.shape = shape;
+
+			m_cloth->CreateWorldShape(csd);
 		}
 
 		m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
@@ -85,9 +94,9 @@ public:
 			b3Vec3 pA = m_clothDragger->GetPointA();
 			b3Vec3 pB = m_clothDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
@@ -95,7 +104,7 @@ public:
 		extern u32 b3_clothSolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
 
-		float32 E = m_cloth->GetEnergy();
+		scalar E = m_cloth->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
 	}
 
@@ -134,11 +143,11 @@ public:
 		return new TableCloth();
 	}
 
-	b3GridClothMesh<10, 10> m_clothMesh;
+	b3GridClothMesh<e_w, e_h> m_clothMesh;
 	b3Cloth* m_cloth;
 	b3ClothDragger* m_clothDragger;
 
-	b3QHull m_tableHull;
+	b3CylinderHull m_tableHull;
 };
 
 #endif

examples/testbed/tests/tension_mapping.h

@@ -21,11 +21,11 @@
 
 // Hot/Cold color map
 // See http://paulbourke.net/miscellaneous/colourspace/
-static inline b3Color Color(float32 x, float32 a, float32 b)
+static inline b3Color Color(scalar x, scalar a, scalar b)
 {
 	x = b3Clamp(x, a, b);
 
-	float32 d = b - a;
+	scalar d = b - a;
 
 	b3Color c(1.0f, 1.0f, 1.0f); 
 
@@ -58,6 +58,12 @@ static inline b3Color Color(float32 x, float32 a, float32 b)
 class TensionMapping : public Test
 {
 public:
+	enum
+	{
+		e_w = 10,
+		e_h = 10
+	};
+
 	TensionMapping()
 	{
 		// Create cloth
@@ -65,24 +71,25 @@ public:
 		def.mesh = &m_clothMesh;
 		def.density = 0.2f;
 		def.streching = 10000.0f;
-		def.shearing = 5000.0f;
-		def.damping = 100.0f;
+		def.strechDamping = 100.0f;
+		def.shearing = 1000.0f;
+		def.shearDamping = 10.0f;
+		def.bending = 1000.0f;
+		def.bendDamping = 10.0f;
 
 		m_cloth = new b3Cloth(def);
 
 		m_cloth->SetGravity(b3Vec3(0.0f, -9.8f, 0.0f));
-		m_cloth->SetWorld(&m_world);
 
 		// Freeze some particles
-		b3AABB3 aabb;
-		aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
-		aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
-
-		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
+		for (u32 i = 0; i < 2; ++i)
 		{
-			if (aabb.Contains(p->GetPosition()))
+			for (u32 j = 0; j < e_w + 1; ++j)
 			{
-				p->SetType(e_staticParticle);
+				u32 v = m_clothMesh.GetVertex(i, j);
+				
+				b3ClothParticle* p = m_cloth->GetParticle(v);
+				p->SetType(e_staticClothParticle);
 			}
 		}
 
@@ -103,8 +110,7 @@ public:
 
 		const b3ClothMesh* mesh = m_cloth->GetMesh();
 
-		b3StackArray<b3Vec3, 256> tension;
-		tension.Resize(mesh->vertexCount);
+		b3Vec3 tension[(e_h + 1) * (e_w + 1)];
 		for (u32 i = 0; i < mesh->vertexCount; ++i)
 		{
 			tension[i].SetZero();
@@ -112,66 +118,100 @@ public:
 
 		for (b3Force* f = m_cloth->GetForceList().m_head; f; f = f->GetNext())
 		{
-			if (f->GetType() == e_strechForce)
+			if (f->GetType() == e_stretchForce)
 			{
-				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;
+				b3StretchForce* s = (b3StretchForce*)f;
 
 				b3Vec3 f1 = s->GetActionForce1();
 				b3Vec3 f2 = s->GetActionForce2();
 				b3Vec3 f3 = s->GetActionForce3();
 
+				b3ClothParticle* p1 = s->GetParticle1();
+				b3ClothParticle* p2 = s->GetParticle2();
+				b3ClothParticle* p3 = s->GetParticle3();
+
+				u32 v1 = p1->GetMeshIndex();
+				u32 v2 = p2->GetMeshIndex();
+				u32 v3 = p3->GetMeshIndex();
+
 				tension[v1] += f1;
 				tension[v2] += f2;
 				tension[v3] += f3;
 			}
 		}
+		
+		for (b3ClothParticle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
+		{
+			if (p->GetType() == e_staticClothParticle)
+			{
+				b3Draw_draw->DrawPoint(p->GetPosition(), 4.0f, b3Color_white);
+			}
+
+			if (p->GetType() == e_kinematicClothParticle)
+			{
+				b3Draw_draw->DrawPoint(p->GetPosition(), 4.0f, b3Color_blue);
+			}
+
+			if (p->GetType() == e_dynamicClothParticle)
+			{
+				b3Draw_draw->DrawPoint(p->GetPosition(), 4.0f, b3Color_green);
+			}
+		}
 
 		for (u32 i = 0; i < mesh->triangleCount; ++i)
 		{
-			b3ClothMeshTriangle* t = mesh->triangles + i;
+			b3ClothMeshTriangle* triangle = mesh->triangles + i;
+
+			b3Vec3 v1 = m_cloth->GetParticle(triangle->v1)->GetPosition();
+			b3Vec3 v2 = m_cloth->GetParticle(triangle->v2)->GetPosition();
+			b3Vec3 v3 = m_cloth->GetParticle(triangle->v3)->GetPosition();
 
-			b3Vec3 v1 = m_cloth->GetParticle(t->v1)->GetPosition();
-			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;
+			scalar s = 0.9f;
 
 			v1 = s * (v1 - c) + c;
 			v2 = s * (v2 - c) + c;
 			v3 = s * (v3 - c) + c;
 
-			b3Vec3 f1 = tension[t->v1];
-			float32 L1 = b3Length(f1);
+			b3Vec3 f1 = tension[triangle->v1];
+			scalar L1 = b3Length(f1);
 
-			b3Vec3 f2 = tension[t->v2];
-			float32 L2 = b3Length(f2);
+			b3Vec3 f2 = tension[triangle->v2];
+			scalar L2 = b3Length(f2);
 
-			b3Vec3 f3 = tension[t->v3];
-			float32 L3 = b3Length(f3);
+			b3Vec3 f3 = tension[triangle->v3];
+			scalar L3 = b3Length(f3);
 
-			float32 L = (L1 + L2 + L3) / 3.0f;
+			scalar L = (L1 + L2 + L3) / 3.0f;
 
-			const float32 kMaxT = 10000.0f;
+			const scalar kMaxT = 10000.0f;
 			b3Color color = Color(L, 0.0f, kMaxT);
 			
 			b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
 			n1.Normalize();
-			g_draw->DrawSolidTriangle(n1, v1, v2, v3, color);
 
-			b3Vec3 n2 = -n1;
-			g_draw->DrawSolidTriangle(n2, v3, v2, v1, color);
+			scalar r = 0.05f;
+
+			{
+				b3Vec3 x1 = v1 + r * n1;
+				b3Vec3 x2 = v2 + r * n1;
+				b3Vec3 x3 = v3 + r * n1;
+
+				g_draw->DrawSolidTriangle(n1, x1, x2, x3, color);
+			}
+
+			{
+				b3Vec3 n2 = -n1;
+
+				b3Vec3 x1 = v1 + r * n2;
+				b3Vec3 x2 = v2 + r * n2;
+				b3Vec3 x3 = v3 + r * n2;
+
+				g_draw->DrawSolidTriangle(n2, x3, x2, x1, color);
+			}
 		}
 
 		if (m_clothDragger->IsDragging())
@@ -179,9 +219,9 @@ public:
 			b3Vec3 pA = m_clothDragger->GetPointA();
 			b3Vec3 pB = m_clothDragger->GetPointB();
 
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
 
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
 
 			g_draw->DrawSegment(pA, pB, b3Color_white);
 		}
@@ -189,7 +229,7 @@ public:
 		extern u32 b3_clothSolverIterations;
 		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
 
-		float32 E = m_cloth->GetEnergy();
+		scalar E = m_cloth->GetEnergy();
 		g_draw->DrawString(b3Color_white, "E = %f", E);
 	}
 
@@ -228,7 +268,7 @@ public:
 		return new TensionMapping();
 	}
 
-	b3GridClothMesh<10, 10> m_clothMesh;
+	b3GridClothMesh<e_w, e_h> m_clothMesh;
 	b3Cloth* m_cloth;
 	b3ClothDragger* m_clothDragger; 
 };

examples/testbed/tests/tetgen_softbody.h

@@ -0,0 +1,413 @@
+/*
+* 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 TETGEN_SOFTBODY_H
+#define TETGEN_SOFTBODY_H
+
+#include <fstream>
+
+struct TetGenMesh : public b3SoftBodyMesh
+{
+	TetGenMesh()
+	{
+		vertexCount = 0;
+		vertices = nullptr;
+		triangleCount = 0;
+		triangles = nullptr;
+		tetrahedronCount = 0;
+		tetrahedrons = nullptr;
+	}
+
+	~TetGenMesh()
+	{
+		free(vertices);
+		free(triangles);
+		free(tetrahedrons);
+	}
+
+	bool Load(const char* node_filename, const char* face_filename, const char* ele_filename)
+	{
+		{
+			std::ifstream file(node_filename);
+			if (!file.good())
+			{
+				printf("Could not open %s \n", node_filename);
+				return false;
+			}
+
+			int nodeCount, nodeDimensions, attributeCount, boundaryMarkCount;
+
+			std::string line;
+			while (std::getline(file, line))
+			{
+				if (line[0] == '#')
+				{
+					continue;
+				}
+
+				if (line[0] == 0)
+				{
+					continue;
+				}
+
+				std::stringstream line_stream(line);
+
+				line_stream >> nodeCount >> nodeDimensions >> attributeCount >> boundaryMarkCount;
+
+				break;
+			}
+
+			if (nodeDimensions != 3)
+			{
+				printf(".node file: Only 3 dimensional nodes supported\n");
+				return false;
+			}
+
+			if (attributeCount != 0)
+			{
+				printf(".node file: Only nodes with 0 attributes supported\n");
+				return false;
+			}
+
+			if (boundaryMarkCount != 0)
+			{
+				printf(".node file: Only nodes with 0 markers supported\n");
+				return false;
+			}
+
+			assert(vertexCount == 0);
+			vertices = (b3Vec3*)malloc(sizeof(b3Vec3) * nodeCount);
+
+			while (std::getline(file, line))
+			{
+				if (line[0] == '#')
+				{
+					continue;
+				}
+
+				if (line[0] == 0)
+				{
+					continue;
+				}
+
+				int nodeId;
+				float x, y, z;
+
+				std::stringstream line_stream(line);
+
+				line_stream >> nodeId >> x >> y >> z;
+
+				assert(nodeId > 0);
+				assert(nodeId <= nodeCount);
+
+				assert(b3IsValid(x));
+				assert(b3IsValid(y));
+				assert(b3IsValid(z));
+
+				vertices[vertexCount].x = x;
+				vertices[vertexCount].y = y;
+				vertices[vertexCount].z = z;
+
+				++vertexCount;
+			}
+
+			assert(vertexCount == nodeCount);
+		}
+		
+		{
+			std::ifstream file(face_filename);
+			if (!file.good())
+			{
+				printf("Could not open %s \n", face_filename);
+				return false;
+			}
+
+			int faceCount, boundaryMarkerCount;
+
+			std::string line;
+			while (std::getline(file, line))
+			{
+				if (line[0] == '#')
+				{
+					continue;
+				}
+
+				if (line[0] == 0)
+				{
+					continue;
+				}
+
+				std::stringstream line_stream(line);
+
+				line_stream >> faceCount >> boundaryMarkerCount;
+
+				break;
+			}
+
+			assert(triangleCount == 0);
+			triangles = (b3SoftBodyMeshTriangle*)malloc(sizeof(b3SoftBodyMeshTriangle) * faceCount);
+
+			while (std::getline(file, line))
+			{
+				if (line[0] == '#')
+				{
+					continue;
+				}
+
+				if (line[0] == 0)
+				{
+					continue;
+				}
+
+				int faceId;
+				int v1, v2, v3;
+				int corner;
+
+				std::stringstream line_stream(line);
+
+				line_stream >> faceId >> v1 >> v2 >> v3 >> corner;
+
+				assert(faceId > 0);
+				assert(faceId <= faceCount);
+
+				// Make CCW
+				b3Swap(v2, v3);
+
+				triangles[triangleCount].v1 = u32(v1 - 1);
+				triangles[triangleCount].v2 = u32(v2 - 1);
+				triangles[triangleCount].v3 = u32(v3 - 1);
+
+				++triangleCount;
+			}
+
+			assert(triangleCount == faceCount);
+		}
+
+		{
+			std::ifstream file(ele_filename);
+			if (!file.good())
+			{
+				printf("Could not open %s \n", ele_filename);
+				return false;
+			}
+
+			int tetCount, nodesPerTet, attributeCount;
+
+			std::string line;
+			while (std::getline(file, line))
+			{
+				if (line[0] == '#')
+				{
+					continue;
+				}
+
+				if (line[0] == 0)
+				{
+					continue;
+				}
+
+				std::stringstream line_stream(line);
+
+				line_stream >> tetCount >> nodesPerTet >> attributeCount;
+
+				break;
+			}
+
+			if (nodesPerTet != 4)
+			{
+				printf(".ele file: Only 4 nodes per tetrahedran supported\n");
+				return false;
+			}
+
+			if (attributeCount != 0)
+			{
+				printf(".ele file: Only elements with 0 attributes supported\n");
+				return false;
+			}
+
+			assert(tetrahedronCount == 0);
+			tetrahedrons = (b3SoftBodyMeshTetrahedron*)malloc(sizeof(b3SoftBodyMeshTetrahedron) * tetCount);
+
+			while (std::getline(file, line))
+			{
+				if (line[0] == '#')
+				{
+					continue;
+				}
+
+				if (line[0] == 0)
+				{
+					continue;
+				}
+
+				int tetId;
+				int v1, v2, v3, v4;
+
+				std::stringstream line_stream(line);
+
+				line_stream >> tetId >> v1 >> v2 >> v3 >> v4;
+
+				assert(tetId > 0);
+				assert(tetId <= tetCount);
+
+				// Make CCW
+				b3Swap(v2, v3);
+
+				tetrahedrons[tetrahedronCount].v1 = u32(v1 - 1);
+				tetrahedrons[tetrahedronCount].v2 = u32(v2 - 1);
+				tetrahedrons[tetrahedronCount].v3 = u32(v3 - 1);
+				tetrahedrons[tetrahedronCount].v4 = u32(v4 - 1);
+
+				++tetrahedronCount;
+			}
+
+			assert(tetrahedronCount == tetCount);
+		}
+
+		return true;
+	}
+};
+
+class TetGenSoftBody : public Test
+{
+public:
+	TetGenSoftBody()
+	{
+		{
+			bool ok = m_mesh.Load("data/octopus.node", "data/octopus.face", "data/octopus.ele");
+			assert(ok);
+		}
+
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			m_mesh.vertices[i].y += 10.0f;
+		}
+
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.2f;
+		def.E = 1000.0f;
+		def.nu = 0.3f;
+		def.radius = 0.05f;
+		def.friction = 0.2f;
+
+		m_body = new b3SoftBody(def);
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+
+		// Create ground
+		{
+			b3BodyDef bd;
+			bd.type = e_staticBody;
+
+			b3Body* b = m_world.CreateBody(bd);
+
+			b3HullShape groundShape;
+			groundShape.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &groundShape;
+			sd.friction = 0.3f;
+
+			b3Shape* s = b->CreateShape(sd);
+
+			b3SoftBodyWorldShapeDef ssd;
+			ssd.shape = s;
+
+			m_body->CreateWorldShape(ssd); 
+		}
+
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~TetGenSoftBody()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 4.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 4.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		scalar E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new TetGenSoftBody();
+	}
+
+	TetGenMesh m_mesh;
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+};
+
+#endif

examples/testbed/tests/time_of_impact.h

@@ -0,0 +1,237 @@
+/*
+* 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 TIME_OF_IMPACT_H
+#define TIME_OF_IMPACT_H
+
+class TimeOfImpact : public Test
+{
+public:
+	TimeOfImpact()
+	{
+		m_shapeA.m_hull = &b3BoxHull_identity;
+		m_shapeA.m_radius = 0.0f;
+
+		m_shapeB.m_hull = &b3BoxHull_identity;
+		m_shapeB.m_radius = 0.0f;
+
+		m_sweepA.localCenter.SetZero();
+		m_sweepA.worldCenter0.Set(0.0f, 0.0f, 5.0f);
+		m_sweepA.worldCenter.Set(0.0f, 0.0f, -5.0f);
+		m_sweepA.orientation0.SetIdentity();
+		m_sweepA.orientation.SetIdentity();
+
+		m_sweepB.localCenter.SetZero();
+		m_sweepB.worldCenter0.Set(5.0f, 0.0f, 0.0f);
+		m_sweepB.worldCenter.Set(-5.0f, 0.0f, 0.0f);
+		m_sweepB.orientation0.SetIdentity();
+		m_sweepB.orientation.SetIdentity();
+		
+		m_proxyA.Set(&m_shapeA, 0);
+		m_proxyB.Set(&m_shapeB, 0);
+
+		m_time = 0.0f;
+	}
+
+	void Step()
+	{
+		b3Color colorA0(1.0f, 0.0f, 0.0f, 1.0f);
+		b3Color colorB0(0.0f, 1.0f, 0.0f, 1.0f);
+
+		// t0
+		b3Transform xfA0 = m_sweepA.GetTransform(0.0f);
+		b3Transform xfB0 = m_sweepB.GetTransform(0.0f);
+
+		g_draw->DrawTransform(xfA0);
+		g_draw->DrawTransform(xfB0);
+
+		m_world.DrawShape(xfA0, &m_shapeA, b3Color_black);
+		m_world.DrawShape(xfB0, &m_shapeB, b3Color_black);
+
+		m_world.DrawSolidShape(xfA0, &m_shapeA, colorA0);
+		m_world.DrawSolidShape(xfB0, &m_shapeB, colorB0);
+
+		// t1
+		b3Transform xfA1 = m_sweepA.GetTransform(1.0f);
+		b3Transform xfB1 = m_sweepB.GetTransform(1.0f);
+
+		g_draw->DrawTransform(xfA1);
+		g_draw->DrawTransform(xfB1);
+
+		m_world.DrawShape(xfA1, &m_shapeA, b3Color_black);
+		m_world.DrawShape(xfB1, &m_shapeB, b3Color_black);
+
+		m_world.DrawSolidShape(xfA1, &m_shapeA, colorA0);
+		m_world.DrawSolidShape(xfB1, &m_shapeB, colorB0);
+
+		// time
+		b3Color colorAt(1.0f, 0.0f, 0.0f, 0.5f);
+		b3Color colorBt(0.0f, 1.0f, 0.0f, 0.5f);
+
+		b3Transform xfAx = m_sweepA.GetTransform(m_time);
+		b3Transform xfBx = m_sweepB.GetTransform(m_time);
+
+		g_draw->DrawTransform(xfAx);
+		g_draw->DrawTransform(xfBx);
+
+		m_world.DrawShape(xfAx, &m_shapeA, b3Color_black);
+		m_world.DrawShape(xfBx, &m_shapeB, b3Color_black);
+
+		m_world.DrawSolidShape(xfAx, &m_shapeA, colorAt);
+		m_world.DrawSolidShape(xfBx, &m_shapeB, colorBt);
+
+		b3TOIInput input;
+		input.proxyA = m_proxyA;
+		input.sweepA = m_sweepA;
+		input.proxyB = m_proxyB;
+		input.sweepB = m_sweepB;
+		input.tMax = 1.0f;
+
+		b3TOIOutput output = b3TimeOfImpact(input);
+
+		if (output.state == b3TOIOutput::e_touching)
+		{
+			b3Transform xfAt = m_sweepA.GetTransform(output.t);
+			b3Transform xfBt = m_sweepB.GetTransform(output.t);
+
+			m_world.DrawShape(xfAt, &m_shapeA, b3Color_black);
+			m_world.DrawShape(xfBt, &m_shapeB, b3Color_black);
+		}
+
+		g_draw->DrawString(b3Color_white, "Left/Right/Up/Down Arrow/W/S - Translate shape");
+		g_draw->DrawString(b3Color_white, "X/Y/Z - Rotate shape");
+		g_draw->DrawString(b3Color_white, "F/B - Advance Time Forwards/Backwards");
+		g_draw->DrawString(b3Color_white, "Iterations = %d", output.iterations);
+
+		if (output.state == b3TOIOutput::e_failed)
+		{
+			g_draw->DrawString(b3Color_white, "State = Failed");
+		}
+		else if (output.state == b3TOIOutput::e_overlapped)
+		{
+			g_draw->DrawString(b3Color_white, "State = Overlapped");
+		}
+		else if (output.state == b3TOIOutput::e_separated)
+		{
+			g_draw->DrawString(b3Color_white, "State = Separated!");
+		}
+		else if (output.state == b3TOIOutput::e_touching)
+		{
+			g_draw->DrawString(b3Color_white, "State = Touching!");
+		}
+
+		g_draw->DrawString(b3Color_white, m_sweepA.worldCenter0, "t0");
+		g_draw->DrawString(b3Color_white, m_sweepA.worldCenter, "t1");
+
+		g_draw->DrawString(b3Color_white, m_sweepB.worldCenter0, "t0");
+		g_draw->DrawString(b3Color_white, m_sweepB.worldCenter, "t1");
+	}
+
+	void KeyDown(int key)
+	{
+		const scalar dt = 0.01f;
+		const scalar d = 0.15f;
+		const scalar theta = 0.05f * B3_PI;
+
+		if (key == GLFW_KEY_F)
+		{
+			m_time += dt;
+			if (m_time > 1.0f)
+			{
+				m_time = 0.0f;
+			}
+		}
+
+		if (key == GLFW_KEY_B)
+		{
+			m_time -= dt;
+			if (m_time < 0.0f)
+			{
+				m_time = 1.0f;
+			}
+		}
+		
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_sweepB.worldCenter0.x -= d;
+		}
+
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_sweepB.worldCenter0.x += d;
+		}
+
+		if (key == GLFW_KEY_UP)
+		{
+			m_sweepB.worldCenter0.y += d;
+		}
+
+		if (key == GLFW_KEY_DOWN)
+		{
+			m_sweepB.worldCenter0.y -= d;
+		}
+
+		if (key == GLFW_KEY_S)
+		{
+			m_sweepB.worldCenter0.z += d;
+		}
+
+		if (key == GLFW_KEY_W)
+		{
+			m_sweepB.worldCenter0.z -= d;
+		}
+		
+		if (key == GLFW_KEY_X)
+		{
+			b3Quat qx = b3QuatRotationX(theta);
+
+			m_sweepB.orientation0 = m_sweepB.orientation0 * qx;
+		}
+
+		if (key == GLFW_KEY_Y)
+		{
+			b3Quat qy = b3QuatRotationY(theta);
+
+			m_sweepB.orientation0 = m_sweepB.orientation0 * qy;
+		}
+
+		if (key == GLFW_KEY_Z)
+		{
+			b3Quat qz = b3QuatRotationZ(theta);
+
+			m_sweepB.orientation0 = m_sweepB.orientation0 * qz;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new TimeOfImpact();
+	}
+
+	scalar m_time;
+
+	b3HullShape m_shapeA;
+	b3Sweep m_sweepA;
+	b3ShapeGJKProxy m_proxyA;
+
+	b3HullShape m_shapeB;
+	b3Sweep m_sweepB;
+	b3ShapeGJKProxy m_proxyB;
+};
+
+#endif

examples/testbed/tests/triangle_contact_test.h

@@ -16,45 +16,55 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef DEGENERATE_CAPSULE_H
-#define DEGENERATE_CAPSULE_H
+#ifndef TRIANGLE_CONTACT_TEST_H
+#define TRIANGLE_CONTACT_TEST_H
 
-class DegenerateCapsule : public Collide
+class TriangleContactTest : public Test
 {
 public:
-	DegenerateCapsule()
+	TriangleContactTest()
 	{
-		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
-		m_xfA.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
 
-		m_sA.m_centers[0].Set(0.0f, 0.0f, 0.0f);
-		m_sA.m_centers[1].Set(0.0f, 0.0f, 0.0f);
-		m_sA.m_radius = 0.05f;
+			b3Body* body = m_world.CreateBody(bdef);
 
-		m_xfB.position.Set(0.f, 0.0f, 0.0f);
-		m_xfB.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
+			b3TriangleShape ts;
+			ts.m_vertex1.Set(-5.0f, 0.0f, 5.0f);
+			ts.m_vertex2.Set(5.0f, 0.0f, 5.0f);
+			ts.m_vertex3.Set(0.0f, 0.0f, -5.0f);
 
-		b3Transform xf;
-		xf.SetIdentity();
-		xf.rotation = b3Diagonal(4.0f, 1.0f, 4.0f);
+			b3ShapeDef sdef;
+			sdef.shape = &ts;
+			sdef.friction = 1.0f;
 
-		m_box.SetTransform(xf);
+			body->CreateShape(sdef);
+		}
 
-		m_sB.m_hull = &m_box;
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_dynamicBody;
+			bdef.position.Set(0.0f, 5.0f, 0.0f);
 
-		m_shapeA = &m_sA;
-		m_shapeB = &m_sB;
-		m_cache.count = 0;
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &b3BoxHull_identity;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.1f;
+			sdef.friction = 0.1f;
+			sdef.shape = &hs;
+
+			body->CreateShape(sdef);
+		}
 	}
 
 	static Test* Create()
 	{
-		return new DegenerateCapsule();
+		return new TriangleContactTest();
 	}
-
-	b3CapsuleShape m_sA;
-	b3HullShape m_sB;
-	b3BoxHull m_box;
 };
 
-#endif
+#endif

examples/testbed/tests/tumbler.h

@@ -39,11 +39,10 @@ public:
 			{
 				static b3BoxHull box;
 
-				b3Transform m;
-				m.position.Set(0.0f, -45.0f, 0.0f);
-				m.rotation = b3Diagonal(50.0f, 1.0f, 200.0f);
-
-				box.SetTransform(m);
+				box.SetExtents(50.0f, 1.0f, 200.0f);
+				
+				b3Vec3 translation(0.0f, -45.0f, 0.0f);
+				box.Translate(translation);
 
 				b3HullShape hs;
 				hs.m_hull = &box;
@@ -58,11 +57,10 @@ public:
 			{
 				static b3BoxHull box;
 
-				b3Transform m;
-				m.position.Set(0.0f, 50.0f, 0.0f);
-				m.rotation = b3Diagonal(50.0f, 1.0f, 200.0f);
-
-				box.SetTransform(m);
+				box.SetExtents(50.0f, 1.0f, 200.0f);
+				
+				b3Vec3 translation(0.0f, 50.0f, 0.0f);
+				box.Translate(translation);
 
 				b3HullShape hs;
 				hs.m_hull = &box;
@@ -77,11 +75,10 @@ public:
 			{
 				static b3BoxHull box;
 
-				b3Transform m;
-				m.position.Set(0.0f, 5.0f, -200.0f);
-				m.rotation = b3Diagonal(50.0f, 50.0f, 1.0f);
-
-				box.SetTransform(m);
+				box.SetExtents(50.0f, 50.0f, 1.0f);
+				
+				b3Vec3 translation(0.0f, 5.0f, -200.0f);
+				box.Translate(translation);
 
 				b3HullShape hs;
 				hs.m_hull = &box;
@@ -96,11 +93,10 @@ public:
 			{
 				static b3BoxHull box;
 
-				b3Transform m;
-				m.position.Set(0.0f, 5.0f, 200.0f);
-				m.rotation = b3Diagonal(50.0f, 50.0f, 1.0f);
-
-				box.SetTransform(m);
+				box.SetExtents(50.0f, 50.0f, 1.0f);
+				
+				b3Vec3 translation(0.0f, 5.0f, 200.0f);
+				box.Translate(translation);
 
 				b3HullShape hs;
 				hs.m_hull = &box;
@@ -115,11 +111,10 @@ public:
 			{
 				static b3BoxHull box;
 
-				b3Transform m;
-				m.position.Set(-50.0f, 5.0f, 0.0f);
-				m.rotation = b3Diagonal(1.0f, 50.0f, 200.0f);
-
-				box.SetTransform(m);
+				box.SetExtents(1.0f, 50.0f, 200.0f);
+				
+				b3Vec3 translation(-50.0f, 5.0f, 0.0f);
+				box.Translate(translation);
 
 				b3HullShape hs;
 				hs.m_hull = &box;
@@ -134,11 +129,11 @@ public:
 			{
 				static b3BoxHull box;
 
-				b3Transform m;
-				m.position.Set(50.0f, 5.0f, 0.0f);
-				m.rotation = b3Diagonal(1.0f, 50.0f, 200.0f);
+				box.SetExtents(1.0f, 50.0f, 200.0f);
+				
+				b3Vec3 translation(50.0f, 5.0f, 0.0f);
 
-				box.SetTransform(m);
+				box.Translate(translation);
 
 				b3HullShape hs;
 				hs.m_hull = &box;
@@ -161,8 +156,8 @@ public:
 			}
 		}
 
-		m_coneHull.SetAsCone();
-		m_cylinderHull.SetAsCylinder();
+		m_coneHull.SetExtents(1.0f, 1.0f);
+		m_cylinderHull.SetExtents(1.0f, 1.0f);
 
 		m_count = 0;
 	}
@@ -204,8 +199,8 @@ public:
 				b3Body* body = m_world.CreateBody(bdef);
 
 				b3CapsuleShape capsule;
-				capsule.m_centers[0].Set(0.0f, 0.0f, -1.0f);
-				capsule.m_centers[1].Set(0.0f, 0.0f, 1.0f);
+				capsule.m_vertex1.Set(0.0f, 0.0f, -1.0f);
+				capsule.m_vertex2.Set(0.0f, 0.0f, 1.0f);
 				capsule.m_radius = 1.0f;
 
 				b3ShapeDef sdef;
@@ -279,8 +274,8 @@ public:
 	}
 
 	u32 m_count;
-	b3QHull m_coneHull;
-	b3QHull m_cylinderHull;
+	b3ConeHull m_coneHull;
+	b3CylinderHull m_cylinderHull;
 };
 
 #endif

examples/testbed/tests/varying_friction.h

@@ -36,19 +36,12 @@ public:
 			ground->CreateShape(sd);
 		}
 
-		static b3BoxHull rampHull;
-
-		{
-			b3Transform xf;
-			xf.position.SetZero();
-			xf.rotation = b3Diagonal(25.0f, 0.5f, 25.0f);
-			rampHull.SetTransform(xf);
-		}
+		static b3BoxHull rampHull(25.0f, 0.5f, 25.0f);
 
 		{
 			b3BodyDef bdef;
 			bdef.position.Set(-20.0f, 20.0f, 0.0f);
-			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), -0.1f * B3_PI);
+			bdef.orientation = b3QuatRotationZ(-0.1f * B3_PI);
 
 			b3Body* ramp = m_world.CreateBody(bdef);
 			
@@ -64,7 +57,7 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.position.Set(20.0f, 30.0f, 0.0f);
-			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.1f * B3_PI);
+			bdef.orientation = b3QuatRotationZ(0.1f * B3_PI);
 
 			b3Body* ramp = m_world.CreateBody(bdef);
 
@@ -80,7 +73,7 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.position.Set(-20.0f, 40.0f, 0.0f);
-			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), -0.1f * B3_PI);
+			bdef.orientation = b3QuatRotationZ(-0.1f * B3_PI);
 
 			b3Body* ramp = m_world.CreateBody(bdef);
 
@@ -96,7 +89,7 @@ public:
 		{
 			b3BodyDef bdef;
 			bdef.position.Set(20.0f, 50.0f, 0.0f);
-			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.1f * B3_PI);
+			bdef.orientation = b3QuatRotationZ(0.1f * B3_PI);
 
 			b3Body* ramp = m_world.CreateBody(bdef);
 

examples/testbed/tests/weld_test.h

@@ -46,8 +46,8 @@ public:
 			bA = m_world.CreateBody(bd);
 
 			b3CapsuleShape shape;
-			shape.m_centers[0].Set(0.0f, -3.5f, 0.0f);
-			shape.m_centers[1].Set(0.0f, 3.5f, 0.0f);
+			shape.m_vertex1.Set(0.0f, -3.5f, 0.0f);
+			shape.m_vertex2.Set(0.0f, 3.5f, 0.0f);
 			shape.m_radius = 0.5f;
 
 			b3ShapeDef sd;
@@ -64,13 +64,7 @@ public:
 
 			bB = m_world.CreateBody(bd);
 
-			static b3BoxHull doorHull;
-			{
-				b3Transform xf;
-				xf.position.SetZero();
-				xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f);
-				doorHull.SetTransform(xf);
-			}
+			static b3BoxHull doorHull(2.0f, 4.0f, 0.5f);
 
 			b3HullShape hull;
 			hull.m_hull = &doorHull;
@@ -86,14 +80,15 @@ public:
 
 				b3WeldJointDef jd;
 				jd.Initialize(bA, bB, anchor);
+				jd.frequencyHz = 2.0f;
+				jd.dampingRatio = 0.3f;
 
 				b3WeldJoint* wj = (b3WeldJoint*)m_world.CreateJoint(jd);
 			}
 
 			// Invalidate the orientation
-			b3Vec3 axis(1.0f, 0.0f, 0.0f);
-			float32 angle = B3_PI;
-			bB->SetTransform(bB->GetPosition(), axis, angle);
+			b3Quat q = b3QuatRotationX(B3_PI);
+			bB->SetTransform(bB->GetPosition(), q);
 		}
 	}
 

examples/testbed/tests/wheel_test.h

@@ -0,0 +1,204 @@
+/*
+* 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 WHEEL_TEST_H
+#define WHEEL_TEST_H
+
+class WheelTest : public Test
+{
+public:
+	WheelTest()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		m_chassisHull.SetExtents(2.0f, 0.5f, 5.0f);
+
+		b3HullShape chassisShape;
+		chassisShape.m_hull = &m_chassisHull;
+
+		m_wheelHull.SetExtents(1.0f, 0.5f);
+
+		b3HullShape wheelShape;
+		wheelShape.m_hull = &m_wheelHull;
+
+		// Chassis
+		b3Body* chassis;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(0.0f, 10.0f, 0.0f);
+
+			chassis = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.density = 0.1f;
+			sdef.friction = 0.3f;
+			sdef.shape = &chassisShape;
+
+			chassis->CreateShape(sdef);
+		}
+
+		b3Quat orientation = b3QuatRotationZ(0.5f * B3_PI);
+
+		b3Body* wheelLF;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(-1.0f, 7.0f, 4.5f);
+			bdef.orientation = orientation;
+
+			wheelLF = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.shape = &wheelShape;
+			sdef.density = 0.1f;
+			sdef.friction = 1.0f;
+
+			wheelLF->CreateShape(sdef);
+		}
+
+		{
+			b3WheelJointDef def;
+			def.Initialize(chassis, wheelLF, wheelLF->GetPosition(), b3Vec3_y, b3Vec3_x);
+			def.motorSpeed = 0.25f * B3_PI;
+			def.maxMotorTorque = 1000.0f;
+
+			m_joint1 = (b3WheelJoint*)m_world.CreateJoint(def);
+		}
+
+		b3Body* wheelRF;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(1.0f, 7.0, 4.5f);
+			bdef.orientation = orientation;
+
+			wheelRF = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.density = 0.1f;
+			sdef.friction = 1.0f;
+			sdef.shape = &wheelShape;
+
+			wheelRF->CreateShape(sdef);
+		}
+
+		{
+			b3WheelJointDef def;
+			def.Initialize(chassis, wheelRF, wheelRF->GetPosition(), b3Vec3_y, b3Vec3_x);
+			def.motorSpeed = 0.25f * B3_PI;
+			def.maxMotorTorque = 1000.0f;
+
+			m_joint2 = (b3WheelJoint*)m_world.CreateJoint(def);
+		}
+
+		b3Body* wheelLB;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(-1.0f, 7.0f, -4.5f);
+			bdef.orientation = orientation;
+
+			wheelLB = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.shape = &wheelShape;
+			sdef.density = 0.1f;
+			sdef.friction = 1.0f;
+
+			wheelLB->CreateShape(sdef);
+		}
+
+		{
+			b3WheelJointDef def;
+			def.Initialize(chassis, wheelLB, wheelLB->GetPosition(), b3Vec3_y, b3Vec3_x);
+
+			m_world.CreateJoint(def);
+		}
+
+		b3Body* wheelRB;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(1.0f, 7.0f, -4.5f);
+			bdef.orientation = orientation;
+
+			wheelRB = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.density = 0.1f;
+			sdef.friction = 1.0f;
+			sdef.shape = &wheelShape;
+
+			wheelRB->CreateShape(sdef);
+		}
+
+		{
+			b3WheelJointDef def;
+			def.Initialize(chassis, wheelRB, wheelRB->GetPosition(), b3Vec3_y, b3Vec3_x);
+
+			m_world.CreateJoint(def);
+		}
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		g_draw->DrawString(b3Color_white, "M - Enable Motor");
+		g_draw->DrawString(b3Color_white, "S - Flip Motor Speed");
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_M)
+		{
+			m_joint1->EnableMotor(!m_joint1->IsMotorEnabled());
+			m_joint2->EnableMotor(!m_joint2->IsMotorEnabled());
+		}
+
+		if (button == GLFW_KEY_S)
+		{
+			m_joint1->SetMotorSpeed(-m_joint1->GetMotorSpeed());
+			m_joint2->SetMotorSpeed(-m_joint2->GetMotorSpeed());
+		}
+	}
+
+	static Test* Create()
+	{
+		return new WheelTest();
+	}
+
+	b3BoxHull m_chassisHull;
+	b3CylinderHull m_wheelHull;
+	b3WheelJoint* m_joint1;
+	b3WheelJoint* m_joint2;
+};
+
+#endif