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

        float mVelocityBleed { 0.01f };

        float mVelocityBleedAngular { 0.01f };

        float mVelocityCutoff { 0.001f };

        float mVelocityCutoffAngular { 0.0005f };

        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 };
 
    template<uint8_t LagrangeCount>
    class Constraint;
 
    template <typename TConfig, typename TParameter, uint8_t LagrangeCount>
    class ConstraintParametrized;

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

        template <typename TConstraint,
            std::enable_if_t<
                std::is_base_of<
                    ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                    TConstraint
                >::value, bool
            > = true
        >
        void setParameter(ParticipantID participant, const typename TConstraint::Parameter& parameter);

        template <typename TConstraint,
            std::enable_if_t<
                std::is_base_of<
                    ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                    TConstraint
                >::value, bool
            > = true
        >
        typename TConstraint::Parameter getParameter(ParticipantID participant) const;

        template <typename TConstraint,
 
            std::enable_if_t<
                std::is_base_of<
                    ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                    TConstraint
                >::value, bool
            > = true
        >
        void setConfig(const typename TConstraint::Config& config);

        template <typename TConstraint,
            std::enable_if_t<
                std::is_base_of<
                    ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                    TConstraint
                >::value, bool
            > = true
        >
        typename TConstraint::Config getConfig() const;
 
        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 TConfig, typename TParameter, uint8_t LagrangeCount>
    class ConstraintParametrized: public Constraint<LagrangeCount> {
    private:
        std::unordered_map<BaseConstraint::ParticipantID, TParameter> mParameters {};

        TConfig mConfig {};
 
    public:
        using Config = TConfig;
        using Parameter = TParameter;
        static const uint8_t NLagrange = LagrangeCount;

        explicit ConstraintParametrized(
            const TConfig& config,
            const std::vector<TParameter>& constraintParameters,
            float compliance
        );

        void setConfig(const TConfig& config);

        TConfig getConfig() const;

        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 ConstraintContact: public Constraint<2> {
    public:
        bool mCollided { false };

        glm::vec3 mPreviousA { 0.f };

        glm::vec3 mCurrentA { 0.f };

        glm::vec3 mRelativeA { 0.f };

        glm::vec3 mPreviousB { 0.f };

        glm::vec3 mCurrentB { 0.f };

        glm::vec3 mRelativeB { 0.f };

        glm::vec3 mContactNormal { 0.f };

        float mCollisionVelocity { 0.f };

        float mPenetration { 0.f };

        ConstraintContact();

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

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

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

    class ConstraintContactManifold: public BaseConstraint {
    public:
        ConstraintContactManifold(): BaseConstraint{ 0.f } {}

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

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

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

        void resetLagrange() override;

        inline uint8_t getNContacts() const { return mNContacts; }

        inline void clear() { mNContacts = 0; }
 
    private:
        uint8_t mNContacts { 0 };

        std::array<ConstraintContact, 5> mContacts {};

        void trim(const ObjectBounds& boundsA, const ObjectBounds& boundsB);
    };

    class ConstraintDampingRigidbody: public ConstraintParametrized<float, PhysicsState, 1> {
    public:
        using ConstraintParametrized<float, PhysicsState, 1>::ConstraintParametrized;

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

    struct Constraint1DOFConfig {
        glm::vec3 mAxis { 1.f, 0.f, 0.f };

        float mBoundLower { 0.f };

        float mBoundUpper { 0.f };

        bool isActive { true };

        inline bool isSensible(bool isRotation=false) const {
            return (
                isFinite(mAxis)
                && isNumber(mAxis)
                && squareDistance(mAxis) != 0.f
                && isFinite(mBoundUpper) && isFinite(mBoundLower)
                && isNumber(mBoundUpper) && isNumber(mBoundLower)
                && mBoundUpper >= mBoundLower
                && (!isRotation || (
                    mBoundLower < glm::pi<float>() && mBoundUpper < glm::pi<float>()
                    && mBoundLower > -glm::pi<float>() && mBoundUpper > -glm::pi<float>()
                ))
            );
        }
    };

    struct Constraint1DOFParam {
        glm::quat mRotateToLocal { 1.f, 0.f, 0.f, 0.f };

        glm::vec3 mVector { 0.f, 0.f, 1.f };
 
        inline bool isSensible(bool isRotation=false) const {
            return (
                isFinite(mVector) && isNumber(mVector)
                && glm::length(mRotateToLocal) == 1.f
            );
        }
    };
 

    class ConstraintRotation1D: public ConstraintParametrized<Constraint1DOFConfig, Constraint1DOFParam, 1> {
    public:
        using ConstraintParametrized<Constraint1DOFConfig, Constraint1DOFParam, 1>::ConstraintParametrized;
        void applyConstraintPosition(const ParticipantTable& states, float substepSeconds);
    };

    class ConstraintDistance1D: public ConstraintParametrized<Constraint1DOFConfig, Constraint1DOFParam, 1> {
    public:
        using ConstraintParametrized<Constraint1DOFConfig, Constraint1DOFParam, 1>::ConstraintParametrized;
        void applyConstraintPosition(const ParticipantTable& states, float substepSeconds);
    };
 
    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("velocity_bleed") != json.end()) {
            physics.mVelocityBleed = json.at("velocity_bleed");
            assert(
                isNumber(physics.mVelocityBleed)
                && physics.mVelocityBleed >= 0.f
                && physics.mVelocityBleed <= 1.f
                &&"Velocity bleed must be a finite positive number in range [0, 1]."
            );
        }
        if(json.find("velocity_cutoff") != json.end()) {
            physics.mVelocityCutoff = json.at("velocity_cutoff");
            assert(
                isNumber(physics.mVelocityCutoff)
                && isFinite(physics.mVelocityCutoff)
                && isNonNegative(physics.mVelocityCutoff)
                && "Velocity cutoff must be a finite non-negative number"
            );
        }
 
        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("angular_velocity_bleed") != json.end()) {
            physics.mVelocityBleedAngular = json.at("angular_velocity_bleed");
            assert(
                isNumber(physics.mVelocityBleedAngular)
                && physics.mVelocityBleedAngular >= 0.f
                && physics.mVelocityBleedAngular <= 1.f
                && "Velocity bleed must be a finite positive number in range [0, 1]."
            );
        }
        if(json.find("angular_velocity_cutoff") != json.end()) {
            physics.mVelocityCutoffAngular = json.at("angular_velocity_cutoff");
            assert(
                isNumber(physics.mVelocityCutoffAngular)
                && isFinite(physics.mVelocityCutoffAngular)
                && isNonNegative(physics.mVelocityCutoffAngular)
                && "Velocity cutoff must be a finite positive number"
            );
        }
 
        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 },
            { "velocity_bleed", physics.mVelocityBleed },
            { "angular_velocity_bleed", physics.mVelocityBleedAngular },
            { "velocity_cutoff", physics.mVelocityCutoff },
            { "velocity_cutoff_angular", physics.mVelocityCutoffAngular },
        };
    }
 
    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 TConfig, typename TParameter, uint8_t LagrangeCount>
    inline void ConstraintParametrized<TConfig, TParameter, LagrangeCount>::setParameter(BaseConstraint::ParticipantID participant, const TParameter& parameter) {
        mParameters[participant] = parameter;
    }
 
    template <typename TConstraint,
        std::enable_if_t<
            std::is_base_of<
                ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                TConstraint
            >::value, bool
        >
    >
    inline void BaseConstraint::setParameter(ParticipantID participant, const typename TConstraint::Parameter& parameter) {
        static_cast<TConstraint&>(*this).setParameter(participant, parameter);
    }
 
    template <typename TConstraint,
        std::enable_if_t<
            std::is_base_of<
                ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                TConstraint
            >::value, bool
        >
    >
    inline typename TConstraint::Parameter BaseConstraint::getParameter(ParticipantID participant) const {
        return static_cast<TConstraint&>(*this).getParameter(participant);
    }
 
 
    template<typename TConfig, typename TParameter, uint8_t LagrangeCount>
    inline void ConstraintParametrized<TConfig, TParameter, LagrangeCount>::setConfig(const TConfig& config) {
        mConfig = config;
    }
 
 
    template<typename TConfig, typename TParameter, uint8_t LagrangeCount>
    inline TConfig ConstraintParametrized<TConfig, TParameter, LagrangeCount>::getConfig() const {
        return mConfig;
    }
 
    template <typename TConstraint,
        std::enable_if_t<
            std::is_base_of<
                ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                TConstraint
            >::value, bool
        >
    >
    inline void BaseConstraint::setConfig(const typename TConstraint::Config& config) {
        static_cast<TConstraint&>(*this).setConfig(config);
    }
 
    template <typename TConstraint,
        std::enable_if_t<
            std::is_base_of<
                ConstraintParametrized<typename TConstraint::Config, typename TConstraint::Parameter, TConstraint::NLagrange>,
                TConstraint
            >::value, bool
        >
    >
    inline typename TConstraint::Config BaseConstraint::getConfig() const {
        return static_cast<TConstraint&>(*this).getConfig();
    }
 
    template<typename TConfig, typename TParameter, uint8_t LagrangeCount>
    inline void ConstraintParametrized<TConfig, TParameter, LagrangeCount>::removeParameter(BaseConstraint::ParticipantID participant) {
        mParameters.erase(participant);
    }
 
    template <typename TConfig, typename TParameter, uint8_t LagrangeCount>
    inline const std::unordered_map<BaseConstraint::ParticipantID, TParameter>& ConstraintParametrized<TConfig, TParameter, LagrangeCount>::getParameters() const {
        return mParameters;
    }
 
    template <typename TConfig, typename TParameter, uint8_t LagrangeCount>
    inline ConstraintParametrized<TConfig, TParameter, LagrangeCount>::ConstraintParametrized(const TConfig& config, const std::vector<TParameter>& constraintParameters, float compliance): Constraint<LagrangeCount> { compliance }, mConfig { config } {
        for(auto i { 0 }; i < constraintParameters.size(); ++i) {
            setParameter(i, constraintParameters[i]);
        }
    }
}
 
#endif