fix and improve circle debug drawing

examples/testbed/framework/debug_draw.cpp

@@ -1149,60 +1149,91 @@ void DebugDraw::DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u
 		m_triangles->Vertex(p3, fillColor, normal);
 	}
 
-	b3Color edgeColor(0.0f, 0.0f, 0.0f, 1.0f);
+	b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f);
-	DrawPolygon(vertices, count, edgeColor);
+	DrawPolygon(vertices, count, frameColor);
 }
 
 void DebugDraw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
 {
+	// Build a tangent vector to normal.
+	b3Vec3 u = b3Cross(normal, b3Vec3(1.0f, 0.0f, 0.0f));
+	b3Vec3 v = b3Cross(normal, b3Vec3(0.0f, 1.0f, 0.0f));
+
+	// Handle edge cases (zero cross product).
+	b3Vec3 n1;
+	if (b3LengthSquared(u) > b3LengthSquared(v))
+	{
+		n1 = u;
+	}
+	else
+	{
+		n1 = v;
+	}
+
+	n1.Normalize();
+	
+	// Build a quaternion to rotate the tangent about the normal.
 	u32 kEdgeCount = 20;
 	float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
-	float32 cosInc = cos(kAngleInc);
+	b3Quat q(normal, kAngleInc);
-	float32 sinInc = sin(kAngleInc);
-	float32 tInc = 1.0f - cosInc;
 
-	b3Vec3 n1 = b3Perp(normal);
+	b3Vec3 p1 = center + radius * n1;
-	b3Vec3 v1 = center + radius * n1;
 	for (u32 i = 0; i < kEdgeCount; ++i)
 	{
-		// Rodrigues' rotation formula
+		b3Vec3 n2 = b3Mul(q, n1);
-		b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal;
+		b3Vec3 p2 = center + radius * n2;
-		b3Vec3 v2 = center + radius * n2;
 
-		m_lines->Vertex(v1, color);
+		m_lines->Vertex(p1, color);
-		m_lines->Vertex(v2, color);
+		m_lines->Vertex(p2, color);
 		
 		n1 = n2;
-		v1 = v2;
+		p1 = p2;
 	}
 }
 
 void DebugDraw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
 {
-	u32 kEdgeCount = 20;
+	b3Color fillColor(color.r, color.g, color.b, color.a);
-	float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
+	b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f);
-	float32 cosInc = cos(kAngleInc);
-	float32 sinInc = sin(kAngleInc);
-	float32 tInc = 1.0f - cosInc;
 	
-	b3Vec3 n1 = b3Perp(normal);
+	// Build a tangent vector to normal.
-	b3Vec3 v1 = center + radius * n1;
+	b3Vec3 u = b3Cross(normal, b3Vec3(1.0f, 0.0f, 0.0f));
-	for (u32 i = 0; i < kEdgeCount; ++i)
+	b3Vec3 v = b3Cross(normal, b3Vec3(0.0f, 1.0f, 0.0f));
+
+	// Handle edge cases (zero cross product).
+	b3Vec3 n1;
+	if (b3LengthSquared(u) > b3LengthSquared(v))
 	{
-		// Rodrigues' rotation formula
+		n1 = u;
-		b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal;
+	}
-		b3Vec3 v2 = center + radius * n2;
+	else
-
+	{
-		m_triangles->Vertex(center, color, normal);
+		n1 = v;
-		m_triangles->Vertex(v1, color, normal);
-		m_triangles->Vertex(v2, color, normal);
-
-		n1 = n2;
-		v1 = v2;
 	}
 
