CoCalc -- BodyManager.h

GitHub Repository: godotengine/godot
Path: blob/master/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h
⁹⁹¹² views
1
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3
// SPDX-License-Identifier: MIT
4

5
#pragma once
6

7
#include <Jolt/Physics/Body/Body.h>
8
#include <Jolt/Core/Mutex.h>
9
#include <Jolt/Core/MutexArray.h>
10

11
JPH_NAMESPACE_BEGIN
12

13
// Classes
14
class BodyCreationSettings;
15
class SoftBodyCreationSettings;
16
class BodyActivationListener;
17
class StateRecorderFilter;
18
struct PhysicsSettings;
19
#ifdef JPH_DEBUG_RENDERER
20
class DebugRenderer;
21
class BodyDrawFilter;
22
#endif // JPH_DEBUG_RENDERER
23

24
#ifdef JPH_DEBUG_RENDERER
25

26
/// Defines how to color soft body constraints
27
enum class ESoftBodyConstraintColor
28
{
29
	ConstraintType,				/// Draw different types of constraints in different colors
30
	ConstraintGroup,			/// Draw constraints in the same group in the same color, non-parallel group will be red
31
	ConstraintOrder,			/// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient
32
};
33

34
#endif // JPH_DEBUG_RENDERER
35

36
/// Array of bodies
37
using BodyVector = Array<Body *>;
38

39
/// Array of body ID's
40
using BodyIDVector = Array<BodyID>;
41

42
/// Class that contains all bodies
43
class JPH_EXPORT BodyManager : public NonCopyable
44
{
45
public:
46
	JPH_OVERRIDE_NEW_DELETE
47

48
	/// Destructor
49
									~BodyManager();
50

51
	/// Initialize the manager
52
	void							Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface);
53

54
	/// Gets the current amount of bodies that are in the body manager
55
	uint							GetNumBodies() const;
56

57
	/// Gets the max bodies that we can support
58
	uint							GetMaxBodies() const						{ return uint(mBodies.capacity()); }
59

60
	/// Helper struct that counts the number of bodies of each type
61
	struct BodyStats
62
	{
63
		uint						mNumBodies					= 0;			///< Total number of bodies in the body manager
64
		uint						mMaxBodies					= 0;			///< Max allowed number of bodies in the body manager (as configured in Init(...))
65

66
		uint						mNumBodiesStatic			= 0;			///< Number of static bodies
67

68
		uint						mNumBodiesDynamic			= 0;			///< Number of dynamic bodies
69
		uint						mNumActiveBodiesDynamic		= 0;			///< Number of dynamic bodies that are currently active
70

71
		uint						mNumBodiesKinematic			= 0;			///< Number of kinematic bodies
72
		uint						mNumActiveBodiesKinematic	= 0;			///< Number of kinematic bodies that are currently active
73

74
		uint						mNumSoftBodies				= 0;			///< Number of soft bodies
75
		uint						mNumActiveSoftBodies		= 0;			///< Number of soft bodies that are currently active
76
	};
77

78
	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
79
	BodyStats						GetBodyStats() const;
80

81
	/// Create a body using creation settings. The returned body will not be part of the body manager yet.
82
	Body *							AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
83

84
	/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
85
	Body *							AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const;
86

87
	/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
88
	void							FreeBody(Body *inBody) const;
89

90
	/// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available.
91
	bool							AddBody(Body *ioBody);
92

93
	/// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid.
94
	bool							AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID);
95

96
	/// Remove a list of bodies from the body manager
97
	void							RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
98

99
	/// Remove a set of bodies from the body manager and frees them.
100
	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
101

102
	/// Activate a list of bodies.
103
	/// This function should only be called when an exclusive lock for the bodies are held.
104
	void							ActivateBodies(const BodyID *inBodyIDs, int inNumber);
105

106
	/// Deactivate a list of bodies.
107
	/// This function should only be called when an exclusive lock for the bodies are held.
108
	void							DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
109

110
	/// Update the motion quality for a body
111
	void							SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
112

113
	/// Get copy of the list of active bodies under protection of a lock.
114
	void							GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const;
115

116
	/// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment.
117
	const BodyID *					GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; }
118

119
	/// Get the number of active bodies.
120
	uint32							GetNumActiveBodies(EBodyType inType) const	{ return mNumActiveBodies[int(inType)].load(memory_order_acquire); }
121

122
	/// Get the number of active bodies that are using continuous collision detection
123
	uint32							GetNumActiveCCDBodies() const				{ return mNumActiveCCDBodies; }
124

125
	/// Listener that is notified whenever a body is activated/deactivated
126
	void							SetBodyActivationListener(BodyActivationListener *inListener);
127
	BodyActivationListener *		GetBodyActivationListener() const			{ return mActivationListener; }
128

129
	/// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist.
130
	static inline bool				sIsValidBodyPointer(const Body *inBody)		{ return (uintptr_t(inBody) & cIsFreedBody) == 0; }
131

132
	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
133
	const BodyVector &				GetBodies() const							{ return mBodies; }
134

135
	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
