Program Listing for File geometry.hpp

Return to documentation for file (/home/runner/work/Legion-Engine/Legion-Engine/legion/engine/core/math/geometry.hpp)

#pragma once
#include <core/math/glm/glm_include.hpp>
#include <core/math/glm/gtx/string_cast.hpp>
#include <core/math/close_enough.hpp>

namespace legion::core::math
{
    inline float pointToLineSegment2D(const vec2& point, const vec2& lineOrigin, const vec2& lineEnd)
    {
        if (point == lineOrigin || point == lineEnd) return 0.0f;
        vec2 lineDirection = lineEnd - lineOrigin;
        vec2 lineNormal = vec2(-lineDirection.y, lineDirection.x);

        vec2 toLineOrigin = point - lineOrigin;

        if (dot(toLineOrigin, lineDirection) <= 0.0)
        {
            // Point is before the start of the line
            // Return to line start
            return length(toLineOrigin);
        }
        vec2 toLineEnd = point - lineEnd;

        if (dot(toLineEnd, lineDirection) >= 0.0)
        {
            // Point is after the end of the line
            // Return to line start
            return length(toLineEnd);
        }

        // Calculate and return point to line projection length
        return dot(point - lineOrigin, normalize(lineNormal));
    }

    inline float pointToLine2D(const vec2& point, const vec2& lineOrigin, const vec2& lineEnd)
    {
        if (point == lineOrigin || point == lineEnd) return 0.0f;
        vec2 lineDirection = lineEnd - lineOrigin;
        vec2 lineNormal = vec2(-lineDirection.y, lineDirection.x);

        return dot(point - lineOrigin, normalize(lineNormal));
    }

    inline float pointToLineSegment(const vec3& point, const vec3& lineOrigin, const vec3& lineEnd)
    {
        // Check if the point is equal to the start or end of the line
        if (point == lineOrigin || point == lineEnd) return 0.0f;
        vec3 dir = lineEnd - lineOrigin;
        vec3 toLineOrigin = point - lineOrigin;

        if (dot(toLineOrigin, dir) <= 0.0)
        {
            // Projected point is before the start of the line
            // Use distance to start of the line
            return length(toLineOrigin);
        }
        vec3 toLineEnd = point - lineEnd;
        if (dot(toLineEnd, dir) >= 0.0f)
        {
            // Projected point is beyond end of line
            // Use distance toward end of line
            return length(toLineEnd);
        }
        return length(cross(dir, toLineOrigin)) / length(dir);
    }

    struct line_segment
    {
        line_segment(vec3 origin, vec3 end) :
            origin(origin), end(end)
        {
        }

        // Line origin, line start
        vec3 origin;
        // Line end
        vec3 end;

        inline vec3 direction() const
        {
            return end - origin;
        }

        float distanceToPoint(const vec3& p) const
        {
            if (p == origin || p == end) return 0.0f;
            vec3 dir = direction();
            vec3 toLineOrigin = p - origin;

            if (dot(toLineOrigin, dir) <= 0.0)
            {
                // Projected point is before the start of the line
                // Use distance to start of the line
                return length(toLineOrigin);
            }
            vec3 toLineEnd = p - end;
            if (dot(toLineEnd, dir) >= 0.0f)
            {
                // Projected point is beyond end of line
                // Use distance toward end of line
                return length(toLineEnd);
            }
            return length(cross(dir, toLineOrigin)) / length(dir);
        }
    };

    inline float triangleSurface(const vec3& p0, const vec3& p1, const vec3& p2)
    {
        if (p0 == p1 || p0 == p2 || p1 == p2) return 0.0;
        // side lengths
        float a = abs(length(p0 - p1));
        float b = abs(length(p1 - p2));
        float c = abs(length(p2 - p0));
        float s = (a + b + c) * 0.5f;
        return sqrt(s * (s - a) * (s - b) * (s - c));
    }

