Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
QuadTree.h
Go to the documentation of this file.
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
8#include <Jolt/Core/Atomics.h>
12
13//#define JPH_DUMP_BROADPHASE_TREE
14
16
21{
22public:
24
25private:
26 // Forward declare
27 class AtomicNodeID;
28
30 class NodeID
31 {
32 public:
34
36 inline NodeID() = default;
37
39 static inline NodeID sInvalid() { return NodeID(cInvalidNodeIndex); }
40 static inline NodeID sFromBodyID(BodyID inID) { NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; }
41 static inline NodeID sFromNodeIndex(uint32 inIdx) { JPH_ASSERT((inIdx & cIsNode) == 0); return NodeID(inIdx | cIsNode); }
42
44 inline bool IsValid() const { return mID != cInvalidNodeIndex; }
45 inline bool IsBody() const { return (mID & cIsNode) == 0; }
46 inline bool IsNode() const { return (mID & cIsNode) != 0; }
47
49 inline BodyID GetBodyID() const { JPH_ASSERT(IsBody()); return BodyID(mID); }
50 inline uint32 GetNodeIndex() const { JPH_ASSERT(IsNode()); return mID & ~cIsNode; }
51
53 inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); }
54 inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; }
55
56 private:
57 friend class AtomicNodeID;
58
59 inline explicit NodeID(uint32 inID) : mID(inID) { }
60
61 static const uint32 cIsNode = BodyID::cBroadPhaseBit;
62
63 uint32 mID;
64 };
65
66 static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs");
67
69 class AtomicNodeID
70 {
71 public:
73 AtomicNodeID() = default;
74 explicit AtomicNodeID(const NodeID &inRHS) : mID(inRHS.mID) { }
75
77 inline void operator = (const NodeID &inRHS) { mID = inRHS.mID; }
78
80 inline operator NodeID () const { return NodeID(mID); }
81
83 inline bool IsValid() const { return mID != cInvalidNodeIndex; }
84
86 inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); }
87 inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; }
88
90 inline bool CompareExchange(NodeID inOld, NodeID inNew) { return mID.compare_exchange_strong(inOld.mID, inNew.mID); }
91
92 private:
93 atomic<uint32> mID;
94 };
95
97 class Node
98 {
99 public:
101 explicit Node(bool inIsChanged);
102
104 void GetNodeBounds(AABox &outBounds) const;
105
107 void GetChildBounds(int inChildIndex, AABox &outBounds) const;
108
110 void SetChildBounds(int inChildIndex, const AABox &inBounds);
111
113 void InvalidateChildBounds(int inChildIndex);
114
116 bool EncapsulateChildBounds(int inChildIndex, const AABox &inBounds);
117
118 // The functions below read the atomic bounds / child IDs using a non-atomic SIMD load while other threads may be modifying them.
119 // This is by design: SetChildBounds / InvalidateChildBounds write the values in an order that ensures readers see either a valid or an empty box.
120 // TSAN reports this as a data race, so we suppress the warning here. Note that these functions should not be marked JPH_INLINE, as force inlining
121 // them into an instrumented function would remove the attribute.
122
125 inline void LoadChildBounds(Vec4 &outMinX, Vec4 &outMinY, Vec4 &outMinZ, Vec4 &outMaxX, Vec4 &outMaxY, Vec4 &outMaxZ) const
126 {
127 outMinX = Vec4::sLoadFloat4Aligned((const Float4 *)&mBoundsMinX);
128 outMinY = Vec4::sLoadFloat4Aligned((const Float4 *)&mBoundsMinY);
129 outMinZ = Vec4::sLoadFloat4Aligned((const Float4 *)&mBoundsMinZ);
130 outMaxX = Vec4::sLoadFloat4Aligned((const Float4 *)&mBoundsMaxX);
131 outMaxY = Vec4::sLoadFloat4Aligned((const Float4 *)&mBoundsMaxY);
132 outMaxZ = Vec4::sLoadFloat4Aligned((const Float4 *)&mBoundsMaxZ);
133 }
134
137 inline UVec4 LoadChildNodeIDs() const
138 {
139 return UVec4::sLoadInt4Aligned((const uint32 *)&mChildNodeID[0]);
140 }
141
143 atomic<float> mBoundsMinX[4];
144 atomic<float> mBoundsMinY[4];
145 atomic<float> mBoundsMinZ[4];
146 atomic<float> mBoundsMaxX[4];
147 atomic<float> mBoundsMaxY[4];
148 atomic<float> mBoundsMaxZ[4];
149
151 AtomicNodeID mChildNodeID[4];
152
155 atomic<uint32> mParentNodeIndex = cInvalidNodeIndex;
156
159 atomic<uint32> mIsChanged;
160
161 // Padding to align to 124 bytes
162 uint32 mPadding = 0;
163 };
164
165 // Maximum size of the stack during tree walk
166 static constexpr int cStackSize = 128;
167
168 static_assert(sizeof(atomic<float>) == 4, "Assuming that an atomic doesn't add any additional storage");
169 static_assert(sizeof(atomic<uint32>) == 4, "Assuming that an atomic doesn't add any additional storage");
170 static_assert(std::is_trivially_destructible<Node>(), "Assuming that we don't have a destructor");
171
172public:
175
176 static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes");
177
179 struct Tracking
180 {
182 Tracking() = default;
183 Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { }
184
186 static const uint32 cInvalidBodyLocation = 0xffffffff;
187
188 atomic<BroadPhaseLayer::Type> mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
189 atomic<ObjectLayer> mObjectLayer = cObjectLayerInvalid;
190 atomic<uint32> mBodyLocation { cInvalidBodyLocation };
191 };
192
194
196 ~QuadTree();
197
198#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
200 void SetName(const char *inName) { mName = inName; }
201 inline const char * GetName() const { return mName; }
202#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
203
205 inline bool HasBodies() const { return mNumBodies != 0; }
206
208 inline bool IsDirty() const { return mIsDirty; }
209
211 inline bool CanBeUpdated() const { return mFreeNodeBatch.mNumObjects == 0; }
212
214 void Init(Allocator &inAllocator);
215
217 {
218 NodeID mRootNodeID;
219 };
220
222 void DiscardOldTree();
223
225 AABox GetBounds() const;
226
229 void UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
230 void UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
231
233 struct AddState
234 {
235 NodeID mLeafID = NodeID::sInvalid();
237 };
238
242 void AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState);
243
245 void AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState);
246
249 void AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState);
250
252 void RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber);
253
255 void NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber);
256
258 void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
259
261 void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
262
264 void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
265
267 void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
268
270 void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
271
273 void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
274
276 void FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const;
277
278#ifdef JPH_TRACK_BROADPHASE_STATS
280 uint64 GetTicks100Pct() const;
281
283 void ReportStats(uint64 inTicks100Pct) const;
284#endif // JPH_TRACK_BROADPHASE_STATS
285
286private:
288 static constexpr uint32 cInvalidNodeIndex = 0xffffffff;
289 static const AABox cInvalidBounds;
290
292 struct RootNode
293 {
295 inline NodeID GetNodeID() const { return NodeID::sFromNodeIndex(mIndex); }
296
298 atomic<uint32> mIndex { cInvalidNodeIndex };
299 };
300
302 void GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const;
303 void SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const;
304 static void sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
305
307 JPH_INLINE const RootNode & GetCurrentRoot() const { return mRootNode[mRootNodeIndex]; }
308 JPH_INLINE RootNode & GetCurrentRoot() { return mRootNode[mRootNodeIndex]; }
309
311 inline AABox GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const;
312
314 inline void MarkNodeAndParentsChanged(uint32 inNodeIndex);
315
317 inline void WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds);
318
320 inline uint32 AllocateNode(bool inIsChanged);
321
323 inline bool TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
324
326 inline bool TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
327
329 NodeID BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds);
330
333 static void sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint);
334
338 static void sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit);
339
340#ifdef JPH_DEBUG
343 void ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
344#endif
345
346#ifdef JPH_DUMP_BROADPHASE_TREE
348 void DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
349#endif
350
352 Allocator * mAllocator = nullptr;
353
355 Allocator::Batch mFreeNodeBatch;
356
360 alignas(JPH_CACHE_LINE_SIZE) atomic<uint32> mNumBodies { 0 };
361
364 RootNode mRootNode[2];
365 atomic<uint32> mRootNodeIndex { 0 };
366
368 atomic<bool> mIsDirty = false;
369
370#ifdef JPH_TRACK_BROADPHASE_STATS
372 mutable Mutex mStatsMutex;
373
374 struct Stat
375 {
376 uint64 mNumQueries = 0;
377 uint64 mNodesVisited = 0;
378 uint64 mBodiesVisited = 0;
379 uint64 mHitsReported = 0;
380 uint64 mTotalTicks = 0;
381 uint64 mCollectorTicks = 0;
382 };
383
384 using LayerToStats = UnorderedMap<String, Stat>;
385
387 uint64 GetTicks100Pct(const LayerToStats &inLayer) const;
388
390 void ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const;
391
392 mutable LayerToStats mCastRayStats;
393 mutable LayerToStats mCollideAABoxStats;
394 mutable LayerToStats mCollideSphereStats;
395 mutable LayerToStats mCollidePointStats;
396 mutable LayerToStats mCollideOrientedBoxStats;
397 mutable LayerToStats mCastAABoxStats;
398#endif // JPH_TRACK_BROADPHASE_STATS
399
401 uint GetMaxTreeDepth(const NodeID &inNodeID) const;
402
404 template <class Visitor>
405 JPH_INLINE void WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const;
406
407#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
409 const char * mName = "Layer";
410#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
411};
412
#define JPH_IF_TRACK_BROADPHASE_STATS(...)
Definition BroadPhase.h:16
#define JPH_EXPORT
Definition Core.h:303
#define JPH_CACHE_LINE_SIZE
Definition Core.h:613
std::uint64_t uint64
Definition Core.h:569
unsigned int uint
Definition Core.h:564
#define JPH_TSAN_NO_SANITIZE
Definition Core.h:704
#define JPH_NAMESPACE_END
Definition Core.h:487
std::uint32_t uint32
Definition Core.h:567
#define JPH_NAMESPACE_BEGIN
Definition Core.h:481
#define JPH_ASSERT(...)
Definition IssueReporting.h:28
#define JPH_OVERRIDE_NEW_DELETE
Macro to override the new and delete functions.
Definition Memory.h:51
Axis aligned box.
Definition AABox.h:16
Definition Array.h:22
ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding ra...
Definition BodyID.h:13
static constexpr uint32 cBroadPhaseBit
This bit is used by the broadphase.
Definition BodyID.h:18
uint32 GetIndexAndSequenceNumber() const
Returns the index and sequence number combined in an uint32.
Definition BodyID.h:59
uint8 Type
Definition BroadPhaseLayer.h:20
Virtual interface that allows collecting multiple collision results.
Definition CollisionCollector.h:45
Class that holds 4 float values. Convert to Vec4 to perform calculations.
Definition Float4.h:11
Definition Mutex.h:124
Class that makes another class non-copyable. Usage: Inherit from NonCopyable.
Definition NonCopyable.h:11
Filter class for object layers.
Definition ObjectLayer.h:27
Filter class to test if two objects can collide based on their object layer. Used while finding colli...
Definition ObjectLayer.h:49
Oriented box.
Definition OrientedBox.h:18
Definition QuadTree.h:21
const char * GetName() const
Definition QuadTree.h:201
bool HasBodies() const
Check if there is anything in the tree.
Definition QuadTree.h:205
bool IsDirty() const
Check if the tree needs an UpdatePrepare/Finalize()
Definition QuadTree.h:208
bool CanBeUpdated() const
Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first.
Definition QuadTree.h:211
void SetName(const char *inName)
Name of the tree for debugging purposes.
Definition QuadTree.h:200
Definition UVec4.h:12
static JPH_INLINE UVec4 sLoadInt4Aligned(const uint32 *inV)
Load 4 ints from memory, aligned to 16 bytes.
Definition UVec4.inl:118
Definition UnorderedMap.h:31
Definition Vec3.h:17
Definition Vec4.h:14
static JPH_INLINE Vec4 sLoadFloat4Aligned(const Float4 *inV)
Load 4 floats from memory, 16 bytes aligned.
Definition Vec4.inl:139
Structure that holds AABox moving linearly through 3d space.
Definition AABoxCast.h:13
Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort.
Definition QuadTree.h:234
AABox mLeafBounds
Definition QuadTree.h:236
Data to track location of a Body in the tree.
Definition QuadTree.h:180
Tracking(const Tracking &inRHS)
Definition QuadTree.h:183
Tracking()=default
Constructor to satisfy the vector class.
Definition QuadTree.h:217
NodeID mRootNodeID
This will be the new root node id.
Definition QuadTree.h:218
Definition RayCast.h:47