624 lines
12 KiB
C++
624 lines
12 KiB
C++
/*
|
|
* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
|
|
*
|
|
* 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 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<b3Shape*, 32> shapes;
|
|
CollectStaticShapes(shapes);
|
|
|
|
b3StackArray<Plane, 32> planes;
|
|
CollectOverlapPlanes(planes, shapes);
|
|
|
|
m_isGrounded = false;
|
|
|
|
for (u32 i = 0; i < planes.Count(); ++i)
|
|
{
|
|
Plane plane = planes[i];
|
|
|
|
float32 cosine = b3Dot(b3Vec3_y, plane.n);
|
|
|
|
// ~80 degrees
|
|
const float32 kTol = 0.17f;
|
|
|
|
if (cosine > kTol)
|
|
{
|
|
m_isGrounded = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void CollectStaticShapes(b3Array<b3Shape*>& 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())
|
|
{
|
|
shapes.PushBack(s);
|
|
}
|
|
}
|
|
}
|
|
|
|
void CollectOverlapPlanes(b3Array<Plane>& planes,
|
|
const b3Array<b3Shape*>& 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();
|
|
|
|
if (shape1->GetType() == e_meshShape)
|
|
{
|
|
b3MeshShape* meshShape1 = (b3MeshShape*)shape1;
|
|
const b3Mesh* mesh1 = meshShape1->m_mesh;
|
|
|
|
for (u32 j = 0; j < mesh1->triangleCount; ++j)
|
|
{
|
|
b3TestSphereOutput output;
|
|
bool overlap = meshShape1->TestSphere(&output, sphere2, xf1, j);
|
|
|
|
if (overlap)
|
|
{
|
|
Plane plane;
|
|
plane.n = output.normal;
|
|
plane.p = output.point;
|
|
|
|
planes.PushBack(plane);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3TestSphereOutput output;
|
|
bool overlap = shape1->TestSphere(&output, sphere2, xf1);
|
|
|
|
if (overlap)
|
|
{
|
|
Plane plane;
|
|
plane.n = output.normal;
|
|
plane.p = output.point;
|
|
|
|
planes.PushBack(plane);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void CollectSweepPlanes(b3Array<Plane>& planes,
|
|
const b3Array<b3Shape*>& 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();
|
|
|
|
if (shape1->GetType() == e_meshShape)
|
|
{
|
|
b3MeshShape* meshShape1 = (b3MeshShape*)shape1;
|
|
const b3Mesh* mesh1 = meshShape1->m_mesh;
|
|
|
|
for (u32 j = 0; j < mesh1->triangleCount; ++j)
|
|
{
|
|
b3ShapeGJKProxy proxy1;
|
|
proxy1.Set(shape1, j);
|
|
|
|
b3GJKShapeCastOutput output;
|
|
bool hit = b3GJKShapeCast(&output, xf1, proxy1, xf2, proxy2, translation2);
|
|
|
|
if (hit)
|
|
{
|
|
Plane plane;
|
|
plane.n = output.normal;
|
|
plane.p = output.point;
|
|
|
|
planes.PushBack(plane);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
b3ShapeGJKProxy proxy1;
|
|
proxy1.Set(shape1, 0);
|
|
|
|
b3GJKShapeCastOutput output;
|
|
bool hit = b3GJKShapeCast(&output, xf1, proxy1, xf2, proxy2, translation2);
|
|
|
|
if (hit)
|
|
{
|
|
Plane plane;
|
|
plane.n = output.normal;
|
|
plane.p = output.point;
|
|
|
|
planes.PushBack(plane);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
b3Vec3 SolvePositionConstraints(const b3Array<Plane>& 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;
|
|
}
|
|
|
|
void Solve()
|
|
{
|
|
b3StackArray<b3Shape*, 32> shapes;
|
|
CollectStaticShapes(shapes);
|
|
|
|
// Collect collision planes
|
|
b3StackArray<Plane, 32> 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 (;;)
|
|
{
|
|
// Collect collision planes
|
|
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;
|
|
}
|
|
|
|
// Synchronize 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;
|
|
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 = 10.0f;
|
|
const float32 gravityAcc = 9.8f;
|
|
|
|
static b3Vec3 velocity = b3Vec3_zero;
|
|
|
|
velocity.x = 0.0f;
|
|
velocity.z = 0.0f;
|
|
|
|
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);
|
|
|
|
bool isGrounded = m_characterController->IsGrounded();
|
|
|
|
// Walk
|
|
if (leftDown)
|
|
{
|
|
velocity.x -= walkSpeed;
|
|
}
|
|
|
|
if (rightDown)
|
|
{
|
|
velocity.x += walkSpeed;
|
|
}
|
|
|
|
if (upDown)
|
|
{
|
|
velocity.z -= walkSpeed;
|
|
}
|
|
|
|
if (downDown)
|
|
{
|
|
velocity.z += walkSpeed;
|
|
}
|
|
|
|
if (isGrounded)
|
|
{
|
|
velocity.y = 0.0f;
|
|
|
|
// Jump
|
|
if (spaceDown)
|
|
{
|
|
velocity.y += jumpSpeed;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Integrate gravity
|
|
velocity.y -= dt * gravityAcc;
|
|
}
|
|
|
|
// 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 |