AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

improve segment vs segment collision, small fixes

Browse Source
This commit is contained in:
Irlan
2017-02-17 13:52:24 -02:00
parent a346a1472d
commit 012460f6b0
16 changed files with 175 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
src/bounce/collision/distance.cpp
View File

@ -157,32 +157,41 @@ void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& N1,
const b3Vec3& P2, const b3Vec3& N2)
{
float32 a12 = -b3Dot(N1, N2);
float32 a21 = -a12;
// sin^2 = 1 - cos^2
// or
// sin = norm( cross(n1, n2) )
const float32 kTol = 0.0f;
float32 det = -1.0f - a12 * a21;
if (det == 0.0f)
float32 c = b3Dot(N1, N2);
float32 den = 1.0f - c * c;
if (den < kTol * kTol)
{
// Nearly paralell lines.
*C1 = P1;
*C2 = P2;
return;
}
det = 1.0f / det;
den = 1.0f / den;
// a = dot(n1, e3)
// b = dot(n2, e3)
// c = dot(n1, n2)
// s - c * t = -dot(n1, e3)
// c * s - t = -dot(n2, e3)
// s = ( c * dot(n2, e3) - dot(n1, e3) ) / den
// t = ( dot(n2, e3) - c * dot(n1, e3) ) / den
b3Vec3 E3 = P1 - P2;
b3Vec2 b;
b.x = -b3Dot(N1, E3);
b.y = -b3Dot(N2, E3);
float32 a = b3Dot(N2, E3);
float32 b = b3Dot(N1, E3);
float32 s = den * (c * a - b);
float32 t = den * (a - c - b);
b3Vec2 x;
x.x = det * (-b.x - a12 * b.y);
x.y = det * (b.y - a21 * b.x);
*C1 = P1 + x.x * N1;
*C2 = P2 + x.y * N2;
*C1 = P1 + s * N1;
*C2 = P2 + t * N2;
}
void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
@ -191,45 +200,72 @@ void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
{
b3Vec3 E1 = Q1 - P1;
float32 L1 = b3Length(E1);
b3Vec3 E2 = Q2 - P2;
float32 L2 = b3Length(E2);
if (L1 < B3_LINEAR_SLOP && L2 < B3_LINEAR_SLOP)
if (L1 < 0.0f && L2 < 0.0f)
{
*C1 = P1;
*C2 = P2;
return;
}
if (L1 < B3_LINEAR_SLOP)
if (L1 < 0.0f)
{
*C1 = P1;
*C2 = b3ClosestPointOnSegment(P1, P2, Q2);
return;
}
if (L2 < B3_LINEAR_SLOP)
if (L2 < 0.0f)
{
*C1 = b3ClosestPointOnSegment(P2, P1, Q1);
*C2 = P2;
return;
}
// |e1xe2| = sin(theta) * |e1| * |e2|
b3Vec3 E1_x_E2 = b3Cross(E1, E2);
float32 L = b3Length(E1_x_E2);
const float32 kTolerance = 0.005f;
if (L < kTolerance * L1 * L2)
// Here and in 3D we need to start "GJK" with the closest points between the two edges
// since the cross product between their direction is a possible separating axis.
b3Vec3 N1 = (1.0f / L1) * E1;
b3Vec3 N2 = (1.0f / L2) * E2;
// sin = norm( cross(n1, n2) )
// or
// sin^2 = 1 - cos^2
// Zero parallelism tolerance used because the colinearity tolerance above is also zero.
const float32 kTol = 0.0f;
float32 c = b3Dot(N1, N2);
float32 den = 1.0f - c * c;
if (den < kTol * kTol)
{
*C1 = P1;
*C2 = P2;
}
else
{
b3Vec3 N1 = (1.0f / L1) * E1;
b3Vec3 N2 = (1.0f / L2) * E2;
b3ClosestPointsOnNormalizedLines(C1, C2, P1, N1, P2, N2);
// a = dot(n1, e3)
// b = dot(n2, e3)
// c = dot(n1, n2)
// s - c * t = -dot(n1, e3)
// c * s - t = -dot(n2, e3)
// s = ( c * dot(n2, e3) - dot(n1, e3) ) / den
// t = ( dot(n2, e3) - c * dot(n1, e3) ) / den
b3Vec3 E3 = P1 - P2;
float32 a = b3Dot(N2, E3);
float32 b = b3Dot(N1, E3);
float32 inv_den = 1.0f / den;
float32 s = inv_den * (c * a - b);
float32 t = inv_den * (a - c - b);
*C1 = P1 + s * N1;
*C2 = P2 + t * N2;
}
*C1 = b3ClosestPointOnSegment(*C1, P1, Q1);

46
src/bounce/dynamics/contacts/collide/collide_capsule_hull.cpp
View File

@ -42,47 +42,29 @@ void b3BuildEdgeContact(b3Manifold& manifold,
b3Vec3 E2 = Q2 - P2;
b3Vec3 N2 = b3Normalize(E2);
b3Vec3 E3 = P1 - P2;
b3Vec2 b;
b.x = -b3Dot(N1, E3);
b.y = -b3Dot(N2, E3);
float32 a12 = -b3Dot(N1, N2), a21 = -a12;
float32 det = -1.0f - a12 * a21;
if (det != 0.0f)
b3Vec3 N = b3Cross(E1, E2);
N.Normalize();
if (b3Dot(N, P2 - C2) > 0.0f)
{
det = 1.0f / det;
N = -N;
}
b3Vec2 x;
x.x = det * (-b.x - a12 * b.y);
x.y = det * (b.y - a21 * b.x);
b3Vec3 point1 = P1 + x.x * N1;
b3Vec3 point2 = P2 + x.y * N2;
b3Vec3 axis = b3Cross(E1, E2);
b3Vec3 normal = b3Normalize(axis);
if (b3Dot(normal, P2 - C2) > 0.0f)
{
normal = -normal;
}
b3Vec3 PA, PB;
b3ClosestPointsOnNormalizedLines(&PA, &PB, P1, E1, P2, E2);
b3FeaturePair pair = b3MakePair(0, 1, index2, index2 + 1);
manifold.pointCount = 1;
manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
manifold.points[0].key = b3MakeKey(pair);
manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
manifold.points[0].localPoint = b3MulT(xf1, point1);
manifold.points[0].localPoint2 = b3MulT(xf2, point2);
manifold.points[0].localNormal = b3MulT(xf1.rotation, N);
manifold.points[0].localPoint = b3MulT(xf1, PA);
manifold.points[0].localPoint2 = b3MulT(xf2, PB);
manifold.center = 0.5f * (point1 + hull1->radius * normal + point2 - B3_HULL_RADIUS * normal);
manifold.normal = normal;
manifold.tangent1 = b3Perp(normal);
manifold.tangent2 = b3Cross(manifold.tangent1, normal);
manifold.center = 0.5f * (PA + hull1->radius * N + PB - B3_HULL_RADIUS * N);
manifold.normal = N;
manifold.tangent1 = b3Perp(N);
manifold.tangent2 = b3Cross(manifold.tangent1, N);
}
void b3BuildFaceContact(b3Manifold& manifold,
@ -207,7 +189,7 @@ void b3CollideCapsuleAndHull(b3Manifold& manifold,
return;
}
b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfA, &hullA, xfB, hullB);
if (faceQueryB.separation > totalRadius)
{

34
src/bounce/dynamics/draw_world.cpp
View File

@ -40,20 +40,20 @@
#include <bounce/collision/shapes/hull.h>
#include <bounce/collision/shapes/mesh.h>
const b3Color b3Color_black(0.0f, 0.0f, 0.0f);
const b3Color b3Color_white(1.0f, 1.0f, 1.0f);
const b3Color b3Color_red(1.0f, 0.0f, 0.0f);
const b3Color b3Color_green(0.0f, 1.0f, 0.0f);
const b3Color b3Color_blue(0.0f, 0.0f, 1.0f);
const b3Color b3Color_yellow(1.0f, 1.0f, 0.0f);
const b3Color b3Color_pink(1.0f, 0.0f, 1.0f);
void b3World::DebugDraw() const
{
B3_ASSERT(m_debugDraw);
u32 flags = m_debugDraw->m_flags;
b3Color black(0.0f, 0.0f, 0.0f, 1.0f);
b3Color white(1.0f, 1.0f, 1.0f, 1.0f);
b3Color red(1.0f, 0.0f, 0.0f, 1.0f);
b3Color green(0.0f, 1.0f, 0.0f, 1.0f);
b3Color blue(0.0f, 0.0f, 1.0f, 1.0f);
b3Color yellow(1.0f, 1.0f, 0.0f, 1.0f);
b3Color purple(1.0f, 0.0f, 1.0f, 1.0f);
if (flags & b3Draw::e_centerOfMassesFlag)
{
for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
@ -83,7 +83,7 @@ void b3World::DebugDraw() const
for (b3Shape* s = b->m_shapeList.m_head; s; s = s->m_next)
{
const b3AABB3& aabb = m_contactMan.m_broadPhase.GetAABB(s->m_broadPhaseID);
m_debugDraw->DrawAABB(aabb, purple);
m_debugDraw->DrawAABB(aabb, b3Color_pink);
}
}
}
@ -122,18 +122,18 @@ void b3World::DebugDraw() const
if (flags & b3Draw::e_contactPointsFlag)
{
m_debugDraw->DrawPoint(p, 4.0f, yellow);
m_debugDraw->DrawPoint(p, 4.0f, b3Color_yellow);
}
if (flags & b3Draw::e_contactNormalsFlag)
{
m_debugDraw->DrawSegment(p, p + n, yellow);
m_debugDraw->DrawSegment(p, p + n, b3Color_yellow);
}
if (flags & b3Draw::e_contactTangentsFlag)
{
m_debugDraw->DrawSegment(p, p + t1, yellow);
m_debugDraw->DrawSegment(p, p + t2, yellow);
m_debugDraw->DrawSegment(p, p + t1, b3Color_yellow);
m_debugDraw->DrawSegment(p, p + t2, b3Color_yellow);
}
}
@ -152,18 +152,18 @@ void b3World::DebugDraw() const
if (flags & b3Draw::e_contactPointsFlag)
{
m_debugDraw->DrawPoint(p, 4.0f, mp->persisting ? green : red);
m_debugDraw->DrawPoint(p, 4.0f, mp->persisting ? b3Color_green : b3Color_red);
}
if (flags & b3Draw::e_contactNormalsFlag)
{
m_debugDraw->DrawSegment(p, p + n, white);
m_debugDraw->DrawSegment(p, p + n, b3Color_white);
}
if (flags & b3Draw::e_contactTangentsFlag)
{
m_debugDraw->DrawSegment(p, p + t1, yellow);
m_debugDraw->DrawSegment(p, p + t2, yellow);
m_debugDraw->DrawSegment(p, p + t1, b3Color_yellow);
m_debugDraw->DrawSegment(p, p + t2, b3Color_yellow);
}
}
}

