////  GeometryUtil.cpp//  libraries/shared/src////  Created by Andrzej Kapolka on 5/21/13.//  Copyright 2013 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "GeometryUtil.h" #include <assert.h>#include <cstring>#include <cmath>#include <bitset>#include <complex>#include <qmath.h>#include <glm/gtx/quaternion.hpp>#include "glm/gtc/matrix_transform.hpp" #include "NumericalConstants.h"#include "GLMHelpers.h"#include "Plane.h" glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) {    // compute the projection of the point vector onto the segment vector    glm::vec3 segmentVector = end - start;    float lengthSquared = glm::dot(segmentVector, segmentVector);    if (lengthSquared < EPSILON) {        return start - point; // start and end the same    }    float proj = glm::dot(point - start, segmentVector) / lengthSquared;    if (proj <= 0.0f) { // closest to the start        return start - point;     } else if (proj >= 1.0f) { // closest to the end        return end - point;     } else { // closest to the middle        return start + segmentVector*proj - point;    }} // Computes the penetration between a point and a sphere (centered at the origin)// if point is inside sphere: returns true and stores the result in 'penetration'// (the vector that would move the point outside the sphere)// otherwise returns falsebool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius,                           glm::vec3& penetration) {    float vectorLength = glm::length(point);    if (vectorLength < EPSILON) {        penetration = defaultDirection * sphereRadius;        return true;    }    float distance = vectorLength - sphereRadius;    if (distance < 0.0f) {        penetration = point * (-distance / vectorLength);        return true;    }    return false;} bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius,                                const glm::vec3& point, glm::vec3& penetration) {    return findSpherePenetration(point - sphereCenter, glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);} bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereCenter,       float sphereRadius, glm::vec3& penetration) {    return findSpherePenetration(sphereCenter - point, glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);} bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius,                                 const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration) {    return findSpherePointPenetration(firstCenter, firstRadius + secondRadius, secondCenter, penetration);} bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,                                  const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration) {    return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd),                                 glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);} bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& capsuleStart,                                  const glm::vec3& capsuleEnd, float capsuleRadius, glm::vec3& penetration) {    return findSphereSegmentPenetration(sphereCenter, sphereRadius + capsuleRadius,        capsuleStart, capsuleEnd, penetration);} bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart,        const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration) {    // compute the projection of the point vector onto the segment vector    glm::vec3 segmentVector = capsuleEnd - capsuleStart;    float lengthSquared = glm::dot(segmentVector, segmentVector);    if (lengthSquared < EPSILON) { // start and end the same        return findPointSpherePenetration(point, capsuleStart,            glm::max(startRadius, endRadius), penetration);    }    float proj = glm::dot(point - capsuleStart, segmentVector) / lengthSquared;    if (proj <= 0.0f) { // closest to the start        return findPointSpherePenetration(point, capsuleStart, startRadius, penetration);     } else if (proj >= 1.0f) { // closest to the end        return findPointSpherePenetration(point, capsuleEnd, endRadius, penetration);     } else { // closest to the middle        return findPointSpherePenetration(point, capsuleStart + segmentVector * proj,            glm::mix(startRadius, endRadius, proj), penetration);    }} bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter,        float sphereRadius, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd,        float startRadius, float endRadius, glm::vec3& penetration) {    return findPointCapsuleConePenetration(sphereCenter, capsuleStart, capsuleEnd,        startRadius + sphereRadius, endRadius + sphereRadius, penetration);} bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,                                const glm::vec4& plane, glm::vec3& penetration) {    float distance = glm::dot(plane, glm::vec4(sphereCenter, 1.0f)) - sphereRadius;    if (distance < 0.0f) {        penetration = glm::vec3(plane) * distance;        return true;    }    return false;} bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,                               const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,                               glm::vec3& penetration) {    glm::vec3 localCenter = sphereCenter - diskCenter;    float axialDistance = glm::dot(localCenter, diskNormal);    if (std::fabs(axialDistance) < (sphereRadius + 0.5f * diskThickness)) {        // sphere hit the plane, but does it hit the disk?        // Note: this algorithm ignores edge hits.        glm::vec3 axialOffset = axialDistance * diskNormal;        if (glm::length(localCenter - axialOffset) < diskRadius) {            // yes, hit the disk            penetration = (std::fabs(axialDistance) - (sphereRadius + 0.5f * diskThickness) ) * diskNormal;            if (axialDistance < 0.0f) {                // hit the backside of the disk, so negate penetration vector                penetration *= -1.0f;            }            return true;        }    }    return false;} bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,                                  const glm::vec3& sphereCenter, float sphereRadius, glm::vec3& penetration) {    if (findSphereCapsulePenetration(sphereCenter, sphereRadius,            capsuleStart, capsuleEnd, capsuleRadius, penetration)) {        penetration = -penetration;        return true;    }    return false;} bool findCapsulePlanePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,                                 const glm::vec4& plane, glm::vec3& penetration) {    float distance = glm::min(glm::dot(plane, glm::vec4(capsuleStart, 1.0f)),        glm::dot(plane, glm::vec4(capsuleEnd, 1.0f))) - capsuleRadius;    if (distance < 0.0f) {        penetration = glm::vec3(plane) * distance;        return true;    }    return false;} glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration) {    // find the component of the new penetration in the direction of the current    float currentLength = glm::length(currentPenetration);    if (currentLength == 0.0f) {        return newPenetration;    }    glm::vec3 currentDirection = currentPenetration / currentLength;    float directionalComponent = glm::dot(newPenetration, currentDirection);     // if orthogonal or in the opposite direction, we can simply add    if (directionalComponent <= 0.0f) {        return currentPenetration + newPenetration;    }     // otherwise, we need to take the maximum component of current and new    return currentDirection * glm::max(directionalComponent, currentLength) +        newPenetration - (currentDirection * directionalComponent);} // finds the intersection between a ray and the facing plane on one axisbool 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 axisbool 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;} // https://tavianator.com/fast-branchless-raybounding-box-intersections/bool findRayAABoxIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& invDirection,                              const glm::vec3& corner, const glm::vec3& scale, float& distance, BoxFace& face, glm::vec3& surfaceNormal) {    float t1, t2, newTmin, newTmax, tmin = -INFINITY, tmax = INFINITY;    int minAxis = -1, maxAxis = -1;     for (int i = 0; i < 3; ++i) {        t1 = (corner[i] - origin[i]) * invDirection[i];        t2 = (corner[i] + scale[i] - origin[i]) * invDirection[i];         newTmin = glm::min(t1, t2);        newTmax = glm::max(t1, t2);         minAxis = newTmin > tmin ? i : minAxis;        tmin = glm::max(tmin, newTmin);        maxAxis = newTmax < tmax ? i : maxAxis;        tmax = glm::min(tmax, newTmax);    }     if (tmax >= glm::max(tmin, 0.0f)) {        if (tmin < 0.0f) {            distance = tmax;            bool positiveDirection = direction[maxAxis] > 0.0f;            surfaceNormal = glm::vec3(0.0f);            surfaceNormal[maxAxis] = positiveDirection ? -1.0f : 1.0f;            face = positiveDirection ? BoxFace(2 * maxAxis + 1) : BoxFace(2 * maxAxis);        } else {            distance = tmin;            bool positiveDirection = direction[minAxis] > 0.0f;            surfaceNormal = glm::vec3(0.0f);            surfaceNormal[minAxis] = positiveDirection ? -1.0f : 1.0f;            face = positiveDirection ? BoxFace(2 * minAxis) : BoxFace(2 * minAxis + 1);        }        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;    float c = glm::dot(relativeOrigin, relativeOrigin) - radius * radius;    if (c < 0.0f) {        distance = 0.0f;        return true; // starts inside the sphere    }    float b = 2.0f * glm::dot(direction, relativeOrigin);    float a = glm::dot(direction, direction);    float radicand = b * b - 4.0f * a * c;    if (radicand < 0.0f) {        return false; // doesn't hit the sphere    }    float t = 0.5f * (-b - sqrtf(radicand)) / a;    if (t < 0.0f) {        return false; // doesn't hit the sphere    }    distance = t;    return true;} bool pointInSphere(const glm::vec3& origin, const glm::vec3& center, float radius) {    glm::vec3 relativeOrigin = origin - center;    float c = glm::dot(relativeOrigin, relativeOrigin) - radius * radius;    return c <= 0.0f;}  bool pointInCapsule(const glm::vec3& origin, const glm::vec3& start, const glm::vec3& end, float radius) {    glm::vec3 relativeOrigin = origin - start;    glm::vec3 relativeEnd = end - start;    float capsuleLength = glm::length(relativeEnd);    relativeEnd /= capsuleLength;    float originProjection = glm::dot(relativeEnd, relativeOrigin);    glm::vec3 constant = relativeOrigin - relativeEnd * originProjection;    float c = glm::dot(constant, constant) - radius * radius;    if (c < 0.0f) { // starts inside cylinder        if (originProjection < 0.0f) { // below start            return pointInSphere(origin, start, radius);        } else if (originProjection > capsuleLength) { // above end            return pointInSphere(origin, end, radius);        } else { // between start and end            return true;        }    }    return false;} bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direction,        const glm::vec3& start, const glm::vec3& end, float radius, float& distance) {    if (start == end) {        return findRaySphereIntersection(origin, direction, start, radius, distance); // handle degenerate case    }    glm::vec3 relativeOrigin = origin - start;    glm::vec3 relativeEnd = end - start;    float capsuleLength = glm::length(relativeEnd);    relativeEnd /= capsuleLength;    float originProjection = glm::dot(relativeEnd, relativeOrigin);    glm::vec3 constant = relativeOrigin - relativeEnd * originProjection;    float c = glm::dot(constant, constant) - radius * radius;    if (c < 0.0f) { // starts inside cylinder        if (originProjection < 0.0f) { // below start            return findRaySphereIntersection(origin, direction, start, radius, distance);         } else if (originProjection > capsuleLength) { // above end            return findRaySphereIntersection(origin, direction, end, radius, distance);         } else { // between start and end            distance = 0.0f;            return true;        }    }    glm::vec3 coefficient = direction - relativeEnd * glm::dot(relativeEnd, direction);    float a = glm::dot(coefficient, coefficient);    if (a == 0.0f) {        return false; // parallel to enclosing cylinder    }    float b = 2.0f * glm::dot(constant, coefficient);    float radicand = b * b - 4.0f * a * c;    if (radicand < 0.0f) {        return false; // doesn't hit the enclosing cylinder    }    float t = (-b - sqrtf(radicand)) / (2.0f * a);    if (t < 0.0f) {        return false; // doesn't hit the enclosing cylinder    }    glm::vec3 intersection = relativeOrigin + direction * t;    float intersectionProjection = glm::dot(relativeEnd, intersection);    if (intersectionProjection < 0.0f) { // below start        return findRaySphereIntersection(origin, direction, start, radius, distance);     } else if (intersectionProjection > capsuleLength) { // above end        return findRaySphereIntersection(origin, direction, end, radius, distance);    }    distance = t; // between start and end    return true;} // reference https://www.opengl.org/wiki/Calculating_a_Surface_Normalglm::vec3 Triangle::getNormal() const {    glm::vec3 edge1 = v1 - v0;    glm::vec3 edge2 = v2 - v0;    return glm::normalize(glm::cross(edge1, edge2));} float Triangle::getArea() const {    glm::vec3 edge1 = v1 - v0;    glm::vec3 edge2 = v2 - v0;    return 0.5f * glm::length(glm::cross(edge1, edge2));} Triangle Triangle::operator*(const glm::mat4& transform) const {    return {        glm::vec3(transform * glm::vec4(v0, 1.0f)),        glm::vec3(transform * glm::vec4(v1, 1.0f)),        glm::vec3(transform * glm::vec4(v2, 1.0f))    };} // https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithmbool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,        const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance, bool allowBackface) {    glm::vec3 firstSide = v1 - v0;    glm::vec3 secondSide = v2 - v0;    glm::vec3 P = glm::cross(direction, secondSide);    float det = glm::dot(firstSide, P);    if (!allowBackface && det < EPSILON) {        return false;    } else if (fabsf(det) < EPSILON) {        return false;    }     float invDet = 1.0f / det;    glm::vec3 T = origin - v0;    float u = glm::dot(T, P) * invDet;    if (u < 0.0f || u > 1.0f) {        return false;    }     glm::vec3 Q = glm::cross(T, firstSide);    float v = glm::dot(direction, Q) * invDet;    if (v < 0.0f || u + v > 1.0f) {        return false;    }     float t = glm::dot(secondSide, Q) * invDet;    if (t > EPSILON) {        distance = t;        return true;    }    return false;} static void getTrianglePlaneIntersectionPoints(const glm::vec3 trianglePoints[3], const float pointPlaneDistances[3],                                               const int clippedPointIndex, const int keptPointIndices[2],                                               glm::vec3 points[2]) {    assert(clippedPointIndex >= 0 && clippedPointIndex < 3);    const auto& clippedPoint = trianglePoints[clippedPointIndex];    const float clippedPointPlaneDistance = pointPlaneDistances[clippedPointIndex];    for (auto i = 0; i < 2; i++) {        assert(keptPointIndices[i] >= 0 && keptPointIndices[i] < 3);        const auto& keptPoint = trianglePoints[keptPointIndices[i]];        const float keptPointPlaneDistance = pointPlaneDistances[keptPointIndices[i]];        auto intersectionEdgeRatio = clippedPointPlaneDistance / (clippedPointPlaneDistance - keptPointPlaneDistance);        points[i] = clippedPoint + (keptPoint - clippedPoint) * intersectionEdgeRatio;    }} int clipTriangleWithPlane(const Triangle& triangle, const Plane& plane, Triangle* clippedTriangles, int maxClippedTriangleCount) {    float pointDistanceToPlane[3];    std::bitset<3> arePointsClipped;    glm::vec3 triangleVertices[3] = { triangle.v0, triangle.v1, triangle.v2 };    int clippedTriangleCount = 0;    int i;     for (i = 0; i < 3; i++) {        pointDistanceToPlane[i] = plane.distance(triangleVertices[i]);        arePointsClipped.set(i, pointDistanceToPlane[i] < 0.0f);    }     switch (arePointsClipped.count()) {        case 0:            // Easy, the entire triangle is kept as is.            *clippedTriangles = triangle;            clippedTriangleCount = 1;            break;         case 1:        {            int clippedPointIndex = 2;            int keptPointIndices[2] = { 0, 1 };            glm::vec3 newVertices[2];             // Determine which point was clipped.            if (arePointsClipped.test(0)) {                clippedPointIndex = 0;                keptPointIndices[0] = 2;            } else if (arePointsClipped.test(1)) {                clippedPointIndex = 1;                keptPointIndices[1] = 2;            }            // We have a quad now, so we need to create two triangles.            getTrianglePlaneIntersectionPoints(triangleVertices, pointDistanceToPlane, clippedPointIndex, keptPointIndices, newVertices);            clippedTriangles->v0 = triangleVertices[keptPointIndices[0]];            clippedTriangles->v1 = triangleVertices[keptPointIndices[1]];            clippedTriangles->v2 = newVertices[1];            clippedTriangles++;            clippedTriangleCount++;             if (clippedTriangleCount < maxClippedTriangleCount) {                clippedTriangles->v0 = triangleVertices[keptPointIndices[0]];                clippedTriangles->v1 = newVertices[0];                clippedTriangles->v2 = newVertices[1];                clippedTriangles++;                clippedTriangleCount++;            }        }        break;         case 2:        {            int keptPointIndex = 2;            int clippedPointIndices[2] = { 0, 1 };            glm::vec3 newVertices[2];             // Determine which point was NOT clipped.            if (!arePointsClipped.test(0)) {                keptPointIndex = 0;                clippedPointIndices[0] = 2;            } else if (!arePointsClipped.test(1)) {                keptPointIndex = 1;                clippedPointIndices[1] = 2;            }            // We have a single triangle            getTrianglePlaneIntersectionPoints(triangleVertices, pointDistanceToPlane, keptPointIndex, clippedPointIndices, newVertices);            clippedTriangles->v0 = triangleVertices[keptPointIndex];            clippedTriangles->v1 = newVertices[0];            clippedTriangles->v2 = newVertices[1];            clippedTriangleCount = 1;        }        break;         default:            // Entire triangle is clipped.           break;

Lines 1–500 of 1840. Load more