types.hpp

 
#ifndef TOYMAKERENGINE_PHYSICSTYPES_H
#define TOYMAKERENGINE_PHYSICSTYPES_H
 
#include <array>
#include <string>
#include <utility>
 
#include <glm/glm.hpp>
#include <nlohmann/json.hpp>
 
#include "../spatial_query/types.hpp"
 
namespace ToyMaker {
    struct PhysicsState;

    float computeGeneralizedInverseMassPositional(
        const ObjectBounds& object,
        const PhysicsState& physics,
        const glm::vec3& correctionPoint,
        const glm::vec3& correctionGradient
    );

    float computeGeneralizedInverseMassRotational(
        const ObjectBounds& object,
        const PhysicsState& physics,
        const glm::vec3& correctionRotational
    );

    glm::mat3 computeInertiaRotationalWorld(const glm::vec3& rotationalInertiaLocal, const glm::quat& orientation);

    ObjectBounds applyImpulseObject(
        ObjectBounds object,
        const PhysicsState& physics,
        const glm::vec3& impulsePositional,
        const glm::vec3& impulsePoint
    );

    ObjectBounds applyImpulseObject(
        ObjectBounds object,
        const PhysicsState& physics,
        const glm::vec3& impulseRotational
    );

    PhysicsState applyImpulsePhysics(
        const ObjectBounds& object,
        PhysicsState physics,
        const glm::vec3& impulsePositional,
        const glm::vec3& impulsePoint
    );

    PhysicsState applyImpulsePhysics(
        const ObjectBounds& object,
        PhysicsState physics,
        const glm::vec3& impulseRotational
    );

    struct PhysicsState {
        using Traits = uint8_t;

        enum Mode: Traits {
            MODE_DYNAMIC=0x0,

            MODE_KINEMATIC=0x1,

            MODE_STATIC=0x2,
        };

        static const Traits MaskMode;

        enum CollisionResponse: Traits {
            COLLISION_SEPARATE=0x4,

            COLLISION_SIGNAL=0x8
        };

        static const Traits MaskCollisionResponse;

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

        glm::vec3 mForce { 0.f };

        glm::vec3 mTorque { 0.f };

        glm::vec3 mVelocity { 0.f };

        glm::vec3 mAngularVelocity { 0.f };

        glm::vec3 mRotationalInertiaInverse { 1.f };

        float mMassInverse { 1.f };

        float mCoefficientFrictionStatic { 0.f };

        float mCoefficientFrictionDynamic { 0.f };

        float mCoefficientRestitution { .8f };

        Traits mTraits { static_cast<Traits>(COLLISION_SEPARATE) | static_cast<Traits>(MODE_DYNAMIC) };

        void applyForceGlobal(const glm::vec3& force, const glm::vec3& atPosition, const ObjectBounds& bounds);

        void applyForceLocal(const glm::vec3& force, const glm::vec3& atPosition, const ObjectBounds& bounds);

        inline float getMass() const {
            if(mMassInverse == 0.f) {
                return std::numeric_limits<float>::max();
            }
            return 1.f / mMassInverse;
        }

        inline void setMass(float mass) {
            assert(isNumber(mass) && isPositiveStrict(mass) && "Mass must be a valid positive number");
            if(mass == std::numeric_limits<float>::max()) {
                mMassInverse = 0.f;
                return;
            }
            mMassInverse = 1.f / mass;
        }

        inline glm::vec3 getRotationalInertia() const {
            return glm::vec3 {
                mRotationalInertiaInverse.x == 0.f? std::numeric_limits<float>::max(): 1.f / mRotationalInertiaInverse.x,
                mRotationalInertiaInverse.y == 0.f? std::numeric_limits<float>::max(): 1.f / mRotationalInertiaInverse.y,
                mRotationalInertiaInverse.z == 0.f? std::numeric_limits<float>::max(): 1.f / mRotationalInertiaInverse.z,
            };
        }

