types.hpp

Go to the documentation of this file.

 
#ifndef TOYMAKERENGINE_SPATIALQUERYTYPES_H
#define TOYMAKERENGINE_SPATIALQUERYTYPES_H
 
#include <cmath>
#include <array>
#include <queue>
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
 
#include "../core/ecs_world.hpp"
 
namespace ToyMaker {
    struct AreaTriangle;
 
    inline float squareDistance(const glm::vec3& vector) {
        return glm::dot(vector, vector);
    }

    using BoxCorner = uint8_t;

    enum BoxCornerSpecifier: BoxCorner {
        RIGHT=0x1,
        TOP=0x2,
        FRONT=0x4
    };
 

    inline bool isFinite(float number) {
        return std::isfinite(number);
    }

    inline bool isFinite(const glm::vec3& vector) {
        return isFinite(vector.x) && isFinite(vector.y) && isFinite(vector.z);
    }

    inline bool isPositiveStrict(float number) {
        return number > 0.f;
    }

    inline bool isPositiveStrict(const glm::vec3& vector) {
        return isPositiveStrict(vector.x) && isPositiveStrict(vector.y) && isPositiveStrict(vector.z);
    }

    inline bool isNonNegative(float number) {
        return number >= 0.f;
    }

    inline bool isNonNegative(const glm::vec3& vector) {
        return isNonNegative(vector.x) && isNonNegative(vector.y) && isNonNegative(vector.z);
    }
 
 
    template <typename TDerived>
    struct Volume;
 

    struct VolumeBase_ {
        inline static constexpr std::array<glm::vec3, 8> GetCornerSignsArray() {
            std::array<glm::vec3, 8> cornerSigns {};
            for(BoxCorner corner {0}; corner < 8; ++corner) {
                cornerSigns[corner].x = corner&BoxCornerSpecifier::RIGHT? 1.f: -1.f;
                cornerSigns[corner].y = corner&BoxCornerSpecifier::TOP? 1.f: -1.f;
                cornerSigns[corner].z = corner&BoxCornerSpecifier::FRONT? 1.f: -1.f;
            }
            return cornerSigns;
        }

        inline static std::array<glm::vec3, 8> ComputeBoxCorners(const glm::vec3& boxDimensions) {
            const std::array<glm::vec3, 8> cornerSignsArray { GetCornerSignsArray() };
            glm::vec3 absoluteCornerOffset { .5f * boxDimensions };
            std::array<glm::vec3, 8> cornerArray { .5f * boxDimensions };
            for(uint8_t corner{0}; corner < 8; ++corner) {
                cornerArray[corner] = cornerSignsArray[corner] * absoluteCornerOffset;
            }
            return cornerArray;
        }

        template <typename TDerived>
        inline std::array<glm::vec3, 8> getVolumeRelativeBoxCorners() const {
            return Volume<TDerived>::getVolumeRelativeBoxCorners();
        }

        template <typename TDerived>
        inline bool isSensible() const {
            return Volume<TDerived>::isSensible();
        }

        template <typename TDerived>
        inline bool isPositiveStrict() const {
            return Volume<TDerived>::isPositiveStrict();
        }
    };

    template <typename TDerived>
    struct Volume: public VolumeBase_ {
        inline std::array<glm::vec3, 8> getVolumeRelativeBoxCorners() const {
            return TDerived::getVolumeRelativeBoxCorners();
        }

        inline bool isSensible() const {
            return TDerived::isSensible();
        }

        inline bool isPositiveStrict() const {
            return TDerived::isPositiveStrict();
        }
    };

    struct VolumeBox: public Volume<VolumeBox> {
        glm::vec3 mDimensions {0.f, 0.f, 0.f};

        inline std::array<glm::vec3, 8> getVolumeRelativeBoxCorners () const {
            return ComputeBoxCorners(mDimensions);
        }

        inline bool isSensible() const {
            return isNonNegative(mDimensions) && isFinite(mDimensions);
        }

        inline bool isPositiveStrict() const {
            return ToyMaker::isPositiveStrict(mDimensions);
        }
    };

    struct VolumeCapsule: public Volume<VolumeCapsule> {
        float mHeight {0.f};