10
src/bounce/dynamics/joints/mouse_joint.cpp
View File

@ -109,14 +109,10 @@ void b3MouseJoint::SetTarget(const b3Vec3& target)
void b3MouseJoint::Draw(b3Draw* draw) const
{
b3Color red(1.0f, 0.0f, 0.0f);
b3Color green(0.0f, 1.0f, 0.0f);
b3Color yellow(1.0f, 1.0f, 0.0f);
b3Vec3 a = GetAnchorA();
b3Vec3 b = GetAnchorB();
draw->DrawPoint(a, 4.0f, green);
draw->DrawPoint(b, 4.0f, red);
draw->DrawSegment(a, b, yellow);
draw->DrawPoint(a, 4.0f, b3Color_green);
draw->DrawPoint(b, 4.0f, b3Color_red);
draw->DrawSegment(a, b, b3Color_yellow);
}

6
src/bounce/dynamics/joints/sphere_joint.cpp
View File

@ -162,7 +162,7 @@ void b3SphereJoint::Draw(b3Draw* draw) const
b3Vec3 a = GetAnchorA();
b3Vec3 b = GetAnchorB();
draw->DrawPoint(a, 4.0f, b3Color(1.0f, 0.0f, 0.0f));
draw->DrawPoint(b, 4.0f, b3Color(0.0f, 1.0f, 0.0f));
draw->DrawSegment(a, b, b3Color(1.0f, 1.0f, 0.0f));
draw->DrawPoint(a, 4.0f, b3Color_red);
draw->DrawPoint(b, 4.0f, b3Color_green);
draw->DrawSegment(a, b, b3Color_yellow);
}

