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 <nlohmann/json.hpp>
#include <string>
 
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 isNumber(float number) {
        return !std::isnan(number);
    };
 
    inline bool isNumber(const glm::vec3& vector) {
        return isNumber(vector.x) && isNumber(vector.y) && isNumber(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;

    using InteractionLayerMask = uint16_t;

    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");
 
            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,
            InteractionLayerMask interactionLayers=0x1,
            InteractionLayerMask interactionMask=std::numeric_limits<InteractionLayerMask>::max()
        );

        static ObjectBounds create(
            const VolumeCapsule& capsule,
            const glm::vec3& positionOffset,
            const glm::quat& orientationOffset,
            InteractionLayerMask interactionLayers=0x1,
            InteractionLayerMask interactionMask=std::numeric_limits<InteractionLayerMask>::max()
        );

        static ObjectBounds create(
            const VolumeSphere& sphere,
            const glm::vec3& positionOffset,
            const glm::quat& orientationOffset,
            InteractionLayerMask interactionLayers=0x1,
            InteractionLayerMask interactionMask=std::numeric_limits<InteractionLayerMask>::max()
        );

        TrueVolumeType mType { TrueVolumeType::BOX };

        TrueVolume mTrueVolume {};

        glm::vec3 mPositionWorld { 0.f };

        glm::vec3 mPositionOrigin { 0.f };

        glm::vec3 mPositionOffset { 0.f };

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

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

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

        InteractionLayerMask mInteractionLayers { 0x1 };

        InteractionLayerMask mInteractionMask { std::numeric_limits<InteractionLayerMask>::max() };

        bool mUpdatedFromTransform { true };

        bool mTransformUpdateRequired { false };

        bool mVolumeSystemComputed { true };

        bool mVolumeUpdateRequired { true };

        bool mPhysicsRecomputeRequired { true };

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

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

        inline glm::mat4 getRotationTransformOrigin() const {
            return glm::mat4_cast(glm::normalize(mOrientationOrigin));
        }

        inline glm::mat4 getTranslationTransformOrigin() const {
            return glm::mat4 {
                {   1.f, 0.f, 0.f, 0.f },
                {   0.f, 1.f, 0.f, 0.f },
                {   0.f, 0.f, 1.f, 0.f },
                { mPositionOrigin, 1.f },
            };
        }

        glm::vec3 getPositionWorld() const;

        void setPositionWorld(const glm::vec3& newPosition);

        void setPositionOffset(const glm::vec3& newPosition);

        void setPositionOrigin(const glm::vec3& newPosition);

        void recomputeWorldPositionOrientation();

        glm::quat getOrientationWorld() const;

        void setOrientationWorld(const glm::quat& newOrientation);

        void setOrientationOffset(const glm::quat& newOrientation);

        void setOrientationOrigin(const glm::quat& newOrientation);

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

        std::pair<TrueVolumeType, TrueVolume> getVolume() const {
            return { mType, mTrueVolume };
        }

        inline void setVolume(TrueVolumeType type, const TrueVolume& volume) {
            mType = type;
            mTrueVolume = volume;
            mPhysicsRecomputeRequired = true;
        }

        inline void setVolumeBox(const glm::vec3& boxDimensions) {
            assert(
                isNonNegative(boxDimensions) && isFinite(boxDimensions)
                && "Invalid box parameters provided"
            );
            setVolume(TrueVolumeType::BOX,
                TrueVolume { .mBox { .mDimensions { boxDimensions } } }
            );
        }

        inline void setVolumeCapsule(float radius, float height) {
            assert(
                isNonNegative(radius) && isNonNegative(height) && isFinite(radius) && isFinite(height)
                && "Invalid capsule parameters provided"
            );
            setVolume(TrueVolumeType::CAPSULE,
                TrueVolume {
                    .mCapsule { .mHeight { height }, .mRadius { radius } }
                }
            );
        }

        inline void setVolumeSphere(float radius) {
            assert(
                isNonNegative(radius) && isFinite(radius)
                && "Invalid sphere parameters provided"
            );
            setVolume(TrueVolumeType::SPHERE,
                TrueVolume {
                    .mSphere { .mRadius { radius } }
                }
            );
        }

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

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

        bool isSensible() const;

        bool isPositiveStrict() const;

        inline InteractionLayerMask getInteractionLayers() const { return mInteractionLayers; }

        inline void setInteractionLayers(InteractionLayerMask newLayers) { mInteractionLayers = newLayers; }

        inline InteractionLayerMask getInteractionMask() const { return mInteractionMask; }

        inline void setInteractionMask(InteractionLayerMask newMask) { mInteractionMask = newMask; }
    };

    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, InteractionLayerMask interactionLayers=0x0);

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

        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 getPositionWorld() 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 InteractionLayerMask getInteractionLayers() const { return mInteractionLayers; }

        inline void setInteractionLayers(InteractionLayerMask newLayers) { mInteractionLayers = newLayers; }

        inline void setPosition(const glm::vec3& position) {
            assert(isFinite(position) && "Invalid position specified. Position must be finite");
            const glm::vec3 deltaPosition { position - getPositionWorld() };
            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 { getPositionWorld() };
            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}};

        InteractionLayerMask mInteractionLayers { 0x0 };
    };

    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]
            }))
        };
        const InteractionLayerMask interactionLayers { json.at("interaction_layers").get<InteractionLayerMask>() };
        const InteractionLayerMask interactionMask { json.at("interaction_mask").get<InteractionLayerMask>() };
 
        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,
                    interactionLayers,
                    interactionMask
                );
                break;
 
            case ObjectBounds::TrueVolumeType::SPHERE:
                objectBounds = ObjectBounds::create(
                    VolumeSphere { .mRadius { json.at("volume_properties").at("radius").get<float>() } },
                    positionOffset,
                    orientationOffset,
                    interactionLayers,
                    interactionMask
                );
                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,
                    interactionLayers,
                    interactionMask
                );
                break;
        }
        objectBounds.mVolumeSystemComputed = false;
    }

    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