        float mRadius {0.f};

        inline std::array<glm::vec3, 8> getVolumeRelativeBoxCorners () const {
            const glm::vec3 boxDimensions { 2.f*mRadius, mHeight + 2.f*mRadius, 2.f*mRadius};
            return ComputeBoxCorners(boxDimensions);
        }

        inline bool isSensible() const {
            return (
                isNonNegative(mHeight) && isFinite(mHeight)
                && isNonNegative(mRadius) && isFinite(mRadius)
            );
        }

        inline bool isPositiveStrict() const {
            return (
                ToyMaker::isPositiveStrict(mRadius)
                // You can still have a capsule which occupies space even if its
                // spine is length 0, so no test for mHeight
            );
        }
    };

    struct VolumeSphere: public Volume<VolumeSphere> {
        float mRadius {0.f};

        inline std::array<glm::vec3, 8> getVolumeRelativeBoxCorners () const {
            return ComputeBoxCorners(glm::vec3{2*mRadius});
        }

        inline bool isSensible() const {
            return isNonNegative(mRadius) && isFinite(mRadius);
        }

        inline bool isPositiveStrict() const {
            return ToyMaker::isPositiveStrict(mRadius);
        }
    };

    struct AreaTriangle {
        std::array<glm::vec3, 3> mPoints {};

        inline bool isSensible() const {
            return (
                (
                    isFinite(mPoints[0]) && isFinite(mPoints[1]) && isFinite(mPoints[2])
                ) && (
                    isNonNegative(
                        glm::length(glm::cross(
                            mPoints[2] - mPoints[0], mPoints[1] - mPoints[0]
                        ))
                    )
                )
            );
        }

        inline bool isPositiveStrict() const {
            return (
                ToyMaker::isPositiveStrict(
                    glm::length(
                        glm::cross(
                            mPoints[2] - mPoints[0], mPoints[1] - mPoints[0]
                        )
                    )
                )
            );
        }
    };

    std::array<AreaTriangle, 12> computeBoxFaceTriangles(const std::array<glm::vec3, 8>& boxCorners);

    struct AreaCircle {
        float mRadius { 0.f };

        glm::vec3 mCenter { 0.f };

        glm::vec3 mNormal { 0.f, -1.f, 0.f };

        inline bool isSensible() const {
            return (
                (
                    isFinite(mRadius) && isNonNegative(mRadius)
                ) && (
                    isFinite(mNormal) && ToyMaker::isPositiveStrict(glm::length(mNormal))
                ) && (
                    isFinite(mCenter)
                )
            );
        }

        inline bool isPositiveStrict() const {
            return (
                ToyMaker::isPositiveStrict(mRadius)
            );
        }
    };

    struct Ray {
        glm::vec3 mStart { 0.f };

        glm::vec3 mDirection { 0.f, 0.f, -1.f };

        float mLength { std::numeric_limits<float>::infinity() };

        inline bool isSensible() const {
            return (
                (
                    isFinite(mDirection) && ToyMaker::isPositiveStrict(glm::length(mDirection))
                ) && (
                    isFinite(mStart)
                ) && (
                    isNonNegative(mLength)
                )
            );
        }

        inline bool isPositiveStrict() const {
            return ToyMaker::isPositiveStrict(mLength);
        }
    };

    struct Plane {
        glm::vec3 mPointOnPlane { 0.f };

        glm::vec3 mNormal { 0.f, 0.f, -1.f };

        inline bool isSensible() const {
            return (
                (
                    isFinite(mNormal) && ToyMaker::isPositiveStrict(glm::length(mNormal))
                ) && (
                    isFinite(mPointOnPlane)
                )
            );
        }

        inline bool isPositiveStrict() const {
            return isSensible();
        }
    };

    struct Simplex {
        std::array<glm::vec3, 4> mPoints;

        std::array<glm::vec3, 4> mPointsSupportA;

        std::array<glm::vec3, 4> mPointsSupportB;

        uint8_t mNPoints { 0 };

