math.hpp

Go to the documentation of this file.

 
#ifndef TOYMAKERENGINE_SPATIALQUERYMATH_H
#define TOYMAKERENGINE_SPATIALQUERYMATH_H
 
#include <glm/glm.hpp>
 
#include "types.hpp"
 
namespace ToyMaker {
 
    static constexpr uint8_t kMaxIterations { 64 };

    std::pair<bool, glm::vec3> computeIntersection(const Ray& ray, const Plane& plane);
    std::pair<bool, glm::vec3> computeIntersection(const Ray& ray, const AreaTriangle& triangle);
    std::pair<uint8_t, std::pair<glm::vec3, glm::vec3>> computeIntersections(const Ray& ray, const AxisAlignedBounds& axisAlignedBounds);
    std::pair<uint8_t, std::pair<glm::vec3, glm::vec3>> computeIntersections(const Ray& ray, const ObjectBounds& objectbounds);

    bool overlaps(const glm::vec3& point, const ObjectBounds& bounds);
    inline bool overlaps(const ObjectBounds& bounds, const glm::vec3& point) {
        return overlaps(point, bounds);
    }

    bool overlaps(const Ray& ray, const ObjectBounds& bounds);
    inline bool overlaps(const ObjectBounds& bounds, const Ray& ray) {
        return overlaps(ray, bounds);
    }

    bool overlaps(const glm::vec3& point, const AxisAlignedBounds& bounds);
    inline bool overlaps(const AxisAlignedBounds& bounds, const glm::vec3& point) {
        return overlaps(point, bounds);
    }

    bool overlaps(const Ray& ray, const AxisAlignedBounds& bounds);
    inline bool overlaps(const AxisAlignedBounds& bounds, const Ray& ray) {
        return overlaps(ray, bounds);
    }

    bool overlaps(const AxisAlignedBounds& one, const AxisAlignedBounds& two);
    bool overlaps(const ObjectBounds& one, const ObjectBounds& two);
    bool overlaps(const AxisAlignedBounds& one, const ObjectBounds& two);
    inline bool overlaps(const ObjectBounds& one, const AxisAlignedBounds& two) {
        return overlaps(two, one);
    }

    template <typename T, typename U>
    std::pair<bool, Simplex> gjkOverlaps(const T& one, const U& two);

    template <typename T, typename U>
    Polytope buildPolytope(const T& one, const U& two, const Simplex& gjkSimplex);

    template <typename T, typename U>
    Collision checkCollision(const T& one, const U& two);

    bool contains(const glm::vec3& point, const ObjectBounds& bounds);
    bool contains(const glm::vec3& point, const AxisAlignedBounds& bounds);
    bool contains(const Ray& ray, const AxisAlignedBounds& bounds);
    bool contains(const Ray& ray, const ObjectBounds& bounds);
    bool contains(const AxisAlignedBounds& one, const AxisAlignedBounds& two);
    bool contains(const ObjectBounds& one, const AxisAlignedBounds& two);

    inline std::pair<glm::vec3, glm::vec3> deriveTangents(const glm::vec3& vector) {
        assert(isFinite(vector) && "Vector must be finite");
        assert(squareDistance(vector) && "Zero vectors are invalid as basis for tangents");
 
        const auto normalized { glm::normalize(vector) };
        const glm::vec3 tangentPrelim { ((glm::abs(normalized.x) >= .57735f) ?
            glm::vec3{ normalized.y, -normalized.x, 0.f } : glm::vec3{ 0.f, normalized.z, -normalized.y }
        ) };
        const glm::vec3 tangent2 {
            glm::normalize(glm::cross(tangentPrelim, normalized))
        };
 
        return { glm::normalize(glm::cross(normalized, tangent2)), tangent2 };
    }

    template<typename T, typename U>
    inline glm::vec3 minkowskiDifference(const T& one, const U& two, const glm::vec3& along) {
        const glm::vec3 supportOne { one.getSupportAlong(along) };
        const glm::vec3 supportTwo { two.getSupportAlong(-along) };
        return supportOne - supportTwo;
    }

