MotionProperties.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/Geometry/Sphere.h>
#include <Jolt/Physics/Body/AllowedDOFs.h>
#include <Jolt/Physics/Body/MotionQuality.h>
#include <Jolt/Physics/Body/BodyAccess.h>
#include <Jolt/Physics/Body/MotionType.h>
#include <Jolt/Physics/Body/BodyType.h>
#include <Jolt/Physics/Body/MassProperties.h>
#include <Jolt/Physics/DeterminismLog.h>
 
JPH_NAMESPACE_BEGIN
 
class StateRecorder;
 
enum class ECanSleep
{
    CannotSleep = 0,                                                                        
    CanSleep = 1,                                                                           
};
 
class JPH_EXPORT MotionProperties
{
public:
    JPH_OVERRIDE_NEW_DELETE
 
    EMotionQuality          GetMotionQuality() const                                        { return mMotionQuality; }
 
    inline EAllowedDOFs     GetAllowedDOFs() const                                          { return mAllowedDOFs; }
 
    inline bool             GetAllowSleeping() const                                        { return mAllowSleeping; }
 
    inline Vec3             GetLinearVelocity() const                                       { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; }
 
    void                    SetLinearVelocity(Vec3Arg inLinearVelocity)                     { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); }
 
    void                    SetLinearVelocityClamped(Vec3Arg inLinearVelocity)              { mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); }
 
    inline Vec3             GetAngularVelocity() const                                      { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; }
 
    void                    SetAngularVelocity(Vec3Arg inAngularVelocity)                   { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); }
 
    void                    SetAngularVelocityClamped(Vec3Arg inAngularVelocity)            { mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); }
 
    inline void             MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
 
 
    inline float            GetMaxLinearVelocity() const                                    { return mMaxLinearVelocity; }
    inline void             SetMaxLinearVelocity(float inLinearVelocity)                    { JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
 
    inline float            GetMaxAngularVelocity() const                                   { return mMaxAngularVelocity; }
    inline void             SetMaxAngularVelocity(float inAngularVelocity)                  { JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
 
    inline void             ClampLinearVelocity();
    inline void             ClampAngularVelocity();
 
    inline float            GetLinearDamping() const                                        { return mLinearDamping; }
    void                    SetLinearDamping(float inLinearDamping)                         { JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; }
 
    inline float            GetAngularDamping() const                                       { return mAngularDamping; }
    void                    SetAngularDamping(float inAngularDamping)                       { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; }
 
    inline float            GetGravityFactor() const                                        { return mGravityFactor; }
    void                    SetGravityFactor(float inGravityFactor)                         { mGravityFactor = inGravityFactor; }
 
    void                    SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties);
 
    inline float            GetInverseMass() const                                          { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
    inline float            GetInverseMassUnchecked() const                                 { return mInvMass; }
 
    void                    SetInverseMass(float inInverseMass)                             { mInvMass = inInverseMass; }
 
    inline Vec3             GetInverseInertiaDiagonal() const                               { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; }
 
    inline Quat             GetInertiaRotation() const                                      { return mInertiaRotation; }
 
    void                    SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot)            { mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
 
    void                    ScaleToMass(float inMass);
 
    inline Mat44            GetLocalSpaceInverseInertia() const;
 
    inline Mat44            GetLocalSpaceInverseInertiaUnchecked() const;
 
    inline Mat44            GetInverseInertiaForRotation(Mat44Arg inRotation) const;
 
    JPH_INLINE Vec3         MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const;
 
    JPH_INLINE Vec3         GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const         { return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); }
 
    // Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
    JPH_INLINE Vec3         GetAccumulatedForce() const                                     { return Vec3::sLoadFloat3Unsafe(mForce); }
 
    // Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
    JPH_INLINE Vec3         GetAccumulatedTorque() const                                    { return Vec3::sLoadFloat3Unsafe(mTorque); }
 
    // Reset the total accumulated force, note that this will be done automatically after every time step.
    JPH_INLINE void         ResetForce()                                                    { mForce = Float3(0, 0, 0); }
 
    // Reset the total accumulated torque, note that this will be done automatically after every time step.
    JPH_INLINE void         ResetTorque()                                                   { mTorque = Float3(0, 0, 0); }
 
    // Reset the current velocity and accumulated force and torque.
    JPH_INLINE void         ResetMotion()
    {
        JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
        mLinearVelocity = mAngularVelocity = Vec3::sZero();
        mForce = mTorque = Float3(0, 0, 0);
    }
 
    JPH_INLINE UVec4        GetLinearDOFsMask() const
    {
        UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0);
        return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
    }
 
    JPH_INLINE Vec3         LockTranslation(Vec3Arg inV) const
    {
        return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat()));
    }
 
    JPH_INLINE UVec4        GetAngularDOFsMask() const
    {
        UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0);
        return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
    }
 
    JPH_INLINE Vec3         LockAngular(Vec3Arg inV) const
    {
        return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat()));
    }
 
    void                    SetNumVelocityStepsOverride(uint inN)                           { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
    uint                    GetNumVelocityStepsOverride() const                             { return mNumVelocityStepsOverride; }
 
    void                    SetNumPositionStepsOverride(uint inN)                           { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
    uint                    GetNumPositionStepsOverride() const                             { return mNumPositionStepsOverride; }
 
#ifdef JPH_TRACK_SIMULATION_STATS
    struct SimulationStats
    {
        void                Reset()                                                         { mBroadPhaseTicks = 0; mNarrowPhaseTicks.store(0, memory_order_relaxed); mVelocityConstraintTicks = 0; mPositionConstraintTicks = 0; mUpdateBoundsTicks = 0; mCCDTicks.store(0, memory_order_relaxed); mNumContactConstraints.store(0, memory_order_relaxed); mNumCollisionSteps = 0; mNumVelocitySteps = 0; mNumPositionSteps = 0; mIsLargeIsland = false; }
 
        uint64              mBroadPhaseTicks = 0;                                           
        atomic<uint64>      mNarrowPhaseTicks = 0;                                          
        uint64              mVelocityConstraintTicks = 0;                                   
        uint64              mPositionConstraintTicks = 0;                                   
        uint64              mUpdateBoundsTicks = 0;                                         
        atomic<uint64>      mCCDTicks = 0;                                                  
        atomic<uint32>      mNumContactConstraints = 0;                                     
        uint8               mNumCollisionSteps = 0;                                         
        uint8               mNumVelocitySteps = 0;                                          
        uint8               mNumPositionSteps = 0;                                          
        bool                mIsLargeIsland = false;                                         
    };
 
    const SimulationStats & GetSimulationStats() const                                      { return mSimulationStats; }
    SimulationStats &       GetSimulationStats()                                            { return mSimulationStats; }
#endif // JPH_TRACK_SIMULATION_STATS
 
    // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
 
    inline void             ApplyLinearVelocityStep(Vec3Arg inLinearVelocity)
    {
        JPH_DET_LOG("ApplyLinearVelocityStep: " << inLinearVelocity);
        JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
        JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
        JPH_ASSERT(!inLinearVelocity.IsNaN());
        mLinearVelocity = LockTranslation(inLinearVelocity);
    }
    inline void             AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)           { ApplyLinearVelocityStep(mLinearVelocity + inLinearVelocityChange); }
    inline void             SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)           { ApplyLinearVelocityStep(mLinearVelocity - inLinearVelocityChange); }
 
    inline void             ApplyAngularVelocityStep(Vec3Arg inAngularVelocity)
    {
        JPH_DET_LOG("ApplyAngularVelocityStep: " << inAngularVelocity);
        JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
        JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
        JPH_ASSERT(!inAngularVelocity.IsNaN());
        mAngularVelocity = inAngularVelocity;
    }
    inline void             AddAngularVelocityStep(Vec3Arg inAngularVelocityChange)         { ApplyAngularVelocityStep(mAngularVelocity + inAngularVelocityChange); }
    inline void             SubAngularVelocityStep(Vec3Arg inAngularVelocityChange)         { ApplyAngularVelocityStep(mAngularVelocity - inAngularVelocityChange); }
 
    inline void             ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime);
 
    inline void             ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
 
    uint32                  GetIslandIndexInternal() const                                  { return mIslandIndex; }
    void                    SetIslandIndexInternal(uint32 inIndex)                          { mIslandIndex = inIndex; }
 
    uint32                  GetIndexInActiveBodiesInternal() const                          { return mIndexInActiveBodies; }
 