        inline bool append(const glm::vec3& candidatePoint, const glm::vec3& supportA, const glm::vec3& supportB) {
            assert(mNPoints < 4 && "We already have 4 (or more) points, there's no more needed");
            assert(supportA - supportB == candidatePoint && "The difference in supports should yield the candidate point");
 
            // Hackery to ensure that we don't add the same point twice (which is a possibility 
            // since the hackery to force 3-simplex might cause us to look in the same direction
            // twice)
            for(uint8_t index { 0 }; index < mNPoints; ++index) {
                if(mPoints[index] == candidatePoint) {
                    return false;
                }
            }
            mPoints[mNPoints] = candidatePoint;
            mPointsSupportA[mNPoints] = supportA;
            mPointsSupportB[mNPoints] = supportB;
            ++mNPoints;
 
            return true;
        }

        inline void reorder(const std::vector<uint8_t> reorderedIndices) {
            assert(reorderedIndices.size() <= 4 && "Invalid point list provided");
            const std::array<glm::vec3, 4> oldPoints { mPoints };
            const std::array<glm::vec3, 4> oldPointsSupportA { mPointsSupportA };
            const std::array<glm::vec3, 4> oldPointsSupportB { mPointsSupportB };
            mNPoints = 0;
            for(const auto& index: reorderedIndices) {
                const bool pointAddedSuccessfully{ append(oldPoints[index], oldPointsSupportA[index], oldPointsSupportB[index]) };
                assert(pointAddedSuccessfully && "Invalid point list provided");
            }
        }

        std::pair<bool, glm::vec3> evaluate();
 
    private:
        std::pair<bool, glm::vec3> doSimplex4();
        glm::vec3 doSimplex3();
        glm::vec3 doSimplex2();
    };

    class Polytope {
    private:
        struct Face {
            std::array<uint16_t, 3>  mIndices {};
        };

        Polytope();
 

        std::priority_queue<Face, std::vector<Face>, std::function<bool(const Face&, const Face&)>> mFaces;

        std::vector<glm::vec3> mPoints {};
        std::vector<glm::vec3> mPointsSupportA {};

        std::vector<glm::vec3> mPointsSupportB {};
 
        inline glm::vec3 getTriangleCross(const Face& face) const {
            return glm::cross(
                mPoints[face.mIndices[1]] - mPoints[face.mIndices[0]],
                mPoints[face.mIndices[2]] - mPoints[face.mIndices[0]]
            );
        }
 
        inline glm::vec3 getTriangleNorm(const Face& face) const {
            return glm::normalize(getTriangleCross(face));
        }
 
    public:
        Polytope(const Simplex& simplex);

        glm::vec3 getNextSearch() const;

        std::size_t getNumFaces() const {
            return mFaces.size();
        }
 
        std::size_t getNumPoints() const {
            return mPoints.size();
        }

        glm::vec3 getClosestPoint() const;

        glm::vec3 getClosestTriangleNormal() const;

        AreaTriangle getClosestTriangle() const;

        AreaTriangle getClosestTriangleSupportA() const;

        AreaTriangle getClosestTriangleSupportB() const;

        bool append(const glm::vec3& newPoint, const glm::vec3& supportA, const glm::vec3& supportB);
    };

    struct Contact {
        glm::vec3 mPoint;

        glm::vec3 mNormal;

        glm::vec3 mTangent1;

        glm::vec3 mTangent2;

        float mPenetrationDepth;
    };

    struct Collision {
        bool mCollided;

        Contact mContactA;

        Contact mContactB;
    };

    struct ObjectBounds {
        inline static std::string getComponentTypeName() { return "ObjectBounds"; }

        enum class TrueVolumeType: uint8_t {
            BOX=0,
            SPHERE,
            CAPSULE,
        };

        union TrueVolume {
            VolumeBox mBox { .mDimensions{ glm::vec3{0.f} } };
            VolumeCapsule mCapsule;
            VolumeSphere mSphere;
        };

        static ObjectBounds create(const VolumeBox& box, const glm::vec3& positionOffset, const glm::quat& orientationOffset);

        static ObjectBounds create(const VolumeCapsule& capsule, const glm::vec3& positionOffset, const glm::quat& orientationOffset);

