Body.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/Core/NonCopyable.h>
#include <Jolt/Geometry/AABox.h>
#include <Jolt/Physics/Collision/Shape/Shape.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
#include <Jolt/Physics/Collision/ObjectLayer.h>
#include <Jolt/Physics/Collision/CollisionGroup.h>
#include <Jolt/Physics/Collision/TransformedShape.h>
#include <Jolt/Physics/Body/MotionProperties.h>
#include <Jolt/Physics/Body/BodyID.h>
#include <Jolt/Physics/Body/BodyAccess.h>
#include <Jolt/Physics/Body/BodyType.h>
#include <Jolt/Core/StringTools.h>
 
JPH_NAMESPACE_BEGIN
 
class StateRecorder;
class BodyCreationSettings;
class SoftBodyCreationSettings;
 
class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public NonCopyable
{
public:
    JPH_OVERRIDE_NEW_DELETE
 
    inline const BodyID &   GetID() const                                                   { return mID; }
 
    inline EBodyType        GetBodyType() const                                             { return mBodyType; }
 
    inline bool             IsRigidBody() const                                             { return mBodyType == EBodyType::RigidBody; }
 
    inline bool             IsSoftBody() const                                              { return mBodyType == EBodyType::SoftBody; }
 
    // See comment at GetIndexInActiveBodiesInternal for reasoning why TSAN is disabled here
    JPH_TSAN_NO_SANITIZE
    inline bool             IsActive() const                                                { return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; }
 
    inline bool             IsStatic() const                                                { return mMotionType == EMotionType::Static; }
 
    inline bool             IsKinematic() const                                             { return mMotionType == EMotionType::Kinematic; }
 
    inline bool             IsDynamic() const                                               { return mMotionType == EMotionType::Dynamic; }
 
    inline bool             CanBeKinematicOrDynamic() const                                 { return mMotionProperties != nullptr; }
 
    inline void             SetIsSensor(bool inIsSensor)                                    { JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); }
 
    inline bool             IsSensor() const                                                { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; }
 
    inline void             SetCollideKinematicVsNonDynamic(bool inCollide)                 { JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); }
 
    inline bool             GetCollideKinematicVsNonDynamic() const                         { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; }
 
    inline void             SetUseManifoldReduction(bool inUseReduction)                    { JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
 
    inline bool             GetUseManifoldReduction() const                                 { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; }
 
    inline bool             GetUseManifoldReductionWithBody(const Body &inBody2) const      { return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; }
 
    inline void             SetApplyGyroscopicForce(bool inApply)                           { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); }
 
    inline bool             GetApplyGyroscopicForce() const                                 { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; }
 
    inline void             SetEnhancedInternalEdgeRemoval(bool inApply)                    { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); }
 
    inline bool             GetEnhancedInternalEdgeRemoval() const                          { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
 
    inline bool             GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
 
    inline EMotionType      GetMotionType() const                                           { return mMotionType; }
 
    void                    SetMotionType(EMotionType inMotionType);
 
    inline BroadPhaseLayer  GetBroadPhaseLayer() const                                      { return mBroadPhaseLayer; }
 
    inline ObjectLayer      GetObjectLayer() const                                          { return mObjectLayer; }
 
    const CollisionGroup &  GetCollisionGroup() const                                       { return mCollisionGroup; }
    CollisionGroup &        GetCollisionGroup()                                             { return mCollisionGroup; }
    void                    SetCollisionGroup(const CollisionGroup &inGroup)                { mCollisionGroup = inGroup; }
 
    bool                    GetAllowSleeping() const                                        { return mMotionProperties->mAllowSleeping; }
    void                    SetAllowSleeping(bool inAllow);
 
    inline void             ResetSleepTimer();
 
    inline float            GetFriction() const                                             { return mFriction; }
    void                    SetFriction(float inFriction)                                   { mFriction = inFriction; }
 
    inline float            GetRestitution() const                                          { return mRestitution; }
    void                    SetRestitution(float inRestitution)                             { mRestitution = inRestitution; }
 
    inline Vec3             GetLinearVelocity() const                                       { return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); }
 
    void                    SetLinearVelocity(Vec3Arg inLinearVelocity)                     { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); }
 
    void                    SetLinearVelocityClamped(Vec3Arg inLinearVelocity)              { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); }
 
    inline Vec3             GetAngularVelocity() const                                      { return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); }
 
    void                    SetAngularVelocity(Vec3Arg inAngularVelocity)                   { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); }
 
    void                    SetAngularVelocityClamped(Vec3Arg inAngularVelocity)            { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); }
 
    inline Vec3             GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const         { return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); }
 
    inline Vec3             GetPointVelocity(RVec3Arg inPoint) const                        { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); }
 
    inline void             AddForce(Vec3Arg inForce)                                       { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }
 
    inline void             AddForce(Vec3Arg inForce, RVec3Arg inPosition);
 
    inline void             AddTorque(Vec3Arg inTorque)                                     { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); }
 
    // Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
    inline Vec3             GetAccumulatedForce() const                                     { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); }
 
    // Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
    inline Vec3             GetAccumulatedTorque() const                                    { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); }
 
    // Reset the total accumulated force, not that this will be done automatically after every time step.
    JPH_INLINE void         ResetForce()                                                    { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); }
 
    // Reset the total accumulated torque, not that this will be done automatically after every time step.
    JPH_INLINE void         ResetTorque()                                                   { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); }
 
    // Reset the current velocity and accumulated force and torque.
    JPH_INLINE void         ResetMotion()                                                   { JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); }
 
    inline Mat44            GetInverseInertia() const;
 
    inline void             AddImpulse(Vec3Arg inImpulse);
 
    inline void             AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition);
 
    inline void             AddAngularImpulse(Vec3Arg inAngularImpulse);
 
    void                    MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
 
    bool                    ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
 
    inline bool             IsInBroadPhase() const                                          { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; }
 
    inline bool             IsCollisionCacheInvalid() const                                 { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; }
 
    inline const Shape *    GetShape() const                                                { return mShape; }
 
    inline RVec3            GetPosition() const                                             { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); }
 
    inline Quat             GetRotation() const                                             { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mRotation; }
 
    inline RMat44           GetWorldTransform() const;
 
    inline RVec3            GetCenterOfMassPosition() const                                 { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition; }
 
    inline RMat44           GetCenterOfMassTransform() const;
 
    inline RMat44           GetInverseCenterOfMassTransform() const;
 
    inline const AABox &    GetWorldSpaceBounds() const                                     { return mBounds; }
 
    const MotionProperties *GetMotionProperties() const                                     { JPH_ASSERT(!IsStatic()); return mMotionProperties; }
    MotionProperties *      GetMotionProperties()                                           { JPH_ASSERT(!IsStatic()); return mMotionProperties; }
 
    const MotionProperties *GetMotionPropertiesUnchecked() const                            { return mMotionProperties; }
    MotionProperties *      GetMotionPropertiesUnchecked()                                  { return mMotionProperties; }
 
    uint64                  GetUserData() const                                             { return mUserData; }
    void                    SetUserData(uint64 inUserData)                                  { mUserData = inUserData; }
 
    inline Vec3             GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const;
 
    inline TransformedShape GetTransformedShape() const                                     { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); }
 
    BodyCreationSettings    GetBodyCreationSettings() const;
 
    SoftBodyCreationSettings GetSoftBodyCreationSettings() const;
 
    static Body             sFixedToWorld;
 
 
    static inline bool      sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2);
 
    inline void             AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)         { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
    inline void             SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)         { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
 
    inline void             AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
    inline void             SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
 
    inline void             SetInBroadPhaseInternal(bool inInBroadPhase)                    { if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); }
 
    inline bool             InvalidateContactCacheInternal()                                { return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; }
 
    inline void             ValidateContactCacheInternal()                                  { JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); }
 
    void                    CalculateWorldSpaceBoundsInternal();
 
    void                    SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true);
 
    void                    UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties);
 
    void                    SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties);
 
    // TSAN detects a race between BodyManager::AddBodyToActiveBodies coming from PhysicsSystem::ProcessBodyPair and Body::GetIndexInActiveBodiesInternal coming from PhysicsSystem::ProcessBodyPair.
    // When PhysicsSystem::ProcessBodyPair activates a body, it updates mIndexInActiveBodies and then updates BodyManager::mNumActiveBodies with release semantics. PhysicsSystem::ProcessBodyPair will
    // then finish its loop of active bodies and at the end of the loop it will read BodyManager::mNumActiveBodies with acquire semantics to see if any bodies were activated during the loop.
    // This means that changes to mIndexInActiveBodies must be visible to the thread, so TSANs report must be a false positive. We suppress the warning here.
    JPH_TSAN_NO_SANITIZE
    uint32                  GetIndexInActiveBodiesInternal() const                          { return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; }
 
    ECanSleep               UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep);
 
    void                    SaveState(StateRecorder &inStream) const;
 
    void                    RestoreState(StateRecorder &inStream);
 
 
    static constexpr uint32 cInactiveIndex = MotionProperties::cInactiveIndex;              
 