-	b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f);
+	n1.Normalize();
-	DrawCircle(normal, center, radius, frameColor);
+
+	// Build a quaternion to rotate the tangent about the normal.
+	const u32 kEdgeCount = 20;
+	const float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
+	b3Quat q(normal, kAngleInc);
+
+	b3Vec3 p1 = center + radius * n1;
+	for (u32 i = 0; i < kEdgeCount; ++i)
+	{
+		b3Vec3 n2 = b3Mul(q, n1);
+		b3Vec3 p2 = center + radius * n2;
+		
+		m_triangles->Vertex(center, fillColor, normal);
+		m_triangles->Vertex(p1, fillColor, normal);
+		m_triangles->Vertex(p2, fillColor, normal);
+
+		m_lines->Vertex(p1, frameColor);
+		m_lines->Vertex(p2, frameColor);
+
+		n1 = n2;
+		p1 = p2;
+	}
 }
 
 void DebugDraw::DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color)

examples/testbed/framework/test.cpp

@@ -358,6 +358,8 @@ void Test::Step()
 
 	ProfileEnd();
 	
+	g_debugDraw->Submit();
+
 	// Draw World
 	u32 drawFlags = 0;
 	drawFlags += g_settings.drawBounds * b3Draw::e_aabbsFlag;
@@ -370,6 +372,18 @@ void Test::Step()
 
 	g_debugDraw->SetFlags(drawFlags);
 	m_world.DebugDraw();
+	
+	if (m_mouseJoint)
+	{
+		b3Shape* shape = m_rayHit.shape;
+		b3Body* body = shape->GetBody();
+
+		b3Vec3 n = body->GetWorldVector(m_rayHit.normal);
+		b3Vec3 p = body->GetWorldPoint(m_rayHit.point);
+		
+		g_debugDraw->DrawSolidCircle(n, p + 0.05f * n, 1.0f, b3Color_white);
+	}
+
 	g_debugDraw->Submit();
 	
 	if (g_settings.drawFaces)
@@ -453,17 +467,21 @@ void Test::MouseLeftDown(const Ray3& pw)
 		m_world.DestroyBody(groundBody);
 	}
 
+	// Perform the ray cast
 	b3Vec3 p1 = pw.Start();
 	b3Vec3 p2 = pw.End();
 
-	RayCastListener listener;
-	listener.hit.shape = NULL;
-
-	// Perform the ray cast
 	b3RayCastSingleOutput out;
 	if (m_world.RayCastSingle(&out, p1, p2))
 	{
-		m_rayHit = out;		
+		b3Shape* shape = out.shape;
+		b3Body* body = shape->GetBody();
+		
+		m_rayHit.shape = out.shape;
+		m_rayHit.point = body->GetLocalPoint(out.point);
+		m_rayHit.normal = body->GetLocalVector(out.normal);
+		m_rayHit.fraction = out.fraction;
+
 		RayHit();
 	}
 }
@@ -491,7 +509,7 @@ void Test::RayHit()
 	b3MouseJointDef def;
 	def.bodyA = bodyA;
 	def.bodyB = bodyB;
-	def.target = m_rayHit.point;
+	def.target = bodyB->GetWorldPoint(m_rayHit.point);
 	def.maxForce = 2000.0f * bodyB->GetMass();
 
 	m_mouseJoint = (b3MouseJoint*)m_world.CreateJoint(def);

examples/testbed/tests/ray_cast.h

@@ -209,7 +209,8 @@ public:
 		{
 			g_debugDraw->DrawSegment(p1, out.point, b3Color(0.0f, 1.0f, 0.0f));
 			g_debugDraw->DrawPoint(out.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(out.point, out.normal, b3Color(1.0f, 1.0f, 1.0f));
+			g_debugDraw->DrawSolidCircle(out.normal, out.point + 0.05f * out.normal, 1.0f, b3Color(1.0f, 1.0f, 1.0f));
 		}
 		else
 		{

examples/testbed/tests/test.h

@@ -134,7 +134,7 @@ public:
 
 	b3World m_world;
 
-	b3RayCastSingleOutput m_rayHit;
+	b3RayCastSingleOutput m_rayHit; // local space
 	b3BoxHull m_groundHull;
 	b3BoxHull m_boxHull;
 	b3BoxHull m_tallHull;