        static ObjectBounds create(const VolumeSphere& sphere, const glm::vec3& positionOffset, const glm::quat& orientationOffset);

        TrueVolumeType mType { TrueVolumeType::BOX };

        TrueVolume mTrueVolume {};

        glm::vec3 mPosition { 0.f };

        glm::vec3 mPositionOffset { 0.f };

        glm::quat mOrientation { glm::vec3{ 0.f } };

        glm::quat mOrientationOffset { glm::vec3{ 0.f } };

        void applyModelMatrix(const glm::mat4& modelMatrix);

        inline glm::mat3 getLocalRotationTransform() const { 
            return glm::mat3_cast(glm::normalize(mOrientationOffset));
        }

        inline glm::mat3 getWorldRotationTransform() const {
            return glm::mat3_cast(glm::normalize(mOrientation));
        }

        glm::vec3 getComputedWorldPosition() const;

        glm::quat getComputedWorldOrientation() const;

        std::array<glm::vec3, 8> getVolumeRelativeBoxCorners() const;

        std::array<glm::vec3, 8> getLocalOrientedBoxCorners() const;

        std::array<glm::vec3, 8> getWorldOrientedBoxCorners() const;

        std::array<AreaTriangle, 12> getWorldOrientedBoxFaceTriangles() const { return computeBoxFaceTriangles(getWorldOrientedBoxCorners()); };

        glm::vec3 getSupportAlong(const glm::vec3& axis) const;

        std::pair<float, float> getProjectionAlong(const glm::vec3& axis) const;

        bool isSensible() const;

        bool isPositiveStrict() const;
    protected:
    };

    class AxisAlignedBounds {
    public:
        inline static std::string getComponentTypeName() { return "AxisAlignedBounds"; }

        using Extents = std::pair<glm::vec3, glm::vec3>;

        AxisAlignedBounds();

        AxisAlignedBounds(const ObjectBounds& objectBounds);

        AxisAlignedBounds(const Extents& axisAlignedExtents);

        AxisAlignedBounds(const glm::vec3& position, const glm::vec3& dimensions):
        AxisAlignedBounds { Extents{{position + .5f*dimensions}, {position - .5f*dimensions}} }
        {}

        AxisAlignedBounds operator+(const AxisAlignedBounds& other) const;

        std::array<glm::vec3, 8> getAxisAlignedBoxCorners() const;

        inline std::array<AreaTriangle, 12> getAxisAlignedBoxFaceTriangles() const { return computeBoxFaceTriangles(getAxisAlignedBoxCorners()); }

        Extents getAxisAlignedBoxExtents() const;

        inline glm::vec3 getDimensions() const { return mExtents.first - mExtents.second; }

        inline glm::vec3 getComputedWorldPosition() const { return mExtents.second + .5f * getDimensions(); }

        inline bool isSensible() const {
            return (
                isFinite(mExtents.first) && isFinite(mExtents.second)
                && isNonNegative(mExtents.first - mExtents.second)
            );
        }

        glm::vec3 getSupportAlong(const glm::vec3& axis) const;

        inline bool isPositiveStrict() const {
            return (
                ToyMaker::isPositiveStrict(mExtents.first - mExtents.second)
            );
        }

        inline void setPosition(const glm::vec3& position) {
            assert(isFinite(position) && "Invalid position specified. Position must be finite");
            const glm::vec3 deltaPosition { position - getComputedWorldPosition() };
            mExtents.first += deltaPosition;
            mExtents.second += deltaPosition;
        }

        inline void setDimensions(const glm::vec3& dimensions) {
            assert(isNonNegative(dimensions) && isFinite(dimensions) && "Invalid dimensions provided.  Dimensions must be non negative and finite");
            const glm::vec3 position { getComputedWorldPosition() };
            const glm::vec3 deltaDimensions { dimensions - getDimensions() };
            mExtents.first += .5f * deltaDimensions;
            mExtents.second -= .5f * deltaDimensions;
        }
    private:
        void setByExtents(const Extents& axisAlignedExtents);

        Extents mExtents {glm::vec3{0.f}, glm::vec3{0.f}};
    };

