From 03ebca5728d66bf09cf3baaa236fa845e0a2e1cc Mon Sep 17 00:00:00 2001 From: Irlan Date: Fri, 3 Mar 2017 16:46:13 -0300 Subject: [PATCH] fix and improve circle debug drawing --- examples/testbed/framework/debug_draw.cpp | 99 +++++++++++++++-------- examples/testbed/framework/test.cpp | 30 +++++-- examples/testbed/tests/ray_cast.h | 3 +- examples/testbed/tests/test.h | 2 +- 4 files changed, 92 insertions(+), 42 deletions(-) diff --git a/examples/testbed/framework/debug_draw.cpp b/examples/testbed/framework/debug_draw.cpp index 22d342d..5d35051 100644 --- a/examples/testbed/framework/debug_draw.cpp +++ b/examples/testbed/framework/debug_draw.cpp @@ -1137,7 +1137,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, color.a); - + b3Vec3 p1 = vertices[0]; for (u32 i = 1; i < count - 1; ++i) { @@ -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); - DrawPolygon(vertices, count, edgeColor); + b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f); + 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); - float32 sinInc = sin(kAngleInc); - float32 tInc = 1.0f - cosInc; + b3Quat q(normal, kAngleInc); - b3Vec3 n1 = b3Perp(normal); - b3Vec3 v1 = center + radius * n1; + b3Vec3 p1 = center + radius * n1; for (u32 i = 0; i < kEdgeCount; ++i) { - // Rodrigues' rotation formula - b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal; - b3Vec3 v2 = center + radius * n2; + b3Vec3 n2 = b3Mul(q, n1); + b3Vec3 p2 = center + radius * n2; - m_lines->Vertex(v1, color); - m_lines->Vertex(v2, color); + m_lines->Vertex(p1, 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; - float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount); - float32 cosInc = cos(kAngleInc); - float32 sinInc = sin(kAngleInc); - float32 tInc = 1.0f - cosInc; + 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); + + // 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)); - b3Vec3 n1 = b3Perp(normal); - b3Vec3 v1 = center + radius * n1; - for (u32 i = 0; i < kEdgeCount; ++i) + // Handle edge cases (zero cross product). + b3Vec3 n1; + if (b3LengthSquared(u) > b3LengthSquared(v)) { - // Rodrigues' rotation formula - b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal; - b3Vec3 v2 = center + radius * n2; - - m_triangles->Vertex(center, color, normal); - m_triangles->Vertex(v1, color, normal); - m_triangles->Vertex(v2, color, normal); - - n1 = n2; - v1 = v2; + n1 = u; + } + else + { + n1 = v; } - b3Color frameColor(0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 1.0f); - DrawCircle(normal, center, radius, frameColor); + n1.Normalize(); + + // 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) diff --git a/examples/testbed/framework/test.cpp b/examples/testbed/framework/test.cpp index 4117533..8e33a3f 100644 --- a/examples/testbed/framework/test.cpp +++ b/examples/testbed/framework/test.cpp @@ -357,6 +357,8 @@ void Test::Step() m_world.Step(dt, g_settings.velocityIterations, g_settings.positionIterations); ProfileEnd(); + + g_debugDraw->Submit(); // Draw World u32 drawFlags = 0; @@ -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); diff --git a/examples/testbed/tests/ray_cast.h b/examples/testbed/tests/ray_cast.h index 80def88..541ccc8 100644 --- a/examples/testbed/tests/ray_cast.h +++ b/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 { diff --git a/examples/testbed/tests/test.h b/examples/testbed/tests/test.h index da43de6..0b49a67 100644 --- a/examples/testbed/tests/test.h +++ b/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;