From c581dee66ebc20fd0f0ff73fe852ea5cfcce0838 Mon Sep 17 00:00:00 2001 From: Irlan Date: Mon, 6 Mar 2017 13:34:24 -0300 Subject: [PATCH] fix capsule to hull contact generation, add weld joint with an alternative constraint model --- examples/testbed/framework/test_entries.cpp | 2 + examples/testbed/tests/weld_test.h | 94 +++++++ include/bounce/bounce.h | 1 + include/bounce/dynamics/body.h | 1 + include/bounce/dynamics/joints/joint.h | 1 + include/bounce/dynamics/joints/weld_joint.h | 99 ++++++++ premake5.lua | 4 +- .../contacts/collide/collide_capsule_hull.cpp | 81 ++++--- src/bounce/dynamics/draw_world.cpp | 30 +-- src/bounce/dynamics/joints/joint.cpp | 15 +- src/bounce/dynamics/joints/weld_joint.cpp | 229 ++++++++++++++++++ {tests => test}/ignore.txt | 0 12 files changed, 492 insertions(+), 65 deletions(-) create mode 100644 examples/testbed/tests/weld_test.h create mode 100644 include/bounce/dynamics/joints/weld_joint.h create mode 100644 src/bounce/dynamics/joints/weld_joint.cpp rename {tests => test}/ignore.txt (100%) diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index ec470e0..15ff225 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ TestEntry g_tests[] = { "Multiple Shapes", &MultipleShapes::Create }, { "Quadric Shapes", &QuadricShapes::Create }, { "Springs", &Spring::Create }, + { "Weld Test", &WeldTest::Create }, { "Newton's Cradle", &NewtonCradle::Create }, { "Hinge Motor", &HingeMotor::Create }, { "Hinge Chain", &HingeChain::Create }, diff --git a/examples/testbed/tests/weld_test.h b/examples/testbed/tests/weld_test.h new file mode 100644 index 0000000..8f2498c --- /dev/null +++ b/examples/testbed/tests/weld_test.h @@ -0,0 +1,94 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef WELD_TEST_H +#define WELD_TEST_H + +class WeldTest : public Test +{ +public: + WeldTest() + { + { + b3BodyDef bd; + b3Body* ground = m_world.CreateBody(bd); + + b3HullShape shape; + shape.m_hull = &m_groundHull; + + b3ShapeDef sd; + sd.shape = &shape; + + ground->CreateShape(sd); + } + + b3Body* hinge, *door; + + { + b3BodyDef bd; + bd.type = b3BodyType::e_dynamicBody; + bd.position.Set(-2.0f, 5.05f, 0.0f); + hinge = 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_radius = 0.5f; + + b3ShapeDef sd; + sd.shape = &shape; + sd.density = 1.0f; + + hinge->CreateShape(sd); + } + + { + b3BodyDef bd; + bd.type = b3BodyType::e_dynamicBody; + bd.orientation.Set(b3Vec3(1.0f, 0.0f, 0.0f), 0.25f * B3_PI); + bd.position.Set(1.0f, 5.05f, 0.0f); + + door = m_world.CreateBody(bd); + + b3HullShape hull; + hull.m_hull = &m_doorHull; + + b3ShapeDef sdef; + sdef.shape = &hull; + sdef.density = 2.0f; + + door->CreateShape(sdef); + } + + { + b3Vec3 hingeAnchor(-2.0f, 5.0f, 0.0f); + + b3WeldJointDef jd; + jd.Initialize(hinge, door, hingeAnchor); + + b3WeldJoint* wj = (b3WeldJoint*)m_world.CreateJoint(jd); + } + } + + static Test* Create() + { + return new WeldTest(); + } +}; + +#endif \ No newline at end of file diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h index 3b26e1b..8c7ec24 100644 --- a/include/bounce/bounce.h +++ b/include/bounce/bounce.h @@ -41,6 +41,7 @@ #include #include +#include #include #include #include diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h index 79a2489..ba294a2 100644 --- a/include/bounce/dynamics/body.h +++ b/include/bounce/dynamics/body.h @@ -243,6 +243,7 @@ private: friend class b3JointSolver; friend class b3MouseJoint; friend class b3SpringJoint; + friend class b3WeldJoint; friend class b3RevoluteJoint; friend class b3SphereJoint; friend class b3ConeJoint; diff --git a/include/bounce/dynamics/joints/joint.h b/include/bounce/dynamics/joints/joint.h index f0df156..59c3e20 100644 --- a/include/bounce/dynamics/joints/joint.h +++ b/include/bounce/dynamics/joints/joint.h @@ -33,6 +33,7 @@ enum b3JointType e_unknownJoint, e_mouseJoint, e_springJoint, + e_weldJoint, e_revoluteJoint, e_sphereJoint, e_coneJoint, diff --git a/include/bounce/dynamics/joints/weld_joint.h b/include/bounce/dynamics/joints/weld_joint.h new file mode 100644 index 0000000..96c1490 --- /dev/null +++ b/include/bounce/dynamics/joints/weld_joint.h @@ -0,0 +1,99 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_WELD_JOINT_H +#define B3_WELD_JOINT_H + +#include + +struct b3WeldJointDef : public b3JointDef +{ + b3WeldJointDef() + { + type = e_weldJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + relativeRotation.SetIdentity(); + } + + // Initialize this definition from bodies and world anchor point. + void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& anchor); + + // The joint anchor relative body A's origin. + b3Vec3 localAnchorA; + + // The joint anchor relative body B's origin. + b3Vec3 localAnchorB; + + // The initial relative rotation from body A to body B. + b3Quat relativeRotation; +}; + +// A weld joint removes the relative rotation between two bodies. +// You need to specify the relative rotation and the local anchor points. +// @todo Soft this constraint. +class b3WeldJoint : public b3Joint +{ +public: + // Get the anchor point on body A in world coordinates. + b3Vec3 GetAnchorA() const; + + // Get the anchor point on body B in world coordinates. + b3Vec3 GetAnchorB() const; + + // Draw this joint. + void Draw(b3Draw* draw) const; +private: + friend class b3Joint; + friend class b3JointManager; + friend class b3JointSolver; + + b3WeldJoint(const b3WeldJointDef* def); + + virtual void InitializeConstraints(const b3SolverData* data); + virtual void WarmStart(const b3SolverData* data); + virtual void SolveVelocityConstraints(const b3SolverData* data); + virtual bool SolvePositionConstraints(const b3SolverData* data); + + // Solver shared + b3Vec3 m_localAnchorA; + b3Vec3 m_localAnchorB; + + b3Quat m_dq0; + + // Solver temp + u32 m_indexA; + u32 m_indexB; + float32 m_mA; + float32 m_mB; + b3Mat33 m_iA; + b3Mat33 m_iB; + b3Vec3 m_localCenterA; + b3Vec3 m_localCenterB; + + // Point-to-point + b3Vec3 m_rA; + b3Vec3 m_rB; + b3Vec3 m_impulse; + b3Mat33 m_mass; + + b3Vec3 m_axisImpulse; + //b3Vec3 m_velocityBias; +}; + +#endif \ No newline at end of file diff --git a/premake5.lua b/premake5.lua index 064f155..d808df3 100644 --- a/premake5.lua +++ b/premake5.lua @@ -11,8 +11,8 @@ bounce_inc_dir = "include/" bounce_src_dir = "src/" examples_inc_dir = "examples/" examples_src_dir = "examples/" -tests_inc_dir = "tests/" -tests_src_dir = "tests/" +tests_inc_dir = "test/" +tests_src_dir = "test/" obj_dir = "/obj/" bin_dir = "/bin/" diff --git a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp index 8662e7d..db2d1b1 100644 --- a/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp +++ b/src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp @@ -50,7 +50,7 @@ void b3BuildEdgeContact(b3Manifold& manifold, } b3Vec3 PA, PB; - b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, E1, P2, E2); + b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, N1, P2, N2); b3FeaturePair pair = b3MakePair(0, 1, index2, index2 + 1); @@ -82,52 +82,55 @@ void b3BuildFaceContact(b3Manifold& manifold, b3ClipVertex clipEdge1[2]; u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, xf2, index2, hull2); + + // Project clipped edge 1 onto face plane 2. + float32 r1 = hull1->radius; + float32 r2 = B3_HULL_RADIUS; + float32 totalRadius = r1 + r2; + + b3Plane localPlane2 = hull2->GetPlane(index2); + b3Plane plane2 = b3Mul(xf2, localPlane2); + const b3Face* face2 = hull2->GetFace(index2); + const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge); + b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin); + + b3Vec3 normal = -plane2.normal; - // Project. - if (clipCount == 2) + b3Vec3 center; + center.SetZero(); + + u32 pointCount = 0; + for (u32 i = 0; i < clipCount; ++i) { - float32 r1 = hull1->radius; - float32 r2 = B3_HULL_RADIUS; - float32 totalRadius = r1 + r2; - - b3Plane localPlane2 = hull2->GetPlane(index2); - b3Plane plane2 = b3Mul(xf2, localPlane2); - const b3Face* face2 = hull2->GetFace(index2); - const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge); - b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin); - - b3Vec3 cp1 = b3ClosestPointOnPlane(clipEdge1[0].position, plane2); - b3Vec3 cp2 = b3ClosestPointOnPlane(clipEdge1[1].position, plane2); - - float32 s1 = b3Distance(clipEdge1[0].position, plane2); - float32 s2 = b3Distance(clipEdge1[1].position, plane2); - - if (s1 <= totalRadius && s2 <= totalRadius) + float32 s = b3Distance(clipEdge1[i].position, plane2); + if (s <= totalRadius) { - b3Vec3 normal = -plane2.normal; - b3Vec3 p1 = 0.5f * (clipEdge1[0].position + r1 * normal + cp1 - r2 * normal); - b3Vec3 p2 = 0.5f * (clipEdge1[1].position + r1 * normal + cp2 - r2 * normal); + b3Vec3 cp = b3ClosestPointOnPlane(clipEdge1[i].position, plane2); + b3Vec3 p = 0.5f * (clipEdge1[i].position + r1 * normal + cp - r2 * normal); - manifold.pointCount = 2; + b3ManifoldPoint* mp = manifold.points + pointCount; + mp->triangleKey = B3_NULL_TRIANGLE; + mp->key = b3MakeKey(clipEdge1[i].pair); + mp->localNormal = b3MulT(xf1.rotation, normal); + mp->localPoint = b3MulT(xf1, clipEdge1[i].position); + mp->localPoint2 = b3MulT(xf2, cp); - manifold.points[0].triangleKey = B3_NULL_TRIANGLE; - manifold.points[0].key = b3MakeKey(clipEdge1[0].pair); - manifold.points[0].localNormal = b3MulT(xf1.rotation, normal); - manifold.points[0].localPoint = b3MulT(xf1, clipEdge1[0].position); - manifold.points[0].localPoint2 = b3MulT(xf2, cp1); + ++pointCount; - manifold.points[1].triangleKey = B3_NULL_TRIANGLE; - manifold.points[1].key = b3MakeKey(clipEdge1[1].pair); - manifold.points[1].localNormal = b3MulT(xf1.rotation, normal); - manifold.points[1].localPoint = b3MulT(xf1, clipEdge1[1].position); - manifold.points[1].localPoint2 = b3MulT(xf2, cp2); - - manifold.center = 0.5f * (p1 + p2); - manifold.normal = normal; - manifold.tangent1 = b3Perp(normal); - manifold.tangent2 = b3Cross(manifold.tangent1, normal); + center += p; } } + + if (pointCount > 0) + { + center /= pointCount; + + manifold.center = center; + manifold.normal = normal; + manifold.tangent1 = b3Perp(normal); + manifold.tangent2 = b3Cross(manifold.tangent1, normal); + manifold.pointCount = pointCount; + } } void b3CollideCapsuleAndHull(b3Manifold& manifold, diff --git a/src/bounce/dynamics/draw_world.cpp b/src/bounce/dynamics/draw_world.cpp index b8f43b8..105e71b 100644 --- a/src/bounce/dynamics/draw_world.cpp +++ b/src/bounce/dynamics/draw_world.cpp @@ -16,29 +16,7 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include +#include const b3Color b3Color_black(0.0f, 0.0f, 0.0f); const b3Color b3Color_white(1.0f, 1.0f, 1.0f); @@ -251,6 +229,12 @@ void b3World::DrawJoint(const b3Joint* joint) const o->Draw(b3_debugDraw); break; } + case e_weldJoint: + { + b3WeldJoint* o = (b3WeldJoint*)joint; + o->Draw(b3_debugDraw); + break; + } case e_revoluteJoint: { b3RevoluteJoint* o = (b3RevoluteJoint*)joint; diff --git a/src/bounce/dynamics/joints/joint.cpp b/src/bounce/dynamics/joints/joint.cpp index 7fb43dd..2d1593c 100644 --- a/src/bounce/dynamics/joints/joint.cpp +++ b/src/bounce/dynamics/joints/joint.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -40,7 +41,12 @@ b3Joint* b3Joint::Create(const b3JointDef* def) joint = new (block) b3SpringJoint((b3SpringJointDef*)def); break; } - case e_revoluteJoint: + case e_weldJoint: + { + void* block = b3Alloc(sizeof(b3WeldJoint)); + joint = new (block) b3WeldJoint((b3WeldJointDef*)def); + break; + }case e_revoluteJoint: { void* block = b3Alloc(sizeof(b3RevoluteJoint)); joint = new (block) b3RevoluteJoint((b3RevoluteJointDef*)def); @@ -88,6 +94,13 @@ void b3Joint::Destroy(b3Joint* joint) b3Free(joint); break; } + case e_weldJoint: + { + b3WeldJoint* o = (b3WeldJoint*)joint; + o->~b3WeldJoint(); + b3Free(joint); + break; + } case b3JointType::e_revoluteJoint: { b3RevoluteJoint* o = (b3RevoluteJoint*)joint; diff --git a/src/bounce/dynamics/joints/weld_joint.cpp b/src/bounce/dynamics/joints/weld_joint.cpp new file mode 100644 index 0000000..e0709c6 --- /dev/null +++ b/src/bounce/dynamics/joints/weld_joint.cpp @@ -0,0 +1,229 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include + +void b3WeldJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchor) +{ + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + + b3Quat qA = bodyA->GetOrientation(); + b3Quat qB = bodyB->GetOrientation(); + + relativeRotation = b3Conjugate(qA) * qB; +} + +b3WeldJoint::b3WeldJoint(const b3WeldJointDef* def) +{ + m_type = e_weldJoint; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_dq0 = def->relativeRotation; + m_impulse.SetZero(); + m_axisImpulse.SetZero(); +} + +void b3WeldJoint::InitializeConstraints(const b3SolverData* data) +{ + b3Body* m_bodyA = GetBodyA(); + b3Body* m_bodyB = GetBodyB(); + + m_indexA = m_bodyA->m_islandID; + m_indexB = m_bodyB->m_islandID; + m_mA = m_bodyA->m_invMass; + m_mB = m_bodyB->m_invMass; + m_iA = m_bodyA->m_worldInvI; + m_iB = m_bodyB->m_worldInvI; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + + b3Quat qA = data->positions[m_indexA].q; + b3Quat qB = data->positions[m_indexB].q; + + { + // Compute effective mass for the block solver + m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB); + + b3Mat33 RA = b3Skew(m_rA); + b3Mat33 RAT = b3Transpose(RA); + b3Mat33 RB = b3Skew(m_rB); + b3Mat33 RBT = b3Transpose(RB); + b3Mat33 M = b3Diagonal(m_mA + m_mB); + + m_mass = M + RA * m_iA * RAT + RB * m_iB * RBT; + } +} + +void b3WeldJoint::WarmStart(const b3SolverData* data) +{ + b3Vec3 vA = data->velocities[m_indexA].v; + b3Vec3 wA = data->velocities[m_indexA].w; + b3Vec3 vB = data->velocities[m_indexB].v; + b3Vec3 wB = data->velocities[m_indexB].w; + + vA -= m_mA * m_impulse; + wA -= m_iA * (b3Cross(m_rA, m_impulse) + m_axisImpulse); + + vB += m_mB * m_impulse; + wB += m_iB * (b3Cross(m_rB, m_impulse) + m_axisImpulse); + + data->velocities[m_indexA].v = vA; + data->velocities[m_indexA].w = wA; + data->velocities[m_indexB].v = vB; + data->velocities[m_indexB].w = wB; +} + +void b3WeldJoint::SolveVelocityConstraints(const b3SolverData* data) +{ + b3Vec3 vA = data->velocities[m_indexA].v; + b3Vec3 wA = data->velocities[m_indexA].w; + b3Vec3 vB = data->velocities[m_indexB].v; + b3Vec3 wB = data->velocities[m_indexB].w; + + b3Quat qA = data->positions[m_indexA].q; + b3Quat qB = data->positions[m_indexB].q; + + { + b3Vec3 Cdot = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA); + b3Vec3 impulse = m_mass.Solve(-Cdot); + + m_impulse += impulse; + + vA -= m_mA * impulse; + wA -= m_iA * b3Cross(m_rA, impulse); + + vB += m_mB * impulse; + wB += m_iB * b3Cross(m_rB, impulse); + } + + { + b3Vec3 Cdot = wB - wA; + b3Vec3 impulse = -Cdot; + + m_axisImpulse += impulse; + + wA -= m_iA * impulse; + wB += m_iB * impulse; + } + + data->velocities[m_indexA].v = vA; + data->velocities[m_indexA].w = wA; + data->velocities[m_indexB].v = vB; + data->velocities[m_indexB].w = wB; +} + +bool b3WeldJoint::SolvePositionConstraints(const b3SolverData* data) +{ + b3Vec3 xA = data->positions[m_indexA].x; + b3Quat qA = data->positions[m_indexA].q; + b3Vec3 xB = data->positions[m_indexB].x; + b3Quat qB = data->positions[m_indexB].q; + + float32 linearError = 0.0f; + + { + // Compute effective mass + b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA); + b3Vec3 rB = b3Mul(qB, m_localAnchorB - m_localCenterB); + + b3Mat33 M = b3Diagonal(m_mA + m_mB); + b3Mat33 RA = b3Skew(rA); + b3Mat33 RAT = b3Transpose(RA); + b3Mat33 RB = b3Skew(rB); + b3Mat33 RBT = b3Transpose(RB); + + b3Mat33 mass = M + RA * m_iA * RAT + RB * m_iB * RBT; + + b3Vec3 C = xB + rB - xA - rA; + b3Vec3 impulse = mass.Solve(-C); + + xA -= m_mA * impulse; + qA -= b3Derivative(qA, b3Mul(m_iA, b3Cross(rA, impulse))); + qA.Normalize(); + + xB += m_mB * impulse; + qB += b3Derivative(qB, b3Mul(m_iB, b3Cross(rB, impulse))); + qB.Normalize(); + + linearError += b3Length(C); + } + + float32 angularError = 0.0f; + + { + // qC = inv(dq0) * dq = q_identity + // qB - qA - q_rel0 + // Small angle approximation + // C = 2 * sin(theta / 2) = theta + b3Quat dq = b3Conjugate(m_dq0) * b3Conjugate(qA) * qB; + if (dq.w < 0.0f) + { + dq.x = -dq.x; + dq.y = -dq.y; + dq.z = -dq.z; + } + b3Vec3 axis(dq.x, dq.y, dq.z); + axis = b3Mul(qA, axis); + + b3Vec3 C = 2.0f * axis; + + b3Vec3 P = -C; + + qA -= b3Derivative(qA, m_iA * P); + qA.Normalize(); + + qB += b3Derivative(qB, m_iB * P); + qB.Normalize(); + + angularError += 2.0f * b3Length(C); + } + + data->positions[m_indexA].x = xA; + data->positions[m_indexA].q = qA; + data->positions[m_indexB].x = xB; + data->positions[m_indexB].q = qB; + + return linearError <= B3_LINEAR_SLOP && angularError <= B3_ANGULAR_SLOP; +} + +b3Vec3 b3WeldJoint::GetAnchorA() const +{ + return GetBodyA()->GetWorldPoint(m_localAnchorA); +} + +b3Vec3 b3WeldJoint::GetAnchorB() const +{ + return GetBodyB()->GetWorldPoint(m_localAnchorB); +} + +void b3WeldJoint::Draw(b3Draw* draw) const +{ + b3Vec3 a = GetAnchorA(); + b3Vec3 b = GetAnchorB(); + + draw->DrawPoint(a, 4.0f, b3Color_red); + draw->DrawPoint(b, 4.0f, b3Color_green); + draw->DrawSegment(a, b, b3Color_yellow); +} diff --git a/tests/ignore.txt b/test/ignore.txt similarity index 100% rename from tests/ignore.txt rename to test/ignore.txt