improve segment vs segment collision, small fixes

include/bounce/common/draw.h

@@ -32,6 +32,15 @@ struct b3Color
 	float32 r, g, b, a;
 };
 
+// Color pallete commonly used by the debug draw interface.
+extern const b3Color b3Color_black;
+extern const b3Color b3Color_white;
+extern const b3Color b3Color_red;
+extern const b3Color b3Color_green;
+extern const b3Color b3Color_blue;
+extern const b3Color b3Color_yellow;
+extern const b3Color b3Color_pink;
+
 // Implement this interface and set to a world so it can draw the physics entities.
 class b3Draw 
 {

include/bounce/dynamics/world.h

@@ -34,6 +34,14 @@ class b3ContactListener;
 class b3ContactFilter;
 class b3Draw;
 
+struct b3RayCastSingleOutput
+{
+	b3Shape* shape; // shape
+	b3Vec3 point; // intersection point on surface
+	b3Vec3 normal; // surface normal of intersection
+	float32 fraction; // time of intersection on segment
+};
+
 // Use a physics world to create/destroy rigid bodies, execute ray cast and volume queries.
 class b3World
 {
@@ -83,6 +91,13 @@ public:
 	// and the number of constraint solver iterations.
 	void Step(float32 dt, u32 velocityIterations, u32 positionIterations);
 	
+	// Perform a ray cast with the world.
+	// If the ray doesn't intersect with a shape in the world then return false.
+	// The ray cast output is the intercepted shape, the intersection 
+	// point in world space, the face normal on the shape associated with the point, 
+	// and the intersection fraction.
+	bool RayCastSingle(b3RayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
+
 	// Perform a ray cast with the world.
 	// The given ray cast listener will be notified when a ray intersects a shape 
 	// in the world. 
@@ -91,13 +106,6 @@ public:
 	// and the intersection fraction.
 	void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
 
-	// Convenience function.
-	// Perform a ray cast with the world.
-	// If there is an intersection then the given ray cast listener will be notified once with 
-	// the shape closest to the ray origin and the associated ray cast output. 
-	// @todo Centralize all queries to a common scene query class?
-	void RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
-
 	// Perform a AABB cast with the world.
 	// The query listener will be notified when two shape AABBs are overlapping.
 	// If the listener returns false then the query is stopped immediately.

include/testbed/framework/debug_draw.h

@@ -106,7 +106,7 @@ public:
 
 	void Draw(const b3World& world);
 
-	void Draw();
+	void Submit();
 private:
 	friend struct DrawShapes;
 

include/testbed/tests/ray_cast.h

@@ -204,18 +204,12 @@ public:
 
 	void CastRay(const b3Vec3 p1, const b3Vec3 p2) const
 	{
-		// Perform the ray cast
+		b3RayCastSingleOutput out;
-		RayCastListener listener;
+		if (m_world.RayCastSingle(&out, p1, p2))
-		listener.hit.shape = NULL;
-		m_world.RayCastFirst(&listener, p1, p2);
-
-		RayCastHit hit = listener.hit;
-		if (hit.shape)
 		{
-			// Replace current hit
+			g_debugDraw->DrawSegment(p1, out.point, b3Color(0.0f, 1.0f, 0.0f));
-			g_debugDraw->DrawSegment(p1, hit.point, b3Color(0.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawPoint(out.point, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
-			g_debugDraw->DrawPoint(hit.point, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
+			g_debugDraw->DrawSegment(out.point, out.point + out.normal, b3Color(1.0f, 1.0f, 1.0f));
-			g_debugDraw->DrawSegment(hit.point, hit.point + hit.normal, b3Color(1.0f, 1.0f, 1.0f));
 		}
 		else
 		{

include/testbed/tests/test.h

@@ -88,14 +88,6 @@ struct TestEntry
 
 extern TestEntry g_tests[];
 
-struct RayCastHit
-{
-	b3Shape* shape;
-	b3Vec3 point;
-	b3Vec3 normal;
-	float32 fraction;
-};
-
 class RayCastListener : public b3RayCastListener
 {
 public:
@@ -108,7 +100,7 @@ public:
 		return 1.0f;
 	}
 
-	RayCastHit hit;
+	b3RayCastSingleOutput hit;
 };
 
 class Test : public b3ContactListener
@@ -142,7 +134,7 @@ public:
 	b3Profile m_profile;
 	b3Profile m_maxProfile;
 
-	RayCastHit m_rayHit;
+	b3RayCastSingleOutput m_rayHit;
 	b3BoxHull m_groundHull;
 	b3BoxHull m_boxHull;
 	b3BoxHull m_tallHull;

premake5.lua

@@ -178,7 +178,7 @@ solution (solution_name)
 		links { "glfw", "glad", "imgui", "bounce" }
 
 		configuration { "windows" }
-			links { "glu32", "opengl32", "winmm" }
+			links { "opengl32", "winmm" }
 			
 		configuration { "not windows", "not macosx" }
 			links 

src/bounce/collision/distance.cpp

@@ -157,32 +157,41 @@ void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
 	const b3Vec3& P1, const b3Vec3& N1,
 	const b3Vec3& P2, const b3Vec3& N2)
 {
-	float32 a12 = -b3Dot(N1, N2);
+	// sin^2 = 1 - cos^2
-	float32 a21 = -a12;
+	// or
+	// sin = norm( cross(n1, n2) )
+	const float32 kTol = 0.0f;
 
-	float32 det = -1.0f - a12 * a21;
+	float32 c = b3Dot(N1, N2);
-	if (det == 0.0f)
+	float32 den = 1.0f - c * c;
+	if (den < kTol * kTol)
 	{
-		// Nearly paralell lines.
 		*C1 = P1;
 		*C2 = P2;
 		return;
 	}
 
-	det = 1.0f / det;
+	den = 1.0f / den;
 
+	// a = dot(n1, e3)
+	// b = dot(n2, e3)
+	// c = dot(n1, n2)
+
+	// s - c * t = -dot(n1, e3)
+	// c * s - t = -dot(n2, e3)
+
+	// s = ( c * dot(n2, e3) - dot(n1, e3) ) / den 
+	// t = ( dot(n2, e3) - c * dot(n1, e3) ) / den 
 	b3Vec3 E3 = P1 - P2;
 
-	b3Vec2 b;
+	float32 a = b3Dot(N2, E3);
-	b.x = -b3Dot(N1, E3);
+	float32 b = b3Dot(N1, E3);
-	b.y = -b3Dot(N2, E3);
 	
-	b3Vec2 x;
+	float32 s = den * (c * a - b);
-	x.x = det * (-b.x - a12 * b.y);
+	float32 t = den * (a - c - b);
-	x.y = det * (b.y - a21 * b.x);
 
-	*C1 = P1 + x.x * N1;
+	*C1 = P1 + s * N1;
-	*C2 = P2 + x.y * N2;
+	*C2 = P2 + t * N2;
 }
 
 void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
@@ -195,41 +204,68 @@ void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
 	b3Vec3 E2 = Q2 - P2;
 	float32 L2 = b3Length(E2);
 
-	if (L1 < B3_LINEAR_SLOP && L2 < B3_LINEAR_SLOP)
+	if (L1 < 0.0f && L2 < 0.0f)
 	{
 		*C1 = P1;
 		*C2 = P2;
 		return;
 	}
 
-	if (L1 < B3_LINEAR_SLOP)
+	if (L1 < 0.0f)
 	{
 		*C1 = P1;
 		*C2 = b3ClosestPointOnSegment(P1, P2, Q2);
 		return;
 	}
 
-	if (L2 < B3_LINEAR_SLOP)
+	if (L2 < 0.0f)
 	{
 		*C1 = b3ClosestPointOnSegment(P2, P1, Q1);
 		*C2 = P2;
 		return;
 	}
 
-	// |e1xe2| = sin(theta) * |e1| * |e2|
+	// Here and in 3D we need to start "GJK" with the closest points between the two edges 
-	b3Vec3 E1_x_E2 = b3Cross(E1, E2);
+	// since the cross product between their direction is a possible separating axis.
-	float32 L = b3Length(E1_x_E2);
+	b3Vec3 N1 = (1.0f / L1) * E1;
-	const float32 kTolerance = 0.005f;
+	b3Vec3 N2 = (1.0f / L2) * E2;
-	if (L < kTolerance * L1 * L2)
+
+	// sin = norm( cross(n1, n2) )
+	// or
+	// sin^2 = 1 - cos^2
+
+	// Zero parallelism tolerance used because the colinearity tolerance above is also zero.
+	const float32 kTol = 0.0f;
+
+	float32 c = b3Dot(N1, N2);
+	float32 den = 1.0f - c * c;
+	if (den < kTol * kTol)
 	{
 		*C1 = P1;
 		*C2 = P2;
 	}
 	else
 	{
-		b3Vec3 N1 = (1.0f / L1) * E1;
+		// a = dot(n1, e3)
-		b3Vec3 N2 = (1.0f / L2) * E2;
+		// b = dot(n2, e3)
-		b3ClosestPointsOnNormalizedLines(C1, C2, P1, N1, P2, N2);
+		// c = dot(n1, n2)
+
+		// s - c * t = -dot(n1, e3)
+		// c * s - t = -dot(n2, e3)
+
+		// s = ( c * dot(n2, e3) - dot(n1, e3) ) / den 
+		// t = ( dot(n2, e3) - c * dot(n1, e3) ) / den 
+		b3Vec3 E3 = P1 - P2;
+
+		float32 a = b3Dot(N2, E3);
+		float32 b = b3Dot(N1, E3);
+		float32 inv_den = 1.0f / den;
+
+		float32 s = inv_den * (c * a - b);
+		float32 t = inv_den * (a - c - b);
+
+		*C1 = P1 + s * N1;
+		*C2 = P2 + t * N2;
 	}
 
 	*C1 = b3ClosestPointOnSegment(*C1, P1, Q1);

src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp

@@ -42,47 +42,29 @@ void b3BuildEdgeContact(b3Manifold& manifold,
 	b3Vec3 E2 = Q2 - P2;
 	b3Vec3 N2 = b3Normalize(E2);
 
-	b3Vec3 E3 = P1 - P2;
+	b3Vec3 N = b3Cross(E1, E2);
-
+	N.Normalize();
-	b3Vec2 b;
+	if (b3Dot(N, P2 - C2) > 0.0f)
-	b.x = -b3Dot(N1, E3);
-	b.y = -b3Dot(N2, E3);
-
-	float32 a12 = -b3Dot(N1, N2), a21 = -a12;
-
-	float32 det = -1.0f - a12 * a21;
-	if (det != 0.0f)
 	{
-		det = 1.0f / det;
+		N = -N;
 	}
 
-	b3Vec2 x;
+	b3Vec3 PA, PB;
-	x.x = det * (-b.x - a12 * b.y);
+	b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, E1, P2, E2);
-	x.y = det * (b.y - a21 * b.x);
-
-	b3Vec3 point1 = P1 + x.x * N1;
-	b3Vec3 point2 = P2 + x.y * N2;
-
-	b3Vec3 axis = b3Cross(E1, E2);
-	b3Vec3 normal = b3Normalize(axis);
-	if (b3Dot(normal, P2 - C2) > 0.0f)
-	{
-		normal = -normal;
-	}
 
 	b3FeaturePair pair = b3MakePair(0, 1, index2, index2 + 1);
 	
 	manifold.pointCount = 1;
 	manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
 	manifold.points[0].key = b3MakeKey(pair);
-	manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
+	manifold.points[0].localNormal = b3MulT(xf1.rotation, N);
-	manifold.points[0].localPoint = b3MulT(xf1, point1);
+	manifold.points[0].localPoint = b3MulT(xf1, PA);
-	manifold.points[0].localPoint2 = b3MulT(xf2, point2);
+	manifold.points[0].localPoint2 = b3MulT(xf2, PB);
 
-	manifold.center = 0.5f * (point1 + hull1->radius * normal + point2 - B3_HULL_RADIUS * normal);
+	manifold.center = 0.5f * (PA + hull1->radius * N + PB - B3_HULL_RADIUS * N);
-	manifold.normal = normal;
+	manifold.normal = N;
-	manifold.tangent1 = b3Perp(normal);
+	manifold.tangent1 = b3Perp(N);
-	manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+	manifold.tangent2 = b3Cross(manifold.tangent1, N);
 }
 
 void b3BuildFaceContact(b3Manifold& manifold,

src/bounce/dynamics/draw_world.cpp

@@ -40,20 +40,20 @@
 #include <bounce/collision/shapes/hull.h>
 #include <bounce/collision/shapes/mesh.h>
 
+const b3Color b3Color_black(0.0f, 0.0f, 0.0f);
+const b3Color b3Color_white(1.0f, 1.0f, 1.0f);
+const b3Color b3Color_red(1.0f, 0.0f, 0.0f);
+const b3Color b3Color_green(0.0f, 1.0f, 0.0f);
+const b3Color b3Color_blue(0.0f, 0.0f, 1.0f);
+const b3Color b3Color_yellow(1.0f, 1.0f, 0.0f);
+const b3Color b3Color_pink(1.0f, 0.0f, 1.0f);
+
 void b3World::DebugDraw() const
 {
 	B3_ASSERT(m_debugDraw);
 
 	u32 flags = m_debugDraw->m_flags;
 
-	b3Color black(0.0f, 0.0f, 0.0f, 1.0f);
-	b3Color white(1.0f, 1.0f, 1.0f, 1.0f);
-	b3Color red(1.0f, 0.0f, 0.0f, 1.0f);
-	b3Color green(0.0f, 1.0f, 0.0f, 1.0f);
-	b3Color blue(0.0f, 0.0f, 1.0f, 1.0f);
-	b3Color yellow(1.0f, 1.0f, 0.0f, 1.0f);
-	b3Color purple(1.0f, 0.0f, 1.0f, 1.0f);
-
 	if (flags & b3Draw::e_centerOfMassesFlag)
 	{
 		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
@@ -83,7 +83,7 @@ void b3World::DebugDraw() const
 			for (b3Shape* s = b->m_shapeList.m_head; s; s = s->m_next)
 			{
 				const b3AABB3& aabb = m_contactMan.m_broadPhase.GetAABB(s->m_broadPhaseID);
-				m_debugDraw->DrawAABB(aabb, purple);
+				m_debugDraw->DrawAABB(aabb, b3Color_pink);
 			}
 		}
 	}
@@ -122,18 +122,18 @@ void b3World::DebugDraw() const
 				
 				if (flags & b3Draw::e_contactPointsFlag)
 				{
-					m_debugDraw->DrawPoint(p, 4.0f, yellow);
+					m_debugDraw->DrawPoint(p, 4.0f, b3Color_yellow);
 				}
 
 				if (flags & b3Draw::e_contactNormalsFlag)
 				{
-					m_debugDraw->DrawSegment(p, p + n, yellow);
+					m_debugDraw->DrawSegment(p, p + n, b3Color_yellow);
 				}
 
 				if (flags & b3Draw::e_contactTangentsFlag)
 				{
-					m_debugDraw->DrawSegment(p, p + t1, yellow);
+					m_debugDraw->DrawSegment(p, p + t1, b3Color_yellow);
-					m_debugDraw->DrawSegment(p, p + t2, yellow);
+					m_debugDraw->DrawSegment(p, p + t2, b3Color_yellow);
 				}
 			}
 
@@ -152,18 +152,18 @@ void b3World::DebugDraw() const
 
 				if (flags & b3Draw::e_contactPointsFlag)
 				{
-					m_debugDraw->DrawPoint(p, 4.0f, mp->persisting ? green : red);
+					m_debugDraw->DrawPoint(p, 4.0f, mp->persisting ? b3Color_green : b3Color_red);
 				}
 				
 				if (flags & b3Draw::e_contactNormalsFlag)
 				{
-					m_debugDraw->DrawSegment(p, p + n, white);
+					m_debugDraw->DrawSegment(p, p + n, b3Color_white);
 				}
 				
 				if (flags & b3Draw::e_contactTangentsFlag)
 				{
-					m_debugDraw->DrawSegment(p, p + t1, yellow);
+					m_debugDraw->DrawSegment(p, p + t1, b3Color_yellow);
-					m_debugDraw->DrawSegment(p, p + t2, yellow);
+					m_debugDraw->DrawSegment(p, p + t2, b3Color_yellow);
 				}
 			}
 		}

src/bounce/dynamics/joints/mouse_joint.cpp

@@ -109,14 +109,10 @@ void b3MouseJoint::SetTarget(const b3Vec3& target)
 
 void b3MouseJoint::Draw(b3Draw* draw) const 
 {
-	b3Color red(1.0f, 0.0f, 0.0f);
-	b3Color green(0.0f, 1.0f, 0.0f);
-	b3Color yellow(1.0f, 1.0f, 0.0f);
-	
 	b3Vec3 a = GetAnchorA();
 	b3Vec3 b = GetAnchorB();
 
-	draw->DrawPoint(a, 4.0f, green);
+	draw->DrawPoint(a, 4.0f, b3Color_green);
-	draw->DrawPoint(b, 4.0f, red);
+	draw->DrawPoint(b, 4.0f, b3Color_red);
-	draw->DrawSegment(a, b, yellow);
+	draw->DrawSegment(a, b, b3Color_yellow);
 }

src/bounce/dynamics/joints/sphere_joint.cpp

@@ -162,7 +162,7 @@ void b3SphereJoint::Draw(b3Draw* draw) const
 	b3Vec3 a = GetAnchorA();
 	b3Vec3 b = GetAnchorB();
 	
-	draw->DrawPoint(a, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
+	draw->DrawPoint(a, 4.0f, b3Color_red);
-	draw->DrawPoint(b, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
+	draw->DrawPoint(b, 4.0f, b3Color_green);
-	draw->DrawSegment(a, b, b3Color(1.0f, 1.0f, 0.0f));
+	draw->DrawSegment(a, b, b3Color_yellow);
 }

src/bounce/dynamics/joints/spring_joint.cpp

@@ -254,14 +254,10 @@ bool b3SpringJoint::SolvePositionConstraints(const b3SolverData* data)
 
 void b3SpringJoint::Draw(b3Draw* draw) const 
 {
-	b3Color red = b3Color(1.0f, 0.0f, 0.0f, 1.0f);
-	b3Color green = b3Color(0.0f, 1.0f, 0.0f, 1.0f);
-	b3Color blue = b3Color(0.0f, 0.0f, 1.0f, 1.0f);
-
 	b3Vec3 a = GetBodyA()->GetWorldPoint(m_localAnchorA);
 	b3Vec3 b = GetBodyB()->GetWorldPoint(m_localAnchorB);
 	
-	draw->DrawPoint(a, 4.0f, red);
+	draw->DrawPoint(a, 4.0f, b3Color_red);
-	draw->DrawPoint(b, 4.0f, green);
+	draw->DrawPoint(b, 4.0f, b3Color_green);
-	draw->DrawSegment(a, b, blue);
+	draw->DrawSegment(a, b, b3Color_yellow);
 }

src/bounce/dynamics/world.cpp

@@ -377,7 +377,7 @@ void b3World::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec
 	m_contactMan.m_broadPhase.RayCast(&callback, input);
 }
 
-struct b3RayCastFirstCallback
+struct b3RayCastSingleCallback
 {
 	float32 Report(const b3RayCastInput& input, i32 proxyId)
 	{
@@ -409,14 +409,14 @@ struct b3RayCastFirstCallback
 	const b3BroadPhase* broadPhase;
 };
 
-void b3World::RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const
+bool b3World::RayCastSingle(b3RayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const
 {
 	b3RayCastInput input;
 	input.p1 = p1;
 	input.p2 = p2;
 	input.maxFraction = 1.0f;
 
-	b3RayCastFirstCallback callback;
+	b3RayCastSingleCallback callback;
 	callback.shape0 = NULL;
 	callback.output0.fraction = B3_MAX_FLOAT;
 	callback.broadPhase = &m_contactMan.m_broadPhase;
@@ -428,15 +428,18 @@ void b3World::RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const
 	{
 		// Ray hits closest shape.
 		float32 fraction = callback.output0.fraction;
-		float32 w1 = 1.0f - fraction;
+		b3Vec3 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
-		float32 w2 = fraction;
-
-		b3Vec3 point = w1 * input.p1 + w2 * input.p2;
 		b3Vec3 normal = callback.output0.normal;
 
-		// Report the intersection to the user.
+		output->shape = callback.shape0;
-		listener->ReportShape(callback.shape0, point, normal, fraction);
+		output->point = point;
+		output->normal = normal;
+		output->fraction = fraction;
+		
+		return true;
 	}
+	
+	return false;
 }
 
 struct b3QueryAABBCallback

src/testbed/framework/debug_draw.cpp

@@ -1115,6 +1115,9 @@ void DebugDraw::DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const
 	m_triangles->Vertex(p1, color, normal);
 	m_triangles->Vertex(p2, color, normal);
 	m_triangles->Vertex(p3, color, normal);
+	
+	b3Color edgeColor(0.0f, 0.0f, 0.0f, 1.0f);
+	DrawTriangle(p2, p3, p3, edgeColor);
 }
 
 void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color)
@@ -1124,7 +1127,8 @@ void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& co
 	{
 		b3Vec3 p2 = vertices[i];
 		
-		DrawSegment(p1, p2, color);
+		m_lines->Vertex(p1, color);
+		m_lines->Vertex(p2, color);
 
 		p1 = p2;
 	}
@@ -1132,7 +1136,7 @@ void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& co
 
 void DebugDraw::DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 count, const b3Color& color)
 {
-	b3Color fillColor(color.r, color.g, color.b, 0.5f);
+	b3Color fillColor(color.r, color.g, color.b, color.a);
 	
 	b3Vec3 p1 = vertices[0];
 	for (u32 i = 1; i < count - 1; ++i)
@@ -1165,7 +1169,8 @@ void DebugDraw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 r
 		b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal;
 		b3Vec3 v2 = center + radius * n2;
 
-		DrawSegment(v1, v2, color);
+		m_lines->Vertex(v1, color);
+		m_lines->Vertex(v2, color);
 		
 		n1 = n2;
 		v1 = v2;
@@ -1367,12 +1372,18 @@ void DebugDraw::DrawMesh(const b3MeshShape* s, const b3Color& c, const b3Transfo
 		b3Vec3 p2 = xf * mesh->vertices[t->v2];
 		b3Vec3 p3 = xf * mesh->vertices[t->v3];
 
-		b3Vec3 n = b3Cross(p2 - p1, p3 - p1);
+		b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1);
-		n.Normalize();
+		n1.Normalize();
 		
-		m_triangles->Vertex(p1, c, n);
+		m_triangles->Vertex(p1, c, n1);
-		m_triangles->Vertex(p2, c, n);
+		m_triangles->Vertex(p2, c, n1);
-		m_triangles->Vertex(p3, c, n);
+		m_triangles->Vertex(p3, c, n1);
+
+		b3Vec3 n2 = -n1;
+		
+		m_triangles->Vertex(p1, c, n2);
+		m_triangles->Vertex(p3, c, n2);
+		m_triangles->Vertex(p2, c, n2);
 	}
 }
 
@@ -1436,10 +1447,10 @@ void DebugDraw::Draw(const b3World& world)
 		}
 	}
 	
