2463 lines
73 KiB
C++
2463 lines
73 KiB
C++
/**
|
||
@file CollisionDetection.cpp
|
||
|
||
@maintainer Morgan McGuire, http://graphics.cs.williams.edu
|
||
|
||
@cite Bounce direction based on Paul Nettle's ftp://ftp.3dmaileffects.com/pub/FluidStudios/CollisionDetection/Fluid_Studios_Generic_Collision_Detection_for_Games_Using_Ellipsoids.pdf and comments by Max McGuire. Ray-sphere code by Eric Haines.
|
||
|
||
@created 2001-11-24
|
||
@edited 2008-12-29
|
||
*/
|
||
|
||
#include "G3D/CoordinateFrame.h"
|
||
#include "G3D/platform.h"
|
||
#include "G3D/CollisionDetection.h"
|
||
#include "G3D/debugAssert.h"
|
||
#include "G3D/vectorMath.h"
|
||
#include "G3D/Capsule.h"
|
||
#include "G3D/Plane.h"
|
||
#include "G3D/Line.h"
|
||
#include "G3D/LineSegment.h"
|
||
#include "G3D/Sphere.h"
|
||
#include "G3D/Box.h"
|
||
#include "G3D/Triangle.h"
|
||
#include "G3D/Vector3.h"
|
||
#include "G3D/AABox.h"
|
||
|
||
#ifdef _MSC_VER
|
||
// Turn on fast floating-point optimizations
|
||
#pragma float_control( push )
|
||
#pragma fp_contract( on )
|
||
#pragma fenv_access( off )
|
||
#pragma float_control( except, off )
|
||
#pragma float_control( precise, off )
|
||
#endif
|
||
|
||
|
||
namespace G3D {
|
||
|
||
bool CollisionDetection::ignoreBool;
|
||
Vector3 CollisionDetection::ignore;
|
||
Array<Vector3> CollisionDetection::ignoreArray;
|
||
|
||
|
||
|
||
Vector3 CollisionDetection::separatingAxisForSolidBoxSolidBox(
|
||
const int separatingAxisIndex,
|
||
const Box & box1,
|
||
const Box & box2) {
|
||
debugAssert(separatingAxisIndex >= 0);
|
||
debugAssert(separatingAxisIndex < 15);
|
||
Vector3 axis;
|
||
if (separatingAxisIndex < 3) {
|
||
axis = box1.axis(separatingAxisIndex);
|
||
} else if (separatingAxisIndex < 6) {
|
||
axis = box2.axis(separatingAxisIndex - 3);
|
||
} else {
|
||
int box1Index = (separatingAxisIndex - 6) / 3;
|
||
int box2Index = (separatingAxisIndex - 6) % 3;
|
||
axis = cross(box1.axis(box1Index), box2.axis(box2Index));
|
||
}
|
||
return axis;
|
||
}
|
||
|
||
#ifdef _MSC_VER
|
||
# pragma warning (push)
|
||
# pragma warning (disable : 4244)
|
||
#endif
|
||
|
||
float CollisionDetection::projectedDistanceForSolidBoxSolidBox(
|
||
const int separatingAxisIndex,
|
||
const Vector3 & a,
|
||
const Vector3 & b,
|
||
const Vector3 & D,
|
||
const double* c,
|
||
const double* ca,
|
||
const double* ad,
|
||
const double* bd)
|
||
{
|
||
(void)D;
|
||
|
||
float R0 = 0.0f;
|
||
float R1 = 0.0f;
|
||
float R = 0.0f;
|
||
switch (separatingAxisIndex) {
|
||
case 0:
|
||
// A0
|
||
R0 = a[0];
|
||
R1 = b[0] * ca[0] + b[1] * ca[1] + b[2] * ca[2];
|
||
R = fabs(ad[0]);
|
||
break;
|
||
case 1:
|
||
// A1
|
||
R0 = a[1];
|
||
R1 = b[0] * ca[3] + b[1] * ca[4] + b[2] * ca[5];
|
||
R = fabs(ad[1]);
|
||
break;
|
||
case 2:
|
||
// A2
|
||
R0 = a[2];
|
||
R1 = b[0] * ca[6] + b[1] * ca[7] + b[2] * ca[8];
|
||
R = fabs(ad[2]);
|
||
break;
|
||
case 3:
|
||
// B0
|
||
R0 = a[0] * ca[0] + a[1] * ca[3] + a[2] * ca[6];
|
||
R1 = b[0];
|
||
R = fabs(bd[0]);
|
||
break;
|
||
case 4:
|
||
// B1
|
||
R0 = a[0] * ca[1] + a[1] * ca[4] + a[2] * ca[7];
|
||
R1 = b[1];
|
||
R = fabs(bd[1]);
|
||
break;
|
||
case 5:
|
||
// B2
|
||
R0 = a[0] * ca[2] + a[1] * ca[5] + a[2] * ca[8];
|
||
R1 = b[2];
|
||
R = fabs(bd[2]);
|
||
break;
|
||
case 6:
|
||
// A0 x B0
|
||
R0 = a[1] * ca[6] + a[2] * ca[3];
|
||
R1 = b[1] * ca[2] + b[2] * ca[1];
|
||
R = fabs(c[3] * ad[2] - c[6] * ad[1]);
|
||
break;
|
||
case 7:
|
||
// A0 x B1
|
||
R0 = a[1] * ca[7] + a[2] * ca[4];
|
||
R1 = b[0] * ca[2] + b[2] * ca[0];
|
||
R = fabs(c[4] * ad[2] - c[7] * ad[1]);
|
||
break;
|
||
case 8:
|
||
// A0 x B2
|
||
R0 = a[1] * ca[8] + a[2] * ca[5];
|
||
R1 = b[0] * ca[1] + b[1] * ca[0];
|
||
R = fabs(c[5] * ad[2] - c[8] * ad[1]);
|
||
break;
|
||
case 9:
|
||
// A1 x B0
|
||
R0 = a[0] * ca[6] + a[2] * ca[0];
|
||
R1 = b[1] * ca[5] + b[2] * ca[4];
|
||
R = fabs(c[6] * ad[0] - c[0] * ad[2]);
|
||
break;
|
||
case 10:
|
||
// A1 x B1
|
||
R0 = a[0] * ca[7] + a[2] * ca[1];
|
||
R1 = b[0] * ca[5] + b[2] * ca[3];
|
||
R = fabs(c[7] * ad[0] - c[1] * ad[2]);
|
||
break;
|
||
case 11:
|
||
// A1 x B2
|
||
R0 = a[0] * ca[8] + a[2] * ca[2];
|
||
R1 = b[0] * ca[4] + b[1] * ca[3];
|
||
R = fabs(c[8] * ad[0] - c[2] * ad[2]);
|
||
break;
|
||
case 12:
|
||
// A2 x B0
|
||
R0 = a[0] * ca[3] + a[1] * ca[0];
|
||
R1 = b[1] * ca[8] + b[2] * ca[7];
|
||
R = fabs(c[0] * ad[1] - c[3] * ad[0]);
|
||
break;
|
||
case 13:
|
||
// A2 x B1
|
||
R0 = a[0] * ca[4] + a[1] * ca[1];
|
||
R1 = b[0] * ca[8] + b[2] * ca[6];
|
||
R = fabs(c[1] * ad[1] - c[4] * ad[0]);
|
||
break;
|
||
case 14:
|
||
// A2 x B2
|
||
R0 = a[0] * ca[5] + a[1] * ca[2];
|
||
R1 = b[0] * ca[7] + b[1] * ca[6];
|
||
R = fabs(c[2] * ad[1] - c[5] * ad[0]);
|
||
break;
|
||
default:
|
||
debugAssertM(false, "fell through switch statement");
|
||
}
|
||
|
||
return (R - (R0 + R1));
|
||
}
|
||
|
||
|
||
bool CollisionDetection::parallelAxisForSolidBoxSolidBox(
|
||
const double* ca,
|
||
const double epsilon,
|
||
int & axis1,
|
||
int & axis2) {
|
||
const double parallelDot = 1.0 - epsilon;
|
||
for (int i = 0; i < 9; ++i) {
|
||
if (ca[i] >= parallelDot) {
|
||
axis1 = i / 3;
|
||
axis2 = i % 3;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
|
||
|
||
|
||
void CollisionDetection::fillSolidBoxSolidBoxInfo(
|
||
const Box & box1,
|
||
const Box & box2,
|
||
Vector3 & a,
|
||
Vector3 & b,
|
||
Vector3 & D,
|
||
double* c,
|
||
double* ca,
|
||
double* ad,
|
||
double* bd) {
|
||
// length between center and each side of box1 and box2
|
||
a = box1.extent() * 0.5;
|
||
b = box2.extent() * 0.5;
|
||
|
||
// difference between centers of box1 and box2
|
||
D = box2.center() - box1.center();
|
||
|
||
// store the value of all possible dot products between the
|
||
// axes of box1 and box2, c_{row, col} in the Eberly paper
|
||
// corresponds to c[row * 3 + col] for this 9 element array.
|
||
//
|
||
// c[] holds signed values, ca[] hold absolute values
|
||
for (int i = 0; i < 9; ++i) {
|
||
c[i] = dot(box1.axis(i / 3), box2.axis(i % 3));
|
||
ca[i] = fabs(c[i]);
|
||
}
|
||
|
||
// store all possible dot products between the axes of box1 and D,
|
||
// as well as the axes of box2 and D
|
||
for (int i = 0; i < 3; ++i) {
|
||
ad[i] = dot(box1.axis(i), D);
|
||
bd[i] = dot(box2.axis(i), D);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
bool CollisionDetection::conservativeBoxBoxTest(
|
||
const Vector3 & a, const Vector3 & b, const Vector3 & D) {
|
||
// do a quick bounding sphere test because it is relatively
|
||
// cheap, (three dot products, two sqrts, and a few others)
|
||
double boxRadius1 = a.magnitude();
|
||
double boxRadius2 = b.magnitude();
|
||
return (D.squaredMagnitude() < square(boxRadius1 + boxRadius2));
|
||
}
|
||
|
||
|
||
|
||
|
||
bool CollisionDetection::fixedSolidBoxIntersectsFixedSolidBox(
|
||
const Box& box1,
|
||
const Box& box2,
|
||
const int lastSeparatingAxis) {
|
||
// for explanations of the variable please refer to the
|
||
// paper and fillSolidBoxSolidBoxInfo()
|
||
Vector3 a;
|
||
Vector3 b;
|
||
Vector3 D;
|
||
double c[9];
|
||
double ca[9];
|
||
double ad[3];
|
||
double bd[3];
|
||
|
||
fillSolidBoxSolidBoxInfo(box1, box2, a, b, D, c, ca, ad, bd);
|
||
|
||
int dummy1, dummy2;
|
||
bool parallelAxes = parallelAxisForSolidBoxSolidBox(ca, 0.00001,
|
||
dummy1, dummy2);
|
||
|
||
// check the separating axis from the last time step
|
||
if (lastSeparatingAxis != -1 &&
|
||
(lastSeparatingAxis < 6 || !parallelAxes)) {
|
||
double projectedDistance = projectedDistanceForSolidBoxSolidBox(
|
||
lastSeparatingAxis, a, b, D, c, ca, ad, bd);
|
||
|
||
// the separating axis from the last time step is still
|
||
// valid, the boxes do not intersect
|
||
if (projectedDistance > 0.0) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// test if the boxes can be separated by a plane normal to
|
||
// any of the three axes of box1, any of the three axes of box2,
|
||
// or any of the 9 possible cross products of axes from box1
|
||
// and box2
|
||
for (int i = 0; i < 15; ++i) {
|
||
// do not need to check edge-edge cases if any two of
|
||
// the axes are parallel
|
||
if (parallelAxes && i == 6) {
|
||
return true;
|
||
}
|
||
|
||
double projectedDistance =
|
||
projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd);
|
||
|
||
// found a separating axis, the boxes do not intersect
|
||
if (projectedDistance > 0.0) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
|
||
void CollisionDetection::closestPointsBetweenLineAndLine(
|
||
const Line & line1,
|
||
const Line & line2,
|
||
Vector3 & closest1,
|
||
Vector3 & closest2) {
|
||
// TODO make accessors for Line that don't make a copy of data
|
||
Vector3 P0 = line1.point();
|
||
Vector3 u = line1.direction();
|
||
Vector3 Q0 = line2.point();
|
||
Vector3 v = line2.direction();
|
||
Vector3 w0 = P0 - Q0;
|
||
|
||
// a = 1.0, c = 1.0
|
||
double b = dot(u, v);
|
||
double d = dot(u, w0);
|
||
double e = dot(v, w0);
|
||
double D = 1.0 - b * b;
|
||
double sc, tc;
|
||
|
||
static const double epsilon = 0.00001;
|
||
|
||
if (D < epsilon) {
|
||
// lines are parallel, choose P0 as one point, find the point
|
||
// on line2 that is closest to P0
|
||
sc = 0.0;
|
||
tc = (b > 1.0) ? (d / b) : (e / 1.0);
|
||
} else {
|
||
// lines are not parallel
|
||
sc = (b * e - 1.0 * d) / D;
|
||
tc = (1.0 * e - b * d) / D;
|
||
}
|
||
|
||
closest1 = P0 + (sc * u);
|
||
closest2 = Q0 + (tc * v);
|
||
}
|
||
|
||
|
||
|
||
float CollisionDetection::penetrationDepthForFixedBoxFixedBox(
|
||
const Box& box1,
|
||
const Box& box2,
|
||
Array<Vector3>& contactPoints,
|
||
Array<Vector3>& contactNormals,
|
||
const int lastSeparatingAxis) {
|
||
|
||
contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
|
||
Vector3 a;
|
||
Vector3 b;
|
||
Vector3 D;
|
||
double c[9];
|
||
double ca[9];
|
||
double ad[3];
|
||
double bd[3];
|
||
|
||
debugAssert(lastSeparatingAxis >= -1);
|
||
debugAssert(lastSeparatingAxis < 15);
|
||
|
||
fillSolidBoxSolidBoxInfo(box1, box2, a, b, D, c, ca, ad, bd);
|
||
|
||
int axis1, axis2;
|
||
bool parallelAxes = parallelAxisForSolidBoxSolidBox(ca, 0.00001,
|
||
axis1, axis2);
|
||
|
||
|
||
// check the separating axis from the last time step
|
||
if (lastSeparatingAxis != -1 &&
|
||
(lastSeparatingAxis < 6 || !parallelAxes)) {
|
||
float projectedDistance = projectedDistanceForSolidBoxSolidBox(
|
||
lastSeparatingAxis, a, b, D, c, ca, ad, bd);
|
||
|
||
// the separating axis from the last time step is still
|
||
// valid, the boxes do not intersect
|
||
if (projectedDistance > 0.0) {
|
||
return -projectedDistance;
|
||
}
|
||
}
|
||
|
||
// test if the boxes can be separated by a plane normal to
|
||
// any of the three axes of box1, any of the three axes of box2,
|
||
// (test 9 possible cross products later)
|
||
float penetration = -finf();
|
||
int penetrationAxisIndex = -1;
|
||
|
||
for (int i = 0; i < 6; ++i) {
|
||
float projectedDistance =
|
||
projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd);
|
||
|
||
// found a separating axis, the boxes do not intersect
|
||
if (projectedDistance > 0.0) {
|
||
return -projectedDistance;
|
||
}
|
||
|
||
// keep track of the axis that is least violated
|
||
if (projectedDistance > penetration) {
|
||
penetration = projectedDistance;
|
||
penetrationAxisIndex = i;
|
||
}
|
||
}
|
||
|
||
|
||
// for each edge-edge case we have to adjust the magnitude of
|
||
// penetration since we did not include the dot(L, L) denominator
|
||
// that can be smaller than 1.0 for the edge-edge cases.
|
||
|
||
if (!parallelAxes) {
|
||
double edgeDistances[9];
|
||
|
||
// run through edge-edge cases to see if we can find a separating axis
|
||
for (int i = 6; i < 15; ++i) {
|
||
float projectedDistance =
|
||
projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd);
|
||
|
||
// found a separating axis, the boxes do not intersect,
|
||
// correct magnitude and return projected distance
|
||
if (projectedDistance > 0.0) {
|
||
Vector3 L = separatingAxisForSolidBoxSolidBox(i, box1, box2);
|
||
projectedDistance /= dot(L, L);
|
||
return -projectedDistance;
|
||
}
|
||
|
||
edgeDistances[i - 6] = projectedDistance;
|
||
}
|
||
|
||
// no separating axis found, the boxes do intersect,
|
||
// correct the magnitudes of the projectedDistance values
|
||
for (int i = 6; i < 15; ++i) {
|
||
// find the negative penetration value with the smallest magnitude,
|
||
// the adjustment done for the edge-edge cases only increases
|
||
// magnitude by dividing by a number smaller than 1 and greater than 0
|
||
float projectedDistance = (float)edgeDistances[i - 6];
|
||
if (projectedDistance > penetration) {
|
||
Vector3 L = separatingAxisForSolidBoxSolidBox(i, box1, box2);
|
||
projectedDistance /= dot(L, L);
|
||
if (projectedDistance > penetration) {
|
||
penetration = projectedDistance;
|
||
penetrationAxisIndex = i;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// get final separating axis vector
|
||
Vector3 L = separatingAxisForSolidBoxSolidBox(penetrationAxisIndex,
|
||
box1, box2);
|
||
|
||
// set L to be the normal that faces away from box1
|
||
if (dot(L, D) < 0) {
|
||
L = -L;
|
||
}
|
||
|
||
Vector3 contactPoint;
|
||
|
||
if (penetrationAxisIndex < 6) {
|
||
// vertex to face collision, find deepest colliding vertex
|
||
const Box* vertexBox;
|
||
Vector3 faceNormal = L;
|
||
|
||
// L will be the outward facing normal for the faceBox
|
||
if (penetrationAxisIndex < 3) {
|
||
vertexBox = & box2;
|
||
if (dot(L, D) < 0) {
|
||
faceNormal = -L;
|
||
}
|
||
} else {
|
||
vertexBox = & box1;
|
||
if (dot(L, D) > 0) {
|
||
faceNormal = -L;
|
||
}
|
||
}
|
||
|
||
// find the vertex that is farthest away in the direction
|
||
// face normal direction
|
||
int deepestPointIndex = 0;
|
||
float deepestPointDot = dot(faceNormal, vertexBox->corner(0));
|
||
for (int i = 1; i < 8; ++i) {
|
||
float dotProduct = dot(faceNormal, vertexBox->corner(i));
|
||
if (dotProduct < deepestPointDot) {
|
||
deepestPointDot = dotProduct;
|
||
deepestPointIndex = i;
|
||
}
|
||
}
|
||
|
||
// return the point half way between the deepest point and the
|
||
// contacting face
|
||
contactPoint = vertexBox->corner(deepestPointIndex) +
|
||
(-penetration * 0.5 * faceNormal);
|
||
} else {
|
||
// edge-edge case, find the two ege lines
|
||
int edge1 = (penetrationAxisIndex - 6) / 3;
|
||
int edge2 = (penetrationAxisIndex - 6) % 3;
|
||
Vector3 linePoint1 = box1.center();
|
||
Vector3 linePoint2 = box2.center();
|
||
Vector3 lineDir1;
|
||
Vector3 lineDir2;
|
||
|
||
// find edge line by finding the edge axis, and the
|
||
// other two axes that are closest to the other box
|
||
for (int i = 0; i < 3; ++i) {
|
||
if (i == edge1) {
|
||
lineDir1 = box1.axis(i);
|
||
} else {
|
||
Vector3 axis = box1.axis(i);
|
||
if (dot(axis, L) < 0) {
|
||
axis = -axis;
|
||
}
|
||
linePoint1 += axis * a[i];
|
||
}
|
||
|
||
if (i == edge2) {
|
||
lineDir2 = box2.axis(i);
|
||
} else {
|
||
Vector3 axis = box2.axis(i);
|
||
if (dot(axis, L) > 0) {
|
||
axis = -axis;
|
||
}
|
||
linePoint2 += axis * b[i];
|
||
}
|
||
}
|
||
|
||
// make lines from the two closest edges, and find
|
||
// the points that on each line that are closest to the other
|
||
Line line1 = Line::fromPointAndDirection(linePoint1, lineDir1);
|
||
Line line2 = Line::fromPointAndDirection(linePoint2, lineDir2);
|
||
Vector3 closest1;
|
||
Vector3 closest2;
|
||
|
||
closestPointsBetweenLineAndLine(line1, line2, closest1, closest2);
|
||
|
||
// take the average of the two closest edge points for the final
|
||
// contact point
|
||
contactPoint = (closest1 + closest2) * 0.5;
|
||
}
|
||
|
||
contactPoints.push(contactPoint);
|
||
contactNormals.push(L);
|
||
|
||
return -penetration;
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
float CollisionDetection::penetrationDepthForFixedSphereFixedBox(
|
||
const Sphere& sphere,
|
||
const Box& box,
|
||
Array<Vector3>& contactPoints,
|
||
Array<Vector3>& contactNormals) {
|
||
|
||
contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
|
||
// In its local coordinate frame, the box measures
|
||
// 2 * halfExtent[a] along dimesion a.
|
||
Vector3 halfExtent(box.extent(0), box.extent(1), box.extent(2));
|
||
halfExtent *= 0.5f;
|
||
|
||
CoordinateFrame boxFrame;
|
||
box.getLocalFrame(boxFrame);
|
||
|
||
// Transform the sphere to the box's coordinate frame.
|
||
Vector3 center = boxFrame.pointToObjectSpace(sphere.center);
|
||
|
||
// Find the square of the distance from the sphere to the box
|
||
|
||
|
||
// Distance along each axis from the closest side of the box
|
||
// to the sphere center. Negative values are *inside* the box.
|
||
Vector3 distOutsideBox;
|
||
|
||
// Divide space up into the 27 regions corresponding
|
||
// to {+|-|0}X, {+|-|0}Y, {+|-|0}Z and classify the
|
||
// sphere center into one of them.
|
||
Vector3 centerRegion;
|
||
|
||
// In the edge collision case, the edge is between vertices
|
||
// (constant + variable) and (constant - variable).
|
||
Vector3 constant, variable;
|
||
|
||
int numNonZero = 0;
|
||
|
||
// Iterate over axes
|
||
for (int a = 0; a < 3; ++a) {
|
||
// For each (box side), see which direction the sphere
|
||
// is outside the box (positive or negative). Add the
|
||
// square of that distance to the total distance from
|
||
// the box.
|
||
|
||
float distanceFromLow = -halfExtent[a] - center[a];
|
||
float distanceFromHigh = center[a] - halfExtent[a];
|
||
|
||
if (fabsf(distanceFromLow) < fabsf(distanceFromHigh)) {
|
||
distOutsideBox[a] = distanceFromLow;
|
||
} else {
|
||
distOutsideBox[a] = distanceFromHigh;
|
||
}
|
||
|
||
if (distanceFromLow < 0.0) {
|
||
if (distanceFromHigh < 0.0) {
|
||
// Inside the box
|
||
centerRegion[a] = 0.0;
|
||
variable[a] = 1.0;
|
||
} else {
|
||
// Off the high side
|
||
centerRegion[a] = 1.0;
|
||
constant[a] = halfExtent[a];
|
||
++numNonZero;
|
||
}
|
||
} else if (distanceFromHigh < 0.0) {
|
||
// Off the low side
|
||
centerRegion[a] = -1.0;
|
||
constant[a] = -halfExtent[a];
|
||
++numNonZero;
|
||
} else {
|
||
debugAssertM(false,
|
||
"distanceFromLow and distanceFromHigh cannot both be positive");
|
||
}
|
||
}
|
||
|
||
// Squared distance between the outside of the box and the
|
||
// sphere center.
|
||
float d2 = Vector3::zero().max(distOutsideBox).squaredMagnitude();
|
||
|
||
if (d2 > square(sphere.radius)) {
|
||
// There is no penetration because the distance is greater
|
||
// than the radius of the sphere. This is the common case
|
||
// and we quickly exit.
|
||
return -1;
|
||
}
|
||
|
||
// We know there is some penetration but need to classify it.
|
||
//
|
||
// Examine the region that contains the center of the sphere. If
|
||
// there is exactly one non-zero axis, the collision is with a
|
||
// plane. If there are exactly two non-zero axes, the collision
|
||
// is with an edge. If all three axes are non-zero, the collision is
|
||
// with a vertex. If there are no non-zero axes, the center is inside
|
||
// the box.
|
||
|
||
double depth = -1;
|
||
switch (numNonZero) {
|
||
case 3: // Vertex collision
|
||
// The collision point is the vertex at constant, the normal
|
||
// is the vector from there to the sphere center.
|
||
contactNormals.append(boxFrame.normalToWorldSpace(constant - center));
|
||
contactPoints.append(boxFrame.pointToWorldSpace(constant));
|
||
depth = sphere.radius - sqrt(d2);
|
||
break;
|
||
|
||
case 2: // Edge collision
|
||
{
|
||
// TODO: unwrapping the edge constructor and closest point
|
||
// code will probably make it faster.
|
||
|
||
// Determine the edge
|
||
Line line = Line::fromPointAndDirection(constant, variable);
|
||
|
||
// Penetration depth:
|
||
depth = sphere.radius - sqrt(d2);
|
||
|
||
// The contact point is the closes point to the sphere on the line
|
||
Vector3 X = line.closestPoint(center);
|
||
contactNormals.append(boxFrame.normalToWorldSpace(X - center).direction());
|
||
contactPoints.append(boxFrame.pointToWorldSpace(X));
|
||
}
|
||
break;
|
||
|
||
case 1: // Plane collision
|
||
{
|
||
// The plane normal is the centerRegion vector,
|
||
// so the sphere normal is the negative. Take
|
||
// it to world space from box-space.
|
||
|
||
// Center region doesn't need to be normalized because
|
||
// it is known to contain only one non-zero value
|
||
// and that value is +/- 1.
|
||
Vector3 N = boxFrame.normalToWorldSpace(-centerRegion);
|
||
contactNormals.append(N);
|
||
|
||
// Penetration depth:
|
||
depth = sphere.radius - sqrtf(d2);
|
||
|
||
// Compute the contact point from the penetration depth
|
||
contactPoints.append(sphere.center + N * (sphere.radius - depth));
|
||
}
|
||
break;
|
||
|
||
case 0: // Volume collision
|
||
|
||
// The sphere center is inside the box. This is an easy case
|
||
// to handle. Note that all axes of distOutsideBox must
|
||
// be negative.
|
||
|
||
// Arbitratily choose the sphere center as a contact point
|
||
contactPoints.append(sphere.center);
|
||
|
||
// Find the least-negative penetration axis.
|
||
//
|
||
// We could have computed this during the loop over the axes,
|
||
// but since volume collisions are rare (they only occur with
|
||
// large time steps), this case will seldom be executed and
|
||
// should not be optimized at the expense of the others.
|
||
if (distOutsideBox.x > distOutsideBox.y) {
|
||
if (distOutsideBox.x > distOutsideBox.z) {
|
||
// Smallest penetration on x-axis
|
||
// Chose normal based on which side we're closest to.
|
||
// Keep in mind that this is a normal to the sphere,
|
||
// so it is the inverse of the box normal.
|
||
if (center.x > 0) {
|
||
contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitX()));
|
||
} else {
|
||
contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitX()));
|
||
}
|
||
depth = -distOutsideBox.x;
|
||
} else {
|
||
// Smallest penetration on z-axis
|
||
goto ZAXIS;
|
||
}
|
||
} else if (distOutsideBox.y > distOutsideBox.z) {
|
||
// Smallest penetration on y-axis
|
||
// Chose normal based on which side we're closest to.
|
||
// Keep in mind that this is a normal to the sphere,
|
||
// so it is the inverse of the box normal.
|
||
if (center.y > 0) {
|
||
contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitY()));
|
||
} else {
|
||
contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitY()));
|
||
}
|
||
depth = -distOutsideBox.y;
|
||
} else {
|
||
// Smallest on z-axis
|
||
ZAXIS:
|
||
// Chose normal based on which side we're closest to.
|
||
// Keep in mind that this is a normal to the sphere,
|
||
// so it is the inverse of the box normal.
|
||
if (center.z > 0) {
|
||
contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitZ()));
|
||
} else {
|
||
contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitZ()));
|
||
}
|
||
depth = -distOutsideBox.z;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
debugAssertM(false, "Fell through switch");
|
||
break;
|
||
}
|
||
|
||
return depth;
|
||
}
|
||
|
||
|
||
float CollisionDetection::penetrationDepthForFixedSphereFixedSphere(
|
||
const Sphere& sphereA,
|
||
const Sphere& sphereB,
|
||
Array<Vector3>& contactPoints,
|
||
Array<Vector3>& contactNormals) {
|
||
|
||
Vector3 axis = sphereB.center - sphereA.center;
|
||
double radius = sphereA.radius + sphereB.radius;
|
||
double mag = axis.magnitude();
|
||
axis /= mag;
|
||
double depth = -(mag - radius);
|
||
|
||
contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
|
||
if (depth >= 0) {
|
||
contactPoints.append(sphereA.center + axis * (sphereA.radius - depth / 2));
|
||
contactNormals.append(axis);
|
||
}
|
||
|
||
return depth;
|
||
}
|
||
|
||
|
||
float CollisionDetection::penetrationDepthForFixedSphereFixedPlane(
|
||
const Sphere& sphereA,
|
||
const Plane& planeB,
|
||
Array<Vector3>& contactPoints,
|
||
Array<Vector3>& contactNormals) {
|
||
|
||
Vector3 N;
|
||
double d;
|
||
|
||
planeB.getEquation(N, d);
|
||
|
||
double depth = -(sphereA.center.dot(N) + d - sphereA.radius);
|
||
|
||
contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
|
||
if (depth >= 0) {
|
||
contactPoints.append(N * (depth - sphereA.radius) + sphereA.center);
|
||
contactNormals.append(N);
|
||
}
|
||
|
||
return depth;
|
||
}
|
||
|
||
|
||
float CollisionDetection::penetrationDepthForFixedBoxFixedPlane(
|
||
const Box& box,
|
||
const Plane& plane,
|
||
Array<Vector3>& contactPoints,
|
||
Array<Vector3>& contactNormals) {
|
||
|
||
Vector3 N;
|
||
double d;
|
||
|
||
plane.getEquation(N, d);
|
||
|
||
contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
|
||
|
||
float lowest = finf();
|
||
for (int i = 0; i < 8; ++i) {
|
||
const Vector3 vertex = box.corner(i);
|
||
|
||
float x = vertex.dot(N) + (float)d;
|
||
|
||
if (x <= 0) {
|
||
// All vertices below the plane should be contact points.
|
||
contactPoints.append(vertex);
|
||
contactNormals.append(-N);
|
||
}
|
||
|
||
lowest = min(lowest, x);
|
||
}
|
||
|
||
// Depth should be a positive number
|
||
return -lowest;
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingPointFixedPlane(
|
||
const Vector3& point,
|
||
const Vector3& velocity,
|
||
const Plane& plane,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
// Solve for the time at which normal.dot(point + velocity) + d == 0.
|
||
float d;
|
||
Vector3 normal;
|
||
plane.getEquation(normal, d);
|
||
|
||
const float vdotN = velocity.dot(normal);
|
||
const float pdotN = point.dot(normal);
|
||
|
||
if (fuzzyEq(pdotN + d, 0.0f)) {
|
||
// The point is *in* the plane.
|
||
location = point;
|
||
outNormal = normal;
|
||
return 0;
|
||
}
|
||
|
||
if (vdotN >= 0.0f) {
|
||
// no collision will occur
|
||
location = Vector3::inf();
|
||
return finf();
|
||
}
|
||
|
||
const float t = -(pdotN + d) / vdotN;
|
||
if (t < 0) {
|
||
location = Vector3::inf();
|
||
return finf();
|
||
} else {
|
||
location = point + velocity * t;
|
||
outNormal = normal;
|
||
return t;
|
||
}
|
||
}
|
||
|
||
bool __fastcall CollisionDetection::rayAABox(
|
||
const Ray& ray,
|
||
const Vector3& invDir,
|
||
const AABox& box,
|
||
const Vector3& boxCenter,
|
||
float boundingRadiusSquared,
|
||
Vector3& location,
|
||
bool& inside) {
|
||
|
||
debugAssertM(fabs(ray.direction().squaredLength() - 1.0f) < 0.01f, format("Length = %f", ray.direction().length()));
|
||
{
|
||
// Pre-emptive partial bounding sphere test
|
||
const Vector3 L(boxCenter - ray.origin());
|
||
float d = L.dot(ray.direction());
|
||
|
||
float L2 = L.dot(L);
|
||
float D2 = square(d);
|
||
float M2 = L2 - D2;
|
||
|
||
if (((d < 0) && (L2 > boundingRadiusSquared)) || (M2 > boundingRadiusSquared)) {
|
||
inside = false;
|
||
return false;
|
||
}
|
||
// Passing here does not mean that the ray hits the bounding sphere;
|
||
// we would still have to perform more expensive tests to determine
|
||
// that.
|
||
}
|
||
|
||
inside = true;
|
||
const Vector3& MinB = box.low();
|
||
const Vector3& MaxB = box.high();
|
||
Vector3 MaxT(-1.0f, -1.0f, -1.0f);
|
||
|
||
// Find candidate planes.
|
||
for (int i = 0; i < 3; ++i) {
|
||
if (ray.origin()[i] < MinB[i]) {
|
||
location[i] = MinB[i];
|
||
inside = false;
|
||
|
||
// Calculate T distances to candidate planes
|
||
if (ray.direction()[i] != 0) {
|
||
MaxT[i] = (MinB[i] - ray.origin()[i]) * invDir[i];
|
||
}
|
||
} else if (ray.origin()[i] > MaxB[i]) {
|
||
location[i] = MaxB[i];
|
||
inside = false;
|
||
|
||
// Calculate T distances to candidate planes
|
||
if (ray.direction()[i] != 0) {
|
||
MaxT[i] = (MaxB[i] - ray.origin()[i]) * invDir[i];
|
||
}
|
||
}
|
||
}
|
||
|
||
if (inside) {
|
||
// Ray origin inside bounding box
|
||
location = ray.origin();
|
||
return true;
|
||
}
|
||
|
||
// Get largest of the maxT's for final choice of intersection
|
||
int WhichPlane = 0;
|
||
if (MaxT[1] > MaxT[WhichPlane]) {
|
||
WhichPlane = 1;
|
||
}
|
||
|
||
if (MaxT[2] > MaxT[WhichPlane]) {
|
||
WhichPlane = 2;
|
||
}
|
||
|
||
// Check final candidate actually inside box
|
||
if (MaxT[WhichPlane] < 0.0f) {
|
||
// Miss the box
|
||
return false;
|
||
}
|
||
|
||
for (int i = 0; i < 3; ++i) {
|
||
if (i != WhichPlane) {
|
||
location[i] = ray.origin()[i] + MaxT[WhichPlane] * ray.direction()[i];
|
||
if ((location[i] < MinB[i]) ||
|
||
(location[i] > MaxB[i])) {
|
||
// On this plane we're outside the box extents, so
|
||
// we miss the box
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
float CollisionDetection::collisionTimeForMovingPointFixedSphere(
|
||
const Vector3& point,
|
||
const Vector3& velocity,
|
||
const Sphere& sphere,
|
||
Vector3& location,
|
||
Vector3& outNormal,
|
||
bool solid) {
|
||
|
||
if (solid && sphere.contains(point)) {
|
||
location = point;
|
||
outNormal = (point - sphere.center).direction();
|
||
return 0.0f;
|
||
}
|
||
|
||
float speed = velocity.magnitude();
|
||
const Vector3& direction = velocity / speed;
|
||
|
||
// length of the axis between the start and the sphere
|
||
const Vector3& L = sphere.center - point;
|
||
float d = L.dot(direction);
|
||
|
||
float L2 = L.dot(L);
|
||
float R2 = square(sphere.radius);
|
||
float D2 = square(d);
|
||
|
||
if ((d < 0.0f) && (L2 > R2)) {
|
||
location = Vector3::inf();
|
||
return finf();
|
||
}
|
||
|
||
const float M2 = L2 - D2;
|
||
|
||
if (M2 > R2) {
|
||
location = Vector3::inf();
|
||
return finf();
|
||
}
|
||
|
||
float q = sqrt(R2 - M2);
|
||
float time;
|
||
|
||
if (L2 > R2) {
|
||
time = d - q;
|
||
} else {
|
||
time = d + q;
|
||
}
|
||
|
||
time /= speed;
|
||
|
||
location = point + velocity * time;
|
||
outNormal = (location - sphere.center).direction();
|
||
|
||
return time;
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingSphereFixedSphere(
|
||
const Sphere& movingSphere,
|
||
const Vector3& velocity,
|
||
const Sphere& fixedSphere,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
const Vector3& sep = (fixedSphere.center - movingSphere.center);
|
||
float sepLen = sep.squaredLength();
|
||
if (sepLen < square(movingSphere.radius + fixedSphere.radius)) {
|
||
// Interpenetrating
|
||
outNormal = sep.directionOrZero();
|
||
location = fixedSphere.center - outNormal * fixedSphere.radius;
|
||
return 0;
|
||
}
|
||
|
||
float time = collisionTimeForMovingPointFixedSphere
|
||
(movingSphere.center, velocity,
|
||
Sphere(fixedSphere.center, fixedSphere.radius + movingSphere.radius),
|
||
location, outNormal);
|
||
|
||
if (time < finf()) {
|
||
// Location is now the center of the moving sphere at the collision time.
|
||
// Adjust for the size of the moving sphere. Two spheres always collide
|
||
// along a line between their centers.
|
||
location += (location - fixedSphere.center) * movingSphere.radius / fixedSphere.radius;
|
||
}
|
||
|
||
return time;
|
||
}
|
||
|
||
|
||
/*
|
||
float CollisionDetection::collisionTimeForMovingPointFixedTriangle(
|
||
const Vector3& point,
|
||
const Vector3& velocity,
|
||
const Triangle& triangle,
|
||
Vector3& outLocation,
|
||
Vector3& outNormal) {
|
||
|
||
double time = collisionTimeForMovingPointFixedPlane(point, velocity, triangle.plane(), outLocation, outNormal);
|
||
|
||
if (time == finf()) {
|
||
// No collision with the plane of the triangle.
|
||
return finf();
|
||
}
|
||
|
||
if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), outLocation, triangle.primaryAxis())) {
|
||
// Collision occured inside the triangle
|
||
return time;
|
||
} else {
|
||
// Missed the triangle
|
||
outLocation = Vector3::inf();
|
||
return finf();
|
||
}
|
||
}*/
|
||
|
||
/*
|
||
float CollisionDetection::collisionTimeForMovingPointFixedTriangle(
|
||
const Vector3& orig,
|
||
const Vector3& dir,
|
||
const Vector3& vert0,
|
||
const Vector3& vert1,
|
||
const Vector3& vert2) {
|
||
|
||
// Barycenteric coords
|
||
double u, v;
|
||
#define EPSILON 0.000001
|
||
#define CROSS(dest,v1,v2) \
|
||
dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
|
||
dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
|
||
dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
|
||
|
||
#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
|
||
|
||
#define SUB(dest,v1,v2) \
|
||
dest[0]=v1[0]-v2[0]; \
|
||
dest[1]=v1[1]-v2[1]; \
|
||
dest[2]=v1[2]-v2[2];
|
||
|
||
double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
|
||
|
||
// find vectors for two edges sharing vert0
|
||
SUB(edge1, vert1, vert0);
|
||
SUB(edge2, vert2, vert0);
|
||
|
||
// begin calculating determinant - also used to calculate U parameter
|
||
CROSS(pvec, dir, edge2);
|
||
|
||
// if determinant is near zero, ray lies in plane of triangle
|
||
const double det = DOT(edge1, pvec);
|
||
|
||
if (det < EPSILON) {
|
||
return finf();
|
||
}
|
||
|
||
// calculate distance from vert0 to ray origin
|
||
SUB(tvec, orig, vert0);
|
||
|
||
// calculate U parameter and test bounds
|
||
u = DOT(tvec, pvec);
|
||
if ((u < 0.0) || (u > det)) {
|
||
// Hit the plane outside the triangle
|
||
return finf();
|
||
}
|
||
|
||
// prepare to test V parameter
|
||
CROSS(qvec, tvec, edge1);
|
||
|
||
// calculate V parameter and test bounds
|
||
v = DOT(dir, qvec);
|
||
if ((v < 0.0) || (u + v > det)) {
|
||
// Hit the plane outside the triangle
|
||
return finf();
|
||
}
|
||
|
||
// calculate t, scale parameters, ray intersects triangle
|
||
// If we want u,v, we can compute this
|
||
// double t = DOT(edge2, qvec);
|
||
//const double inv_det = 1.0 / det;
|
||
//t *= inv_det;
|
||
//u *= inv_det;
|
||
//v *= inv_det;
|
||
// return t;
|
||
|
||
// Case where we don't need correct (u, v):
|
||
|
||
const double t = DOT(edge2, qvec);
|
||
|
||
if (t >= 0) {
|
||
// Note that det must be positive
|
||
return t / det;
|
||
} else {
|
||
// We had to travel backwards in time to intersect
|
||
return finf();
|
||
}
|
||
|
||
#undef EPSILON
|
||
#undef CROSS
|
||
#undef DOT
|
||
#undef SUB
|
||
}
|
||
*/
|
||
|
||
float CollisionDetection::collisionTimeForMovingPointFixedBox(
|
||
const Vector3& point,
|
||
const Vector3& velocity,
|
||
const Box& box,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
double bestTime;
|
||
|
||
Vector3 normal;
|
||
Vector3 v[4];
|
||
|
||
// Prime the loop
|
||
int f = 0;
|
||
box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
|
||
bestTime = collisionTimeForMovingPointFixedRectangle(point, velocity, v[0], v[1], v[2], v[3], location, normal);
|
||
outNormal = normal;
|
||
|
||
// Check other faces
|
||
for (f = 1; f < 6; ++f) {
|
||
Vector3 pos;
|
||
box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
|
||
float time = collisionTimeForMovingPointFixedRectangle(point, velocity, v[0], v[1], v[2], v[3], pos, normal);
|
||
if (time < bestTime) {
|
||
bestTime = time;
|
||
outNormal = normal;
|
||
location = pos;
|
||
}
|
||
}
|
||
|
||
return bestTime;
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingPointFixedAABox(
|
||
const Vector3& origin,
|
||
const Vector3& dir,
|
||
const AABox& box,
|
||
Vector3& location,
|
||
bool& Inside,
|
||
Vector3& normal) {
|
||
|
||
if (collisionLocationForMovingPointFixedAABox(origin, dir, box, location, Inside, normal)) {
|
||
return (location - origin).magnitude();
|
||
} else {
|
||
return (float)finf();
|
||
}
|
||
}
|
||
|
||
|
||
bool CollisionDetection::collisionLocationForMovingPointFixedAABox(
|
||
const Vector3& origin,
|
||
const Vector3& dir,
|
||
const AABox& box,
|
||
Vector3& location,
|
||
bool& Inside,
|
||
Vector3& normal) {
|
||
|
||
// Integer representation of a floating-point value.
|
||
#define IR(x) ((uint32&)x)
|
||
|
||
Inside = true;
|
||
const Vector3& MinB = box.low();
|
||
const Vector3& MaxB = box.high();
|
||
Vector3 MaxT(-1.0f, -1.0f, -1.0f);
|
||
|
||
// Find candidate planes.
|
||
for (int i = 0; i < 3; ++i) {
|
||
if (origin[i] < MinB[i]) {
|
||
location[i] = MinB[i];
|
||
Inside = false;
|
||
|
||
// Calculate T distances to candidate planes
|
||
if (IR(dir[i])) {
|
||
MaxT[i] = (MinB[i] - origin[i]) / dir[i];
|
||
}
|
||
} else if (origin[i] > MaxB[i]) {
|
||
location[i] = MaxB[i];
|
||
Inside = false;
|
||
|
||
// Calculate T distances to candidate planes
|
||
if (IR(dir[i])) {
|
||
MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
|
||
}
|
||
}
|
||
}
|
||
|
||
if (Inside) {
|
||
// Ray origin inside bounding box
|
||
location = origin;
|
||
return false;
|
||
}
|
||
|
||
// Get largest of the maxT's for final choice of intersection
|
||
int WhichPlane = 0;
|
||
if (MaxT[1] > MaxT[WhichPlane]) {
|
||
WhichPlane = 1;
|
||
}
|
||
|
||
if (MaxT[2] > MaxT[WhichPlane]) {
|
||
WhichPlane = 2;
|
||
}
|
||
|
||
// Check final candidate actually inside box
|
||
if (IR(MaxT[WhichPlane]) & 0x80000000) {
|
||
// Miss the box
|
||
return false;
|
||
}
|
||
|
||
for (int i = 0; i < 3; ++i) {
|
||
if (i != WhichPlane) {
|
||
location[i] = origin[i] + MaxT[WhichPlane] * dir[i];
|
||
if ((location[i] < MinB[i]) ||
|
||
(location[i] > MaxB[i])) {
|
||
// On this plane we're outside the box extents, so
|
||
// we miss the box
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Choose the normal to be the plane normal facing into the ray
|
||
normal = Vector3::zero();
|
||
normal[WhichPlane] = (dir[WhichPlane] > 0) ? -1.0 : 1.0;
|
||
|
||
return true;
|
||
|
||
#undef IR
|
||
}
|
||
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingPointFixedRectangle(
|
||
const Vector3& point,
|
||
const Vector3& velocity,
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& v3,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
Plane plane = Plane(v0, v1, v2);
|
||
|
||
float time = collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal);
|
||
|
||
if (time == finf()) {
|
||
// No collision is ever going to happen
|
||
return time;
|
||
}
|
||
|
||
if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), location)) {
|
||
// The intersection point is inside the rectangle; that is the location where
|
||
// the point hits the rectangle.
|
||
return time;
|
||
} else {
|
||
return finf();
|
||
}
|
||
}
|
||
|
||
/** Used by findRayCapsuleIntersection.
|
||
@cite From magic software http://www.magic-software.com/Source/Intersection3D/MgcIntr3DLinCap.cpp */
|
||
static int findRayCapsuleIntersectionAux(
|
||
const Vector3& rkOrigin,
|
||
const Vector3& rkDirection,
|
||
const Capsule& rkCapsule,
|
||
double afT[2]) {
|
||
|
||
Vector3 capsuleDirection = rkCapsule.point(1) - rkCapsule.point(0);
|
||
|
||
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
|
||
Vector3 kW = capsuleDirection;
|
||
float fWLength = kW.length();
|
||
kW = kW.direction();
|
||
|
||
Vector3 kU, kV;
|
||
kW.getTangents(kU, kV);
|
||
Vector3 kD(kU.dot(rkDirection), kV.dot(rkDirection), kW.dot(rkDirection));
|
||
|
||
float fDLength = kD.length();
|
||
kD = kD.direction();
|
||
|
||
float fEpsilon = 1e-6f;
|
||
|
||
float fInvDLength = 1.0f/fDLength;
|
||
Vector3 kDiff = rkOrigin - rkCapsule.point(0);
|
||
Vector3 kP(kU.dot(kDiff),kV.dot(kDiff),kW.dot(kDiff));
|
||
float fRadiusSqr = square(rkCapsule.radius());
|
||
|
||
float fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp;
|
||
|
||
// Is the velocity parallel to the capsule direction? (or zero)
|
||
if ((abs(kD.z) >= 1.0f - fEpsilon) || (fDLength < fEpsilon)) {
|
||
|
||
float fAxisDir = rkDirection.dot(capsuleDirection);
|
||
|
||
fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
|
||
if ((fAxisDir < 0) && (fDiscr >= 0.0f)) {
|
||
// Velocity anti-parallel to the capsule direction
|
||
fRoot = sqrt(fDiscr);
|
||
afT[0] = (kP.z + fRoot)*fInvDLength;
|
||
afT[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
|
||
return 2;
|
||
} else if ((fAxisDir > 0) && (fDiscr >= 0.0f)) {
|
||
// Velocity parallel to the capsule direction
|
||
fRoot = sqrt(fDiscr);
|
||
afT[0] = -(kP.z + fRoot)*fInvDLength;
|
||
afT[1] = (fWLength - kP.z + fRoot)*fInvDLength;
|
||
return 2;
|
||
} else {
|
||
// sphere heading wrong direction, or no velocity at all
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
// test intersection with infinite cylinder
|
||
fA = kD.x*kD.x + kD.y*kD.y;
|
||
fB = kP.x*kD.x + kP.y*kD.y;
|
||
fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
|
||
fDiscr = fB*fB - fA*fC;
|
||
if (fDiscr < 0.0f) {
|
||
// line does not intersect infinite cylinder
|
||
return 0;
|
||
}
|
||
|
||
int iQuantity = 0;
|
||
|
||
if (fDiscr > 0.0f) {
|
||
// line intersects infinite cylinder in two places
|
||
fRoot = sqrt(fDiscr);
|
||
fInv = 1.0f/fA;
|
||
fT = (-fB - fRoot)*fInv;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if ((0.0f <= fTmp) && (fTmp <= fWLength)) {
|
||
afT[iQuantity] = fT * fInvDLength;
|
||
++iQuantity;
|
||
}
|
||
|
||
fT = (-fB + fRoot)*fInv;
|
||
fTmp = kP.z + fT*kD.z;
|
||
|
||
if ((0.0f <= fTmp) && (fTmp <= fWLength)) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
}
|
||
|
||
if (iQuantity == 2) {
|
||
// line intersects capsule wall in two places
|
||
return 2;
|
||
}
|
||
} else {
|
||
// line is tangent to infinite cylinder
|
||
fT = -fB/fA;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if ((0.0f <= fTmp) && (fTmp <= fWLength)) {
|
||
afT[0] = fT*fInvDLength;
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
// test intersection with bottom hemisphere
|
||
// fA = 1
|
||
fB += kP.z*kD.z;
|
||
fC += kP.z*kP.z;
|
||
fDiscr = fB*fB - fC;
|
||
if (fDiscr > 0.0f) {
|
||
fRoot = sqrt(fDiscr);
|
||
fT = -fB - fRoot;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if (fTmp <= 0.0f) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
if (iQuantity == 2) {
|
||
return 2;
|
||
}
|
||
}
|
||
|
||
fT = -fB + fRoot;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if (fTmp <= 0.0f) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
if (iQuantity == 2) {
|
||
return 2;
|
||
}
|
||
}
|
||
} else if (fDiscr == 0.0f) {
|
||
fT = -fB;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if (fTmp <= 0.0f) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
if (iQuantity == 2) {
|
||
return 2;
|
||
}
|
||
}
|
||
}
|
||
|
||
// test intersection with top hemisphere
|
||
// fA = 1
|
||
fB -= kD.z*fWLength;
|
||
fC += fWLength*(fWLength - 2.0f*kP.z);
|
||
|
||
fDiscr = fB*fB - fC;
|
||
if (fDiscr > 0.0f) {
|
||
fRoot = sqrt(fDiscr);
|
||
fT = -fB - fRoot;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if (fTmp >= fWLength) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
if (iQuantity == 2) {
|
||
return 2;
|
||
}
|
||
}
|
||
|
||
fT = -fB + fRoot;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if (fTmp >= fWLength) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
if (iQuantity == 2) {
|
||
return 2;
|
||
}
|
||
}
|
||
} else if (fDiscr == 0.0f) {
|
||
fT = -fB;
|
||
fTmp = kP.z + fT*kD.z;
|
||
if (fTmp >= fWLength) {
|
||
afT[iQuantity++] = fT*fInvDLength;
|
||
if (iQuantity == 2) {
|
||
return 2;
|
||
}
|
||
}
|
||
}
|
||
|
||
return iQuantity;
|
||
}
|
||
|
||
|
||
/** Used by collisionTimeForMovingPointFixedCapsule.
|
||
@cite From magic software http://www.magic-software.com/Source/Intersection3D/MgcIntr3DLinCap.cpp
|
||
|
||
@param rkRay The ray
|
||
@param rkCapsule The capsule
|
||
@param riQuantity The number of intersections found
|
||
@param akPoint The intersections found
|
||
@return True if there is at least one intersection
|
||
*/
|
||
static bool findRayCapsuleIntersection(
|
||
const Ray& rkRay,
|
||
const Capsule& rkCapsule,
|
||
int& riQuantity,
|
||
Vector3 akPoint[2]) {
|
||
|
||
double afT[2];
|
||
riQuantity = findRayCapsuleIntersectionAux(rkRay.origin(), rkRay.direction(), rkCapsule, afT);
|
||
|
||
// Only return intersections that occur in the future
|
||
int iClipQuantity = 0;
|
||
int i;
|
||
for (i = 0; i < riQuantity; ++i) {
|
||
if (afT[i] >= 0.0f) {
|
||
akPoint[iClipQuantity] = rkRay.origin() + afT[i] * rkRay.direction();
|
||
++iClipQuantity;
|
||
}
|
||
}
|
||
|
||
riQuantity = iClipQuantity;
|
||
return (riQuantity > 0);
|
||
}
|
||
|
||
float CollisionDetection::collisionTimeForMovingPointFixedCapsule(
|
||
const Vector3& _point,
|
||
const Vector3& velocity,
|
||
const Capsule& capsule,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
float timeScale = velocity.magnitude();
|
||
|
||
if (timeScale == 0.0f) {
|
||
timeScale = 1;
|
||
}
|
||
|
||
Vector3 direction = velocity / timeScale;
|
||
int numIntersections;
|
||
Vector3 intersection[2];
|
||
findRayCapsuleIntersection(Ray::fromOriginAndDirection(_point, direction), capsule, numIntersections, intersection);
|
||
|
||
if (numIntersections == 2) {
|
||
// A collision can only occur if there are two intersections. If there is one
|
||
// intersection, that one is exiting the capsule.
|
||
|
||
// Find the entering intersection (the first one that occurs).
|
||
float d0 = (intersection[0] - _point).squaredMagnitude();
|
||
float d1 = (intersection[1] - _point).squaredMagnitude();
|
||
|
||
// Compute the surface normal (if we aren't ignoring the result)
|
||
if (&outNormal != &ignore) {
|
||
Vector3 p2 = LineSegment::fromTwoPoints(capsule.point(0), capsule.point(1)).closestPoint(_point);
|
||
outNormal = (_point - p2).direction();
|
||
}
|
||
|
||
if (d0 > d1) {
|
||
location = intersection[1];
|
||
return sqrt(d1) / timeScale;
|
||
} else {
|
||
location = intersection[0];
|
||
return sqrt(d0) / timeScale;
|
||
}
|
||
} else {
|
||
// No entering intersection discovered; return no intersection.
|
||
location = Vector3::inf();
|
||
return finf();
|
||
}
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingSphereFixedPlane(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Plane& plane,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
if (sphere.radius == 0) {
|
||
// Optimization for zero radius sphere
|
||
return collisionTimeForMovingPointFixedPlane(sphere.center, velocity, plane, location, outNormal);
|
||
}
|
||
|
||
// The world-space collision point, which lies on the surface of the sphere, will be the point at
|
||
// center + velocity * time - (radius * planeNormal). Collisions only occur when
|
||
// the sphere is travelling into the plane.
|
||
|
||
float d;
|
||
plane.getEquation(outNormal, d);
|
||
|
||
// Rate at which the sphere is approaching the plane
|
||
const float vdotN = velocity.dot(outNormal);
|
||
|
||
if (fuzzyGt(vdotN, 0)) {
|
||
// No collision when the sphere is moving towards a backface.
|
||
location = Vector3::inf();
|
||
return (float)finf();
|
||
}
|
||
|
||
// Initial distance from the sphere center to the plane
|
||
const float distance = sphere.center.dot(outNormal) + d;
|
||
|
||
if (fuzzyLe(G3D::abs(distance), sphere.radius)) {
|
||
// Already interpenetrating
|
||
location = sphere.center - distance * outNormal;
|
||
return 0;
|
||
} else {
|
||
// The point on the sphere (in world space) that will eventually first contact the plane
|
||
const Point3& point = sphere.center - (sphere.radius * outNormal);
|
||
|
||
// The problem is now reduced to finding when the point hits the plane
|
||
return collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingSphereFixedTriangle
|
||
(const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Triangle& triangle,
|
||
Vector3& outLocation,
|
||
float b[3]) {
|
||
|
||
if (velocity.dot(triangle.normal()) > 0.0f) {
|
||
// No collision if moving towards a backface
|
||
return finf();
|
||
}
|
||
Vector3 dummy;
|
||
const float time = collisionTimeForMovingSphereFixedPlane(sphere, velocity, triangle.plane(),
|
||
outLocation, dummy);
|
||
|
||
if (time == finf()) {
|
||
// No collision is ever going to happen
|
||
return time;
|
||
}
|
||
|
||
// We will hit the plane of the triangle at *time*. See if
|
||
// the intersection point actually is within the triangle.
|
||
|
||
if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(),
|
||
outLocation, b, triangle.primaryAxis())) {
|
||
|
||
// The intersection point is inside the triangle; that is the location where
|
||
// the sphere hits the triangle.
|
||
|
||
# ifdef G3D_DEBUG
|
||
{
|
||
// Internal consistency checks
|
||
debugAssertM(b[0] >= 0.0 && b[0] <= 1.0f, "Intersection is outside triangle.");
|
||
debugAssertM(b[1] >= 0.0 && b[1] <= 1.0f, "Intersection is outside triangle.");
|
||
debugAssertM(b[2] >= 0.0 && b[2] <= 1.0f, "Intersection is outside triangle.");
|
||
const Vector3& blend =
|
||
b[0] * triangle.vertex(0) +
|
||
b[1] * triangle.vertex(1) +
|
||
b[2] * triangle.vertex(2);
|
||
debugAssertM(blend.fuzzyEq(outLocation), "Barycentric coords don't match intersection.");
|
||
// Call again so that we can debug the problem
|
||
// isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(),
|
||
// outLocation, b, triangle.primaryAxis());
|
||
}
|
||
# endif
|
||
|
||
return time;
|
||
}
|
||
|
||
// The collision (if it exists) is with a point on the triangle perimeter.
|
||
// Switch over to moving the triangle towards a fixed sphere and see at what time
|
||
// they will hit.
|
||
|
||
// Closest point on the triangle to the sphere intersection with the plane.
|
||
int edgeIndex;
|
||
const Vector3& point = closestPointOnTrianglePerimeter(triangle._vertex, triangle.edgeDirection,
|
||
triangle.edgeMagnitude, outLocation, edgeIndex);
|
||
|
||
float t = 0;
|
||
if (! sphere.contains(point)) {
|
||
// The point is outside the sphere--see when it will hit
|
||
t = collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, dummy, dummy);
|
||
}
|
||
|
||
if (t < finf()) {
|
||
outLocation = point;
|
||
// Compute Barycentric coords
|
||
|
||
// Index of the next vertex
|
||
static const int next[] = {1, 2, 0};
|
||
|
||
// Project along the edge in question.
|
||
// Avoid sqrt by taking advantage of the existing edgeDirection unit vector.
|
||
b[next[edgeIndex]] = (outLocation - triangle._vertex[edgeIndex]).dot
|
||
(triangle.edgeDirection[edgeIndex]) / triangle.edgeMagnitude[edgeIndex];
|
||
|
||
b[edgeIndex] = 1.0f - b[next[edgeIndex]];
|
||
|
||
b[next[next[edgeIndex]]] = 0.0f;
|
||
|
||
# ifdef G3D_DEBUG
|
||
{
|
||
// Internal consistency checks
|
||
for (int i = 0; i < 3; ++i) {
|
||
debugAssertM(fuzzyGe(b[i], 0.0f) && fuzzyLe(b[i], 1.0f), "Intersection is outside triangle.");
|
||
}
|
||
Vector3 blend =
|
||
b[0] * triangle.vertex(0) +
|
||
b[1] * triangle.vertex(1) +
|
||
b[2] * triangle.vertex(2);
|
||
debugAssertM(blend.fuzzyEq(outLocation),
|
||
format("Barycentric coords don't match intersection. %s != %s",
|
||
blend.toString().c_str(),
|
||
outLocation.toString().c_str()));
|
||
|
||
// Call again so that we can debug the problem
|
||
collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, dummy, dummy);
|
||
}
|
||
# endif
|
||
|
||
// Due to tiny roundoffs, these values might be slightly out of bounds.
|
||
// Ensure that they are legal. Note that the above debugging code
|
||
// verifies that we are not clamping truly illegal values.
|
||
for (int i = 0; i < 3; ++i) {
|
||
b[i] = clamp(b[i], 0.0f, 1.0f);
|
||
}
|
||
}
|
||
|
||
// The collision occured at the point, if it occured. The normal
|
||
// was the plane normal, computed above.
|
||
|
||
return t;
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingSphereFixedRectangle(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& v3,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
Plane plane(v0, v1, v2);
|
||
|
||
float time = collisionTimeForMovingSphereFixedPlane(sphere, velocity, plane, location, outNormal);
|
||
|
||
if (time == finf()) {
|
||
// No collision is ever going to happen
|
||
return time;
|
||
}
|
||
|
||
if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), location)) {
|
||
// The intersection point is inside the rectangle; that is the location where
|
||
// the sphere hits the rectangle.
|
||
return time;
|
||
}
|
||
|
||
// Switch over to moving the rectangle towards a fixed sphere and see at what time
|
||
// they will hit.
|
||
|
||
Vector3 point = closestPointToRectanglePerimeter(v0, v1, v2, v3, sphere.center);
|
||
|
||
Vector3 dummy;
|
||
double t = collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, location, dummy);
|
||
|
||
// Normal is the plane normal, location is the original location of the point.
|
||
location = point;
|
||
|
||
return t;
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingSphereFixedBox(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Box& box,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
if (fixedSolidSphereIntersectsFixedSolidBox(sphere, box)) {
|
||
// TODO: Compute more useful location and normal?
|
||
location = sphere.center;
|
||
outNormal = Vector3::zero();
|
||
return 0;
|
||
}
|
||
|
||
float bestTime;
|
||
|
||
Vector3 v[4];
|
||
int f = 0;
|
||
box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
|
||
bestTime = collisionTimeForMovingSphereFixedRectangle(sphere, velocity, v[0], v[1], v[2], v[3], location, outNormal);
|
||
|
||
for (f = 1; f < 6; ++f) {
|
||
Vector3 pos, normal;
|
||
box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
|
||
float time = collisionTimeForMovingSphereFixedRectangle(sphere, velocity, v[0], v[1], v[2], v[3], pos, normal);
|
||
if (time < bestTime) {
|
||
bestTime = time;
|
||
location = pos;
|
||
outNormal = normal;
|
||
}
|
||
}
|
||
|
||
return bestTime;
|
||
}
|
||
|
||
|
||
float CollisionDetection::collisionTimeForMovingSphereFixedCapsule(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Capsule& capsule,
|
||
Vector3& location,
|
||
Vector3& outNormal) {
|
||
|
||
(void)outNormal;
|
||
|
||
Capsule _capsule(capsule.point(0), capsule.point(1), capsule.radius() + sphere.radius);
|
||
|
||
Vector3 normal;
|
||
double time = collisionTimeForMovingPointFixedCapsule(sphere.center, velocity, _capsule, location, normal);
|
||
|
||
if (time < finf()) {
|
||
// Location is now the position of the center of the sphere at the time of collision.
|
||
// We have to adjust the collision location for the size of the sphere.
|
||
location -= sphere.radius * normal;
|
||
}
|
||
|
||
return time;
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::bounceDirection(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const float collisionTime,
|
||
const Vector3& collisionLocation,
|
||
const Vector3& collisionNormal) {
|
||
|
||
// Location when the collision occurs
|
||
Vector3 sphereLocation = sphere.center + velocity * collisionTime;
|
||
|
||
Vector3 normal = (sphereLocation - collisionLocation);
|
||
if (fuzzyEq(normal.squaredMagnitude(), 0)) {
|
||
normal = collisionNormal;
|
||
} else {
|
||
normal = normal.direction();
|
||
}
|
||
|
||
Vector3 direction = velocity.direction();
|
||
|
||
// Reflect direction about the normal
|
||
return direction - 2.0 * normal * normal.dot(direction);
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::slideDirection(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const float collisionTime,
|
||
const Vector3& collisionLocation) {
|
||
|
||
Vector3 sphereLocation = sphere.center + velocity * collisionTime;
|
||
Vector3 normal = (sphereLocation - collisionLocation).direction();
|
||
Vector3 direction = velocity.direction();
|
||
|
||
// subtract off the part in the direction away from the normal.
|
||
return direction - normal * normal.dot(direction);
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::closestPointOnLineSegment(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& point) {
|
||
|
||
const Vector3& edge = (v1 - v0);
|
||
float edgeLength = edge.magnitude();
|
||
|
||
if (edgeLength == 0) {
|
||
// The line segment is a point
|
||
return v0;
|
||
}
|
||
|
||
return closestPointOnLineSegment(v0, v1, edge / edgeLength, edgeLength, point);
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::closestPointOnLineSegment(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& edgeDirection,
|
||
const float edgeLength,
|
||
const Vector3& point) {
|
||
|
||
debugAssert((v1 - v0).direction().fuzzyEq(edgeDirection));
|
||
debugAssert(fuzzyEq((v1 - v0).magnitude(), edgeLength));
|
||
|
||
// Vector towards the point
|
||
const Vector3& c = point - v0;
|
||
|
||
// Projected onto the edge itself
|
||
float t = edgeDirection.dot(c);
|
||
|
||
if (t <= 0) {
|
||
// Before the start
|
||
return v0;
|
||
} else if (t >= edgeLength) {
|
||
// After the end
|
||
return v1;
|
||
} else {
|
||
// At distance t along the edge
|
||
return v0 + edgeDirection * t;
|
||
}
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::closestPointOnTrianglePerimeter(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& point) {
|
||
|
||
Vector3 v[3] = {v0, v1, v2};
|
||
Vector3 edgeDirection[3] = {(v1 - v0), (v2 - v1), (v0 - v2)};
|
||
float edgeLength[3];
|
||
|
||
for (int i = 0; i < 3; ++i) {
|
||
edgeLength[i] = edgeDirection[i].magnitude();
|
||
edgeDirection[i] /= edgeLength[i];
|
||
}
|
||
|
||
int edgeIndex;
|
||
return closestPointOnTrianglePerimeter(v, edgeDirection, edgeLength, point, edgeIndex);
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::closestPointOnTrianglePerimeter(
|
||
const Vector3 v[3],
|
||
const Vector3 edgeDirection[3],
|
||
const float edgeLength[3],
|
||
const Vector3& point,
|
||
int& edgeIndex) {
|
||
|
||
// Closest point on segment from v[i] to v[i + 1]
|
||
Vector3 r[3];
|
||
|
||
// Distance squared from r[i] to point
|
||
float d[3];
|
||
|
||
// Index of the next point
|
||
static const int next[] = {1, 2, 0};
|
||
|
||
for (int i = 0; i < 3; ++i) {
|
||
r[i] = closestPointOnLineSegment(v[i], v[next[i]], edgeDirection[i], edgeLength[i], point);
|
||
d[i] = (r[i] - point).squaredMagnitude();
|
||
}
|
||
|
||
if (d[0] < d[1]) {
|
||
if (d[0] < d[2]) {
|
||
// Between v0 and v1
|
||
edgeIndex = 0;
|
||
} else {
|
||
// Between v2 and v0
|
||
edgeIndex = 2;
|
||
}
|
||
} else {
|
||
if (d[1] < d[2]) {
|
||
// Between v1 and v2
|
||
edgeIndex = 1;
|
||
} else {
|
||
// Between v2 and v0
|
||
edgeIndex = 2;
|
||
}
|
||
}
|
||
|
||
# ifdef G3D_DEBUG
|
||
{
|
||
Vector3 diff = r[edgeIndex] - v[edgeIndex];
|
||
debugAssertM(fuzzyEq(diff.direction().dot(edgeDirection[edgeIndex]), 1.0f) ||
|
||
diff.fuzzyEq(Vector3::zero()), "Point not on correct triangle edge");
|
||
float frac = diff.dot(edgeDirection[edgeIndex])/edgeLength[edgeIndex];
|
||
debugAssertM(frac >= -0.000001, "Point off low side of edge.");
|
||
debugAssertM(frac <= 1.000001, "Point off high side of edge.");
|
||
}
|
||
# endif
|
||
|
||
return r[edgeIndex];
|
||
}
|
||
|
||
|
||
bool CollisionDetection::isPointInsideTriangle(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& normal,
|
||
const Vector3& point,
|
||
float b[3],
|
||
Vector3::Axis primaryAxis) {
|
||
|
||
if (primaryAxis == Vector3::DETECT_AXIS) {
|
||
primaryAxis = normal.primaryAxis();
|
||
}
|
||
|
||
// Check that the point is within the triangle using a Barycentric
|
||
// coordinate test on a two dimensional plane.
|
||
int i, j;
|
||
|
||
switch (primaryAxis) {
|
||
case Vector3::X_AXIS:
|
||
i = Vector3::Y_AXIS;
|
||
j = Vector3::Z_AXIS;
|
||
break;
|
||
|
||
case Vector3::Y_AXIS:
|
||
i = Vector3::Z_AXIS;
|
||
j = Vector3::X_AXIS;
|
||
break;
|
||
|
||
case Vector3::Z_AXIS:
|
||
i = Vector3::X_AXIS;
|
||
j = Vector3::Y_AXIS;
|
||
break;
|
||
|
||
default:
|
||
// This case is here to supress a warning on Linux
|
||
i = j = 0;
|
||
debugAssertM(false, "Should not get here.");
|
||
break;
|
||
}
|
||
|
||
// See if all barycentric coordinates are non-negative
|
||
|
||
// 2D area via cross product
|
||
# define AREA2(d, e, f) (((e)[i] - (d)[i]) * ((f)[j] - (d)[j]) - ((f)[i] - (d)[i]) * ((e)[j] - (d)[j]))
|
||
|
||
// Area of the polygon
|
||
float area = AREA2(v0, v1, v2);
|
||
if (area == 0) {
|
||
// This triangle has zero area, so the point must not
|
||
// be in it unless the triangle point is the test point.
|
||
return (v0 == point);
|
||
}
|
||
|
||
debugAssert(area != 0);
|
||
|
||
float invArea = 1.0f / area;
|
||
|
||
// (avoid normalization until absolutely necessary)
|
||
b[0] = AREA2(point, v1, v2) * invArea;
|
||
|
||
if ((b[0] < 0.0f) || (b[0] > 1.0f)) {
|
||
return false;
|
||
}
|
||
|
||
b[1] = AREA2(v0, point, v2) * invArea;
|
||
if ((b[1] < 0.0f) || (b[1] > 1.0f)) {
|
||
return false;
|
||
}
|
||
|
||
b[2] = 1.0f - b[0] - b[1];
|
||
|
||
# undef AREA2
|
||
|
||
return (b[2] >= 0.0f) && (b[2] <= 1.0f);
|
||
}
|
||
|
||
|
||
bool CollisionDetection::isPointInsideRectangle(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& v3,
|
||
const Vector3& normal,
|
||
const Vector3& point) {
|
||
|
||
return isPointInsideTriangle(v0, v1, v2, normal, point) ||
|
||
isPointInsideTriangle(v2, v3, v0, normal, point);
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::closestPointToRectanglePerimeter(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& v3,
|
||
const Vector3& point) {
|
||
|
||
Vector3 r0 = closestPointOnLineSegment(v0, v1, point);
|
||
Vector3 r1 = closestPointOnLineSegment(v1, v2, point);
|
||
Vector3 r2 = closestPointOnLineSegment(v2, v3, point);
|
||
Vector3 r3 = closestPointOnLineSegment(v3, v0, point);
|
||
|
||
double d0 = (r0 - point).squaredMagnitude();
|
||
double d1 = (r1 - point).squaredMagnitude();
|
||
double d2 = (r2 - point).squaredMagnitude();
|
||
double d3 = (r3 - point).squaredMagnitude();
|
||
|
||
if (d0 < d1) {
|
||
if (d0 < d2) {
|
||
if (d0 < d3) {
|
||
return r0;
|
||
} else {
|
||
return r3;
|
||
}
|
||
} else {
|
||
if (d2 < d3) {
|
||
return r2;
|
||
} else {
|
||
return r3;
|
||
}
|
||
}
|
||
} else {
|
||
if (d1 < d2) {
|
||
if (d1 < d3) {
|
||
return r1;
|
||
} else {
|
||
return r3;
|
||
}
|
||
} else {
|
||
if (d2 < d3) {
|
||
return r2;
|
||
} else {
|
||
return r3;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
Vector3 CollisionDetection::closestPointToRectangle(
|
||
const Vector3& v0,
|
||
const Vector3& v1,
|
||
const Vector3& v2,
|
||
const Vector3& v3,
|
||
const Vector3& point) {
|
||
|
||
Plane plane(v0, v1, v2);
|
||
|
||
// Project the point into the plane
|
||
double a, b, c, d;
|
||
plane.getEquation(a, b, c, d);
|
||
|
||
double distance = a*point.x + b*point.y + c*point.z + d;
|
||
Vector3 planePoint = point - distance * plane.normal();
|
||
|
||
if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), planePoint)) {
|
||
return planePoint;
|
||
} else {
|
||
return closestPointToRectanglePerimeter(v0, v1, v2, v3, planePoint);
|
||
}
|
||
}
|
||
|
||
|
||
bool CollisionDetection::fixedSolidSphereIntersectsFixedSolidSphere(
|
||
const Sphere& sphere1,
|
||
const Sphere& sphere2) {
|
||
|
||
return (sphere1.center - sphere2.center).squaredMagnitude() < square(sphere1.radius + sphere2.radius);
|
||
}
|
||
|
||
|
||
bool CollisionDetection::fixedSolidSphereIntersectsFixedSolidBox(
|
||
const Sphere& sphere,
|
||
const Box& box) {
|
||
|
||
// If the center of the sphere is within the box, the whole
|
||
// sphere is within the box.
|
||
if (box.contains(sphere.center)) {
|
||
return true;
|
||
}
|
||
|
||
float r2 = square(sphere.radius);
|
||
|
||
// Find the closest point on the surface of the box to the sphere. If
|
||
// this point is within the sphere's radius, they intersect.
|
||
int f;
|
||
for (f = 0; f < 6; ++f) {
|
||
Vector3 v0, v1, v2, v3;
|
||
box.getFaceCorners(f, v0, v1, v2, v3);
|
||
if ((closestPointToRectangle(v0, v1, v2, v3, sphere.center) - sphere.center).squaredMagnitude() <= r2) {
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
|
||
bool CollisionDetection::movingSpherePassesThroughFixedBox(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Box& box,
|
||
double timeLimit) {
|
||
|
||
// If they intersect originally, they definitely pass through each other.
|
||
if (fixedSolidSphereIntersectsFixedSolidBox(sphere, box)) {
|
||
return true;
|
||
}
|
||
|
||
// See if the sphere hits the box during the time period.
|
||
Vector3 dummy1, dummy2;
|
||
|
||
return (collisionTimeForMovingSphereFixedBox(sphere, velocity, box, dummy1, dummy2) < timeLimit);
|
||
}
|
||
|
||
|
||
bool CollisionDetection::movingSpherePassesThroughFixedSphere(
|
||
const Sphere& sphere,
|
||
const Vector3& velocity,
|
||
const Sphere& fixedSphere,
|
||
double timeLimit) {
|
||
|
||
if (fixedSolidSphereIntersectsFixedSolidSphere(sphere, fixedSphere)) {
|
||
return true;
|
||
}
|
||
|
||
// Extend the fixed sphere by the radius of the moving sphere
|
||
Sphere bigFixed(fixedSphere.center, fixedSphere.radius + sphere.radius);
|
||
Vector3 dummy1, dummy2;
|
||
|
||
// If the sphere collides with the other sphere during the time limit, it passes through
|
||
return (collisionTimeForMovingPointFixedSphere(sphere.center, velocity, bigFixed, dummy1, dummy2) < timeLimit);
|
||
}
|
||
|
||
|
||
|
||
bool CollisionDetection::fixedSolidSphereIntersectsFixedTriangle(
|
||
const Sphere& sphere,
|
||
const Triangle& triangle) {
|
||
|
||
// How far is the sphere from the plane of the triangle
|
||
const Plane& plane = triangle.plane();
|
||
|
||
// Does the closest point to the sphere center lie within the triangle?
|
||
Vector3 v = plane.closestPoint(sphere.center);
|
||
|
||
// Is the closest point to the plane within the sphere?
|
||
if ((v - sphere.center).squaredLength() <= square(sphere.radius)) {
|
||
// Is it also within the triangle?
|
||
float b[3];
|
||
if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(),
|
||
v, b, triangle.primaryAxis())){
|
||
// The closest point is inside the triangle
|
||
return true;
|
||
}
|
||
}
|
||
|
||
// ignored
|
||
int edgeIndex;
|
||
|
||
v = closestPointOnTrianglePerimeter(triangle._vertex, triangle.edgeDirection, triangle.edgeMagnitude, sphere.center, edgeIndex);
|
||
|
||
// Is the closest point within the sphere?
|
||
return ((v - sphere.center).squaredLength() <= square(sphere.radius));
|
||
}
|
||
|
||
|
||
////////////////////////////////////////////////////////////////////////////////
|
||
// AABB-triangle overlap test code based on Tomas Akenine-M<><4D>ller's
|
||
// http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/tribox3.txt
|
||
// Ported 2008-12-28
|
||
|
||
#define X 0
|
||
#define Y 1
|
||
#define Z 2
|
||
|
||
#define FINDMINMAX(x0, x1, x2, min, max) \
|
||
min = max = x0; \
|
||
if(x1<min) min=x1;\
|
||
if(x1>max) max=x1;\
|
||
if(x2<min) min=x2;\
|
||
if(x2>max) max=x2;
|
||
|
||
static bool planeBoxOverlap(const Vector3& normal, const Vector3& vert, const Vector3& maxbox) {
|
||
Vector3 vmin, vmax;
|
||
float v;
|
||
|
||
// for each axis
|
||
for(int a = 0; a < 3; ++a) {
|
||
v = vert[a];
|
||
|
||
if (normal[a] > 0.0f) {
|
||
vmin[a] = -maxbox[a] - v;
|
||
vmax[a] = maxbox[a] - v;
|
||
} else {
|
||
vmin[a] = maxbox[a] - v;
|
||
vmax[a] = -maxbox[a] - v;
|
||
}
|
||
}
|
||
|
||
if (normal.dot(vmin) > 0.0f) {
|
||
return false;
|
||
} else if (normal.dot(vmax) >= 0.0f) {
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/*======================== X-tests ========================*/
|
||
|
||
#define AXISTEST_X01(a, b, fa, fb) \
|
||
p0 = a*v0[Y] - b*v0[Z]; \
|
||
p2 = a*v2[Y] - b*v2[Z]; \
|
||
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
|
||
rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
|
||
if(min>rad || max<-rad) return false;
|
||
|
||
|
||
#define AXISTEST_X2(a, b, fa, fb) \
|
||
p0 = a*v0[Y] - b*v0[Z]; \
|
||
p1 = a*v1[Y] - b*v1[Z]; \
|
||
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
|
||
rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
|
||
if(min>rad || max<-rad) return false;
|
||
|
||
/*======================== Y-tests ========================*/
|
||
|
||
#define AXISTEST_Y02(a, b, fa, fb) \
|
||
p0 = -a*v0[X] + b*v0[Z]; \
|
||
p2 = -a*v2[X] + b*v2[Z]; \
|
||
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
|
||
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
|
||
if(min>rad || max<-rad) return false;
|
||
|
||
#define AXISTEST_Y1(a, b, fa, fb) \
|
||
p0 = -a*v0[X] + b*v0[Z]; \
|
||
p1 = -a*v1[X] + b*v1[Z]; \
|
||
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
|
||
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
|
||
if(min>rad || max<-rad) return false;
|
||
|
||
/*======================== Z-tests ========================*/
|
||
|
||
#define AXISTEST_Z12(a, b, fa, fb) \
|
||
p1 = a*v1[X] - b*v1[Y]; \
|
||
p2 = a*v2[X] - b*v2[Y]; \
|
||
if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
|
||
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
|
||
if(min>rad || max<-rad) return false;
|
||
|
||
#define AXISTEST_Z0(a, b, fa, fb) \
|
||
p0 = a*v0[X] - b*v0[Y]; \
|
||
p1 = a*v1[X] - b*v1[Y]; \
|
||
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
|
||
rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
|
||
if(min>rad || max<-rad) return false;
|
||
|
||
bool CollisionDetection::fixedSolidBoxIntersectsFixedTriangle(
|
||
const AABox& box, const Triangle& tri) {
|
||
|
||
// use separating axis theorem to test overlap between triangle and box
|
||
// need to test for overlap in these directions:
|
||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||
// we do not even need to test these)
|
||
// 2) normal of the triangle
|
||
// 3) crossproduct(edge from tri, {x,y,z}-direction)
|
||
// this gives 3x3=9 more tests
|
||
|
||
// This is the fastest branch (on Sun).
|
||
// Move the triangle to the object space of the box
|
||
// Triangle vertices in box object space
|
||
|
||
const Vector3& boxcenter = box.center();
|
||
const Vector3& boxhalfsize = box.extent() * 0.5f;
|
||
|
||
const Vector3& v0 = tri.vertex(0) - boxcenter;
|
||
const Vector3& v1 = tri.vertex(1) - boxcenter;
|
||
const Vector3& v2 = tri.vertex(2) - boxcenter;
|
||
|
||
// Compute triangle edges in object space
|
||
const Vector3& e0 = v1 - v0;
|
||
const Vector3& e1 = v2 - v1;
|
||
const Vector3& e2 = v0 - v2;
|
||
|
||
// Bullet 3:
|
||
// test the 9 tests first (this was faster)
|
||
float min,max,p0,p1,p2,rad;
|
||
Vector3 fe;
|
||
|
||
fe = abs(e0);
|
||
AXISTEST_X01(e0[Z], e0[Y], fe[Z], fe[Y]);
|
||
AXISTEST_Y02(e0[Z], e0[X], fe[Z], fe[X]);
|
||
AXISTEST_Z12(e0[Y], e0[X], fe[Y], fe[X]);
|
||
|
||
fe = abs(e1);
|
||
AXISTEST_X01(e1[Z], e1[Y], fe[Z], fe[Y]);
|
||
AXISTEST_Y02(e1[Z], e1[X], fe[Z], fe[X]);
|
||
AXISTEST_Z0 (e1[Y], e1[X], fe[Y], fe[X]);
|
||
|
||
fe = abs(e2);
|
||
AXISTEST_X2 (e2[Z], e2[Y], fe[Z], fe[Y]);
|
||
AXISTEST_Y1 (e2[Z], e2[X], fe[Z], fe[X]);
|
||
AXISTEST_Z12(e2[Y], e2[X], fe[Y], fe[X]);
|
||
|
||
// Bullet 1:
|
||
// first test overlap in the {x,y,z}-directions
|
||
// find min, max of the triangle each direction, and test for overlap in
|
||
// that direction -- this is equivalent to testing a minimal AABB around
|
||
// the triangle against the AABB
|
||
|
||
// test in X-direction
|
||
FINDMINMAX(v0[X],v1[X],v2[X],min,max);
|
||
if (min > boxhalfsize[X] || max < -boxhalfsize[X]) {
|
||
return false;
|
||
}
|
||
|
||
// test in Y-direction
|
||
FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
|
||
if (min > boxhalfsize[Y] || max < -boxhalfsize[Y]) {
|
||
return false;
|
||
}
|
||
|
||
// test in Z-direction
|
||
FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
|
||
if (min > boxhalfsize[Z] || max < -boxhalfsize[Z]) {
|
||
return false;
|
||
}
|
||
|
||
// Bullet 2:
|
||
// test if the box intersects the plane of the triangle
|
||
// compute plane equation of triangle: normal*x+d=0
|
||
|
||
if (! planeBoxOverlap(tri.normal(), v0, boxhalfsize)) {
|
||
return false;
|
||
}
|
||
|
||
// box and triangle overlap
|
||
return true;
|
||
}
|
||
#undef X
|
||
#undef Y
|
||
#undef Z
|
||
|
||
////////////////////////////////////////////////////////////////////////////////
|
||
|
||
|
||
} // namespace
|
||
|
||
#ifdef _MSC_VER
|
||
// Turn off fast floating-point optimizations
|
||
#pragma float_control( pop )
|
||
#pragma warning (pop)
|
||
#endif
|