    template<typename T, typename U>
    inline std::tuple<glm::vec3, glm::vec3, glm::vec3> minkowskiDifferenceFull(const T& one, const U& two, const glm::vec3& along) {
        const glm::vec3 supportOne { one.getSupportAlong(along) };
        const glm::vec3 supportTwo { two.getSupportAlong(-along) };
        return { supportOne - supportTwo, supportOne, supportTwo };
    }
 
    template <typename T, typename U>
    inline std::pair<bool, Simplex> gjkOverlaps(const T& one, const U& two) {
        assert(one.isSensible() && two.isSensible() && "Invalid object bounds provided");
 
        // Both need to be non-degenerate bounds to actually overlap
        if(!one.isPositiveStrict() || !two.isPositiveStrict()) {
            return { false, Simplex {} };
        }
 
 
        glm::vec3 searchDirection { two.getComputedWorldPosition() - one.getComputedWorldPosition() };
 
        // Two non-degenerate objects share the same origin, obviously they overlap
        if(squareDistance(searchDirection) == 0.f) {
            return { true, Simplex {} };
        }
 
        Simplex simplex {};
        auto fullDifference { minkowskiDifferenceFull(one, two, searchDirection) };
        const auto& candidatePoint { std::get<0>(fullDifference) };
        const auto& supportA { std::get<1>(fullDifference) };
        const auto& supportB { std::get<2>(fullDifference) };
        simplex.append(candidatePoint, supportA, supportB);
 
        // we go towards the origin from the first point we found
        searchDirection = squareDistance(simplex.mPoints[0]) ? -simplex.mPoints[0]: glm::vec3 { 1.f, 0.f, 0.f };
        bool found { false };
        for(uint8_t iteration { 0 }; iteration < kMaxIterations; ++iteration) {
 
            // We force a tetrahedron to form by picking arbitrary search directions
            // in the event our degenerate simplex contains (0, 0, 0)
            if(!squareDistance(searchDirection)) {
                const float signMult { (iteration & 1)? -1.f: 1.f };
 
                // Pick a new search direction to expand 1-simplex (line) to 2-simplex (triangle)
                if(simplex.mNPoints == 2) {
                    const float signMult2 { (iteration & 2)? -1.f: 1.f };
                    const glm::vec3 dirAB { glm::normalize(simplex.mPoints[1] - simplex.mPoints[0]) };
                    assert(squareDistance(dirAB) > 0 && "A and B should never be the same point");
 
                    const glm::vec3 dirOffset { (dirAB.x != 0.f || dirAB.z != 0.f)? 
                        glm::vec3{ 0.f, signMult * 1.f, 0.f } : glm::vec3{ signMult2 * 1.f, 0.f, signMult * 1.f }
                    };
 
                    searchDirection = -dirAB + dirOffset;
 
                // Pick a new search direction to expand 2-simplex (triangle) to 3-simplex (tetrahedron)
                } else if(simplex.mNPoints == 3) {
                    const glm::vec3 normABC { glm::normalize(glm::cross(simplex.mPoints[1] - simplex.mPoints[0], simplex.mPoints[2] - simplex.mPoints[0])) };
                    searchDirection = signMult * normABC;
 
                } else {
                    assert(false && "A simplex with 4 points is not degenerate, and there's no solving necessary here");
                }
            }
 
            fullDifference = minkowskiDifferenceFull(one, two, searchDirection);
 
            // We couldn't find a point past the origin in this direction, so obviously there is no intersection
            if(glm::dot(candidatePoint, searchDirection) <= 0) {
                return { false, simplex };
            }
 
            // This will return false generally when we're trying to fabricate
            // a search direction.  In that case we just fabricate another one at
            // the start of the loop.
            if(!simplex.append(candidatePoint, supportA, supportB)) {
                searchDirection = glm::vec3 { 0.f, 0.f, 0.f };
                continue;
            }
 
            const auto result { simplex.evaluate() };
            if(result.first) {
                return { true, simplex };
            }
 
            searchDirection = result.second;
        }
        assert(false && "We should not have exited the loop without finding a simplex");
        return { false, simplex };
    }
 