        inline void setRotationalInertia(const glm::vec3& rotationalInertia) {
            assert(
                isNumber(rotationalInertia) && isPositiveStrict(rotationalInertia)
                && "Rotational inertia must be valid positive number for each axis"
            );
            mRotationalInertiaInverse = glm::vec3 {
                rotationalInertia.x == std::numeric_limits<float>::max()? 0.f: 1.f / rotationalInertia.x,
                rotationalInertia.y == std::numeric_limits<float>::max()? 0.f: 1.f / rotationalInertia.y,
                rotationalInertia.z == std::numeric_limits<float>::max()? 0.f: 1.f / rotationalInertia.z,
            };
        }

        inline void setCoefficientRestitution(float newCoefficient) {
            assert(
                isNonNegative(newCoefficient) && newCoefficient <= 1.f
                && "Restitution coefficient must be non negative and cannot exceed 1"
            );
            mCoefficientRestitution = newCoefficient;
        }

        inline Mode getMode() const {
            switch(mTraits&MaskMode) {
                case MODE_DYNAMIC:
                    return MODE_DYNAMIC;
                case MODE_KINEMATIC:
                    return MODE_KINEMATIC;
                case MODE_STATIC:
                    return MODE_STATIC;
                default:
                    return MODE_STATIC;
                    assert(false && "Unrecognized physics type specified");
            }
        }

        inline void setMode(Mode mode) {
            switch(mode&MaskMode) {
                case MODE_DYNAMIC:
                case MODE_KINEMATIC:
                case MODE_STATIC:
                    mTraits = (mTraits&(~MaskMode)) | mode;
                    return;
                default:
                    assert(false && "Unrecognized physics type specified");
            }
        }

        inline bool separatesOnCollision() const {
            return mTraits&static_cast<Traits>(COLLISION_SEPARATE);
        }

        inline bool signalsOnCollision() const {
            return mTraits&static_cast<Traits>(COLLISION_SIGNAL);
        }

        inline void setCollisionResponse(CollisionResponse response) {
            mTraits |= response;
        }

        inline void unsetCollisionResponse(CollisionResponse response) {
            mTraits &= ~response;
        }
    };
 
    inline const PhysicsState::Traits PhysicsState::MaskMode { 0x3 };
    inline const PhysicsState::Traits PhysicsState::MaskCollisionResponse { 0xC };

    class BaseConstraint {
    private:
        float mCompliance { 0.f };
 
    protected:
        BaseConstraint(float compliance) { setCompliance(compliance); }
 
    public:
        using ParticipantID = std::size_t;
        using ParticipantTable = std::unordered_map<
            ParticipantID,
            std::pair<
                std::reference_wrapper<ObjectBounds>,
                std::reference_wrapper<PhysicsState>
            >
        >;

        float getCompliance() const;

        virtual void applyConstraintPosition(
            const ParticipantTable& states,
            float substepSeconds
        ) {}

        virtual void applyConstraintVelocity(
            const ParticipantTable& states,
            float substepSeconds
        ) {}

        virtual void resetLagrange() {}

        void setCompliance(float newCompliance);
 
        virtual ~BaseConstraint() {};
    };
 

    template<uint8_t LagrangeCount>
    class Constraint: public BaseConstraint {
    private:
        template<uint8_t... ints>
        void resetLagrange(std::integer_sequence<uint8_t, ints...> index);

        std::array<float, LagrangeCount> mLagrangeMultipliers { 0.f };

        std::array<float, LagrangeCount> mLagrangeDeltas { 0.f };
 
    protected:
        Constraint(float compliance): BaseConstraint { compliance } {}
 
    public:
        void applyLagrangeDelta(float delta, uint8_t index);

        const std::array<float, LagrangeCount>& getLagrange() const;

        const std::array<float, LagrangeCount>& getLagrangeDelta() const;

        void resetLagrange() override;
    };

    template <typename TParameter, uint8_t LagrangeCount>
    class ParametrizedConstraint: public Constraint<LagrangeCount> {
    private:
        std::unordered_map<BaseConstraint::ParticipantID, TParameter> mParameters {};
 
    protected:
        ParametrizedConstraint(float compliance): Constraint<LagrangeCount> { compliance } {}
 
    public:
        void setParameter(BaseConstraint::ParticipantID participant, const TParameter& parameter);

