math.hpp

Go to the documentation of this file.

 
#ifndef TOYMAKERENGINE_SPATIALQUERYMATH_H
#define TOYMAKERENGINE_SPATIALQUERYMATH_H
 
#include <iostream>
#include <glm/glm.hpp>
 
#include "../util.hpp"
#include "types.hpp"
 
namespace ToyMaker {
 
    static constexpr uint8_t kMaxIterations { 32 };
    static constexpr float kSimplexEpsilon { 0.0001 };
 

    glm::mat3 computeBarycentricSolver(const AreaTriangle& triangle);

    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);
    }

    Collision checkCollisionSphereSphere(const ObjectBounds& one, const ObjectBounds& two);

    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);
 

    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.getPositionWorld() - one.getPositionWorld() };
 
        // avoid handling cases where 2 objects overlap at their origin
        if(squareDistance(searchDirection) == 0.f) {
            std::cerr << "Invalid overlap where 2 objects share same origin detected.\n";
            return { false, Simplex {} };
        }
 
        Simplex simplex {};
        auto fullDifference { minkowskiDifferenceFull(one, two, searchDirection) };
        const auto& candidatePoint { std::get<0>(fullDifference) }; // updated with fullDifference
        const auto& supportA { std::get<1>(fullDifference) }; // updated with fullDifference
        const auto& supportB { std::get<2>(fullDifference) }; // updated with fullDifference
        simplex.append(candidatePoint, supportA, supportB);
 
        // we go towards the origin from the first point we found
        glm::vec3 toOrigin { -simplex.mPoints[0] };
        searchDirection = toOrigin;
        for(uint8_t iteration { 0 }; squareDistance(toOrigin) && iteration < kMaxIterations; ++iteration) {
            // find a new search direction
            searchDirection = toOrigin;
 
            // find a new candidate point
            fullDifference = minkowskiDifferenceFull(one, two, searchDirection);
 
            // we couldn't find a point far enough past the origin in this direction, so
            // obviously there is no intersection
            if(glm::dot(candidatePoint, searchDirection) <= 0.f) {
                return { false, simplex };
            }
 
            simplex.append(candidatePoint, supportA, supportB);
            const auto result { simplex.evaluate() };
            toOrigin = result.second;
            if(result.first) {
                return { true, simplex };
            }
        }
 
        // handle degenerate simplexes by forcing them to expand to a full simplex
        while(simplex.mNPoints < 4) {
            switch(simplex.mNPoints) {
                case 1:
                    simplex.mPoints[simplex.mNPoints] = glm::vec3 { 2.f * kSimplexEpsilon, 0.f, 0.f };
                    simplex.mPointsSupportA[simplex.mNPoints] = glm::vec3 { kSimplexEpsilon, 0.f, 0.f };
                    simplex.mPointsSupportB[simplex.mNPoints] = -glm::vec3 { kSimplexEpsilon, 0.f, 0.f };
                    ++simplex.mNPoints;
                    break;
                case 2:
                    {
                        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, 1.f, 0.f } : glm::vec3{ 1.f, 0.f, 1.f }
                        };
                        searchDirection = glm::normalize(-dirAB + dirOffset);
                        simplex.mPoints[simplex.mNPoints] = 2.f * kSimplexEpsilon * dirOffset;
                        simplex.mPointsSupportA[simplex.mNPoints] = kSimplexEpsilon * dirOffset;
                        simplex.mPointsSupportB[simplex.mNPoints] = -kSimplexEpsilon * dirOffset;
                        ++simplex.mNPoints;
                    }
                    break;
                case 3:
                    {
                        const glm::vec3 normABC { glm::normalize(glm::cross(simplex.mPoints[1] - simplex.mPoints[0], simplex.mPoints[2] - simplex.mPoints[0])) };
                        assert(isNumber(normABC) && isFinite(normABC) && "Invalid norm computed, degenerate triangle detected.");
                        simplex.mPoints[simplex.mNPoints] = 2.f * kSimplexEpsilon * normABC;
                        simplex.mPointsSupportA[simplex.mNPoints] = kSimplexEpsilon * normABC;
                        simplex.mPointsSupportB[simplex.mNPoints] = -kSimplexEpsilon * normABC;
                        ++simplex.mNPoints;
                    }
                    break;
                default:
                    assert(false && "Simplex with invalid number of points for this section of the algorithm provided");
                    break;
            };
        }
 
        return { true, simplex };
    }
 
    // TODO: This shouldn't be a template, since both parameters will _have_ to be ObjectBounds
    template <typename T, typename U>
    inline Collision checkCollision(const T& one, const U& two) {
        // perform inexpensive AABB test first
        if(!overlaps(AxisAlignedBounds(one), AxisAlignedBounds(two))) {
            return {
                .mCollided { false }
            };
        }
 
        // special case: sphere sphere collision, solvable in constant time
        if(
            one.mType == ObjectBounds::TrueVolumeType::SPHERE
            && two.mType == ObjectBounds::TrueVolumeType::SPHERE
        ) {
            return checkCollisionSphereSphere(one, 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 { computeBarycentricSolver(closestTriangle) };
        // If we can't solve this, then there's nothing for us to do
        if(!isSensible(barycentricSolver)) {
            return { false };
        }
 
        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;
        assert(
            isNumber(collisionResult.mContactA.mPoint) && isFinite(collisionResult.mContactA.mPoint)
            && "Invalid contact point computed for object A"
        );
        collisionResult.mContactB.mPoint = barycentricTwo * barycentricCoordinates;
        assert(
            isNumber(collisionResult.mContactB.mPoint) && isFinite(collisionResult.mContactB.mPoint)
            && "Invalid contact point computed for object B"
        );
 
        // build tangent vectors to the normal (used for friction calculations)
        const auto contactNormal {
            squareDistance(closestPoint)? closestPoint: one.getPositionWorld() - two.getPositionWorld()
        };
        const auto tangentPair { getTangents(contactNormal) };
        collisionResult.mContactB.mNormal = glm::normalize(contactNormal);
        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