MotionProperties.h 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188
  1. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  2. // SPDX-License-Identifier: MIT
  3. #pragma once
  4. #include <Jolt/Geometry/Sphere.h>
  5. #include <Jolt/Physics/Body/MotionQuality.h>
  6. #include <Jolt/Physics/Body/BodyAccess.h>
  7. #include <Jolt/Physics/Body/MotionType.h>
  8. #include <Jolt/Physics/Body/MassProperties.h>
  9. #include <Jolt/Physics/DeterminismLog.h>
  10. JPH_NAMESPACE_BEGIN
  11. class StateRecorder;
  12. /// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body.
  13. class MotionProperties
  14. {
  15. public:
  16. JPH_OVERRIDE_NEW_DELETE
  17. /// Motion quality, or how well it detects collisions when it has a high velocity
  18. EMotionQuality GetMotionQuality() const { return mMotionQuality; }
  19. void SetMotionQuality(EMotionQuality inQuality) { mMotionQuality = inQuality; }
  20. /// Get world space linear velocity of the center of mass
  21. inline Vec3 GetLinearVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mLinearVelocity; }
  22. /// Set world space linear velocity of the center of mass
  23. void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = inLinearVelocity; }
  24. /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity
  25. void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { mLinearVelocity = inLinearVelocity; ClampLinearVelocity(); }
  26. /// Get world space angular velocity of the center of mass
  27. inline Vec3 GetAngularVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mAngularVelocity; }
  28. /// Set world space angular velocity of the center of mass
  29. void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = inAngularVelocity; }
  30. /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
  31. void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { mAngularVelocity = inAngularVelocity; ClampAngularVelocity(); }
  32. /// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds.
  33. inline void MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
  34. ///@name Velocity limits
  35. ///@{
  36. /// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding.
  37. inline float GetMaxLinearVelocity() const { return mMaxLinearVelocity; }
  38. inline void SetMaxLinearVelocity(float inLinearVelocity) { JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
  39. /// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding.
  40. inline float GetMaxAngularVelocity() const { return mMaxAngularVelocity; }
  41. inline void SetMaxAngularVelocity(float inAngularVelocity) { JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
  42. ///@}
  43. /// Clamp velocity according to limit
  44. inline void ClampLinearVelocity();
  45. inline void ClampAngularVelocity();
  46. /// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
  47. inline float GetLinearDamping() const { return mLinearDamping; }
  48. void SetLinearDamping(float inLinearDamping) { JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; }
  49. /// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
  50. inline float GetAngularDamping() const { return mAngularDamping; }
  51. void SetAngularDamping(float inAngularDamping) { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; }
  52. /// Get gravity factor (1 = normal gravity, 0 = no gravity)
  53. inline float GetGravityFactor() const { return mGravityFactor; }
  54. void SetGravityFactor(float inGravityFactor) { mGravityFactor = inGravityFactor; }
  55. /// Set the mass and inertia tensor
  56. inline void SetMassProperties(const MassProperties &inMassProperties);
  57. /// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0)
  58. inline float GetInverseMass() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
  59. inline float GetInverseMassUnchecked() const { return mInvMass; }
  60. /// Set the inverse mass (1 / mass).
  61. /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
  62. /// If you change mass, inertia should probably change as well. See MassProperties::ScaleToMass.
  63. void SetInverseMass(float inInverseMass) { mInvMass = inInverseMass; }
  64. /// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0)
  65. inline Vec3 GetInverseInertiaDiagonal() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; }
  66. /// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$
  67. inline Quat GetInertiaRotation() const { return mInertiaRotation; }
  68. /// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
  69. /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
  70. /// If you change inertia, mass should probably change as well. See MassProperties::ScaleToMass.
  71. void SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot) { mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
  72. /// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
  73. inline Mat44 GetLocalSpaceInverseInertia() const;
  74. /// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic
  75. inline Mat44 GetLocalSpaceInverseInertiaUnchecked() const;
  76. /// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic.
  77. inline Mat44 GetInverseInertiaForRotation(Mat44Arg inRotation) const;
  78. /// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic.
  79. JPH_INLINE Vec3 MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const;
  80. /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
  81. JPH_INLINE Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); }
  82. ////////////////////////////////////////////////////////////
  83. // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
  84. ////////////////////////////////////////////////////////////
  85. ///@name Update linear and angular velocity (used during constraint solving)
  86. ///@{
  87. inline void AddLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity += inLinearVelocityChange; JPH_ASSERT(!mLinearVelocity.IsNaN()); }
  88. inline void SubLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity -= inLinearVelocityChange; JPH_ASSERT(!mLinearVelocity.IsNaN()); }
  89. inline void AddAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
  90. inline void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
  91. ///@}
  92. /// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
  93. inline void ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
  94. /// At the end of a simulation update the forces and torques need to be reset for the next frame
  95. inline void ResetForceAndTorqueInternal() { mForce = Float3(0, 0, 0); mTorque = Float3(0, 0, 0); }
  96. /// Access to the island index
  97. uint32 GetIslandIndexInternal() const { return mIslandIndex; }
  98. void SetIslandIndexInternal(uint32 inIndex) { mIslandIndex = inIndex; }
  99. /// Access to the index in the active bodies array
  100. uint32 GetIndexInActiveBodiesInternal() const { return mIndexInActiveBodies; }
  101. /// Reset spheres to center around inPoints with radius 0
  102. inline void ResetSleepTestSpheres(const Vec3 *inPoints);
  103. /// Saving state for replay
  104. void SaveState(StateRecorder &inStream) const;
  105. /// Restoring state for replay
  106. void RestoreState(StateRecorder &inStream);
  107. private:
  108. friend class BodyManager;
  109. friend class Body;
  110. // 1st cache line
  111. // 16 byte aligned
  112. Vec3 mLinearVelocity { Vec3::sZero() }; ///< World space linear velocity of the center of mass (m/s)
  113. Vec3 mAngularVelocity { Vec3::sZero() }; ///< World space angular velocity (rad/s)
  114. Vec3 mInvInertiaDiagonal; ///< Diagonal of inverse inertia matrix: D
  115. Quat mInertiaRotation; ///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1
  116. // 2nd cache line
  117. // 4 byte aligned
  118. Float3 mForce { 0, 0, 0 }; ///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
  119. Float3 mTorque { 0, 0, 0 }; ///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
  120. float mInvMass; ///< Inverse mass of the object (1/kg)
  121. float mLinearDamping; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
  122. float mAngularDamping; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
  123. float mMaxLinearVelocity; ///< Maximum linear velocity that this body can reach (m/s)
  124. float mMaxAngularVelocity; ///< Maximum angular velocity that this body can reach (rad/s)
  125. float mGravityFactor; ///< Factor to multiply gravity with
  126. uint32 mIndexInActiveBodies; ///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active
  127. uint32 mIslandIndex; ///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex
  128. // 1 byte aligned
  129. EMotionQuality mMotionQuality; ///< Motion quality, or how well it detects collisions when it has a high velocity
  130. bool mAllowSleeping; ///< If this body can go to sleep
  131. // 3rd cache line (least frequently used)
  132. // 4 byte aligned
  133. Sphere mSleepTestSpheres[3]; ///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis
  134. float mSleepTestTimer; ///< How long this body has been within the movement tolerance
  135. #ifdef JPH_ENABLE_ASSERTS
  136. EMotionType mCachedMotionType; ///< Copied from Body::mMotionType and cached for asserting purposes
  137. #endif
  138. };
  139. JPH_NAMESPACE_END
  140. #include "MotionProperties.inl"