    template <typename T, typename U>
    inline Collision checkCollision(const T& one, const U& two) {
        // check via GJK whether there's any overlap at all
        const std::pair<bool, Simplex> overlapResult { gjkOverlaps(one, two) };
        if(!overlapResult.first) {
            return {
                .mCollided { false },
            };
        }
 
        // allocate result storage
        Collision collisionResult { Collision {
            .mCollided { true },
        } };
 
        
        // build a polytope containing the closest surface to the origin in 
        // the Minkowski difference between the objects
        const Polytope polytope { buildPolytope(one, two, overlapResult.second) };
 
        // derive barycentric coordinates for the contact point relative to the 
        // triangle in the difference shape it lies on
        const glm::vec3 closestPoint { polytope.getClosestPoint() };
        const AreaTriangle closestTriangle { polytope.getClosestTriangle() };
        const glm::mat3 barycentricSolver { glm::inverse(glm::mat3 {
            closestTriangle.mPoints[0], closestTriangle.mPoints[1], closestTriangle.mPoints[2]
        }) };
        const glm::vec3 barycentricCoordinates {
            barycentricSolver * closestPoint
        };
 
        // Use the barycentric coordinates to derive contact point for both shapes
        const AreaTriangle closestTriangleOne {
            polytope.getClosestTriangleSupportA()
        };
        const AreaTriangle closestTriangleTwo {
            polytope.getClosestTriangleSupportB()
        };
        const glm::mat3 barycentricOne {
            closestTriangleOne.mPoints[0], closestTriangleOne.mPoints[1], closestTriangleOne.mPoints[2]
        };
        const glm::mat3 barycentricTwo {
            closestTriangleTwo.mPoints[0], closestTriangleTwo.mPoints[1], closestTriangleTwo.mPoints[2]
        };
        collisionResult.mContactA.mPoint = barycentricOne * barycentricCoordinates;
        collisionResult.mContactB.mPoint = barycentricTwo * barycentricCoordinates;
 
        // build tangent vectors to the normal (used for friction calculations)
        const auto tangentPair { deriveTangents(closestPoint) };
        collisionResult.mContactB.mNormal = glm::normalize(closestPoint);
        collisionResult.mContactB.mTangent1 = glm::normalize(tangentPair.first);
        collisionResult.mContactB.mTangent2 = glm::normalize(tangentPair.second);
        collisionResult.mContactA.mNormal = -collisionResult.mContactB.mNormal;
        collisionResult.mContactA.mTangent1 = -collisionResult.mContactB.mTangent1;
        collisionResult.mContactA.mTangent2 = -collisionResult.mContactB.mTangent2;
 
        // set penetration depth value for both contacts
        collisionResult.mContactA.mPenetrationDepth = collisionResult.mContactB.mPenetrationDepth = glm::length(closestPoint);
 
        return collisionResult;
    }
 
    template <typename T, typename U>
    inline Polytope buildPolytope(const T& one, const U& two, const Simplex& simplex) {
        Polytope polytope { simplex };
        glm::vec3 searchDirection { polytope.getNextSearch() };
        assert(squareDistance(searchDirection) > 0 && "Search direction should always be non-zero");
        assert(isFinite(searchDirection) && "Search direction should always be finite");
 
        // Compute first support point
        std::tuple<glm::vec3, glm::vec3, glm::vec3> differenceFull { minkowskiDifferenceFull(one, two, searchDirection) };
 
        // These reference types can be thought of as "views" into the 
        // differenceFull struct
        const glm::vec3& supportA { std::get<1>(differenceFull) };
        const glm::vec3& supportB { std::get<2>(differenceFull) };
        const glm::vec3& difference { std::get<0>(differenceFull) };
 
        // Keep building out the expanding polytope until either the minimum translation vector is found or we run out of iterations
        uint8_t iterationsLeft { kMaxIterations };
        while(polytope.append(difference, supportA, supportB) && iterationsLeft--) {
            searchDirection = polytope.getNextSearch();
            differenceFull = minkowskiDifferenceFull(one, two, searchDirection);
        }
 
        return polytope;
    }
}
 
#endif