    NLOHMANN_JSON_SERIALIZE_ENUM( ObjectBounds::TrueVolumeType, {
        {ObjectBounds::TrueVolumeType::BOX, "box"},
        {ObjectBounds::TrueVolumeType::SPHERE, "sphere"},
        {ObjectBounds::TrueVolumeType::CAPSULE, "capsule"},
    })

    inline void to_json(nlohmann::json& json, const ObjectBounds& objectBounds) {
        json = {
            {"type", ObjectBounds::getComponentTypeName()},
            {"volume_type", objectBounds.mType},
            {"position_offset", { objectBounds.mPositionOffset.x, objectBounds.mPositionOffset.y, objectBounds.mPositionOffset.z}},
            {"orientation_offset", { objectBounds.mOrientationOffset.w, objectBounds.mOrientationOffset.x, objectBounds.mOrientationOffset.y, objectBounds.mOrientationOffset.z }},
        };
        switch(objectBounds.mType) {
            case ObjectBounds::TrueVolumeType::BOX:
                json["volume_properties"] = {
                    {"width", objectBounds.mTrueVolume.mBox.mDimensions.x},
                    {"height", objectBounds.mTrueVolume.mBox.mDimensions.y},
                    {"depth", objectBounds.mTrueVolume.mBox.mDimensions.z},
                };
                break;
            case ObjectBounds::TrueVolumeType::SPHERE:
                json["volume_properties"] = {
                    {"radius", objectBounds.mTrueVolume.mSphere.mRadius},
                };
                break;
            case ObjectBounds::TrueVolumeType::CAPSULE:
                json["volume_properties"] = {
                    {"radius", objectBounds.mTrueVolume.mCapsule.mRadius},
                    {"height", objectBounds.mTrueVolume.mCapsule.mHeight},
                };
                break;
        };
    }

    inline void from_json(const nlohmann::json& json, ObjectBounds& objectBounds) {
        assert(json.at("type") == ObjectBounds::getComponentTypeName() && "Incorrect type property for an objectBounds component");
        const glm::vec3 positionOffset {
            json.at("position_offset")[0],
            json.at("position_offset")[1],
            json.at("position_offset")[2],
        };
        const glm::vec3 orientationOffset { 
            glm::eulerAngles(glm::normalize(glm::quat {
                json.at("orientation_offset")[0],
                json.at("orientation_offset")[1],
                json.at("orientation_offset")[2],
                json.at("orientation_offset")[3]
            }))
        };
 
        switch (static_cast<ObjectBounds::TrueVolumeType>(json.at("volume_type"))) {
            case ObjectBounds::TrueVolumeType::BOX:
                objectBounds = ObjectBounds::create(
                    VolumeBox{ .mDimensions {
                        json.at("volume_properties").at("width").get<float>(),
                        json.at("volume_properties").at("height").get<float>(),
                        json.at("volume_properties").at("depth").get<float>(),
                    } },
                    positionOffset,
                    orientationOffset
                );
                break;
 
            case ObjectBounds::TrueVolumeType::SPHERE:
                objectBounds = ObjectBounds::create(
                    VolumeSphere { .mRadius { json.at("volume_properties").at("radius").get<float>() }},
                    positionOffset,
                    orientationOffset
                );
                break;
 
            case ObjectBounds::TrueVolumeType::CAPSULE:
                objectBounds = ObjectBounds::create(
                    VolumeCapsule {
                        .mHeight { json.at("volume_properties").at("height").get<float>() },
                        .mRadius { json.at("volume_properties").at("radius").get<float>() },
                    },
                    positionOffset,
                    orientationOffset
                );
                break;
        }
    }

    inline void to_json(nlohmann::json& json, const AxisAlignedBounds& axisAlignedBounds) { /* never used, so pass */
        (void)json; // prevent unused parameter warnings
        (void)axisAlignedBounds; // prevent unused parameter warnings
    }

    inline void from_json(const nlohmann::json& json, AxisAlignedBounds& objectBounds) { /* never used, so pass */ 
        (void)json; // prevent unused parameter warnings
        (void)objectBounds; // prevent unused parameter warnings
    }
}
 
#endif