generate contact when body type changes at runtime, decoupling, add first ray cast hit query to world query (more to add later such as sphere/box/convex casts), hotfix

This commit is contained in:
Irlan Robson 2017-01-13 17:17:02 -02:00
parent caa9d703b5
commit 7d0f06fea2
18 changed files with 209 additions and 174 deletions

3
.gitignore vendored
View File

@ -1,4 +1,5 @@
*.sln
*.VC
*.vcxproj
*.vcxproj.user
*.vcxproj.filters
@ -8,6 +9,7 @@
*.opendb
*.ilk
*.pdb
*.ini
*.idb
*.pch
*.ipch
@ -26,5 +28,6 @@
*.o
*.a
*.i
*.DS_Store
build/
doc/

View File

@ -49,6 +49,10 @@ public:
// Return true if the proxy has moved.
bool MoveProxy(i32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement);
// Add a proxy to the list of moved proxies.
// Only moved proxies will be used internally as an AABB query reference object.
void BufferMove(i32 proxyId);
// Get the AABB of a given proxy.
const b3AABB3& GetAABB(i32 proxyId) const;
@ -77,10 +81,6 @@ public:
private :
friend class b3DynamicTree;
// Add a proxy to the list of moved proxies.
// Only moved proxies will be used as an AABB query reference object.
void BufferMove(i32 proxyId);
// The client callback used to add an overlapping pair
// to the overlapping pair buffer.
bool Report(i32 proxyId);

View File

@ -208,34 +208,6 @@ inline b3Mat33 b3Outer(const b3Vec3& a, const b3Vec3& b)
return b3Mat33(b.x * a, b.y * a, b.z * a);
}
// Move an inertia tensor from the its current center
// to another.
inline b3Mat33 b3MoveToCOM(const b3Mat33& inertia, float32 mass, const b3Vec3& center)
{
// Paralell Axis Theorem
// J = I + m * dot(r, r) * E - outer(r, r)
// where
// I - inertia about the center of mass
// m - mass
// E - identity 3x3
// r - displacement vector from the current com to the new com
// J - inertia tensor at the new center of rotation
float32 dd = b3Dot(center, center);
b3Mat33 A = b3Diagonal(mass * dd);
b3Mat33 B = b3Outer(center, center);
return inertia + A - B;
}
// Compute the inertia matrix of a body measured in
// inertial frame (variable over time) given the
// inertia matrix in body-fixed frame (constant)
// and a rotation matrix representing the orientation
// of the body frame relative to the inertial frame.
inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Mat33& rotation)
{
return rotation * inertia * b3Transpose(rotation);
}
// Compute an orthogonal basis given one of its vectors.
// The vector must be normalized.
inline b3Mat33 b3Basis(const b3Vec3& a)
@ -250,6 +222,7 @@ inline b3Mat33 b3Basis(const b3Vec3& a)
{
A.y.Set(0.0f, a.z, -a.y);
}
A.x = a;
A.y = b3Normalize(A.y);
A.z = b3Cross(a, A.y);
return A;

View File

