SwingTwistConstraint.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/AngleConstraintPart.h>
#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
 
JPH_NAMESPACE_BEGIN
 
class SwingTwistConstraintSettings final : public TwoBodyConstraintSettings
{
public:
    JPH_DECLARE_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings)
 
    // 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                        mTwistAxis1 = Vec3::sAxisX();
    Vec3                        mPlaneAxis1 = Vec3::sAxisY();
 
    RVec3                       mPosition2 = RVec3::sZero();
    Vec3                        mTwistAxis2 = Vec3::sAxisX();
    Vec3                        mPlaneAxis2 = Vec3::sAxisY();
    
    float                       mNormalHalfConeAngle = 0.0f;                                
    float                       mPlaneHalfConeAngle = 0.0f;                                 
 
    float                       mTwistMinAngle = 0.0f;                                      
    float                       mTwistMaxAngle = 0.0f;                                      
 
    float                       mMaxFrictionTorque = 0.0f;                                  
 
    MotorSettings               mSwingMotorSettings;
    MotorSettings               mTwistMotorSettings;
 
protected:
    // See: ConstraintSettings::RestoreBinaryState
    virtual void                RestoreBinaryState(StreamIn &inStream) override;
};
 
class SwingTwistConstraint final : public TwoBodyConstraint
{
public:
    JPH_OVERRIDE_NEW_DELETE
 
                                SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings);
 
    virtual EConstraintSubType  GetSubType() const override                                 { return EConstraintSubType::SwingTwist; }
    virtual void                NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
    virtual void                SetupVelocityConstraint(float inDeltaTime) 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); }
 
    inline Vec3                 GetLocalSpacePosition1() const                              { return mLocalSpacePosition1; }
    inline Vec3                 GetLocalSpacePosition2() const                              { return mLocalSpacePosition2; }
    inline Quat                 GetConstraintToBody1() const                                { return mConstraintToBody1; }
    inline Quat                 GetConstraintToBody2() const                                { return mConstraintToBody2; }
 
    inline float                GetNormalHalfConeAngle() const                              { return mNormalHalfConeAngle; }
    inline void                 SetNormalHalfConeAngle(float inAngle)                       { mNormalHalfConeAngle = inAngle; UpdateLimits(); }
    inline float                GetPlaneHalfConeAngle() const                               { return mPlaneHalfConeAngle; }
    inline void                 SetPlaneHalfConeAngle(float inAngle)                        { mPlaneHalfConeAngle = inAngle; UpdateLimits(); }
    inline float                GetTwistMinAngle() const                                    { return mTwistMinAngle; }
    inline void                 SetTwistMinAngle(float inAngle)                             { mTwistMinAngle = inAngle; UpdateLimits(); }
    inline float                GetTwistMaxAngle() const                                    { return mTwistMaxAngle; }
    inline void                 SetTwistMaxAngle(float inAngle)                             { mTwistMaxAngle = inAngle; UpdateLimits(); }
 
    const MotorSettings &       GetSwingMotorSettings() const                               { return mSwingMotorSettings; }
    MotorSettings &             GetSwingMotorSettings()                                     { return mSwingMotorSettings; }
    const MotorSettings &       GetTwistMotorSettings() const                               { return mTwistMotorSettings; }
    MotorSettings &             GetTwistMotorSettings()                                     { return mTwistMotorSettings; }
 
    void                        SetMaxFrictionTorque(float inFrictionTorque)                { mMaxFrictionTorque = inFrictionTorque; }
    float                       GetMaxFrictionTorque() const                                { return mMaxFrictionTorque; }
 
 
    void                        SetSwingMotorState(EMotorState inState);
    EMotorState                 GetSwingMotorState() const                                  { return mSwingMotorState; }
    void                        SetTwistMotorState(EMotorState inState);
    EMotorState                 GetTwistMotorState() const                                  { return mTwistMotorState; }
 
    void                        SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)       { mTargetAngularVelocity = inAngularVelocity; }
    Vec3                        GetTargetAngularVelocityCS() const                          { return mTargetAngularVelocity; }
 
    void                        SetTargetOrientationCS(QuatArg inOrientation);
    Quat                        GetTargetOrientationCS() const                              { return mTargetOrientation; }
 
    void                        SetTargetOrientationBS(QuatArg inOrientation)               { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
 
    inline Quat                 GetRotationInConstraintSpace() const;
 
    inline Vec3                 GetTotalLambdaPosition() const                              { return mPointConstraintPart.GetTotalLambda(); }
    inline float                GetTotalLambdaTwist() const                                 { return mSwingTwistConstraintPart.GetTotalTwistLambda(); }
    inline float                GetTotalLambdaSwingY() const                                { return mSwingTwistConstraintPart.GetTotalSwingYLambda(); }
    inline float                GetTotalLambdaSwingZ() const                                { return mSwingTwistConstraintPart.GetTotalSwingZLambda(); }
    inline Vec3                 GetTotalLambdaMotor() const                                 { return Vec3(mMotorConstraintPart[0].GetTotalLambda(), mMotorConstraintPart[1].GetTotalLambda(), mMotorConstraintPart[2].GetTotalLambda()); }
 
private:
    // Update the limits in the swing twist constraint part
    void                        UpdateLimits();
 
    // CONFIGURATION PROPERTIES FOLLOW
 
    // Local space constraint positions
    Vec3                        mLocalSpacePosition1;
    Vec3                        mLocalSpacePosition2;
    
    // Transforms from constraint space to body space
    Quat                        mConstraintToBody1;
    Quat                        mConstraintToBody2;
 
    // Limits
    float                       mNormalHalfConeAngle;
    float                       mPlaneHalfConeAngle;
    float                       mTwistMinAngle;
    float                       mTwistMaxAngle;
 
    // Friction
    float                       mMaxFrictionTorque;
 
    // Motor controls
    MotorSettings               mSwingMotorSettings;
    MotorSettings               mTwistMotorSettings;
    EMotorState                 mSwingMotorState = EMotorState::Off;
    EMotorState                 mTwistMotorState = EMotorState::Off;
    Vec3                        mTargetAngularVelocity = Vec3::sZero();
    Quat                        mTargetOrientation = Quat::sIdentity();
 
    // RUN TIME PROPERTIES FOLLOW
 
    // Rotation axis for motor constraint parts
    Vec3                        mWorldSpaceMotorAxis[3];
 
    // The constraint parts
    PointConstraintPart         mPointConstraintPart;
    SwingTwistConstraintPart    mSwingTwistConstraintPart;
    AngleConstraintPart         mMotorConstraintPart[3];
};
 
JPH_NAMESPACE_END