diff options
Diffstat (limited to 'dep/src/g3dlite/CollisionDetection.cpp')
| -rw-r--r-- | dep/src/g3dlite/CollisionDetection.cpp | 2455 |
1 files changed, 0 insertions, 2455 deletions
diff --git a/dep/src/g3dlite/CollisionDetection.cpp b/dep/src/g3dlite/CollisionDetection.cpp deleted file mode 100644 index 77eef0a5500..00000000000 --- a/dep/src/g3dlite/CollisionDetection.cpp +++ /dev/null @@ -1,2455 +0,0 @@ -/** - @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; - const Box* faceBox; - Vector3 faceNormal = L; - - // L will be the outward facing normal for the faceBox - if (penetrationAxisIndex < 3) { - faceBox = & box1; - vertexBox = & box2; - if (dot(L, D) < 0) { - faceNormal = -L; - } - } else { - faceBox = & box2; - 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. - double d; - Vector3 normal; - plane.getEquation(normal, d); - - float vdotN = velocity.dot(normal); - float pdotN = point.dot(normal); - - if (fuzzyEq(pdotN + d, 0)) { - // The point is *in* the plane. - location = point; - outNormal = normal; - return 0; - } - - if (vdotN >= 0) { - // no collision will occur - location = Vector3::inf(); - return finf(); - } - - 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 kU, kV, kW = capsuleDirection; - float fWLength = kW.unitize(); - Vector3::generateOrthonormalBasis(kU, kV, kW); - Vector3 kD(kU.dot(rkDirection), kV.dot(rkDirection), kW.dot(rkDirection)); - float fDLength = kD.unitize(); - - 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 collision point on the sphere will be the point at - // center - (radius * normal). Collisions only occur when - // the sphere is travelling into the plane. - - double d; - plane.getEquation(outNormal, d); - - double vdotN = velocity.dot(outNormal); - - if (fuzzyGt(vdotN, 0)) { - // No collision when the sphere is moving towards a backface. - location = Vector3::inf(); - return (float)finf(); - } - - float cdotN = sphere.center.dot(outNormal); - - // Distance from the center to the plane - float distance = cdotN + (float)d; - - // Where is the collision on the sphere? - Vector3 point = sphere.center - (sphere.radius * outNormal); - - if (fuzzyLe(G3D::abs(distance), sphere.radius)) { - // Already interpenetrating - location = sphere.center - distance * outNormal; - return 0; - } else { - return collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal); - } - -} - - -float CollisionDetection::collisionTimeForMovingSphereFixedTriangle( - const class Sphere& sphere, - const Vector3& velocity, - const Triangle& triangle, - Vector3& outLocation, - float b[3]) { - - Vector3 dummy; - 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."); - 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.unitize(); - } - - 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 |