From 0494ffcf38f4ad6cae1cd078497e5fd5aed1a296 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 10 Sep 2014 17:01:51 -0700 Subject: [PATCH] implement Ray-vs-Capsule and Ray-vs-AACube --- libraries/shared/src/AACubeShape.cpp | 63 ++++++++++- libraries/shared/src/CapsuleShape.cpp | 130 ++++++++++++++++++++++- libraries/shared/src/CapsuleShape.h | 1 + libraries/shared/src/PlaneShape.cpp | 2 + libraries/shared/src/SphereShape.cpp | 1 + tests/physics/src/ShapeColliderTests.cpp | 12 +++ tests/physics/src/ShapeColliderTests.h | 2 + 7 files changed, 207 insertions(+), 4 deletions(-) diff --git a/libraries/shared/src/AACubeShape.cpp b/libraries/shared/src/AACubeShape.cpp index 16db9fc510..1fa41bb521 100644 --- a/libraries/shared/src/AACubeShape.cpp +++ b/libraries/shared/src/AACubeShape.cpp @@ -9,9 +9,68 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include +#include + #include "AACubeShape.h" +#include "SharedUtil.h" // for SQUARE_ROOT_OF_3 + +glm::vec3 planeNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) }; bool AACubeShape::findRayIntersection(RayIntersectionInfo& intersection) const { - // TODO: Andrew to implement this - return false; + // A = ray point + // B = cube center + glm::vec3 BA = _translation - intersection._rayStart; + + // check for ray intersection with cube's bounding sphere + // a = distance along line to closest approach to B + float a = glm::dot(intersection._rayDirection, BA); + // b2 = squared distance from cube center to point of closest approach + float b2 = glm::length2(a * intersection._rayDirection - BA); + // r = bounding radius of cube + float halfSide = 0.5f * _scale; + const float r = SQUARE_ROOT_OF_3 * halfSide; + if (b2 > r * r) { + // line doesn't hit cube's bounding sphere + return false; + } + + // check for tuncated/short ray + const float maxBA = glm::min(intersection._rayLength, intersection._hitDistance) + halfSide; + if (maxBA * maxBA > a * a + b2) { + // ray is not long enough to reach cube's bounding sphere + // NOTE: we don't fall in here when ray's length if FLT_MAX because maxBA^2 will be NaN + // and all NaN comparisons are false + return false; + } + + // the trivial checks have been exhausted, so must trace to each face + bool hit = false; + for (int i = 0; i < 3; ++i) { + for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) { + glm::vec3 planeNormal = sign * planeNormals[i]; + float rayDotPlane = glm::dot(intersection._rayDirection, planeNormal); + if (glm::abs(rayDotPlane) > EPSILON) { + float distanceToPlane = (halfSide + glm::dot(BA, planeNormal)) / rayDotPlane; + if (distanceToPlane >= 0.0f) { + glm::vec3 point = distanceToPlane * intersection._rayDirection - BA; + int j = (i + 1) % 3; + int k = (i + 2) % 3; + glm::vec3 secondNormal = planeNormals[j]; + glm::vec3 thirdNormal = planeNormals[k]; + if (glm::abs(glm::dot(point, secondNormal)) > halfSide || + glm::abs(glm::dot(point, thirdNormal)) > halfSide) { + continue; + } + if (distanceToPlane < intersection._hitDistance && distanceToPlane < intersection._rayLength) { + intersection._hitDistance = distanceToPlane; + intersection._hitNormal = planeNormal; + intersection._hitShape = const_cast(this); + hit = true; + } + } + } + } + } + return hit; } diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index f6bad6d5d1..5bb118d36e 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -78,9 +78,135 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en updateBoundingRadius(); } -bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const { - // TODO: Andrew to implement this +// helper +bool findRayIntersectionWithCap(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) { + float r2 = sphereRadius * sphereRadius; + + // compute closest approach (CA) + float a = glm::dot(sphereCenter - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA + float b2 = glm::distance2(sphereCenter, intersection._rayStart + a * intersection._rayDirection); // b2 = squared distance from sphere-center to CA + if (b2 > r2) { + // ray does not hit sphere return false; + } + float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection + float d2 = glm::distance2(intersection._rayStart, sphereCenter); // d2 = squared distance from sphere-center to ray-start + float distance = FLT_MAX; + if (a < 0.0f) { + // ray points away from sphere-center + if (d2 > r2) { + // ray starts outside sphere + return false; + } + // ray starts inside sphere + distance = c + a; + } else if (d2 > r2) { + // ray starts outside sphere + distance = a - c; + } else { + // ray starts inside sphere + distance = a + c; + } + if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) { + glm::vec3 sphereCenterToHitPoint = intersection._rayStart + distance * intersection._rayDirection - sphereCenter; + if (glm::dot(sphereCenterToHitPoint, sphereCenter - capsuleCenter) >= 0.0f) { + intersection._hitDistance = distance; + intersection._hitNormal = glm::normalize(sphereCenterToHitPoint); + return true; + } + } + return false; +} + +bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const { + glm::vec3 capCenter; + getStartPoint(capCenter); + bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection); + getEndPoint(capCenter); + hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit; + if (hit) { + intersection._hitShape = const_cast(this); + } + return hit; +} + +bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const { + // ray is U, capsule is V + glm::vec3 axisV; + computeNormalizedAxis(axisV); + glm::vec3 centerV = getTranslation(); + + // first handle parallel case + float uDotV = glm::dot(axisV, intersection._rayDirection); + glm::vec3 UV = intersection._rayStart - centerV; + if (glm::abs(1.0f - glm::abs(uDotV)) < EPSILON) { + // line and cylinder are parallel + float distanceV = glm::dot(UV, intersection._rayDirection); + if (glm::length2(UV - distanceV * intersection._rayDirection) <= _radius * _radius) { + // ray is inside cylinder's radius and might intersect caps + return findRayIntersectionWithCaps(centerV, intersection); + } + return false; + } + + // Given a line with point 'U' and normalized direction 'u' and + // a cylinder with axial point 'V', radius 'r', and normalized direction 'v' + // the intersection of the two is on the line at distance 't' from 'U'. + // + // Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0 + // + // where: + // + // UV = U-V + // w = u-(u.v)v + // Q = UV-(UV.v)v + // + // A = w^2 + // B = 2(w.Q) + // C = Q^2 - r^2 + + glm::vec3 w = intersection._rayDirection - uDotV * axisV; + glm::vec3 Q = UV - glm::dot(UV, axisV) * axisV; + + // we save a few multiplies by storing 2*A rather than just A + float A2 = 2.0f * glm::dot(w, w); + float B = 2.0f * glm::dot(w, Q); + + // since C is only ever used once (in the determinant) we compute it inline + float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - _radius * _radius); + if (determinant < 0.0f) { + return false; + } + float hitLow = (-B - sqrtf(determinant)) / A2; + float hitHigh = -(hitLow + 2.0f * B / A2); + + if (hitLow > hitHigh) { + // re-arrange so hitLow is always the smaller value + float temp = hitHigh; + hitHigh = hitLow; + hitLow = temp; + } + if (hitLow < 0.0f) { + if (hitHigh < 0.0f) { + // capsule is completely behind rayStart + return false; + } + hitLow = hitHigh; + } + + glm::vec3 p = intersection._rayStart + hitLow * intersection._rayDirection; + float d = glm::dot(p - centerV, axisV); + if (glm::abs(d) <= getHalfHeight()) { + // we definitely hit the cylinder wall + intersection._hitDistance = hitLow; + intersection._hitNormal = glm::normalize(p - centerV - d * axisV); + intersection._hitShape = const_cast(this); + return true; + } + + // ray still might hit the caps + return findRayIntersectionWithCaps(centerV, intersection); } // static diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h index 423aa850ba..6e889f6566 100644 --- a/libraries/shared/src/CapsuleShape.h +++ b/libraries/shared/src/CapsuleShape.h @@ -52,6 +52,7 @@ public: virtual float getVolume() const { return (PI * _radius * _radius) * (1.3333333333f * _radius + getHalfHeight()); } protected: + bool findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const; virtual void updateBoundingRadius() { _boundingRadius = _radius + getHalfHeight(); } static glm::quat computeNewRotation(const glm::vec3& newAxis); diff --git a/libraries/shared/src/PlaneShape.cpp b/libraries/shared/src/PlaneShape.cpp index d302bcb656..b844bac299 100644 --- a/libraries/shared/src/PlaneShape.cpp +++ b/libraries/shared/src/PlaneShape.cpp @@ -48,6 +48,7 @@ bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const { // ray starts on the plane intersection._hitDistance = 0.0f; intersection._hitNormal = n; + intersection._hitShape = const_cast(this); return true; } } else { @@ -56,6 +57,7 @@ bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const { // ray points toward plane intersection._hitDistance = d; intersection._hitNormal = n; + intersection._hitShape = const_cast(this); return true; } } diff --git a/libraries/shared/src/SphereShape.cpp b/libraries/shared/src/SphereShape.cpp index d5db68086d..4c47ae91c0 100644 --- a/libraries/shared/src/SphereShape.cpp +++ b/libraries/shared/src/SphereShape.cpp @@ -44,6 +44,7 @@ bool SphereShape::findRayIntersection(RayIntersectionInfo& intersection) const { if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) { intersection._hitDistance = distance; intersection._hitNormal = glm::normalize(intersection._rayStart + distance * intersection._rayDirection - _translation); + intersection._hitShape = const_cast(this); return true; } return false; diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index c7bef547f4..d1ba30ec36 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -2150,6 +2150,7 @@ void ShapeColliderTests::rayHitsPlane() { } void ShapeColliderTests::rayMissesPlane() { + // TODO: Andrew to test RayIntersectionInfo::_hitShape // make a simple plane float planeDistanceFromOrigin = 3.579f; glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); @@ -2229,6 +2230,14 @@ void ShapeColliderTests::rayMissesPlane() { } } +void ShapeColliderTests::rayHitsAACube() { + // TODO: Andrew to implement this +} + +void ShapeColliderTests::rayMissesAACube() { + // TODO: Andrew to implement this +} + void ShapeColliderTests::measureTimeOfCollisionDispatch() { /* KEEP for future manual testing // create two non-colliding spheres @@ -2288,4 +2297,7 @@ void ShapeColliderTests::runAllTests() { rayMissesCapsule(); rayHitsPlane(); rayMissesPlane(); + + rayHitsAACube(); + rayMissesAACube(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a7495d32bf..fa6887f685 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -38,6 +38,8 @@ namespace ShapeColliderTests { void rayMissesCapsule(); void rayHitsPlane(); void rayMissesPlane(); + void rayHitsAACube(); + void rayMissesAACube(); void measureTimeOfCollisionDispatch();