PhysicsSystem.h 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. #pragma once
  5. #include <Jolt/Physics/Body/BodyInterface.h>
  6. #include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
  7. #include <Jolt/Physics/Collision/ContactListener.h>
  8. #include <Jolt/Physics/Constraints/ContactConstraintManager.h>
  9. #include <Jolt/Physics/Constraints/ConstraintManager.h>
  10. #include <Jolt/Physics/IslandBuilder.h>
  11. #include <Jolt/Physics/LargeIslandSplitter.h>
  12. #include <Jolt/Physics/PhysicsUpdateContext.h>
  13. #include <Jolt/Physics/PhysicsSettings.h>
  14. JPH_NAMESPACE_BEGIN
  15. class JobSystem;
  16. class StateRecorder;
  17. class TempAllocator;
  18. class PhysicsStepListener;
  19. class SoftBodyContactListener;
  20. /// The main class for the physics system. It contains all rigid bodies and simulates them.
  21. ///
  22. /// The main simulation is performed by the Update() call on multiple threads (if the JobSystem is configured to use them). Please refer to the general architecture overview in the Docs folder for more information.
  23. class JPH_EXPORT PhysicsSystem : public NonCopyable
  24. {
  25. public:
  26. JPH_OVERRIDE_NEW_DELETE
  27. /// Constructor / Destructor
  28. PhysicsSystem() : mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { }
  29. ~PhysicsSystem();
  30. /// Initialize the system.
  31. /// @param inMaxBodies Maximum number of bodies to support.
  32. /// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect.
  33. /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching.
  34. /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world).
  35. /// @param inBroadPhaseLayerInterface Information on the mapping of object layers to broad phase layers. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
  36. /// @param inObjectVsBroadPhaseLayerFilter Filter callback function that is used to determine if an object layer collides with a broad phase layer. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
  37. /// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
  38. void Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter);
  39. /// Listener that is notified whenever a body is activated/deactivated
  40. void SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); }
  41. BodyActivationListener * GetBodyActivationListener() const { return mBodyManager.GetBodyActivationListener(); }
  42. /// Listener that is notified whenever a contact point between two bodies is added/updated/removed
  43. void SetContactListener(ContactListener *inListener) { mContactManager.SetContactListener(inListener); }
  44. ContactListener * GetContactListener() const { return mContactManager.GetContactListener(); }
  45. /// Listener that is notified whenever a contact point between a soft body and another body
  46. void SetSoftBodyContactListener(SoftBodyContactListener *inListener) { mSoftBodyContactListener = inListener; }
  47. SoftBodyContactListener * GetSoftBodyContactListener() const { return mSoftBodyContactListener; }
  48. /// Set the function that combines the friction of two bodies and returns it
  49. /// Default method is the geometric mean: sqrt(friction1 * friction2).
  50. void SetCombineFriction(ContactConstraintManager::CombineFunction inCombineFriction) { mContactManager.SetCombineFriction(inCombineFriction); }
  51. ContactConstraintManager::CombineFunction GetCombineFriction() const { return mContactManager.GetCombineFriction(); }
  52. /// Set the function that combines the restitution of two bodies and returns it
  53. /// Default method is max(restitution1, restitution1)
  54. void SetCombineRestitution(ContactConstraintManager::CombineFunction inCombineRestition) { mContactManager.SetCombineRestitution(inCombineRestition); }
  55. ContactConstraintManager::CombineFunction GetCombineRestitution() const { return mContactManager.GetCombineRestitution(); }
  56. /// Control the main constants of the physics simulation
  57. void SetPhysicsSettings(const PhysicsSettings &inSettings) { mPhysicsSettings = inSettings; }
  58. const PhysicsSettings & GetPhysicsSettings() const { return mPhysicsSettings; }
  59. /// Access to the body interface. This interface allows to to create / remove bodies and to change their properties.
  60. const BodyInterface & GetBodyInterface() const { return mBodyInterfaceLocking; }
  61. BodyInterface & GetBodyInterface() { return mBodyInterfaceLocking; }
  62. const BodyInterface & GetBodyInterfaceNoLock() const { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care!
  63. BodyInterface & GetBodyInterfaceNoLock() { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care!
  64. /// Access to the broadphase interface that allows coarse collision queries
  65. const BroadPhaseQuery & GetBroadPhaseQuery() const { return *mBroadPhase; }
  66. /// Interface that allows fine collision queries against first the broad phase and then the narrow phase.
  67. const NarrowPhaseQuery & GetNarrowPhaseQuery() const { return mNarrowPhaseQueryLocking; }
  68. const NarrowPhaseQuery & GetNarrowPhaseQueryNoLock() const { return mNarrowPhaseQueryNoLock; } ///< Version that does not lock the bodies, use with great care!
  69. /// Add constraint to the world
  70. void AddConstraint(Constraint *inConstraint) { mConstraintManager.Add(&inConstraint, 1); }
  71. /// Remove constraint from the world
  72. void RemoveConstraint(Constraint *inConstraint) { mConstraintManager.Remove(&inConstraint, 1); }
  73. /// Batch add constraints. Note that the inConstraints array is allowed to have nullptrs, these will be ignored.
  74. void AddConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Add(inConstraints, inNumber); }
  75. /// Batch remove constraints. Note that the inConstraints array is allowed to have nullptrs, these will be ignored.
  76. void RemoveConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Remove(inConstraints, inNumber); }
  77. /// Get a list of all constraints
  78. Constraints GetConstraints() const { return mConstraintManager.GetConstraints(); }
  79. /// Optimize the broadphase, needed only if you've added many bodies prior to calling Update() for the first time.
  80. void OptimizeBroadPhase();
  81. /// Adds a new step listener
  82. void AddStepListener(PhysicsStepListener *inListener);
  83. /// Removes a step listener
  84. void RemoveStepListener(PhysicsStepListener *inListener);
  85. /// Simulate the system.
  86. /// The world steps for a total of inDeltaTime seconds. This is divided in inCollisionSteps iterations.
  87. /// Each iteration consists of collision detection followed by an integration step.
  88. /// This function internally spawns jobs using inJobSystem and waits for them to complete, so no jobs will be running when this function returns.
  89. EPhysicsUpdateError Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem);
  90. /// Saving state for replay
  91. void SaveState(StateRecorder &inStream, EStateRecorderState inState = EStateRecorderState::All, const StateRecorderFilter *inFilter = nullptr) const;
  92. /// Restoring state for replay. Returns false if failed.
  93. bool RestoreState(StateRecorder &inStream);
  94. /// Saving state of a single body.
  95. void SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
  96. /// Restoring state of a single body.
  97. void RestoreBodyState(Body &ioBody, StateRecorder &inStream);
  98. #ifdef JPH_DEBUG_RENDERER
  99. // Drawing properties
  100. static bool sDrawMotionQualityLinearCast; ///< Draw debug info for objects that perform continuous collision detection through the linear cast motion quality
  101. /// Draw the state of the bodies (debugging purposes)
  102. void DrawBodies(const BodyManager::DrawSettings &inSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr) { mBodyManager.Draw(inSettings, mPhysicsSettings, inRenderer, inBodyFilter); }
  103. /// Draw the constraints only (debugging purposes)
  104. void DrawConstraints(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraints(inRenderer); }
  105. /// Draw the constraint limits only (debugging purposes)
  106. void DrawConstraintLimits(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintLimits(inRenderer); }
  107. /// Draw the constraint reference frames only (debugging purposes)
  108. void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintReferenceFrame(inRenderer); }
  109. #endif // JPH_DEBUG_RENDERER
  110. /// Set gravity value
  111. void SetGravity(Vec3Arg inGravity) { mGravity = inGravity; }
  112. Vec3 GetGravity() const { return mGravity; }
  113. /// Returns a locking interface that won't actually lock the body. Use with great care!
  114. inline const BodyLockInterfaceNoLock & GetBodyLockInterfaceNoLock() const { return mBodyLockInterfaceNoLock; }
  115. /// Returns a locking interface that locks the body so other threads cannot modify it.
  116. inline const BodyLockInterfaceLocking & GetBodyLockInterface() const { return mBodyLockInterfaceLocking; }
  117. /// Get an broadphase layer filter that uses the default pair filter and a specified object layer to determine if broadphase layers collide
  118. DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const { return DefaultBroadPhaseLayerFilter(*mObjectVsBroadPhaseLayerFilter, inLayer); }
  119. /// Get an object layer filter that uses the default pair filter and a specified layer to determine if layers collide
  120. DefaultObjectLayerFilter GetDefaultLayerFilter(ObjectLayer inLayer) const { return DefaultObjectLayerFilter(*mObjectLayerPairFilter, inLayer); }
  121. /// Gets the current amount of bodies that are in the body manager
  122. uint GetNumBodies() const { return mBodyManager.GetNumBodies(); }
  123. /// Gets the current amount of active bodies that are in the body manager
  124. uint32 GetNumActiveBodies(EBodyType inType) const { return mBodyManager.GetNumActiveBodies(inType); }
  125. /// Get the maximum amount of bodies that this physics system supports
  126. uint GetMaxBodies() const { return mBodyManager.GetMaxBodies(); }
  127. /// Helper struct that counts the number of bodies of each type
  128. using BodyStats = BodyManager::BodyStats;
  129. /// Get stats about the bodies in the body manager (slow, iterates through all bodies)
  130. BodyStats GetBodyStats() const { return mBodyManager.GetBodyStats(); }
  131. /// Get copy of the list of all bodies under protection of a lock.
  132. /// @param outBodyIDs On return, this will contain the list of BodyIDs
  133. void GetBodies(BodyIDVector &outBodyIDs) const { return mBodyManager.GetBodyIDs(outBodyIDs); }
  134. /// Get copy of the list of active bodies under protection of a lock.
  135. /// @param inType The type of bodies to get
  136. /// @param outBodyIDs On return, this will contain the list of BodyIDs
  137. void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const { return mBodyManager.GetActiveBodies(inType, outBodyIDs); }
  138. /// Get the list of active bodies, use GetNumActiveBodies() to find out how long the list is.
  139. /// Note: Not thread safe. The active bodies list can change at any moment when other threads are doing work. Use GetActiveBodies() if you need a thread safe version.
  140. const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mBodyManager.GetActiveBodiesUnsafe(inType); }
  141. /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work.
  142. /// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards.
  143. /// This function can be called from any thread when the PhysicsSystem::Update is not running. During PhysicsSystem::Update this function is only valid during contact callbacks:
  144. /// - During the ContactListener::OnContactAdded callback this function can be used to determine if a different contact pair between the bodies was active in the previous simulation step (function returns true) or if this is the first step that the bodies are touching (function returns false).
  145. /// - During the ContactListener::OnContactRemoved callback this function can be used to determine if this is the last contact pair between the bodies (function returns false) or if there are other contacts still present (function returns true).
  146. bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const { return mContactManager.WereBodiesInContact(inBody1ID, inBody2ID); }
  147. /// Get the bounding box of all bodies in the physics system
  148. AABox GetBounds() const { return mBroadPhase->GetBounds(); }
  149. #ifdef JPH_TRACK_BROADPHASE_STATS
  150. /// Trace the accumulated broadphase stats to the TTY
  151. void ReportBroadphaseStats() { mBroadPhase->ReportStats(); }
  152. #endif // JPH_TRACK_BROADPHASE_STATS
  153. private:
  154. using CCDBody = PhysicsUpdateContext::Step::CCDBody;
  155. // Various job entry points
  156. void JobStepListeners(PhysicsUpdateContext::Step *ioStep);
  157. void JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const;
  158. void JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  159. void JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const;
  160. void JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  161. void JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex);
  162. void JobFinalizeIslands(PhysicsUpdateContext *ioContext);
  163. void JobBodySetIslandIndex();
  164. void JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  165. void JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  166. void JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  167. void JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const;
  168. void JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  169. void JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  170. void JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep);
  171. void JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  172. void JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
  173. void JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const;
  174. void JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const;
  175. void JobSoftBodyFinalize(PhysicsUpdateContext *ioContext);
  176. /// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet
  177. void TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const;
  178. using ContactAllocator = ContactConstraintManager::ContactAllocator;
  179. /// Process narrow phase for a single body pair
  180. void ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair);
  181. /// This helper batches up bodies that need to put to sleep to avoid contention on the activation mutex
  182. class BodiesToSleep;
  183. /// Called at the end of JobSolveVelocityConstraints to check if bodies need to go to sleep and to update their bounding box in the broadphase
  184. void CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep);
  185. /// Number of constraints to process at once in JobDetermineActiveConstraints
  186. static constexpr int cDetermineActiveConstraintsBatchSize = 64;
  187. /// Number of constraints to process at once in JobSetupVelocityConstraints, we want a low number of threads working on this so we take fairly large batches
  188. static constexpr int cSetupVelocityConstraintsBatchSize = 256;
  189. /// Number of bodies to process at once in JobApplyGravity
  190. static constexpr int cApplyGravityBatchSize = 64;
  191. /// Number of active bodies to test for collisions per batch
  192. static constexpr int cActiveBodiesBatchSize = 16;
  193. /// Number of active bodies to integrate velocities for
  194. static constexpr int cIntegrateVelocityBatchSize = 64;
  195. /// Number of contacts that need to be queued before another narrow phase job is started
  196. static constexpr int cNarrowPhaseBatchSize = 16;
  197. /// Number of continuous collision shape casts that need to be queued before another job is started
  198. static constexpr int cNumCCDBodiesPerJob = 4;
  199. /// Broadphase layer filter that decides if two objects can collide
  200. const ObjectVsBroadPhaseLayerFilter *mObjectVsBroadPhaseLayerFilter = nullptr;
  201. /// Object layer filter that decides if two objects can collide
  202. const ObjectLayerPairFilter *mObjectLayerPairFilter = nullptr;
  203. /// The body manager keeps track which bodies are in the simulation
  204. BodyManager mBodyManager;
  205. /// Body locking interfaces
  206. BodyLockInterfaceNoLock mBodyLockInterfaceNoLock { mBodyManager };
  207. BodyLockInterfaceLocking mBodyLockInterfaceLocking { mBodyManager };
  208. /// Body interfaces
  209. BodyInterface mBodyInterfaceNoLock;
  210. BodyInterface mBodyInterfaceLocking;
  211. /// Narrow phase query interface
  212. NarrowPhaseQuery mNarrowPhaseQueryNoLock;
  213. NarrowPhaseQuery mNarrowPhaseQueryLocking;
  214. /// The broadphase does quick collision detection between body pairs
  215. BroadPhase * mBroadPhase = nullptr;
  216. /// The soft body contact listener
  217. SoftBodyContactListener * mSoftBodyContactListener = nullptr;
  218. /// Simulation settings
  219. PhysicsSettings mPhysicsSettings;
  220. /// The contact manager resolves all contacts during a simulation step
  221. ContactConstraintManager mContactManager;
  222. /// All non-contact constraints
  223. ConstraintManager mConstraintManager;
  224. /// Keeps track of connected bodies and builds islands for multithreaded velocity/position update
  225. IslandBuilder mIslandBuilder;
  226. /// Will split large islands into smaller groups of bodies that can be processed in parallel
  227. LargeIslandSplitter mLargeIslandSplitter;
  228. /// Mutex protecting mStepListeners
  229. Mutex mStepListenersMutex;
  230. /// List of physics step listeners
  231. using StepListeners = Array<PhysicsStepListener *>;
  232. StepListeners mStepListeners;
  233. /// This is the global gravity vector
  234. Vec3 mGravity = Vec3(0, -9.81f, 0);
  235. /// Previous frame's delta time of one sub step to allow scaling previous frame's constraint impulses
  236. float mPreviousStepDeltaTime = 0.0f;
  237. };
  238. JPH_NAMESPACE_END