diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index 6416061..69737b1 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include #include #include @@ -108,7 +108,7 @@ TestEntry g_tests[] = { "Box Pyramid Rows", &Pyramids::Create }, { "Ray Cast", &RayCast::Create }, { "Sensor Test", &SensorTest::Create }, - { "Point & Click", &PointClick::Create }, + { "Character", &CharacterTest::Create }, { "Body Types", &BodyTypes::Create }, { "Varying Friction", &VaryingFriction::Create }, { "Varying Restitution", &VaryingRestitution::Create }, diff --git a/examples/testbed/tests/character_test.h b/examples/testbed/tests/character_test.h new file mode 100644 index 0000000..8cf3c44 --- /dev/null +++ b/examples/testbed/tests/character_test.h @@ -0,0 +1,569 @@ +/* +* 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 CHARACTER_TEST_H +#define CHARACTER_TEST_H + +struct Plane +{ + b3Vec3 n; + b3Vec3 p; +}; + +class CharacterController +{ +public: + CharacterController(b3SphereShape* shape) + { + m_characterShape = shape; + m_characterBody = m_characterShape->GetBody(); + m_world = m_characterBody->GetWorld(); + m_translation.SetZero(); + m_isGrounded = false; + + UpdateGrounded(); + } + + ~CharacterController() + { + + } + + bool IsGrounded() const + { + return m_isGrounded; + } + + void Move(const b3Vec3& translation) + { + 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(); + + UpdateGrounded(); + } +private: + b3Sphere GetCharacterSphere() const + { + b3Sphere sphere; + sphere.vertex = m_characterBody->GetWorldPoint(m_characterShape->m_center); + sphere.radius = m_characterShape->m_radius; + return sphere; + } + + void UpdateGrounded() + { + b3StackArray shapes; + CollectStaticShapes(shapes); + + b3StackArray planes; + CollectOverlapPlanes(planes, shapes); + + m_isGrounded = false; + + for (u32 i = 0; i < planes.Count(); ++i) + { + Plane plane = planes[i]; + + if (b3Dot(b3Vec3_y, plane.n) > 0.0f) + { + m_isGrounded = true; + break; + } + } + } + + void CollectStaticShapes(b3Array& shapes) const + { + for (b3Body* b = m_world->GetBodyList().m_head; b; b = b->GetNext()) + { + if (b == m_characterBody) + { + continue; + } + + if (b->GetType() != e_staticBody) + { + continue; + } + + for (b3Shape* s = b->GetShapeList().m_head; s; s = s->GetNext()) + { + if (s->GetType() == e_meshShape) + { + continue; + } + + shapes.PushBack(s); + } + } + } + + void CollectOverlapPlanes(b3Array& planes, + const b3Array& shapes) const + { + b3Sphere sphere2 = GetCharacterSphere(); + + for (u32 i = 0; i < shapes.Count(); ++i) + { + b3Shape* shape1 = shapes[i]; + b3Body* body1 = shape1->GetBody(); + b3Transform xf1 = body1->GetTransform(); + + b3TestSphereOutput output; + bool overlap = shape1->TestSphere(&output, sphere2, xf1); + + if (overlap == false) + { + continue; + } + + Plane plane; + plane.n = output.normal; + plane.p = output.point; + + planes.PushBack(plane); + } + } + + void CollectSweepPlanes(b3Array& planes, + const b3Array& shapes, + const b3Vec3& translation2) const + { + if (b3LengthSquared(translation2) < B3_EPSILON * B3_EPSILON) + { + return; + } + + b3Transform xf2 = m_characterBody->GetTransform(); + + b3ShapeGJKProxy proxy2; + proxy2.Set(m_characterShape, 0); + + for (u32 i = 0; i < shapes.Count(); ++i) + { + b3Shape* shape1 = shapes[i]; + b3Body* body1 = shape1->GetBody(); + + b3Transform xf1 = body1->GetTransform(); + + b3ShapeGJKProxy proxy1; + proxy1.Set(shape1, 0); + + b3GJKShapeCastOutput output; + bool hit = b3GJKShapeCast(&output, xf1, proxy1, xf2, proxy2, translation2); + + if (hit == false) + { + continue; + } + + Plane plane; + plane.n = output.normal; + plane.p = output.point; + + planes.PushBack(plane); + } + } + + b3Vec3 SolvePositionConstraints(const b3Array& planes, const b3Vec3& position) const + { + b3Vec3 localCenter = m_characterBody->GetLocalCenter(); + b3Vec3 c = position; + b3Quat q = m_characterBody->GetOrientation(); + + const u32 kMaxPositionIterations = 10; + + for (u32 i = 0; i < kMaxPositionIterations; ++i) + { + for (u32 j = 0; j < planes.Count(); ++j) + { + Plane nc = planes[j]; + b3Plane plane(nc.n, nc.p); + + b3Transform xf; + xf.rotation = b3QuatMat33(q); + xf.position = c - b3Mul(xf.rotation, localCenter); + + b3Sphere sphere; + sphere.vertex = b3Mul(xf, m_characterShape->m_center); + sphere.radius = m_characterShape->m_radius; + + float32 separation = b3Distance(sphere.vertex, plane); + + if (separation > sphere.radius) + { + continue; + } + + separation -= sphere.radius; + + float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); + + b3Vec3 normal = plane.normal; + b3Vec3 P = C * normal; + + c -= P; + } + } + + return c; + } + + b3World* m_world; + b3Body* m_characterBody; + b3SphereShape* m_characterShape; + b3Vec3 m_translation; + bool m_isGrounded; +}; + +class CharacterTest : public Test +{ +public: + CharacterTest() + { + { + b3BodyDef bdef; + b3Body* body = m_world.CreateBody(bdef); + + b3HullShape hs; + hs.m_hull = &m_groundHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(-10.0f, 3.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(2.0f, 2.0f, 5.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(-10.0f, 9.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(1.0f, 4.0f, 1.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(-10.0f, 2.7f, 7.0f); + bdef.orientation.Set(b3Vec3_x, 0.25f * B3_PI); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(2.0f, 0.25f, 3.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(-10.0f, 2.7f, -7.0f); + bdef.orientation.Set(b3Vec3_x, -0.25f * B3_PI); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(2.0f, 0.25f, 3.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(10.0f, 3.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(2.0f, 2.0f, 5.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(10.0f, 9.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(1.0f, 4.0f, 1.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(10.0f, 2.7f, 7.0f); + bdef.orientation.Set(b3Vec3_x, 0.25f * B3_PI); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(2.0f, 0.25f, 3.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(10.0f, 2.7f, -7.0f); + bdef.orientation.Set(b3Vec3_x, -0.25f * B3_PI); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(2.0f, 0.25f, 3.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.position.Set(0.0f, 4.0f, 0.0f); + + b3Body* body = m_world.CreateBody(bdef); + + static b3BoxHull boxHull; + boxHull.Set(8.0f, 1.0f, 2.0f); + + b3HullShape hs; + hs.m_hull = &boxHull; + + b3ShapeDef sdef; + sdef.shape = &hs; + + b3Shape* shape = body->CreateShape(sdef); + } + + { + b3BodyDef bdef; + bdef.type = b3BodyType::e_kinematicBody; + bdef.position.Set(0.0f, 1.0f, 20.0f); + + b3Body* body = m_world.CreateBody(bdef); + + b3SphereShape sphere; + sphere.m_center.SetZero(); + sphere.m_radius = 1.0f; + + b3ShapeDef sdef; + sdef.shape = &sphere; + + b3SphereShape* shape = (b3SphereShape*)body->CreateShape(sdef); + + m_characterController = new CharacterController(shape); + } + } + + ~CharacterTest() + { + delete m_characterController; + } + + void Step() + { + g_draw->DrawString(b3Color_white, "Arrows - Walk"); + g_draw->DrawString(b3Color_white, "Space - Jump"); + + float32 dt = g_testSettings->inv_hertz; + + const float32 walkSpeed = 10.0f; + const float32 jumpSpeed = 300.0f; + const float32 gravity = 50.0f * 9.8f; + b3Vec3 velocity = b3Vec3_zero; + + bool isGounded = m_characterController->IsGrounded(); + + if (isGounded) + { + extern GLFWwindow* g_window; + + bool leftDown = glfwGetKey(g_window, GLFW_KEY_LEFT); + bool rightDown = glfwGetKey(g_window, GLFW_KEY_RIGHT); + bool downDown = glfwGetKey(g_window, GLFW_KEY_DOWN); + bool upDown = glfwGetKey(g_window, GLFW_KEY_UP); + bool spaceDown = glfwGetKey(g_window, GLFW_KEY_SPACE); + + // Walk + if (leftDown) + { + velocity.x -= walkSpeed; + } + + if (rightDown) + { + velocity.x += walkSpeed; + } + + if (upDown) + { + velocity.z -= walkSpeed; + } + + if (downDown) + { + velocity.z += walkSpeed; + } + + // Jump + if (spaceDown) + { + velocity.y += jumpSpeed; + } + } + + // Integrate gravity + velocity.y -= dt * gravity; + + // Compute translation + b3Vec3 translation = dt * velocity; + + // Move character + m_characterController->Move(translation); + + // Step controllers + m_characterController->Step(); + + // Step world + Test::Step(); + } + + static Test* Create() + { + return new CharacterTest(); + } + + CharacterController* m_characterController; +}; + +#endif \ No newline at end of file diff --git a/examples/testbed/tests/point_click.h b/examples/testbed/tests/point_click.h deleted file mode 100644 index 0f1f3a1..0000000 --- a/examples/testbed/tests/point_click.h +++ /dev/null @@ -1,98 +0,0 @@ -/* -* 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 POINT_CLICK_H -#define POINT_CLICK_H - -class PointClick : public Test -{ -public: - PointClick() - { - { - b3BodyDef bdef; - b3Body* ground = m_world.CreateBody(bdef); - - b3MeshShape ms; - ms.m_mesh = &m_groundMesh; - - b3ShapeDef sd; - sd.shape = &ms; - - ground->CreateShape(sd); - } - - { - b3BodyDef bdef; - bdef.type = b3BodyType::e_dynamicBody; - bdef.fixedRotationY = true; - bdef.position.Set(0.0f, 5.0f, 0.0f); - - m_character = m_world.CreateBody(bdef); - - b3CapsuleShape cap; - cap.m_centers[0].Set(0.0f, 1.0f, 0.0f); - cap.m_centers[1].Set(0.0f, -1.0f, 0.0f); - cap.m_radius = 1.0f; - - b3ShapeDef sdef; - sdef.shape = ∩ - sdef.density = 1.0f; - sdef.friction = 0.5f; - - m_character->CreateShape(sdef); - } - } - - void BeginDragging() - { - if (m_bodyDragger.GetBody() == m_character) - { - m_bodyDragger.StopDragging(); - } - } - - void Step() - { - if (m_bodyDragger.IsDragging()) - { - if (m_bodyDragger.GetBody() != m_character) - { - b3Vec3 p1 = m_character->GetPosition(); - b3Vec3 p2 = m_bodyDragger.GetPointA(); - - b3Vec3 n = b3Normalize(p2 - p1); - const float32 k = 1000.0f; - b3Vec3 f = k * n; - - m_character->ApplyForceToCenter(f, true); - } - } - - Test::Step(); - } - - static Test* Create() - { - return new PointClick(); - } - - b3Body* m_character; -}; - -#endif \ No newline at end of file