diff --git a/libraries/shared/src/AABox.cpp b/libraries/shared/src/AABox.cpp index d8433e7fda..0d2e6295d0 100644 --- a/libraries/shared/src/AABox.cpp +++ b/libraries/shared/src/AABox.cpp @@ -114,9 +114,7 @@ bool AABox::contains(const Triangle& triangle) const { } bool AABox::contains(const glm::vec3& point) const { - return isWithin(point.x, _corner.x, _scale.x) && - isWithin(point.y, _corner.y, _scale.y) && - isWithin(point.z, _corner.z, _scale.z); + return aaBoxContains(point, _corner, _scale); } bool AABox::contains(const AABox& otherBox) const { @@ -170,30 +168,6 @@ bool AABox::expandedContains(const glm::vec3& point, float expansion) const { isWithinExpanded(point.z, _corner.z, _scale.z, expansion); } -// finds the intersection between a ray and the facing plane on one axis -static bool findIntersection(float origin, float direction, float corner, float size, float& distance) { - if (direction > EPSILON) { - distance = (corner - origin) / direction; - return true; - } else if (direction < -EPSILON) { - distance = (corner + size - origin) / direction; - return true; - } - return false; -} - -// finds the intersection between a ray and the inside facing plane on one axis -static bool findInsideOutIntersection(float origin, float direction, float corner, float size, float& distance) { - if (direction > EPSILON) { - distance = -1.0f * (origin - (corner + size)) / direction; - return true; - } else if (direction < -EPSILON) { - distance = -1.0f * (origin - corner) / direction; - return true; - } - return false; -} - bool AABox::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const { // handle the trivial cases where the expanded box contains the start or end if (expandedContains(start, expansion) || expandedContains(end, expansion)) { @@ -220,219 +194,12 @@ bool AABox::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& e bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face, glm::vec3& surfaceNormal) const { - // handle the trivial case where the box contains the origin - if (contains(origin)) { - // We still want to calculate the distance from the origin to the inside out plane - float axisDistance; - if ((findInsideOutIntersection(origin.x, direction.x, _corner.x, _scale.x, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale.y) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale.z))) { - distance = axisDistance; - face = direction.x > 0 ? MAX_X_FACE : MIN_X_FACE; - surfaceNormal = glm::vec3(direction.x > 0 ? 1.0f : -1.0f, 0.0f, 0.0f); - return true; - } - if ((findInsideOutIntersection(origin.y, direction.y, _corner.y, _scale.y, axisDistance) && axisDistance >= 0 && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale.x) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale.z))) { - distance = axisDistance; - face = direction.y > 0 ? MAX_Y_FACE : MIN_Y_FACE; - surfaceNormal = glm::vec3(0.0f, direction.y > 0 ? 1.0f : -1.0f, 0.0f); - return true; - } - if ((findInsideOutIntersection(origin.z, direction.z, _corner.z, _scale.z, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale.y) && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale.x))) { - distance = axisDistance; - face = direction.z > 0 ? MAX_Z_FACE : MIN_Z_FACE; - surfaceNormal = glm::vec3(0.0f, 0.0f, direction.z > 0 ? 1.0f : -1.0f); - return true; - } - // This case is unexpected, but mimics the previous behavior for inside out intersections - distance = 0; - return true; - } - - // check each axis - float axisDistance; - if ((findIntersection(origin.x, direction.x, _corner.x, _scale.x, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale.y) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale.z))) { - distance = axisDistance; - face = direction.x > 0 ? MIN_X_FACE : MAX_X_FACE; - surfaceNormal = glm::vec3(direction.x > 0 ? -1.0f : 1.0f, 0.0f, 0.0f); - return true; - } - if ((findIntersection(origin.y, direction.y, _corner.y, _scale.y, axisDistance) && axisDistance >= 0 && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale.x) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale.z))) { - distance = axisDistance; - face = direction.y > 0 ? MIN_Y_FACE : MAX_Y_FACE; - surfaceNormal = glm::vec3(0.0f, direction.y > 0 ? -1.0f : 1.0f, 0.0f); - return true; - } - if ((findIntersection(origin.z, direction.z, _corner.z, _scale.z, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale.y) && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale.x))) { - distance = axisDistance; - face = direction.z > 0 ? MIN_Z_FACE : MAX_Z_FACE; - surfaceNormal = glm::vec3(0.0f, 0.0f, direction.z > 0 ? -1.0f : 1.0f); - return true; - } - return false; -} - -void AABox::checkPossibleParabolicIntersection(float t, int i, float& minDistance, - const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, bool& hit) const { - if (t < minDistance && t > 0.0f && - isWithin(origin[(i + 1) % 3] + velocity[(i + 1) % 3] * t + 0.5f * acceleration[(i + 1) % 3] * t * t, _corner[(i + 1) % 3], _scale[(i + 1) % 3]) && - isWithin(origin[(i + 2) % 3] + velocity[(i + 2) % 3] * t + 0.5f * acceleration[(i + 2) % 3] * t * t, _corner[(i + 2) % 3], _scale[(i + 2) % 3])) { - minDistance = t; - hit = true; - } + return findRayAABoxIntersection(origin, direction, _corner, _scale, distance, face, surfaceNormal); } bool AABox::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, - float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const { - float minDistance = FLT_MAX; - BoxFace minFace; - glm::vec3 minNormal; - glm::vec2 possibleDistances; - float a, b, c; - - // Solve the intersection for each face of the cube. As we go, keep track of the smallest, positive, real distance - // that is within the bounds of the other two dimensions - for (int i = 0; i < 3; i++) { - if (fabsf(acceleration[i]) < EPSILON) { - // Handle the degenerate case where we only have a line in this axis - if (origin[i] < _corner[i]) { - { // min - if (velocity[i] > 0.0f) { - float possibleDistance = (_corner[i] - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } else if (origin[i] > _corner[i] + _scale[i]) { - { // max - if (velocity[i] < 0.0f) { - float possibleDistance = (_corner[i] + _scale[i] - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - } else { - { // min - if (velocity[i] < 0.0f) { - float possibleDistance = (_corner[i] - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - { // max - if (velocity[i] > 0.0f) { - float possibleDistance = (_corner[i] + _scale[i] - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } - } else { - a = 0.5f * acceleration[i]; - b = velocity[i]; - if (origin[i] < _corner[i]) { - // If we're below _corner, we only need to check the min face - { // min - c = origin[i] - _corner[i]; - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } else if (origin[i] > _corner[i] + _scale[i]) { - // If we're above _corner + _scale, we only need to check the max face - { // max - c = origin[i] - (_corner[i] + _scale[i]); - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - } else { - // If we're inside on this axis, we could hit either face depending on our velocity and acceleration, so we need to check both - { // min - c = origin[i] - _corner[i]; - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - { // max - c = origin[i] - (_corner[i] + _scale[i]); - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } - } - } - - if (minDistance < FLT_MAX) { - parabolicDistance = minDistance; - face = minFace; - surfaceNormal = minNormal; - return true; - } - return false; + float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const { + return findParabolaAABoxIntersection(origin, velocity, acceleration, _corner, _scale, parabolicDistance, face, surfaceNormal); } bool AABox::rayHitsBoundingSphere(const glm::vec3& origin, const glm::vec3& direction) const { diff --git a/libraries/shared/src/AACube.cpp b/libraries/shared/src/AACube.cpp index 43e97f9e3e..dc1003215d 100644 --- a/libraries/shared/src/AACube.cpp +++ b/libraries/shared/src/AACube.cpp @@ -111,9 +111,7 @@ glm::vec3 AACube::getNearestVertex(const glm::vec3& normal) const { } bool AACube::contains(const glm::vec3& point) const { - return isWithin(point.x, _corner.x, _scale) && - isWithin(point.y, _corner.y, _scale) && - isWithin(point.z, _corner.z, _scale); + return aaBoxContains(point, _corner, glm::vec3(_scale)); } bool AACube::contains(const AACube& otherCube) const { @@ -165,30 +163,6 @@ bool AACube::expandedContains(const glm::vec3& point, float expansion) const { isWithinExpanded(point.z, _corner.z, _scale, expansion); } -// finds the intersection between a ray and the facing plane on one axis -static bool findIntersection(float origin, float direction, float corner, float size, float& distance) { - if (direction > EPSILON) { - distance = (corner - origin) / direction; - return true; - } else if (direction < -EPSILON) { - distance = (corner + size - origin) / direction; - return true; - } - return false; -} - -// finds the intersection between a ray and the inside facing plane on one axis -static bool findInsideOutIntersection(float origin, float direction, float corner, float size, float& distance) { - if (direction > EPSILON) { - distance = -1.0f * (origin - (corner + size)) / direction; - return true; - } else if (direction < -EPSILON) { - distance = -1.0f * (origin - corner) / direction; - return true; - } - return false; -} - bool AACube::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const { // handle the trivial cases where the expanded box contains the start or end if (expandedContains(start, expansion) || expandedContains(end, expansion)) { @@ -215,220 +189,12 @@ bool AACube::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& bool AACube::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face, glm::vec3& surfaceNormal) const { - // handle the trivial case where the box contains the origin - if (contains(origin)) { - - // We still want to calculate the distance from the origin to the inside out plane - float axisDistance; - if ((findInsideOutIntersection(origin.x, direction.x, _corner.x, _scale, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) { - distance = axisDistance; - face = direction.x > 0 ? MAX_X_FACE : MIN_X_FACE; - surfaceNormal = glm::vec3(direction.x > 0 ? 1.0f : -1.0f, 0.0f, 0.0f); - return true; - } - if ((findInsideOutIntersection(origin.y, direction.y, _corner.y, _scale, axisDistance) && axisDistance >= 0 && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) { - distance = axisDistance; - face = direction.y > 0 ? MAX_Y_FACE : MIN_Y_FACE; - surfaceNormal = glm::vec3(0.0f, direction.y > 0 ? 1.0f : -1.0f, 0.0f); - return true; - } - if ((findInsideOutIntersection(origin.z, direction.z, _corner.z, _scale, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale))) { - distance = axisDistance; - face = direction.z > 0 ? MAX_Z_FACE : MIN_Z_FACE; - surfaceNormal = glm::vec3(0.0f, 0.0f, direction.z > 0 ? 1.0f : -1.0f); - return true; - } - // This case is unexpected, but mimics the previous behavior for inside out intersections - distance = 0; - return true; - } - - // check each axis - float axisDistance; - if ((findIntersection(origin.x, direction.x, _corner.x, _scale, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) { - distance = axisDistance; - face = direction.x > 0 ? MIN_X_FACE : MAX_X_FACE; - surfaceNormal = glm::vec3(direction.x > 0 ? -1.0f : 1.0f, 0.0f, 0.0f); - return true; - } - if ((findIntersection(origin.y, direction.y, _corner.y, _scale, axisDistance) && axisDistance >= 0 && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale) && - isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) { - distance = axisDistance; - face = direction.y > 0 ? MIN_Y_FACE : MAX_Y_FACE; - surfaceNormal = glm::vec3(0.0f, direction.y > 0 ? -1.0f : 1.0f, 0.0f); - return true; - } - if ((findIntersection(origin.z, direction.z, _corner.z, _scale, axisDistance) && axisDistance >= 0 && - isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) && - isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale))) { - distance = axisDistance; - face = direction.z > 0 ? MIN_Z_FACE : MAX_Z_FACE; - surfaceNormal = glm::vec3(0.0f, 0.0f, direction.z > 0 ? -1.0f : 1.0f); - return true; - } - return false; -} - -void AACube::checkPossibleParabolicIntersection(float t, int i, float& minDistance, - const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, bool& hit) const { - if (t < minDistance && t > 0.0f && - isWithin(origin[(i + 1) % 3] + velocity[(i + 1) % 3] * t + 0.5f * acceleration[(i + 1) % 3] * t * t, _corner[(i + 1) % 3], _scale) && - isWithin(origin[(i + 2) % 3] + velocity[(i + 2) % 3] * t + 0.5f * acceleration[(i + 2) % 3] * t * t, _corner[(i + 2) % 3], _scale)) { - minDistance = t; - hit = true; - } + return findRayAABoxIntersection(origin, direction, _corner, glm::vec3(_scale), distance, face, surfaceNormal); } bool AACube::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, - float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const { - float minDistance = FLT_MAX; - BoxFace minFace; - glm::vec3 minNormal; - glm::vec2 possibleDistances; - float a, b, c; - - // Solve the intersection for each face of the cube. As we go, keep track of the smallest, positive, real distance - // that is within the bounds of the other two dimensions - for (int i = 0; i < 3; i++) { - if (fabsf(acceleration[i]) < EPSILON) { - // Handle the degenerate case where we only have a line in this axis - if (origin[i] < _corner[i]) { - { // min - if (velocity[i] > 0.0f) { - float possibleDistance = (_corner[i] - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } else if (origin[i] > _corner[i] + _scale) { - { // max - if (velocity[i] < 0.0f) { - float possibleDistance = (_corner[i] + _scale - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - } else { - { // min - if (velocity[i] < 0.0f) { - float possibleDistance = (_corner[i] - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - { // max - if (velocity[i] > 0.0f) { - float possibleDistance = (_corner[i] + _scale - origin[i]) / velocity[i]; - bool hit = false; - checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } - } else { - a = 0.5f * acceleration[i]; - b = velocity[i]; - if (origin[i] < _corner[i]) { - // If we're below _corner, we only need to check the min face - { // min - c = origin[i] - _corner[i]; - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } else if (origin[i] > _corner[i] + _scale) { - // If we're above _corner + _scale, we only need to check the max face - { // max - c = origin[i] - (_corner[i] + _scale); - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - } else { - // If we're inside on this axis, we could hit either face depending on our velocity and acceleration, so we need to check both - { // min - c = origin[i] - _corner[i]; - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i + 1); - minNormal = glm::vec3(0.0f); - minNormal[i] = 1.0f; - } - } - } - { // max - c = origin[i] - (_corner[i] + _scale); - possibleDistances = { FLT_MAX, FLT_MAX }; - if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { - bool hit = false; - checkPossibleParabolicIntersection(possibleDistances.x, i, minDistance, origin, velocity, acceleration, hit); - checkPossibleParabolicIntersection(possibleDistances.y, i, minDistance, origin, velocity, acceleration, hit); - if (hit) { - minFace = BoxFace(2 * i); - minNormal = glm::vec3(0.0f); - minNormal[i] = -1.0f; - } - } - } - } - } - } - - if (minDistance < FLT_MAX) { - parabolicDistance = minDistance; - face = minFace; - surfaceNormal = minNormal; - return true; - } - return false; + float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const { + return findParabolaAABoxIntersection(origin, velocity, acceleration, _corner, glm::vec3(_scale), parabolicDistance, face, surfaceNormal); } bool AACube::touchesSphere(const glm::vec3& center, float radius) const { diff --git a/libraries/shared/src/AACube.h b/libraries/shared/src/AACube.h index df00b8aefc..72aed31999 100644 --- a/libraries/shared/src/AACube.h +++ b/libraries/shared/src/AACube.h @@ -78,9 +78,6 @@ private: static BoxFace getOppositeFace(BoxFace face); - void checkPossibleParabolicIntersection(float t, int i, float& minDistance, - const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, bool& hit) const; - glm::vec3 _corner; float _scale; }; diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 750dd464a6..1d470c42c3 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -190,6 +190,94 @@ glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration - (currentDirection * directionalComponent); } +// finds the intersection between a ray and the facing plane on one axis +bool findIntersection(float origin, float direction, float corner, float size, float& distance) { + if (direction > EPSILON) { + distance = (corner - origin) / direction; + return true; + } else if (direction < -EPSILON) { + distance = (corner + size - origin) / direction; + return true; + } + return false; +} + +// finds the intersection between a ray and the inside facing plane on one axis +bool findInsideOutIntersection(float origin, float direction, float corner, float size, float& distance) { + if (direction > EPSILON) { + distance = -1.0f * (origin - (corner + size)) / direction; + return true; + } else if (direction < -EPSILON) { + distance = -1.0f * (origin - corner) / direction; + return true; + } + return false; +} + +bool findRayAABoxIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& corner, const glm::vec3& scale, float& distance, + BoxFace& face, glm::vec3& surfaceNormal) { + // handle the trivial case where the box contains the origin + if (aaBoxContains(origin, corner, scale)) { + // We still want to calculate the distance from the origin to the inside out plane + float axisDistance; + if ((findInsideOutIntersection(origin.x, direction.x, corner.x, scale.x, axisDistance) && axisDistance >= 0 && + isWithin(origin.y + axisDistance * direction.y, corner.y, scale.y) && + isWithin(origin.z + axisDistance * direction.z, corner.z, scale.z))) { + distance = axisDistance; + face = direction.x > 0 ? MAX_X_FACE : MIN_X_FACE; + surfaceNormal = glm::vec3(direction.x > 0 ? 1.0f : -1.0f, 0.0f, 0.0f); + return true; + } + if ((findInsideOutIntersection(origin.y, direction.y, corner.y, scale.y, axisDistance) && axisDistance >= 0 && + isWithin(origin.x + axisDistance * direction.x, corner.x, scale.x) && + isWithin(origin.z + axisDistance * direction.z, corner.z, scale.z))) { + distance = axisDistance; + face = direction.y > 0 ? MAX_Y_FACE : MIN_Y_FACE; + surfaceNormal = glm::vec3(0.0f, direction.y > 0 ? 1.0f : -1.0f, 0.0f); + return true; + } + if ((findInsideOutIntersection(origin.z, direction.z, corner.z, scale.z, axisDistance) && axisDistance >= 0 && + isWithin(origin.y + axisDistance * direction.y, corner.y, scale.y) && + isWithin(origin.x + axisDistance * direction.x, corner.x, scale.x))) { + distance = axisDistance; + face = direction.z > 0 ? MAX_Z_FACE : MIN_Z_FACE; + surfaceNormal = glm::vec3(0.0f, 0.0f, direction.z > 0 ? 1.0f : -1.0f); + return true; + } + // This case is unexpected, but mimics the previous behavior for inside out intersections + distance = 0; + return true; + } + + // check each axis + float axisDistance; + if ((findIntersection(origin.x, direction.x, corner.x, scale.x, axisDistance) && axisDistance >= 0 && + isWithin(origin.y + axisDistance * direction.y, corner.y, scale.y) && + isWithin(origin.z + axisDistance * direction.z, corner.z, scale.z))) { + distance = axisDistance; + face = direction.x > 0 ? MIN_X_FACE : MAX_X_FACE; + surfaceNormal = glm::vec3(direction.x > 0 ? -1.0f : 1.0f, 0.0f, 0.0f); + return true; + } + if ((findIntersection(origin.y, direction.y, corner.y, scale.y, axisDistance) && axisDistance >= 0 && + isWithin(origin.x + axisDistance * direction.x, corner.x, scale.x) && + isWithin(origin.z + axisDistance * direction.z, corner.z, scale.z))) { + distance = axisDistance; + face = direction.y > 0 ? MIN_Y_FACE : MAX_Y_FACE; + surfaceNormal = glm::vec3(0.0f, direction.y > 0 ? -1.0f : 1.0f, 0.0f); + return true; + } + if ((findIntersection(origin.z, direction.z, corner.z, scale.z, axisDistance) && axisDistance >= 0 && + isWithin(origin.y + axisDistance * direction.y, corner.y, scale.y) && + isWithin(origin.x + axisDistance * direction.x, corner.x, scale.x))) { + distance = axisDistance; + face = direction.z > 0 ? MIN_Z_FACE : MAX_Z_FACE; + surfaceNormal = glm::vec3(0.0f, 0.0f, direction.z > 0 ? -1.0f : 1.0f); + return true; + } + return false; +} + bool findRaySphereIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& center, float radius, float& distance) { glm::vec3 relativeOrigin = origin - center; @@ -719,6 +807,12 @@ bool isWithin(float value, float corner, float size) { return value >= corner && value <= corner + size; } +bool aaBoxContains(const glm::vec3& point, const glm::vec3& corner, const glm::vec3& scale) { + return isWithin(point.x, corner.x, scale.x) && + isWithin(point.y, corner.y, scale.y) && + isWithin(point.z, corner.z, scale.z); +} + void checkPossibleParabolicIntersectionWithZPlane(float t, float& minDistance, const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, const glm::vec2& corner, const glm::vec2& scale) { if (t < minDistance && t > 0.0f && @@ -979,6 +1073,391 @@ bool findParabolaCapsuleIntersection(const glm::vec3& origin, const glm::vec3& v return minDistance != FLT_MAX; } +void checkPossibleParabolicIntersection(float t, int i, float& minDistance, const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, + const glm::vec3& corner, const glm::vec3& scale, bool& hit) { + if (t < minDistance && t > 0.0f && + isWithin(origin[(i + 1) % 3] + velocity[(i + 1) % 3] * t + 0.5f * acceleration[(i + 1) % 3] * t * t, corner[(i + 1) % 3], scale[(i + 1) % 3]) && + isWithin(origin[(i + 2) % 3] + velocity[(i + 2) % 3] * t + 0.5f * acceleration[(i + 2) % 3] * t * t, corner[(i + 2) % 3], scale[(i + 2) % 3])) { + minDistance = t; + hit = true; + } +} + +inline float parabolaVelocityAtT(float velocity, float acceleration, float t) { + return velocity + acceleration * t; +} + +bool findParabolaAABoxIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, + const glm::vec3& corner, const glm::vec3& scale, float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) { + float minDistance = FLT_MAX; + BoxFace minFace; + glm::vec3 minNormal; + glm::vec2 possibleDistances; + float a, b, c; + + // Solve the intersection for each face of the cube. As we go, keep track of the smallest, positive, real distance + // that is within the bounds of the other two dimensions + for (int i = 0; i < 3; i++) { + if (fabsf(acceleration[i]) < EPSILON) { + // Handle the degenerate case where we only have a line in this axis + if (origin[i] < corner[i]) { + { // min + if (velocity[i] > 0.0f) { + float possibleDistance = (corner[i] - origin[i]) / velocity[i]; + bool hit = false; + checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, corner, scale, hit); + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } else if (origin[i] > corner[i] + scale[i]) { + { // max + if (velocity[i] < 0.0f) { + float possibleDistance = (corner[i] + scale[i] - origin[i]) / velocity[i]; + bool hit = false; + checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, corner, scale, hit); + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + } else { + { // min + if (velocity[i] < 0.0f) { + float possibleDistance = (corner[i] - origin[i]) / velocity[i]; + bool hit = false; + checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, corner, scale, hit); + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + { // max + if (velocity[i] > 0.0f) { + float possibleDistance = (corner[i] + scale[i] - origin[i]) / velocity[i]; + bool hit = false; + checkPossibleParabolicIntersection(possibleDistance, i, minDistance, origin, velocity, acceleration, corner, scale, hit); + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } + } else { + a = 0.5f * acceleration[i]; + b = velocity[i]; + if (origin[i] < corner[i]) { + // If we're below corner, we have the following cases: + // - within bounds on other axes + // - if +velocity or +acceleration + // - can only hit MIN_FACE with -normal + // - else + // - if +acceleration + // - can only hit MIN_FACE with -normal + // - else if +velocity + // - can hit MIN_FACE with -normal iff velocity at intersection is + + // - else can hit MAX_FACE with +normal iff velocity at intersection is - + if (origin[(i + 1) % 3] > corner[(i + 1) % 3] && origin[(i + 1) % 3] < corner[(i + 1) % 3] + scale[(i + 1) % 3] && + origin[(i + 2) % 3] > corner[(i + 2) % 3] && origin[(i + 2) % 3] < corner[(i + 2) % 3] + scale[(i + 2) % 3]) { + if (velocity[i] > 0.0f || acceleration[i] > 0.0f) { + { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } + } else { + if (acceleration[i] > 0.0f) { + { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } else if (velocity[i] > 0.0f) { + bool hit = false; + { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) > 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + if (!hit) { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) < 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + } + } + } else if (origin[i] > corner[i] + scale[i]) { + // If we're above corner + scale, we have the following cases: + // - within bounds on other axes + // - if -velocity or -acceleration + // - can only hit MAX_FACE with +normal + // - else + // - if -acceleration + // - can only hit MAX_FACE with +normal + // - else if -velocity + // - can hit MAX_FACE with +normal iff velocity at intersection is - + // - else can hit MIN_FACE with -normal iff velocity at intersection is + + if (origin[(i + 1) % 3] > corner[(i + 1) % 3] && origin[(i + 1) % 3] < corner[(i + 1) % 3] + scale[(i + 1) % 3] && + origin[(i + 2) % 3] > corner[(i + 2) % 3] && origin[(i + 2) % 3] < corner[(i + 2) % 3] + scale[(i + 2) % 3]) { + if (velocity[i] < 0.0f || acceleration[i] < 0.0f) { + { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + } + } else { + if (acceleration[i] < 0.0f) { + { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + } else if (velocity[i] < 0.0f) { + bool hit = false; + { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) < 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + if (!hit) { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) > 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } + } + } else { + // If we're between corner and corner + scale, we have the following cases: + // - within bounds on other axes + // - if -velocity and -acceleration + // - can only hit MIN_FACE with +normal + // - else if +velocity and +acceleration + // - can only hit MAX_FACE with -normal + // - else + // - can hit MIN_FACE with +normal iff velocity at intersection is - + // - can hit MAX_FACE with -normal iff velocity at intersection is + + // - else + // - if -velocity and +acceleration + // - can hit MIN_FACE with -normal iff velocity at intersection is + + // - else if +velocity and -acceleration + // - can hit MAX_FACE with +normal iff velocity at intersection is - + if (origin[(i + 1) % 3] > corner[(i + 1) % 3] && origin[(i + 1) % 3] < corner[(i + 1) % 3] + scale[(i + 1) % 3] && + origin[(i + 2) % 3] > corner[(i + 2) % 3] && origin[(i + 2) % 3] < corner[(i + 2) % 3] + scale[(i + 2) % 3]) { + if (velocity[i] < 0.0f && acceleration[i] < 0.0f) { + { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + } else if (velocity[i] > 0.0f && acceleration[i] > 0.0f) { + { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } else { + { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + bool hit = false; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) < 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + bool hit = false; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) > 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } + } else { + if (velocity[i] < 0.0f && acceleration[i] > 0.0f) { + { // min + c = origin[i] - corner[i]; + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) > 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i); + minNormal = glm::vec3(0.0f); + minNormal[i] = -1.0f; + } + } + } + } else if (velocity[i] > 0.0f && acceleration[i] < 0.0f) { + { // max + c = origin[i] - (corner[i] + scale[i]); + possibleDistances = { FLT_MAX, FLT_MAX }; + if (computeRealQuadraticRoots(a, b, c, possibleDistances)) { + bool hit = false; + for (int j = 0; j < 2; j++) { + if (parabolaVelocityAtT(velocity[i], acceleration[i], possibleDistances[j]) < 0.0f) { + checkPossibleParabolicIntersection(possibleDistances[j], i, minDistance, origin, velocity, acceleration, corner, scale, hit); + } + } + if (hit) { + minFace = BoxFace(2 * i + 1); + minNormal = glm::vec3(0.0f); + minNormal[i] = 1.0f; + } + } + } + } + } + } + } + } + + if (minDistance < FLT_MAX) { + parabolicDistance = minDistance; + face = minFace; + surfaceNormal = minNormal; + return true; + } + return false; +} + void swingTwistDecomposition(const glm::quat& rotation, const glm::vec3& direction, glm::quat& swing, diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 50d8d1f801..54f9062469 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -14,6 +14,7 @@ #include #include +#include "BoxBase.h" class Plane; @@ -73,6 +74,11 @@ bool findCapsulePlanePenetration(const glm::vec3& capsuleStart, const glm::vec3& glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration); +bool findIntersection(float origin, float direction, float corner, float size, float& distance); +bool findInsideOutIntersection(float origin, float direction, float corner, float size, float& distance); +bool findRayAABoxIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& corner, const glm::vec3& scale, float& distance, + BoxFace& face, glm::vec3& surfaceNormal); + bool findRaySphereIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& center, float radius, float& distance); @@ -100,6 +106,9 @@ bool findParabolaTriangleIntersection(const glm::vec3& origin, const glm::vec3& bool findParabolaCapsuleIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, const glm::vec3& start, const glm::vec3& end, float radius, const glm::quat& rotation, float& parabolicDistance); +bool findParabolaAABoxIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, + const glm::vec3& corner, const glm::vec3& scale, float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal); + /// \brief decomposes rotation into its components such that: rotation = swing * twist /// \param rotation[in] rotation to decompose /// \param direction[in] normalized axis about which the twist happens (typically original direction before rotation applied) @@ -202,6 +211,7 @@ bool solve_quartic(float a, float b, float c, float d, glm::vec4& roots); bool computeRealQuarticRoots(float a, float b, float c, float d, float e, glm::vec4& roots); bool isWithin(float value, float corner, float size); +bool aaBoxContains(const glm::vec3& point, const glm::vec3& corner, const glm::vec3& scale); void checkPossibleParabolicIntersectionWithZPlane(float t, float& minDistance, const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration, const glm::vec2& corner, const glm::vec2& scale);