        inline TParameter getParameter(BaseConstraint::ParticipantID participant) const {
            return mParameters.at(participant);
        }

        void removeParameter(BaseConstraint::ParticipantID participant);

        const std::unordered_map<BaseConstraint::ParticipantID, TParameter>& getParameters() const;
    };

    struct CollisionConstraint: public Constraint<2> {
    public:
        bool mCollided { false };

        glm::vec3 mLastPointContactA { 0.f };

        glm::vec3 mCurrentPointContactA { 0.f };

        glm::vec3 mRelativePointContactA { 0.f };

        glm::vec3 mLastPointContactB { 0.f };

        glm::vec3 mCurrentPointContactB { 0.f };

        glm::vec3 mRelativePointContactB { 0.f };

        glm::vec3 mContactNormal { 0.f };

        float mCollisionVelocity { 0.f };

        float mPenetrationDepth { 0.f };

        CollisionConstraint();

        void updateCollisionData(
            const Collision& collision,
            const PhysicsState& physicsA,
            const ObjectBounds& boundsA,
            const ObjectBounds& boundsAPrev,
            const PhysicsState& physicsB,
            const ObjectBounds& boundsB,
            const ObjectBounds& boundsBPrev
        );

        void updateCollisionDataPartial(
            const PhysicsState& physicsA,
            const ObjectBounds& boundsA,
            const PhysicsState& physicsB,
            const ObjectBounds& boundsB
        );

        void applyConstraintPosition(
            const ParticipantTable& states,
            float substepSeconds
        ) override;

        void applyConstraintVelocity(
            const ParticipantTable& states,
            float substepSeconds
        ) override;
    };
 
    NLOHMANN_JSON_SERIALIZE_ENUM(PhysicsState::Mode, {
        { PhysicsState::MODE_DYNAMIC, "dynamic" },
        { PhysicsState::MODE_KINEMATIC, "kinematic" },
        { PhysicsState::MODE_STATIC, "static" },
    });
 
    NLOHMANN_JSON_SERIALIZE_ENUM(PhysicsState::CollisionResponse, {
        { PhysicsState::COLLISION_SEPARATE, "separate" },
        { PhysicsState::COLLISION_SIGNAL, "signal" },
    });
 
    inline void from_json(
        const nlohmann::json& json,
        PhysicsState& physics
    ) {
        assert(json.at("type") == PhysicsState::getComponentTypeName() && "Incorrect type property for an physics property component");
        physics = {};
 
        float mass;
        if(json.at("mass").is_string() && json.at("mass") == "infinity") {
            mass = std::numeric_limits<float>::max();
        } else {
            mass = json.at("mass");
        }
 
        physics.setMass(mass);
        physics.setMode(json.at("mode"));
 
        if(json.find("collision_response") != json.end()) {
            physics.mTraits &= ~PhysicsState::MaskCollisionResponse;
            for(const PhysicsState::CollisionResponse response: json.at("collision_response")) {
                physics.setCollisionResponse(response);
            }
        }
 
        if(json.find("velocity") != json.end()) {
            physics.mVelocity = glm::vec3 {
                json.at("velocity")[0],
                json.at("velocity")[1],
                json.at("velocity")[2],
            };
            assert(isNumber(physics.mVelocity) && isFinite(physics.mVelocity) && "Velocity must be sensible");
        }
 
        if(json.find("angular_velocity") != json.end()) {
            physics.mAngularVelocity = glm::vec3 {
                json.at("angular_velocity")[0],
                json.at("angular_velocity")[1],
                json.at("angular_velocity")[2],
            };
            assert(isNumber(physics.mAngularVelocity) && isFinite(physics.mAngularVelocity) && "Angular velocity must be sensible");
        }
 
        if(json.find("force") != json.end()) {
            physics.mForce = glm::vec3 {
                json.at("force")[0],
                json.at("force")[1],
                json.at("force")[2],
            };
            assert(isNumber(physics.mForce) && isFinite(physics.mForce) && "Force must be sensible");
        }
 
        if(json.find("torque") != json.end()) {
            physics.mForce = glm::vec3 {
                json.at("torque")[0],
                json.at("torque")[1],
                json.at("torque")[2],
            };
            assert(isNumber(physics.mTorque) && isFinite(physics.mTorque) && "Torque must be sensible");
        }
 
        if(json.find("coefficient_friction_static") != json.end()) {
            physics.mCoefficientFrictionStatic = json.at("coefficient_friction_static");
            assert(physics.mCoefficientFrictionStatic >= 0.f && "Coefficient friction must be non-negative");
        }
 
        if(json.find("coefficient_friction_dynamic") != json.end()) {
            physics.mCoefficientFrictionDynamic = json.at("coefficient_friction_dynamic");
            assert(physics.mCoefficientFrictionDynamic >= 0.f && "Coefficient friction must be non-negative");
        }
 
        if(json.find("coefficient_restitution") != json.end()) {
            physics.setCoefficientRestitution(json.at("coefficient_restitution"));
        }
    }
 
