SixDOFConstraint.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
#include <Jolt/Physics/Constraints/MotorSettings.h>
#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
 
JPH_NAMESPACE_BEGIN
 
class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings
{
public:
    JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings)
 
    
    enum EAxis
    {
        TranslationX,
        TranslationY,
        TranslationZ,
 
        RotationX,
        RotationY,
        RotationZ,
 
        Num,
        NumTranslation = TranslationZ + 1,
    };
 
    // See: ConstraintSettings::SaveBinaryState
    virtual void                SaveBinaryState(StreamOut &inStream) const override;
 
    virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override;
 
    EConstraintSpace            mSpace = EConstraintSpace::WorldSpace;
 
    RVec3                       mPosition1 = RVec3::sZero();
    Vec3                        mAxisX1 = Vec3::sAxisX();
    Vec3                        mAxisY1 = Vec3::sAxisY();
 
    RVec3                       mPosition2 = RVec3::sZero();
    Vec3                        mAxisX2 = Vec3::sAxisX();
    Vec3                        mAxisY2 = Vec3::sAxisY();
 
    float                       mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
 
    ESwingType                  mSwingType = ESwingType::Cone;
 
    float                       mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
    float                       mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
 
    SpringSettings              mLimitsSpringSettings[EAxis::NumTranslation];
 
    void                        MakeFreeAxis(EAxis inAxis)                                  { mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
    bool                        IsFreeAxis(EAxis inAxis) const                              { return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
 
    void                        MakeFixedAxis(EAxis inAxis)                                 { mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; }
    bool                        IsFixedAxis(EAxis inAxis) const                             { return mLimitMin[inAxis] >= mLimitMax[inAxis]; }
 
    void                        SetLimitedAxis(EAxis inAxis, float inMin, float inMax)      { mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
 
    MotorSettings               mMotorSettings[EAxis::Num];
 
protected:
    // See: ConstraintSettings::RestoreBinaryState
    virtual void                RestoreBinaryState(StreamIn &inStream) override;
};
 
class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint
{
public:
    JPH_OVERRIDE_NEW_DELETE
 
    using EAxis = SixDOFConstraintSettings::EAxis;
 
                                SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings);
 
    virtual EConstraintSubType  GetSubType() const override                                 { return EConstraintSubType::SixDOF; }
    virtual void                NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
    virtual void                SetupVelocityConstraint(float inDeltaTime) override;
    virtual void                ResetWarmStart() override;
    virtual void                WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
    virtual bool                SolveVelocityConstraint(float inDeltaTime) override;
    virtual bool                SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
#ifdef JPH_DEBUG_RENDERER
    virtual void                DrawConstraint(DebugRenderer *inRenderer) const override;
    virtual void                DrawConstraintLimits(DebugRenderer *inRenderer) const override;
#endif // JPH_DEBUG_RENDERER
    virtual void                SaveState(StateRecorder &inStream) const override;
    virtual void                RestoreState(StateRecorder &inStream) override;
    virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
 
    // See: TwoBodyConstraint
    virtual Mat44               GetConstraintToBody1Matrix() const override                 { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
    virtual Mat44               GetConstraintToBody2Matrix() const override                 { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
 
    void                        SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
 
    void                        SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
 
    float                       GetLimitsMin(EAxis inAxis) const                            { return mLimitMin[inAxis]; }
    float                       GetLimitsMax(EAxis inAxis) const                            { return mLimitMax[inAxis]; }
    Vec3                        GetTranslationLimitsMin() const                             { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::TranslationX])); }
    Vec3                        GetTranslationLimitsMax() const                             { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::TranslationX])); }
    Vec3                        GetRotationLimitsMin() const                                { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::RotationX])); }
    Vec3                        GetRotationLimitsMax() const                                { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::RotationX])); }
 
    inline bool                 IsFixedAxis(EAxis inAxis) const                             { return (mFixedAxis & (1 << inAxis)) != 0; }
    inline bool                 IsFreeAxis(EAxis inAxis) const                              { return (mFreeAxis & (1 << inAxis)) != 0; }
 
    const SpringSettings &      GetLimitsSpringSettings(EAxis inAxis) const                 { JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; }
    void                        SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); }
 
    void                        SetMaxFriction(EAxis inAxis, float inFriction);
    float                       GetMaxFriction(EAxis inAxis) const                          { return mMaxFriction[inAxis]; }
 
    Quat                        GetRotationInConstraintSpace() const;
 
    MotorSettings &             GetMotorSettings(EAxis inAxis)                              { return mMotorSettings[inAxis]; }
    const MotorSettings &       GetMotorSettings(EAxis inAxis) const                        { return mMotorSettings[inAxis]; }
 
    void                        SetMotorState(EAxis inAxis, EMotorState inState);
    EMotorState                 GetMotorState(EAxis inAxis) const                           { return mMotorState[inAxis]; }
 
    Vec3                        GetTargetVelocityCS() const                                 { return mTargetVelocity; }
    void                        SetTargetVelocityCS(Vec3Arg inVelocity)                     { mTargetVelocity = inVelocity; }
 
    void                        SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)       { mTargetAngularVelocity = inAngularVelocity; }
    Vec3                        GetTargetAngularVelocityCS() const                          { return mTargetAngularVelocity; }
 
    Vec3                        GetTargetPositionCS() const                                 { return mTargetPosition; }
    void                        SetTargetPositionCS(Vec3Arg inPosition)                     { mTargetPosition = inPosition; }
 
    void                        SetTargetOrientationCS(QuatArg inOrientation);
    Quat                        GetTargetOrientationCS() const                              { return mTargetOrientation; }
 
    void                        SetTargetOrientationBS(QuatArg inOrientation)               { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
 
    inline Vec3                 GetTotalLambdaPosition() const                              { return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); }
    inline Vec3                 GetTotalLambdaRotation() const                              { return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); }
    inline Vec3                 GetTotalLambdaMotorTranslation() const                      { return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); }
    inline Vec3                 GetTotalLambdaMotorRotation() const                         { return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); }
 
