/**
@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ö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