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