private:
    // Calculate properties needed for the position constraint
    inline void                 GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const;
 
    // Sanitize the translation limits
    inline void                 UpdateTranslationLimits();
 
    // Propagate the rotation limits to the constraint part
    inline void                 UpdateRotationLimits();
 
    // Update the cached state of which axis are free and which ones are fixed
    inline void                 UpdateFixedFreeAxis();
 
    // Cache the state of mTranslationMotorActive
    void                        CacheTranslationMotorActive();
 
    // Cache the state of mRotationMotorActive
    void                        CacheRotationMotorActive();
 
    // Cache the state of mRotationPositionMotorActive
    void                        CacheRotationPositionMotorActive();
 
    void                        CacheHasSpringLimits();
 
    // Constraint settings helper functions
    inline bool                 IsTranslationConstrained() const                            { return (mFreeAxis & 0b111) != 0b111; }
    inline bool                 IsTranslationFullyConstrained() const                       { return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; }
    inline bool                 IsRotationConstrained() const                               { return (mFreeAxis & 0b111000) != 0b111000; }
    inline bool                 IsRotationFullyConstrained() const                          { return (mFixedAxis & 0b111000) == 0b111000; }
    inline bool                 HasFriction(EAxis inAxis) const                             { return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
 
    // CONFIGURATION PROPERTIES FOLLOW
 
    // Local space constraint positions
    Vec3                        mLocalSpacePosition1;
    Vec3                        mLocalSpacePosition2;
 
    // Transforms from constraint space to body space
    Quat                        mConstraintToBody1;
    Quat                        mConstraintToBody2;
 
    // Limits
    uint8                       mFreeAxis = 0;                                              // Bitmask of free axis (bit 0 = TranslationX)
    uint8                       mFixedAxis = 0;                                             // Bitmask of fixed axis (bit 0 = TranslationX)
    bool                        mTranslationMotorActive = false;                            // If any of the translational frictions / motors are active
    bool                        mRotationMotorActive = false;                               // If any of the rotational frictions / motors are active
    uint8                       mRotationPositionMotorActive = 0;                           // Bitmask of axis that have position motor active (bit 0 = RotationX)
    bool                        mHasSpringLimits = false;                                   // If any of the limit springs have a non-zero frequency/stiffness
    float                       mLimitMin[EAxis::Num];
    float                       mLimitMax[EAxis::Num];
    SpringSettings              mLimitsSpringSettings[EAxis::NumTranslation];
 
    // Motor settings for each axis
    MotorSettings               mMotorSettings[EAxis::Num];
 
    // Friction settings for each axis
    float                       mMaxFriction[EAxis::Num];
 
    // Motor controls
    EMotorState                 mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off };
    Vec3                        mTargetVelocity = Vec3::sZero();
    Vec3                        mTargetAngularVelocity = Vec3::sZero();
    Vec3                        mTargetPosition = Vec3::sZero();
    Quat                        mTargetOrientation = Quat::sIdentity();
 
    // RUN TIME PROPERTIES FOLLOW
 
    // Constraint space axis in world space
    Vec3                        mTranslationAxis[3];
    Vec3                        mRotationAxis[3];
 
    // Translation displacement (valid when translation axis has a range limit)
    float                       mDisplacement[3];
 
    // Individual constraint parts for translation, or a combined point constraint part if all axis are fixed
    AxisConstraintPart          mTranslationConstraintPart[3];
    PointConstraintPart         mPointConstraintPart;
 
    // Individual constraint parts for rotation or a combined constraint part if rotation is fixed
    SwingTwistConstraintPart    mSwingTwistConstraintPart;
    RotationEulerConstraintPart mRotationConstraintPart;
 
    // Motor or friction constraints
    AxisConstraintPart          mMotorTranslationConstraintPart[3];
    AngleConstraintPart         mMotorRotationConstraintPart[3];
};
 
JPH_NAMESPACE_END