////  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 <cstring>#include <cmath> #include "GeometryUtil.h"#include "NumericalConstants.h"#include "PlaneShape.h"#include "RayIntersectionInfo.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);} 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 = glm::dot(direction, relativeOrigin);    float radicand = b * b - c;    if (radicand < 0.0f) {        return false; // doesn't hit the sphere    }    float t = -b - sqrtf(radicand);    if (t < 0.0f) {        return false; // doesn't hit the sphere    }    distance = t;    return true;} 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;} bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,        const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance) {    glm::vec3 firstSide = v0 - v1;    glm::vec3 secondSide = v2 - v1;    glm::vec3 normal = glm::cross(secondSide, firstSide);    float dividend = glm::dot(normal, v1) - glm::dot(origin, normal);    if (dividend > 0.0f) {        return false; // origin below plane    }    float divisor = glm::dot(normal, direction);    if (divisor >= 0.0f) {        return false;    }    float t = dividend / divisor;    glm::vec3 point = origin + direction * t;    if (glm::dot(normal, glm::cross(point - v1, firstSide)) > 0.0f &&            glm::dot(normal, glm::cross(secondSide, point - v1)) > 0.0f &&            glm::dot(normal, glm::cross(point - v0, v2 - v0)) > 0.0f) {        distance = t;        return true;    }    return false;} // Do line segments (r1p1.x, r1p1.y)--(r1p2.x, r1p2.y) and (r2p1.x, r2p1.y)--(r2p2.x, r2p2.y) intersect?// from: http://ptspts.blogspot.com/2010/06/how-to-determine-if-two-line-segments.htmlbool doLineSegmentsIntersect(glm::vec2 r1p1, glm::vec2 r1p2, glm::vec2 r2p1, glm::vec2 r2p2) {  int d1 = computeDirection(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p1.x, r1p1.y);  int d2 = computeDirection(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p2.x, r1p2.y);  int d3 = computeDirection(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p1.x, r2p1.y);  int d4 = computeDirection(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p2.x, r2p2.y);  return (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) &&          ((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) ||         (d1 == 0 && isOnSegment(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p1.x, r1p1.y)) ||         (d2 == 0 && isOnSegment(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p2.x, r1p2.y)) ||         (d3 == 0 && isOnSegment(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p1.x, r2p1.y)) ||         (d4 == 0 && isOnSegment(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p2.x, r2p2.y));} bool isOnSegment(float xi, float yi, float xj, float yj, float xk, float yk)  {  return (xi <= xk || xj <= xk) && (xk <= xi || xk <= xj) &&         (yi <= yk || yj <= yk) && (yk <= yi || yk <= yj);} int computeDirection(float xi, float yi, float xj, float yj, float xk, float yk) {  float a = (xk - xi) * (yj - yi);  float b = (xj - xi) * (yk - yi);  return a < b ? -1 : a > b ? 1 : 0;}  //// Polygon Clipping routines inspired by, pseudo code found here: http://www.cs.rit.edu/~icss571/clipTrans/PolyClipBack.html//// Coverage Map's polygon coordinates are from -1 to 1 in the following mapping to screen space.////         (0,0)                   (windowWidth, 0)//         -1,1                    1,1//           +-----------------------+ //           |           |           |//           |           |           |//           | -1,0      |           |//           |-----------+-----------|//           |          0,0          |//           |           |           |//           |           |           |//           |           |           |//           +-----------------------+//           -1,-1                  1,-1// (0,windowHeight)                (windowWidth,windowHeight)// const float PolygonClip::TOP_OF_CLIPPING_WINDOW    =  1.0f;const float PolygonClip::BOTTOM_OF_CLIPPING_WINDOW = -1.0f;const float PolygonClip::LEFT_OF_CLIPPING_WINDOW   = -1.0f;const float PolygonClip::RIGHT_OF_CLIPPING_WINDOW  =  1.0f; const glm::vec2 PolygonClip::TOP_LEFT_CLIPPING_WINDOW       ( LEFT_OF_CLIPPING_WINDOW , TOP_OF_CLIPPING_WINDOW    );const glm::vec2 PolygonClip::TOP_RIGHT_CLIPPING_WINDOW      ( RIGHT_OF_CLIPPING_WINDOW, TOP_OF_CLIPPING_WINDOW    );const glm::vec2 PolygonClip::BOTTOM_LEFT_CLIPPING_WINDOW    ( LEFT_OF_CLIPPING_WINDOW , BOTTOM_OF_CLIPPING_WINDOW );const glm::vec2 PolygonClip::BOTTOM_RIGHT_CLIPPING_WINDOW   ( RIGHT_OF_CLIPPING_WINDOW, BOTTOM_OF_CLIPPING_WINDOW ); void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength, glm::vec2*& outputVertexArray, int& outLength) {    int tempLengthA = inLength;    int tempLengthB;    int maxLength = inLength * 2;    glm::vec2* tempVertexArrayA = new glm::vec2[maxLength];    glm::vec2* tempVertexArrayB = new glm::vec2[maxLength];        // set up our temporary arrays    memcpy(tempVertexArrayA, inputVertexArray, sizeof(glm::vec2) * inLength);        // Left edge    LineSegment2 edge;    edge[0] = TOP_LEFT_CLIPPING_WINDOW;    edge[1] = BOTTOM_LEFT_CLIPPING_WINDOW;    // clip the array from tempVertexArrayA and copy end result to tempVertexArrayB    sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);    // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA    copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);        // Bottom Edge    edge[0] = BOTTOM_LEFT_CLIPPING_WINDOW;    edge[1] = BOTTOM_RIGHT_CLIPPING_WINDOW;    // clip the array from tempVertexArrayA and copy end result to tempVertexArrayB    sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);    // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA    copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);        // Right Edge    edge[0] = BOTTOM_RIGHT_CLIPPING_WINDOW;    edge[1] = TOP_RIGHT_CLIPPING_WINDOW;    // clip the array from tempVertexArrayA and copy end result to tempVertexArrayB    sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);    // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA    copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);        // Top Edge    edge[0] = TOP_RIGHT_CLIPPING_WINDOW;    edge[1] = TOP_LEFT_CLIPPING_WINDOW;    // clip the array from tempVertexArrayA and copy end result to tempVertexArrayB    sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);    // clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA    copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);        // copy final output to outputVertexArray    outputVertexArray = tempVertexArrayA;    outLength         = tempLengthA;     // cleanup our unused temporary buffer...    delete[] tempVertexArrayB;        // Note: we don't delete tempVertexArrayA, because that's the caller's responsibility} void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,                                                int inLength, int& outLength,  const LineSegment2& clipBoundary) {    glm::vec2 start, end; // Start, end point of current polygon edge    glm::vec2 intersection;   // Intersection point with a clip boundary     outLength = 0;    start = inVertexArray[inLength - 1]; // Start with the last vertex in inVertexArray    for (int j = 0; j < inLength; j++) {        end = inVertexArray[j]; // Now start and end correspond to the vertices                // Cases 1 and 4 - the endpoint is inside the boundary         if (pointInsideBoundary(end,clipBoundary)) {            // Case 1 - Both inside            if (pointInsideBoundary(start, clipBoundary)) {                appendPoint(end, outLength, outVertexArray);            } else { // Case 4 - end is inside, but start is outside                segmentIntersectsBoundary(start, end, clipBoundary, intersection);                appendPoint(intersection, outLength, outVertexArray);                appendPoint(end, outLength, outVertexArray);            }        } else { // Cases 2 and 3 - end is outside            if (pointInsideBoundary(start, clipBoundary))  {                 // Cases 2 - start is inside, end is outside                segmentIntersectsBoundary(start, end, clipBoundary, intersection);                appendPoint(intersection, outLength, outVertexArray);            } else {                // Case 3 - both are outside, No action            }       }        start = end;  // Advance to next pair of vertices    }} bool PolygonClip::pointInsideBoundary(const glm::vec2& testVertex, const LineSegment2& clipBoundary) {    // bottom edge    if (clipBoundary[1].x > clipBoundary[0].x) {        if (testVertex.y >= clipBoundary[0].y) {            return true;        }    }    // top edge    if (clipBoundary[1].x < clipBoundary[0].x) {        if (testVertex.y <= clipBoundary[0].y) {            return true;        }    }    // right edge    if (clipBoundary[1].y > clipBoundary[0].y) {        if (testVertex.x <= clipBoundary[1].x) {            return true;        }    }    // left edge    if (clipBoundary[1].y < clipBoundary[0].y) {        if (testVertex.x >= clipBoundary[1].x) {            return true;        }    }    return false;} void PolygonClip::segmentIntersectsBoundary(const glm::vec2& first, const glm::vec2&  second,                                           const LineSegment2& clipBoundary, glm::vec2& intersection) {    // horizontal    if (clipBoundary[0].y==clipBoundary[1].y) {        intersection.y = clipBoundary[0].y;        intersection.x = first.x + (clipBoundary[0].y - first.y) * (second.x - first.x) / (second.y - first.y);    } else { // Vertical        intersection.x = clipBoundary[0].x;        intersection.y = first.y + (clipBoundary[0].x - first.x) * (second.y - first.y) / (second.x - first.x);    }} void PolygonClip::appendPoint(glm::vec2 newVertex, int& outLength, glm::vec2* outVertexArray) {    outVertexArray[outLength].x = newVertex.x;    outVertexArray[outLength].y = newVertex.y;    outLength++;} // The copyCleanArray() function sets the resulting polygon of the previous step up to be the input polygon for next step of the// clipping algorithm. As the Sutherland-Hodgman algorithm is a polygon clipping algorithm, it does not handle line // clipping very well. The modification so that lines may be clipped as well as polygons is included in this function.// when completed vertexArrayA will be ready for output and/or next step of clipping void PolygonClip::copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB) {    // Fix lines: they will come back with a length of 3, from an original of length of 2    if ((lengthA == 2) && (lengthB == 3)) {        // The first vertex should be copied as is.         vertexArrayA[0] = vertexArrayB[0];         // If the first two vertices of the "B" array are same, then collapse them down to be the 2nd vertex        if (vertexArrayB[0].x == vertexArrayB[1].x)  {            vertexArrayA[1] = vertexArrayB[2];        } else {             // Otherwise the first vertex should be the same as third vertex            vertexArrayA[1] = vertexArrayB[1];        }        lengthA=2;    } else {         // for all other polygons, then just copy the vertexArrayB to vertextArrayA for next step        lengthA = lengthB;        for (int i = 0; i < lengthB; i++) {            vertexArrayA[i] = vertexArrayB[i];        }    }} bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::quat& rotation,        const glm::vec3& position, const glm::vec2& dimensions, float& distance) {    RayIntersectionInfo rayInfo;    rayInfo._rayStart = origin;

Lines 1–500 of 526. Load more