@ -24,6 +24,7 @@
#include <bounce/common/math/quat.h>
#include <bounce/common/math/transform.h>
#include <bounce/common/template/list.h>
#include <bounce/dynamics/time_step.h>
class b3World;
class b3Shape;
@ -328,32 +329,6 @@ inline b3BodyType b3Body::GetType() const
return m_type;
}
inline void b3Body::SetType(b3BodyType type)
{
if (m_type == type)
{
return;
}
m_type = type;
ResetMass();
if (m_type == e_staticBody)
{
m_linearVelocity.SetZero();
m_angularVelocity.SetZero();
SynchronizeShapes();
}
SetAwake(true);
m_force.SetZero();
m_torque.SetZero();
DestroyContacts();
}
inline void* b3Body::GetUserData() const
{
return m_userData;
@ -430,6 +405,11 @@ inline const b3Sweep& b3Body::GetSweep() const
return m_sweep;
}
inline bool b3Body::IsAwake() const
{
return (m_flags & e_awakeFlag) != 0;
}
inline void b3Body::SetAwake(bool flag)
{
if (flag)
@ -451,11 +431,6 @@ inline void b3Body::SetAwake(bool flag)
}
}
inline bool b3Body::IsAwake() const
{
return (m_flags & e_awakeFlag) != 0;
}
inline float32 b3Body::GetGravityScale() const
{
return m_gravityScale;

View File

@ -51,6 +51,34 @@ enum b3LimitState
e_equalLimits
};
// Move an inertia tensor from the its current center
// to another.
inline b3Mat33 b3MoveToCOM(const b3Mat33& inertia, float32 mass, const b3Vec3& center)
{
// Paralell Axis Theorem
// J = I + m * dot(r, r) * E - outer(r, r)
// where
// I - inertia about the center of mass
// m - mass
// E - identity 3x3
// r - displacement vector from the current com to the new com
// J - inertia tensor at the new center of rotation
float32 dd = b3Dot(center, center);
b3Mat33 A = b3Diagonal(mass * dd);
b3Mat33 B = b3Outer(center, center);
return inertia + A - B;
}
// Compute the inertia matrix of a body measured in
// inertial frame (variable over time) given the
// inertia matrix in body-fixed frame (constant)
// and a rotation matrix representing the orientation
// of the body frame relative to the inertial frame.
inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Mat33& rotation)
{
return rotation * inertia * b3Transpose(rotation);
}
// Compute the time derivative of an orientation given
// the angular velocity of the rotating frame represented by the orientation.
inline b3Quat b3Derivative(const b3Quat& orientation, const b3Vec3& velocity)

View File

@ -85,11 +85,19 @@ public:
// Perform a ray cast with the world.
// The given ray cast listener will be notified when a ray intersects a shape
// in the world. The ray cast output is the intercepted shape, the intersection
// in the world.
// The ray cast output is the intercepted shape, the intersection
// point in world space, the face normal on the shape associated with the point,
// and the intersection fraction.
void RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
// Convenience function.
// Perform a ray cast with the world.
// If there is an intersection then the given ray cast listener will be notified once with
// the shape closest to the ray origin and the associated ray cast output.
// @todo Centralize all queries to a common scene query class?
void RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
// Perform a AABB cast with the world.
// The query listener will be notified when two shape AABBs are overlapping.
// If the listener returns false then the query is stopped immediately.

View File