    inline float pointToTriangle(const vec3& p, const vec3& triPoint0, const vec3& triPoint1, const vec3& triPoint2, const vec3& triNormal,bool debug = false)
    {
        if (p == triPoint0 ||
            p == triPoint1 ||
            p == triPoint2) return 0.f;

        // Get the angle between the triangle normal and p to triPoint0
        float cosAngle = dot(triNormal, p - triPoint0) / (distance(p, triPoint0) * length(triNormal));
        // Get the length of the projection of point p onto the triangle
        float projectionLength = length(p - triPoint0) * cosAngle;
        // From the projectionLength we can determine if the point is above or under the triangle plane
        float positive = 1;
        if (projectionLength < 0) positive = -1;
        // A vector when added to p gives the projected point
        vec3 towardProjection = -projectionLength * (triNormal / length(triNormal));
        // Q is the projection of p onto the plane
        vec3 q = p + towardProjection;

        // We need to determine if projected p is inside the triangle.
        // By putting point q (projected point p) on the triangle, three new triangles are created
        // If point q is in fact inside the triangle, the areas of the three triangles will add up to same area as the original triangle

        double q01Area = triangleSurface(q, triPoint0, triPoint1);
        double q02Area = triangleSurface(q, triPoint0, triPoint2);
        double q12Area = triangleSurface(q, triPoint1, triPoint2);

        // If the area of q to each set of two points is equal to the triangle surface area, q is on the triangle
        if (abs(q01Area + q02Area + q12Area) - triangleSurface(triPoint0, triPoint1, triPoint2) < math::epsilon<float>())
        {
            return projectionLength;
        }
        //Point q is not inside the triangle, check distance toward each edge of the triangle
        // therefore the smallest distance is distance toward a side or end point
        float distance01 = pointToLineSegment(p, triPoint1, triPoint0);
        float distance02 = pointToLineSegment(p, triPoint2, triPoint0);
        float distance12 = pointToLineSegment(p, triPoint2, triPoint1);

        // Assume the shortest distance is sqDistance01
        // Then check if this is true
        float shortestDistance = distance01;
        if (distance02 < distance12)
        {
            if (distance02 < distance01) shortestDistance = distance02;
        }
        else if (distance12 < distance01) shortestDistance = distance12;

        // Return the shortest distance toward one of the sides, either positive or negative, which was determined before
        return shortestDistance*positive;
    }

    inline float pointToTriangle(const vec3& p, const vec3& triPoint0, const vec3& triPoint1, const vec3& triPoint2)
    {
        vec3 normal = normalize(cross(triPoint1 - triPoint0, triPoint2 - triPoint0));
        return pointToTriangle(p, triPoint0, triPoint1, triPoint2, normal);
    }

    inline bool pointProjectionOntoTriangle(const vec3& p, const vec3& triPoint0, const vec3& triPoint1, const vec3& triPoint2, const vec3& triNormal)
    {
        if (p == triPoint0 ||
            p == triPoint1 ||
            p == triPoint2) return true;

        float cosAngle = dot(triNormal, p - triPoint0) / (distance(p, triPoint0) * length(triNormal));
        float projectionLength = length(p - triPoint0) * cosAngle;
        vec3 towardProjection = -projectionLength * (triNormal / length(triNormal));
        // Q is the projection of p onto the plane
        vec3 q = p + towardProjection;

        // Old way of finding if the point is in the triangle
        double q01Area = triangleSurface(q, triPoint0, triPoint1);
        double q02Area = triangleSurface(q, triPoint0, triPoint2);
        double q12Area = triangleSurface(q, triPoint1, triPoint2);

        // If the area of q to each set of two points is equal to the triangle surface area, q is on the triangle
        if (math::close_enough((q01Area + q02Area + q12Area), triangleSurface(triPoint0, triPoint1, triPoint2)))
        {
            return true;
        }
        return false;
    }

    struct triangle
    {
        triangle(vec3 p0, vec3 p1, vec3 p2)
        {
            points[0] = p0;
            points[1] = p1;
            points[2] = p2;
            normal = normalize(cross(p1 - p0, p2 - p0));
        }