136
	BodyVector &					GetBodies()									{ return mBodies; }
137

138
	/// Get all body IDs under the protection of a lock
139
	void							GetBodyIDs(BodyIDVector &outBodies) const;
140

141
	/// Access a body (not protected by lock)
142
	const Body &					GetBody(const BodyID &inID) const			{ return *mBodies[inID.GetIndex()]; }
143

144
	/// Access a body (not protected by lock)
145
	Body &							GetBody(const BodyID &inID)					{ return *mBodies[inID.GetIndex()]; }
146

147
	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
148
	const Body *					TryGetBody(const BodyID &inID) const
149
	{
150
		uint32 idx = inID.GetIndex();
151
		if (idx >= mBodies.size())
152
			return nullptr;
153

154
		const Body *body = mBodies[idx];
155
		if (sIsValidBodyPointer(body) && body->GetID() == inID)
156
			return body;
157

158
		return nullptr;
159
	}
160

161
	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
162
	Body *							TryGetBody(const BodyID &inID)
163
	{
164
		uint32 idx = inID.GetIndex();
165
		if (idx >= mBodies.size())
166
			return nullptr;
167

168
		Body *body = mBodies[idx];
169
		if (sIsValidBodyPointer(body) && body->GetID() == inID)
170
			return body;
171

172
		return nullptr;
173
	}
174

175
	/// Access the mutex for a single body
176
	SharedMutex &					GetMutexForBody(const BodyID &inID) const	{ return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); }
177

178
	/// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex.
179
	using MutexMask = uint64;
180

181
	///@name Batch body mutex access (do not use directly)
182
	///@{
183
	MutexMask						GetAllBodiesMutexMask() const				{ return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; }
184
	MutexMask						GetMutexMask(const BodyID *inBodies, int inNumber) const;
185
	void							LockRead(MutexMask inMutexMask) const;
186
	void							UnlockRead(MutexMask inMutexMask) const;
187
	void							LockWrite(MutexMask inMutexMask) const;
188
	void							UnlockWrite(MutexMask inMutexMask) const;
189
	///@}
190

191
	/// Lock all bodies. This should only be done during PhysicsSystem::Update().
192
	void							LockAllBodies() const;
193

194
	/// Unlock all bodies. This should only be done during PhysicsSystem::Update().
195
	void							UnlockAllBodies() const;
196

197
	/// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase)
198
	inline void						SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); }
199

200
	/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
201
	void							InvalidateContactCacheForBody(Body &ioBody);
202

203
	/// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again.
204
	void							ValidateContactCacheForAllBodies();
205

206
	/// Saving state for replay
207
	void							SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
208

209
	/// Restoring state for replay. Returns false if failed.
210
	bool							RestoreState(StateRecorder &inStream);
211

212
	/// Save the state of a single body for replay
213
	void							SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
214

215
	/// Save the state of a single body for replay
216
	void							RestoreBodyState(Body &inBody, StateRecorder &inStream);
217

218
#ifdef JPH_DEBUG_RENDERER
219
	enum class EShapeColor
220
	{
221
		InstanceColor,				///< Random color per instance
222
		ShapeTypeColor,				///< Convex = green, scaled = yellow, compound = orange, mesh = red
223
		MotionTypeColor,			///< Static = grey, keyframed = green, dynamic = random color per instance
224
		SleepColor,					///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red
225
		IslandColor,				///< Static = grey, active = random color per island, sleeping = light grey
226
		MaterialColor,				///< Color as defined by the PhysicsMaterial of the shape
227
	};
228

229
	/// Draw settings
230
	struct DrawSettings
231
	{
232
		bool						mDrawGetSupportFunction = false;				///< Draw the GetSupport() function, used for convex collision detection
233
		bool						mDrawSupportDirection = false;					///< When drawing the support function, also draw which direction mapped to a specific support point
234
		bool						mDrawGetSupportingFace = false;					///< Draw the faces that were found colliding during collision detection
235
		bool						mDrawShape = true;								///< Draw the shapes of all bodies
236
		bool						mDrawShapeWireframe = false;					///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid.
237
		EShapeColor					mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes
238
		bool						mDrawBoundingBox = false;						///< Draw a bounding box per body
239
		bool						mDrawCenterOfMassTransform = false;				///< Draw the center of mass for each body
240
		bool						mDrawWorldTransform = false;					///< Draw the world transform (which can be different than the center of mass) for each body
241
		bool						mDrawVelocity = false;							///< Draw the velocity vector for each body
242
		bool						mDrawMassAndInertia = false;					///< Draw the mass and inertia (as the box equivalent) for each body
243
		bool						mDrawSleepStats = false;						///< Draw stats regarding the sleeping algorithm of each body
244
		bool						mDrawSoftBodyVertices = false;					///< Draw the vertices of soft bodies
245
		bool						mDrawSoftBodyVertexVelocities = false;			///< Draw the velocities of the vertices of soft bodies
246
		bool						mDrawSoftBodyEdgeConstraints = false;			///< Draw the edge constraints of soft bodies
247
		bool						mDrawSoftBodyBendConstraints = false;			///< Draw the bend constraints of soft bodies
248
		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
249
		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
250
		bool						mDrawSoftBodyLRAConstraints = false;			///< Draw the LRA constraints of soft bodies
251
		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
252
		ESoftBodyConstraintColor	mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
253
	};