-	g_debugDraw->Draw();
+	g_debugDraw->Submit();
 }
 
-void DebugDraw::Draw()
+void DebugDraw::Submit()
 {
 	m_triangles->Submit();
 	m_lines->Submit();

src/testbed/framework/main.cpp

@@ -339,7 +339,7 @@ void Step()
 	}
 	
 	g_test->Step();	
-	g_debugDraw->Draw();
+	g_debugDraw->Submit();
 }
 
 void Run()

src/testbed/framework/test.cpp

@@ -376,7 +376,7 @@ void Test::Step()
 
 	g_debugDraw->SetFlags(drawFlags);
 	m_world.DebugDraw();
-	g_debugDraw->Draw();
+	g_debugDraw->Submit();
 	
 	if (g_settings.drawFaces)
 	{
@@ -459,12 +459,10 @@ void Test::MouseLeftDown(const Ray3& pw)
 	listener.hit.shape = NULL;
 
 	// Perform the ray cast
-	m_world.RayCastFirst(&listener, p1, p2);
+	b3RayCastSingleOutput out;
-
+	if (m_world.RayCastSingle(&out, p1, p2))
-	if (listener.hit.shape)
 	{
-		m_rayHit = listener.hit;
+		m_rayHit = out;		
-		
 		RayHit();
 	}
 }