@ -61,9 +61,9 @@ public:
void RayHit()
{
if (m_rayHit.m_shape)
if (m_rayHit.shape)
{
if (m_rayHit.m_shape->GetBody() != m_character)
if (m_rayHit.shape->GetBody() != m_character)
{
Test::RayHit();
}
@ -72,12 +72,12 @@ public:
void Step()
{
if (m_rayHit.m_shape)
if (m_rayHit.shape)
{
if (m_rayHit.m_shape->GetBody() != m_character)
if (m_rayHit.shape->GetBody() != m_character)
{
b3Vec3 point = m_rayHit.m_point;
b3Vec3 normal = m_rayHit.m_normal;
b3Vec3 point = m_rayHit.point;
b3Vec3 normal = m_rayHit.normal;
const b3Transform& xf = m_character->GetTransform();
b3Vec3 n = point - xf.position;

View File

@ -206,16 +206,16 @@ public:
{
// Perform the ray cast
RayCastListener listener;
m_world.RayCast(&listener, p1, p2);
listener.hit.shape = NULL;
m_world.RayCastFirst(&listener, p1, p2);
i32 hitId = listener.FindClosestHit();
if (hitId >= 0)
RayCastHit hit = listener.hit;
if (hit.shape)
{
// Replace current hit
RayCastHit hit = listener.m_hits[hitId];
g_debugDraw->DrawSegment(p1, hit.m_point, b3Color(0.0f, 1.0f, 0.0f));
g_debugDraw->DrawPoint(hit.m_point, b3Color(1.0f, 0.0f, 0.0f));
g_debugDraw->DrawSegment(hit.m_point, hit.m_point + hit.m_normal, b3Color(1.0f, 1.0f, 1.0f));
g_debugDraw->DrawSegment(p1, hit.point, b3Color(0.0f, 1.0f, 0.0f));
g_debugDraw->DrawPoint(hit.point, b3Color(1.0f, 0.0f, 0.0f));
g_debugDraw->DrawSegment(hit.point, hit.point + hit.normal, b3Color(1.0f, 1.0f, 1.0f));
}
else
{

View File

@ -19,10 +19,11 @@
#ifndef TEST_H
#define TEST_H
#include <bounce/bounce.h>
#include <glfw/glfw3.h>
#include <imgui/imgui.h>
#include "../framework/debug_draw.h"
#include <bounce/bounce.h>
struct Settings
{
@ -83,53 +84,14 @@ struct TestEntry
TestCreate create;
};
enum TestType
{
// Collision
e_QHull,
e_Cluster,
e_Distance,
e_CapsuleDistance,
e_CapsuleAndCapsule,
e_CapsuleAndHull,
e_HullAndHull,
// Dynamics
// Joints
e_NewtonCradle,
e_Vehicle,
e_Door,
e_HingeChain,
e_Ragdoll,
// Contacts
e_Quadric,
e_MeshContact,
// World
e_SphereStack,
e_CapsuleStack,
e_BoxStack,
e_ShapeStack,
e_Jenga,
e_Thin,
e_Pyramid,
e_Pyramids,
// World Queries
e_RayCast,
e_SensorTest,
e_Character,
e_BodyTypes,
e_VaryingFriction,
e_VaryingRestitution,
e_testCount
};
extern TestEntry g_tests[e_testCount];
extern TestEntry g_tests[];
struct RayCastHit
{
b3Shape* m_shape;
b3Vec3 m_point;
b3Vec3 m_normal;
float32 m_fraction;
b3Shape* shape;
b3Vec3 point;
b3Vec3 normal;
float32 fraction;
};
class RayCastListener : public b3RayCastListener
@ -137,35 +99,14 @@ class RayCastListener : public b3RayCastListener
public:
float32 ReportShape(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction)
{
RayCastHit hit;
hit.m_shape = shape;
hit.m_point = point;
hit.m_normal = normal;
hit.m_fraction = fraction;
m_hits.PushBack(hit);
// Continue.
hit.shape = shape;
hit.point = point;
hit.normal = normal;
hit.fraction = fraction;
return 1.0f;
}
int FindClosestHit() const
{
float32 minFraction = FLT_MAX;
int minIndex = -1;
for (u32 i = 0; i < m_hits.Count(); ++i)
{
const RayCastHit* hit = m_hits.Get(i);
if (hit->m_fraction < minFraction)
{
minFraction = hit->m_fraction;
minIndex = i;
}
}
return minIndex;
}
b3StackArray<RayCastHit, 256> m_hits;
RayCastHit hit;
};
class Test : public b3ContactListener

View File

@ -195,7 +195,7 @@ if os.is "windows" then
newaction
{
trigger = "solution",
description = "Build solution",
description = "Generate solution",
execute = function ()
os.execute ( "premake5 clean" )
os.execute ( "premake5 vs2015" )
@ -205,7 +205,7 @@ if os.is "windows" then
newaction
{
trigger = "doc",
description = "Build documentation",
description = "Generate documentation",
execute = function ()
os.execute ( "doxygen doxyfile" )
os.execute ( "start doc\\api\\html\\index.html" )

View File

@ -335,6 +335,41 @@ bool b3Body::ShouldCollide(const b3Body* other) const
return true;
}
void b3Body::SetType(b3BodyType type)
{
if (m_type == type)
{
return;
}
m_type = type;
ResetMass();
m_force.SetZero();
m_torque.SetZero();
if (m_type == e_staticBody)
{
m_linearVelocity.SetZero();
m_angularVelocity.SetZero();
m_sweep.worldCenter0 = m_sweep.worldCenter;
m_sweep.orientation0 = m_sweep.orientation;
SynchronizeShapes();
}
SetAwake(true);
DestroyContacts();
// Move the shape proxies so new contacts can be created.
b3BroadPhase* phase = &m_world->m_contactMan.m_broadPhase;
for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
{
phase->BufferMove(s->m_broadPhaseID);
}
}
void b3Body::Dump() const
{
i32 bodyIndex = m_islandID;

View File

@ -79,7 +79,7 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
float32 d1 = b3Distance(clipEdgeA[0].position, cp1);
float32 d2 = b3Distance(clipEdgeA[1].position, cp2);
if (d1 <= totalRadius && d2 <= totalRadius)
if (d1 > B3_EPSILON && d1 <= totalRadius && d2 > B3_EPSILON && d2 <= totalRadius)
{
b3Vec3 n1 = (cp1 - clipEdgeA[0].position) / d1;
b3Vec3 n2 = (cp2 - clipEdgeA[1].position) / d2;
@ -123,7 +123,7 @@ void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
float32 distance = b3Distance(pointA, pointB);
if (distance > 0.0f)
if (distance > B3_EPSILON)
{
b3Vec3 normal = (pointB - pointA) / distance;
b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal);

View File

@ -17,6 +17,7 @@
*/
#include <bounce/dynamics/shapes/capsule_shape.h>
#include <bounce/dynamics/time_step.h>
b3CapsuleShape::b3CapsuleShape()
{

View File

@ -17,6 +17,7 @@
*/
#include <bounce/dynamics/shapes/sphere_shape.h>
#include <bounce/dynamics/time_step.h>
b3SphereShape::b3SphereShape()
{

View File

@ -377,6 +377,68 @@ void b3World::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec
m_contactMan.m_broadPhase.RayCast(&callback, input);
}
struct b3RayCastFirstCallback
{
float32 Report(const b3RayCastInput& input, i32 proxyId)
{
// Get shape associated with the proxy.
void* userData = broadPhase->GetUserData(proxyId);
b3Shape* shape = (b3Shape*)userData;
// Get map from shape local space to world space.
b3Transform xf = shape->GetBody()->GetTransform();
b3RayCastOutput output;
bool hit = shape->RayCast(&output, input, xf);
if (hit)
{
// Track minimum time of impact to require less memory.
if (output.fraction < output0.fraction)
{
shape0 = shape;
output0 = output;
}
}
// Continue the search from where we stopped.
return input.maxFraction;
}
b3Shape* shape0;
b3RayCastOutput output0;
const b3BroadPhase* broadPhase;
};
void b3World::RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const
{
b3RayCastInput input;
input.p1 = p1;
input.p2 = p2;
input.maxFraction = 1.0f;
b3RayCastFirstCallback callback;
callback.shape0 = NULL;
callback.output0.fraction = B3_MAX_FLOAT;
callback.broadPhase = &m_contactMan.m_broadPhase;
// Perform the ray cast.
m_contactMan.m_broadPhase.RayCast(&callback, input);
if (callback.shape0)
{
// Ray hits closest shape.
float32 fraction = callback.output0.fraction;
float32 w1 = 1.0f - fraction;
float32 w2 = fraction;
b3Vec3 point = w1 * input.p1 + w2 * input.p2;
b3Vec3 normal = callback.output0.normal;
// Report the intersection to the user.
listener->ReportShape(callback.shape0, point, normal, fraction);
}
}
struct b3QueryAABBCallback
{
bool Report(i32 proxyID)

View File

@ -25,6 +25,7 @@
GLFWwindow* g_window;
Settings g_settings;
Test* g_test;
u32 g_testCount;
Camera g_camera;
DebugDraw* g_debugDraw;
bool g_leftDown;
@ -234,7 +235,7 @@ void Interface()
ImGui::PushItemWidth(-1.0f);
ImGui::Text("Test");
if (ImGui::Combo("##Test", &g_settings.testID, GetTestName, NULL, e_testCount, e_testCount))
if (ImGui::Combo("##Test", &g_settings.testID, GetTestName, NULL, g_testCount, g_testCount))
{
delete g_test;
g_test = g_tests[g_settings.testID].create();
@ -248,12 +249,12 @@ void Interface()
}
if (ImGui::Button("Previous", buttonSize))
{
g_settings.testID = b3Clamp(g_settings.testID - 1, 0, int(e_testCount) - 1);
g_settings.testID = b3Clamp(g_settings.testID - 1, 0, int(g_testCount) - 1);
g_settings.lastTestID = -1;
}
if (ImGui::Button("Next", buttonSize))
{
g_settings.testID = b3Clamp(g_settings.testID + 1, 0, int(e_testCount) - 1);
g_settings.testID = b3Clamp(g_settings.testID + 1, 0, int(g_testCount) - 1);
g_settings.lastTestID = -1;
}
if (ImGui::Button("Exit", buttonSize))
@ -419,12 +420,12 @@ int main(int argc, char** args)
sprintf(title, "Bounce Testbed Version %d.%d.%d", b3_version.major, b3_version.minor, b3_version.revision);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
g_window = glfwCreateWindow(1024, 768, title, NULL, NULL);
if (g_window == NULL)
{
fprintf(stderr, "Failed to opengl GLFW window\n");
fprintf(stderr, "Failed to open GLFW window\n");
glfwTerminate();
return -1;
}
@ -436,11 +437,13 @@ int main(int argc, char** args)
glfwSetKeyCallback(g_window, KeyButton);
glfwSetCharCallback(g_window, Char);
glfwSwapInterval(1);
if (gladLoadGL() == 0)
{
fprintf(stderr, "Failed to load OpenGL extensions\n");
fprintf(stderr, "Error: %d\n", glad_glGetError());
glfwTerminate();
exit(EXIT_FAILURE);
return -1;
}
printf("OpenGL %s, GLSL %s\n", glGetString(GL_VERSION), glGetString(GL_SHADING_LANGUAGE_VERSION));
@ -457,7 +460,13 @@ int main(int argc, char** args)
g_debugDraw = new DebugDraw();
// Run the testbed
g_testCount = 0;
while (g_tests[g_testCount].create != NULL)
{
++g_testCount;
}
g_test = NULL;
Run();
// Destroy the last test
@ -475,4 +484,5 @@ int main(int argc, char** args)
// Destroy g_window
glfwTerminate();
}
return 0;
}

View File

@ -49,7 +49,7 @@ Test::Test()
g_camera.m_center.SetZero();
g_settings.drawGrid = false;
m_rayHit.m_shape = NULL;
m_rayHit.shape = NULL;
m_mouseJoint = NULL;
{
@ -358,7 +358,7 @@ void Test::MouseMove(const Ray3& pw)
{
if (m_mouseJoint)
{
float32 hitFraction = m_rayHit.m_fraction;
float32 hitFraction = m_rayHit.fraction;
float32 w1 = 1.0f - hitFraction;
float32 w2 = hitFraction;
@ -370,7 +370,7 @@ void Test::MouseMove(const Ray3& pw)
void Test::MouseLeftDown(const Ray3& pw)
{
// Clear the current hit
m_rayHit.m_shape = NULL;
m_rayHit.shape = NULL;
if (m_mouseJoint)
{
b3Body* groundBody = m_mouseJoint->GetBodyA();
@ -384,25 +384,23 @@ void Test::MouseLeftDown(const Ray3& pw)
b3Vec3 p1 = pw.Start();
b3Vec3 p2 = pw.End();
// Perform the ray cast
RayCastListener listener;
m_world.RayCast(&listener, p1, p2);
listener.hit.shape = NULL;
int hitId = listener.FindClosestHit();
// Perform the ray cast
m_world.RayCastFirst(&listener, p1, p2);
if (hitId >= 0)
if (listener.hit.shape)
{
// Hit
// Replace current hit
m_rayHit = listener.m_hits[hitId];
m_rayHit = listener.hit;
RayHit();
}
}
void Test::MouseLeftUp(const Ray3& pw)
{
m_rayHit.m_shape = NULL;
m_rayHit.shape = NULL;
if (m_mouseJoint)
{
b3Body* groundBody = m_mouseJoint->GetBodyA();
@ -418,12 +416,12 @@ void Test::RayHit()
{
b3BodyDef bdef;
b3Body* bodyA = m_world.CreateBody(bdef);
b3Body* bodyB = m_rayHit.m_shape->GetBody();
b3Body* bodyB = m_rayHit.shape->GetBody();
b3MouseJointDef def;
def.bodyA = bodyA;
def.bodyB = bodyB;
def.target = m_rayHit.m_point;
def.target = m_rayHit.point;
def.maxForce = 2000.0f * bodyB->GetMass();
m_mouseJoint = (b3MouseJoint*)m_world.CreateJoint(def);

View File

@ -47,7 +47,7 @@
#include <testbed/tests/varying_friction.h>
#include <testbed/tests/varying_restitution.h>
TestEntry g_tests[e_testCount] =
TestEntry g_tests[] =
{
{ "Quickhull Test", &QuickhullTest::Create },
{ "Cluster Test", &Cluster::Create },