diff --git a/examples/testbed/tests/character_test.h b/examples/testbed/tests/character_test.h index 8cf3c44..bc4b7b2 100644 --- a/examples/testbed/tests/character_test.h +++ b/examples/testbed/tests/character_test.h @@ -54,52 +54,6 @@ public: m_translation += translation; } - void Solve() - { - b3StackArray shapes; - CollectStaticShapes(shapes); - - b3StackArray planes; - CollectOverlapPlanes(planes, shapes); - - b3Vec3 startPosition = m_characterBody->GetWorldCenter(); - - b3Vec3 targetPosition = startPosition + m_translation; - - m_translation.SetZero(); - - b3Vec3 solvePosition = SolvePositionConstraints(planes, targetPosition); - - b3Vec3 oldSolvePosition = solvePosition; - - for (;;) - { - b3Vec3 translation = solvePosition - startPosition; - - CollectSweepPlanes(planes, shapes, translation); - - solvePosition = SolvePositionConstraints(planes, targetPosition); - - const float32 tolerance = 0.05f; - - if (b3DistanceSquared(oldSolvePosition, solvePosition) < tolerance * tolerance) - { - break; - } - - oldSolvePosition = solvePosition; - } - - // Update body - b3Quat orientation = m_characterBody->GetOrientation(); - - b3Vec3 axis; - float32 angle; - orientation.GetAxisAngle(&axis, &angle); - - m_characterBody->SetTransform(solvePosition, axis, angle); - } - void Step() { Solve(); @@ -274,6 +228,52 @@ private: return c; } + void Solve() + { + b3StackArray shapes; + CollectStaticShapes(shapes); + + b3StackArray planes; + CollectOverlapPlanes(planes, shapes); + + b3Vec3 startPosition = m_characterBody->GetWorldCenter(); + + b3Vec3 targetPosition = startPosition + m_translation; + + m_translation.SetZero(); + + b3Vec3 solvePosition = SolvePositionConstraints(planes, targetPosition); + + b3Vec3 oldSolvePosition = solvePosition; + + for (;;) + { + b3Vec3 translation = solvePosition - startPosition; + + CollectSweepPlanes(planes, shapes, translation); + + solvePosition = SolvePositionConstraints(planes, targetPosition); + + const float32 tolerance = 0.05f; + + if (b3DistanceSquared(oldSolvePosition, solvePosition) < tolerance * tolerance) + { + break; + } + + oldSolvePosition = solvePosition; + } + + // Update body + b3Quat orientation = m_characterBody->GetOrientation(); + + b3Vec3 axis; + float32 angle; + orientation.GetAxisAngle(&axis, &angle); + + m_characterBody->SetTransform(solvePosition, axis, angle); + } + b3World* m_world; b3Body* m_characterBody; b3SphereShape* m_characterShape;