        triangle(vec3 p0, vec3 p1, vec3 p2, vec3 normal)
        {
            points[0] = p0;
            points[1] = p1;
            points[2] = p2;
            normal = normalize(normal);
        }

        // The three points of the triangle
        vec3 points[3];
        // The normalized normal of the triangle plane
        vec3 normal;

        float distanceToPoint(const vec3& p) const
        {
            if (p == points[0] ||
                p == points[1] ||
                p == points[2]) return 0.f;

            float cosAngle = dot(normal, p - points[0]) / (distance(p, points[0]) * length(normal));
            float projectionLength = length(p - points[0]) * cosAngle;
            float positive = 1;
            if (projectionLength < 0) positive = -1;
            vec3 towardProjection = -projectionLength * (normal / length(normal));
            // Q is the projection of p onto the plane
            vec3 q = p + towardProjection;

            // Old way of finding if the point is in the triangle
            double q01Area = triangleSurface(q, points[0], points[1]);
            double q02Area = triangleSurface(q, points[0], points[2]);
            double q12Area = triangleSurface(q, points[1], points[2]);

            // If the area of q to each set of two points is equal to the triangle surface area, q is on the triangle
            if (math::close_enough((q01Area + q02Area + q12Area), surface()))
            {
                return projectionLength;
            }

            //Point q is not on the triangle, check distance toward each edge of the triangle
            float distance01 = pointToLineSegment(p, points[1], points[0]);
            float distance02 = pointToLineSegment(p, points[2], points[0]);
            float distance12 = pointToLineSegment(p, points[2], points[1]);

            // Assume the shortest distance is sqDistance01
            // Then check if this is true
            float shortestDistance = distance01;
            if (distance02 < distance12)
            {
                if (distance02 < distance01) shortestDistance = distance02;
            }
            else if (distance12 < distance01) shortestDistance = distance12;

            return shortestDistance * positive;
        }

        float surface() const
        {
            // side lengths
            float a = abs(length(points[0] - points[1]));
            float b = abs(length(points[1] - points[2]));
            float c = abs(length(points[2] - points[0]));
            float s = (a + b + c) * 0.5f;
            return sqrt(s * (s - a) * (s - b) * (s - c));
        }
    };

    inline float pointToPlane(const vec3& point, const vec3& planePosition, const vec3& planeNormal)
    {
        return dot(normalize(planeNormal), point - planePosition);
    }

    inline float angleToPlane(const vec3& point, const vec3& planePosition, const vec3& planeNormal)
    {
        return normalizeDot(planeNormal, point - planePosition);
    }

    struct plane
    {
        plane(vec3 position, vec3 normal) :
            position(position)
        {
            normal = normalize(normal);
        }

        plane(vec3 p0, vec3 p1, vec3 p2)
        {
            position = p0;
            normal = normalize(cross(p1 - p0, p2 - p0));
        }

        // Position on the plane, or a point on the plane
        vec3 position;
        // Normalized normal of the plane
        vec3 normal;

        float distanceToPoint(const vec3& p) const
        {
            return dot(normal, p-position);
        }
    };

    inline mat4 planeMatrix(const vec3& p0, const vec3& p1, const vec3& p2, const vec3& centroid)
    {
        vec3 xAxis = p2 - p0;
        vec3 zAxis = p1 - p0;

        mat4 scale(
            length(xAxis), 0, 0, 0,
            0, length(zAxis), 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1
            );

        vec3 rotX = xAxis / length(xAxis);
        vec3 rotZ = zAxis / length(zAxis);
        vec3 rotY = cross(rotX, rotZ);

        mat4 rot(
            rotX.x, rotX.y, rotX.z, 0,
            rotY.x, rotY.y, rotY.z, 0,
            rotZ.x, rotZ.y, rotZ.z, 0,
            0, 0, 0, 1
        );

        mat4 translation(
            1, 0, 0, centroid.x,
            0, 1, 0, centroid.y,
            0, 0, 1, centroid.z,
            0, 0, 0, 1
        );

        return translation * rot * scale;
    }
}