254

255
	/// Draw the state of the bodies (debugging purposes)
256
	void							Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr);
257
#endif // JPH_DEBUG_RENDERER
258

259
#ifdef JPH_ENABLE_ASSERTS
260
	/// Lock the active body list, asserts when Activate/DeactivateBody is called.
261
	void							SetActiveBodiesLocked(bool inLocked)		{ mActiveBodiesLocked = inLocked; }
262

263
	/// Per thread override of the locked state, to be used by the PhysicsSystem only!
264
	class GrantActiveBodiesAccess
265
	{
266
	public:
267
		inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation)
268
		{
269
			JPH_ASSERT(!sGetOverrideAllowActivation());
270
			sSetOverrideAllowActivation(inAllowActivation);
271

272
			JPH_ASSERT(!sGetOverrideAllowDeactivation());
273
			sSetOverrideAllowDeactivation(inAllowDeactivation);
274
		}
275

276
		inline ~GrantActiveBodiesAccess()
277
		{
278
			sSetOverrideAllowActivation(false);
279
			sSetOverrideAllowDeactivation(false);
280
		}
281
	};
282
#endif
283

284
#ifdef JPH_DEBUG
285
	/// Validate if the cached bounding boxes are correct for all active bodies
286
	void							ValidateActiveBodyBounds();
287
#endif // JPH_DEBUG
288

289
private:
290
	/// Increment and get the sequence number of the body
291
#ifdef JPH_COMPILER_CLANG
292
	__attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number
293
#endif
294
	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
295

296
	/// Add a single body to mActiveBodies, note doesn't lock the active body mutex!
297
	inline void						AddBodyToActiveBodies(Body &ioBody);
298

299
	/// Remove a single body from mActiveBodies, note doesn't lock the active body mutex!
300
	inline void						RemoveBodyFromActiveBodies(Body &ioBody);
301

302
	/// Helper function to remove a body from the manager
303
	JPH_INLINE Body *				RemoveBodyInternal(const BodyID &inBodyID);
304

305
	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
306
	inline static void				sDeleteBody(Body *inBody);
307

308
#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
309
	/// Function to check that the free list is not corrupted
310
	void							ValidateFreeList() const;
311
#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
312

313
	/// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array.
314
	BodyVector						mBodies;
315

316
	/// Current number of allocated bodies
317
	uint							mNumBodies = 0;
318

319
	/// Indicates that there are no more freed body IDs
320
	static constexpr uintptr_t		cBodyIDFreeListEnd = ~uintptr_t(0);
321

322
	/// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit.
323
	static constexpr uintptr_t		cIsFreedBody = uintptr_t(1);
324

325
	/// Amount of bits to shift to get an index to the next freed body
326
	static constexpr uint			cFreedBodyIndexShift = 1;
327

328
	/// Index of first entry in mBodies that is unused
329
	uintptr_t						mBodyIDFreeListStart = cBodyIDFreeListEnd;
330

331
	/// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart
332
	mutable Mutex					mBodiesMutex;
333

334
	/// An array of mutexes protecting the bodies in the mBodies array
335
	using BodyMutexes = MutexArray<SharedMutex>;
336
	mutable BodyMutexes				mBodyMutexes;
337

338
	/// List of next sequence number for a body ID
339
	Array<uint8>					mBodySequenceNumbers;
340

341
	/// Mutex that protects the mActiveBodies array
342
	mutable Mutex					mActiveBodiesMutex;
343

344
	/// List of all active dynamic bodies (size is equal to max amount of bodies)
345
	BodyID *						mActiveBodies[cBodyTypeCount] = { };
346

347
	/// How many bodies there are in the list of active bodies
348
	atomic<uint32>					mNumActiveBodies[cBodyTypeCount] = { };
349

350
	/// How many of the active bodies have continuous collision detection enabled
351
	uint32							mNumActiveCCDBodies = 0;
352

353
	/// Mutex that protects the mBodiesCacheInvalid array
354
	mutable Mutex					mBodiesCacheInvalidMutex;
355

356
	/// List of all bodies that should have their cache invalidated
357
	BodyIDVector					mBodiesCacheInvalid;
358

359
	/// Listener that is notified whenever a body is activated/deactivated
360
	BodyActivationListener *		mActivationListener = nullptr;
361

362
	/// Cached broadphase layer interface
363
	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
364

365
#ifdef JPH_ENABLE_ASSERTS
366
	static bool						sGetOverrideAllowActivation();
367
	static void						sSetOverrideAllowActivation(bool inValue);
368

369
	static bool						sGetOverrideAllowDeactivation();
370
	static void						sSetOverrideAllowDeactivation(bool inValue);
371

372
	/// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update()
373
	bool							mActiveBodiesLocked = false;
374
#endif
375
};
376

377
JPH_NAMESPACE_END
378

379
Product

Resources

Company