10
src/bounce/dynamics/joints/spring_joint.cpp
View File

@ -254,14 +254,10 @@ bool b3SpringJoint::SolvePositionConstraints(const b3SolverData* data)
void b3SpringJoint::Draw(b3Draw* draw) const
{
b3Color red = b3Color(1.0f, 0.0f, 0.0f, 1.0f);
b3Color green = b3Color(0.0f, 1.0f, 0.0f, 1.0f);
b3Color blue = b3Color(0.0f, 0.0f, 1.0f, 1.0f);
b3Vec3 a = GetBodyA()->GetWorldPoint(m_localAnchorA);
b3Vec3 b = GetBodyB()->GetWorldPoint(m_localAnchorB);
draw->DrawPoint(a, 4.0f, red);
draw->DrawPoint(b, 4.0f, green);
draw->DrawSegment(a, b, blue);
draw->DrawPoint(a, 4.0f, b3Color_red);
draw->DrawPoint(b, 4.0f, b3Color_green);
draw->DrawSegment(a, b, b3Color_yellow);
}

21
src/bounce/dynamics/world.cpp
View File

@ -377,7 +377,7 @@ void b3World::RayCast(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec
m_contactMan.m_broadPhase.RayCast(&callback, input);
}
struct b3RayCastFirstCallback
struct b3RayCastSingleCallback
{
float32 Report(const b3RayCastInput& input, i32 proxyId)
{
@ -409,14 +409,14 @@ struct b3RayCastFirstCallback
const b3BroadPhase* broadPhase;
};
void b3World::RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const
bool b3World::RayCastSingle(b3RayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const
{
b3RayCastInput input;
input.p1 = p1;
input.p2 = p2;
input.maxFraction = 1.0f;
b3RayCastFirstCallback callback;
b3RayCastSingleCallback callback;
callback.shape0 = NULL;
callback.output0.fraction = B3_MAX_FLOAT;
callback.broadPhase = &m_contactMan.m_broadPhase;
@ -428,15 +428,18 @@ void b3World::RayCastFirst(b3RayCastListener* listener, const b3Vec3& p1, const
{
// 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 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
b3Vec3 normal = callback.output0.normal;
// Report the intersection to the user.
listener->ReportShape(callback.shape0, point, normal, fraction);
output->shape = callback.shape0;
output->point = point;
output->normal = normal;
output->fraction = fraction;
return true;
}
return false;
}
struct b3QueryAABBCallback

33
src/testbed/framework/debug_draw.cpp
View File

@ -1115,6 +1115,9 @@ void DebugDraw::DrawSolidTriangle(const b3Vec3& normal, const b3Vec3& p1, const
m_triangles->Vertex(p1, color, normal);
m_triangles->Vertex(p2, color, normal);
m_triangles->Vertex(p3, color, normal);
b3Color edgeColor(0.0f, 0.0f, 0.0f, 1.0f);
DrawTriangle(p2, p3, p3, edgeColor);
}
void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color)
@ -1124,7 +1127,8 @@ void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& co
{
b3Vec3 p2 = vertices[i];
DrawSegment(p1, p2, color);
m_lines->Vertex(p1, color);
m_lines->Vertex(p2, color);
p1 = p2;
}
@ -1132,7 +1136,7 @@ void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& co
void DebugDraw::DrawSolidPolygon(const b3Vec3& normal, const b3Vec3* vertices, u32 count, const b3Color& color)
{
b3Color fillColor(color.r, color.g, color.b, 0.5f);
b3Color fillColor(color.r, color.g, color.b, color.a);
b3Vec3 p1 = vertices[0];
for (u32 i = 1; i < count - 1; ++i)
@ -1165,8 +1169,9 @@ void DebugDraw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 r
b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal;
b3Vec3 v2 = center + radius * n2;
DrawSegment(v1, v2, color);
m_lines->Vertex(v1, color);
m_lines->Vertex(v2, color);
n1 = n2;
v1 = v2;
}
@ -1367,12 +1372,18 @@ void DebugDraw::DrawMesh(const b3MeshShape* s, const b3Color& c, const b3Transfo
b3Vec3 p2 = xf * mesh->vertices[t->v2];
b3Vec3 p3 = xf * mesh->vertices[t->v3];
b3Vec3 n = b3Cross(p2 - p1, p3 - p1);
n.Normalize();
b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1);
n1.Normalize();
m_triangles->Vertex(p1, c, n);
m_triangles->Vertex(p2, c, n);
m_triangles->Vertex(p3, c, n);
m_triangles->Vertex(p1, c, n1);
m_triangles->Vertex(p2, c, n1);
m_triangles->Vertex(p3, c, n1);
b3Vec3 n2 = -n1;
m_triangles->Vertex(p1, c, n2);
m_triangles->Vertex(p3, c, n2);
m_triangles->Vertex(p2, c, n2);
}
}
@ -1436,10 +1447,10 @@ void DebugDraw::Draw(const b3World& world)
}
}
g_debugDraw->Draw();
g_debugDraw->Submit();
}
void DebugDraw::Draw()
void DebugDraw::Submit()
{
m_triangles->Submit();
m_lines->Submit();

2
src/testbed/framework/main.cpp
View File

@ -339,7 +339,7 @@ void Step()
}
g_test->Step();
g_debugDraw->Draw();
g_debugDraw->Submit();
}
void Run()

10
src/testbed/framework/test.cpp
View File

@ -376,7 +376,7 @@ void Test::Step()
g_debugDraw->SetFlags(drawFlags);
m_world.DebugDraw();
g_debugDraw->Draw();
g_debugDraw->Submit();
if (g_settings.drawFaces)
{
@ -459,12 +459,10 @@ void Test::MouseLeftDown(const Ray3& pw)
listener.hit.shape = NULL;
// Perform the ray cast
m_world.RayCastFirst(&listener, p1, p2);
if (listener.hit.shape)
b3RayCastSingleOutput out;
if (m_world.RayCastSingle(&out, p1, p2))
{
m_rayHit = listener.hit;
m_rayHit = out;
RayHit();
}
}