    inline void to_json(
        nlohmann::json& json,
        const PhysicsState& physics
    ) {
        const float mass { physics.getMass() };
        std::vector<PhysicsState::CollisionResponse> collisionResponse {};
        if(physics.mTraits&PhysicsState::COLLISION_SEPARATE) {
            collisionResponse.push_back(PhysicsState::COLLISION_SEPARATE);
        }
        if(physics.mTraits&PhysicsState::COLLISION_SIGNAL) {
            collisionResponse.push_back(PhysicsState::COLLISION_SIGNAL);
        }
        json = {
            { "type", PhysicsState::getComponentTypeName() },
            mass != std::numeric_limits<float>::max()?
                nlohmann::json::object({ "mass", mass }):
                nlohmann::json::object({ "mass", "infinity" }),
            { "mode", physics.getMode() },
            { "collision_response", collisionResponse },
            { "coefficient_friction_static", physics.mCoefficientFrictionStatic },
            { "coefficient_friction_dynamic", physics.mCoefficientFrictionDynamic },
            { "coefficient_restitution", physics.mCoefficientRestitution },
        };
    }
 
    template <uint8_t LagrangeCount>
    inline const std::array<float, LagrangeCount>& Constraint<LagrangeCount>::getLagrange() const {
        return mLagrangeMultipliers;
    }
 
    template <uint8_t LagrangeCount>
    inline const std::array<float, LagrangeCount>& Constraint<LagrangeCount>::getLagrangeDelta() const {
        return mLagrangeMultipliers;
    }
 
    template <uint8_t LagrangeCount>
    inline void Constraint<LagrangeCount>::applyLagrangeDelta(float delta, uint8_t index) {
        mLagrangeDeltas[index] = delta;
        mLagrangeMultipliers[index] += delta;
    }
 
    template <uint8_t LagrangeCount>
    inline void Constraint<LagrangeCount>::resetLagrange() {
        resetLagrange(std::make_integer_sequence<uint8_t, LagrangeCount>());
    }
 
    template<uint8_t LagrangeCount>
    template<uint8_t ...indices>
    inline void Constraint<LagrangeCount>::resetLagrange(std::integer_sequence<uint8_t, indices...> sequence) {
        ((mLagrangeMultipliers[indices] = mLagrangeDeltas[indices] = 0.f), ...);
    }
 
    template<typename TParameter, uint8_t LagrangeCount>
    inline void ParametrizedConstraint<TParameter, LagrangeCount>::setParameter(BaseConstraint::ParticipantID participant, const TParameter& parameter) {
        mParameters[participant] = parameter;
    }
 
    template<typename TParameter, uint8_t LagrangeCount>
    inline void ParametrizedConstraint<TParameter, LagrangeCount>::removeParameter(BaseConstraint::ParticipantID participant) {
        mParameters.erase(participant);
    }
 
    template <typename TParameter, uint8_t LagrangeCount>
    inline const std::unordered_map<BaseConstraint::ParticipantID, TParameter>& ParametrizedConstraint<TParameter, LagrangeCount>::getParameters() const {
        return mParameters;
    }
}
 
#endif