#ifdef JPH_DOUBLE_PRECISION
    inline DVec3            GetSleepTestOffset() const                                      { return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); }
#endif // JPH_DOUBLE_PRECISION
 
    inline void             ResetSleepTestSpheres(const RVec3 *inPoints);
 
    inline void             ResetSleepTestTimer()                                           { mSleepTestTimer = 0.0f; }
 
    inline ECanSleep        AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep);
 
    void                    SaveState(StateRecorder &inStream) const;
 
    void                    RestoreState(StateRecorder &inStream);
 
    static constexpr uint32 cInactiveIndex = uint32(-1);                                    
 
private:
    friend class BodyManager;
    friend class Body;
 
    // 1st cache line
    // 16 byte aligned
    Vec3                    mLinearVelocity { Vec3::sZero() };                              
    Vec3                    mAngularVelocity { Vec3::sZero() };                             
    Vec3                    mInvInertiaDiagonal;                                            
    Quat                    mInertiaRotation;                                               
 
    // 2nd cache line
    // 4 byte aligned
    Float3                  mForce { 0, 0, 0 };                                             
    Float3                  mTorque { 0, 0, 0 };                                            
    float                   mInvMass;                                                       
    float                   mLinearDamping;                                                 
    float                   mAngularDamping;                                                
    float                   mMaxLinearVelocity;                                             
    float                   mMaxAngularVelocity;                                            
    float                   mGravityFactor;                                                 
    uint32                  mIndexInActiveBodies = cInactiveIndex;                          
    uint32                  mIslandIndex = cInactiveIndex;                                  
 
    // 1 byte aligned
    EMotionQuality          mMotionQuality;                                                 
    bool                    mAllowSleeping;                                                 
    EAllowedDOFs            mAllowedDOFs = EAllowedDOFs::All;                               
    uint8                   mNumVelocityStepsOverride = 0;                                  
    uint8                   mNumPositionStepsOverride = 0;                                  
 
    // 3rd cache line (least frequently used)
    // 4 byte aligned (or 8 byte if running in double precision)
#ifdef JPH_DOUBLE_PRECISION
    Double3                 mSleepTestOffset;                                               
#endif // JPH_DOUBLE_PRECISION
    Sphere                  mSleepTestSpheres[3];                                           
    float                   mSleepTestTimer;                                                
 
#ifdef JPH_ENABLE_ASSERTS
    EBodyType               mCachedBodyType;                                                
    EMotionType             mCachedMotionType;                                              
#endif
 
#ifdef JPH_TRACK_SIMULATION_STATS
    SimulationStats         mSimulationStats;
#endif // JPH_TRACK_SIMULATION_STATS
};
 
JPH_NAMESPACE_END
 
#include "MotionProperties.inl"