private:
    friend class BodyManager;
    friend class BodyWithMotionProperties;
    friend class SoftBodyWithMotionPropertiesAndShape;
 
                            Body() = default;                                               
 
    explicit                Body(bool);                                                     
 
                            ~Body()                                                         { JPH_ASSERT(mMotionProperties == nullptr); } 
 
    inline void             GetSleepTestPoints(RVec3 *outPoints) const;                     
 
    enum class EFlags : uint8
    {
        IsSensor                        = 1 << 0,                                           
        CollideKinematicVsNonDynamic    = 1 << 1,                                           
        IsInBroadPhase                  = 1 << 2,                                           
        InvalidateContactCache          = 1 << 3,                                           
        UseManifoldReduction            = 1 << 4,                                           
        ApplyGyroscopicForce            = 1 << 5,                                           
        EnhancedInternalEdgeRemoval     = 1 << 6,                                           
    };
 
    // 16 byte aligned
    RVec3                   mPosition;                                                      
    Quat                    mRotation;                                                      
    AABox                   mBounds;                                                        
 
    // 8 byte aligned
    RefConst<Shape>         mShape;                                                         
    MotionProperties *      mMotionProperties = nullptr;                                    
    uint64                  mUserData = 0;                                                  
    CollisionGroup          mCollisionGroup;                                                
 
    // 4 byte aligned
    float                   mFriction;                                                      
    float                   mRestitution;                                                   
    BodyID                  mID;                                                            
 
    // 2 or 4 bytes aligned
    ObjectLayer             mObjectLayer;                                                   
 
    // 1 byte aligned
    EBodyType               mBodyType;                                                      
    BroadPhaseLayer         mBroadPhaseLayer;                                               
    EMotionType             mMotionType;                                                    
    atomic<uint8>           mFlags = 0;                                                     
 
    // 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer)
};
 
static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect");
static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align");
 
JPH_NAMESPACE_END
 
#include "Body.inl"