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) { NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; }
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
119 atomic<float> mBoundsMinX[4];
120 atomic<float> mBoundsMinY[4];
121 atomic<float> mBoundsMinZ[4];
122 atomic<float> mBoundsMaxX[4];
123 atomic<float> mBoundsMaxY[4];
124 atomic<float> mBoundsMaxZ[4];
125
127 AtomicNodeID mChildNodeID[4];
128
131 atomic<uint32> mParentNodeIndex = cInvalidNodeIndex;
132
135 atomic<uint32> mIsChanged;
136
137 // Padding to align to 124 bytes
138 uint32 mPadding = 0;
139 };
140
141 // Maximum size of the stack during tree walk
142 static constexpr int cStackSize = 128;
143
144 static_assert(sizeof(atomic<float>) == 4, "Assuming that an atomic doesn't add any additional storage");
145 static_assert(sizeof(atomic<uint32>) == 4, "Assuming that an atomic doesn't add any additional storage");
146 static_assert(is_trivially_destructible<Node>(), "Assuming that we don't have a destructor");
147
148public:
151
152 static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes");
153
155 struct Tracking
156 {
158 Tracking() = default;
159 Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { }
160
162 static const uint32 cInvalidBodyLocation = 0xffffffff;
163
164 atomic<BroadPhaseLayer::Type> mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
165 atomic<ObjectLayer> mObjectLayer = cObjectLayerInvalid;
166 atomic<uint32> mBodyLocation { cInvalidBodyLocation };
167 };
168
170
172 ~QuadTree();
173
174#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
176 void SetName(const char *inName) { mName = inName; }
177 inline const char * GetName() const { return mName; }
178#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
179
181 inline bool HasBodies() const { return mNumBodies != 0; }
182
184 inline bool IsDirty() const { return mIsDirty; }
185
187 inline bool CanBeUpdated() const { return mFreeNodeBatch.mNumObjects == 0; }
188
190 void Init(Allocator &inAllocator);
191
193 {
194 NodeID mRootNodeID;
195 };
196
198 void DiscardOldTree();
199
201 AABox GetBounds() const;
202
205 void UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
206 void UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
207
209 struct AddState
210 {
211 NodeID mLeafID = NodeID::sInvalid();
213 };
214
218 void AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState);
219
221 void AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState);
222
225 void AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState);
226
228 void RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber);
229
231 void NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber);
232
234 void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
235
237 void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
238
240 void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
241
243 void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
244
246 void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
247
249 void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
250
252 void FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const;
253
254#ifdef JPH_TRACK_BROADPHASE_STATS
256 uint64 GetTicks100Pct() const;
257
259 void ReportStats(uint64 inTicks100Pct) const;
260#endif // JPH_TRACK_BROADPHASE_STATS
261
262private:
264 static const uint32 cInvalidNodeIndex = 0xffffffff;
265 static const float cLargeFloat;
266 static const AABox cInvalidBounds;
267
269 struct RootNode
270 {
272 inline NodeID GetNodeID() const { return NodeID::sFromNodeIndex(mIndex); }
273
275 atomic<uint32> mIndex { cInvalidNodeIndex };
276 };
277
279 void GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const;
280 void SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const;
281 static void sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
282
284 JPH_INLINE const RootNode & GetCurrentRoot() const { return mRootNode[mRootNodeIndex]; }
285 JPH_INLINE RootNode & GetCurrentRoot() { return mRootNode[mRootNodeIndex]; }
286
288 inline AABox GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const;
289
291 inline void MarkNodeAndParentsChanged(uint32 inNodeIndex);
292
294 inline void WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds);
295
297 inline uint32 AllocateNode(bool inIsChanged);
298
300 inline bool TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
301
303 inline bool TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
304
306 NodeID BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds);
307
310 static void sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint);
311
315 static void sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit);
316
317#ifdef _DEBUG
320 void ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
321#endif
322
323#ifdef JPH_DUMP_BROADPHASE_TREE
325 void DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
326#endif
327
328#ifdef JPH_TRACK_BROADPHASE_STATS
330 mutable Mutex mStatsMutex;
331
332 struct Stat
333 {
334 uint64 mNumQueries = 0;
335 uint64 mNodesVisited = 0;
336 uint64 mBodiesVisited = 0;
337 uint64 mHitsReported = 0;
338 uint64 mTotalTicks = 0;
339 uint64 mCollectorTicks = 0;
340 };
341
342 using LayerToStats = UnorderedMap<String, Stat>;
343
345 uint64 GetTicks100Pct(const LayerToStats &inLayer) const;
346
348 void ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const;
349
350 mutable LayerToStats mCastRayStats;
351 mutable LayerToStats mCollideAABoxStats;
352 mutable LayerToStats mCollideSphereStats;
353 mutable LayerToStats mCollidePointStats;
354 mutable LayerToStats mCollideOrientedBoxStats;
355 mutable LayerToStats mCastAABoxStats;
356#endif // JPH_TRACK_BROADPHASE_STATS
357
359 uint GetMaxTreeDepth(const NodeID &inNodeID) const;
360
362 template <class Visitor>
363 JPH_INLINE void WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const;
364
365#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
367 const char * mName = "Layer";
368#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
369
371 atomic<uint32> mNumBodies { 0 };
372
375 RootNode mRootNode[2];
376 atomic<uint32> mRootNodeIndex { 0 };
377
379 Allocator * mAllocator = nullptr;
380
382 Allocator::Batch mFreeNodeBatch;
383
385 atomic<bool> mIsDirty = false;
386};
387
Array< Body * > BodyVector
Array of bodies.
Definition: BodyManager.h:25
#define JPH_IF_TRACK_BROADPHASE_STATS(...)
Definition: BroadPhase.h:16
#define JPH_EXPORT
Definition: Core.h:227
std::uint64_t uint64
Definition: Core.h:443
unsigned int uint
Definition: Core.h:439
#define JPH_NAMESPACE_END
Definition: Core.h:367
std::uint32_t uint32
Definition: Core.h:442
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
#define JPH_OVERRIDE_NEW_DELETE
Macro to override the new and delete functions.
Definition: Memory.h:29
std::vector< T, STLAllocator< T > > Array
Definition: STLAllocator.h:81
std::unordered_map< Key, T, Hash, KeyEqual, STLAllocator< pair< const Key, T > > > UnorderedMap
Definition: UnorderedMap.h:13
Axis aligned box.
Definition: AABox.h:16
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:58
uint8 Type
Definition: BroadPhaseLayer.h:20
Virtual interface that allows collecting multiple collision results.
Definition: CollisionCollector.h:45
Definition: Mutex.h:122
Class that makes another class non-copyable. Usage: Inherit from NonCopyable.
Definition: NonCopyable.h:11
Filter class for object layers.
Definition: ObjectLayer.h:28
Filter class to test if two objects can collide based on their object layer. Used while finding colli...
Definition: ObjectLayer.h:50
Oriented box.
Definition: OrientedBox.h:18
Definition: QuadTree.h:21
const char * GetName() const
Definition: QuadTree.h:177
bool HasBodies() const
Check if there is anything in the tree.
Definition: QuadTree.h:181
bool IsDirty() const
Check if the tree needs an UpdatePrepare/Finalize()
Definition: QuadTree.h:184
bool CanBeUpdated() const
Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first.
Definition: QuadTree.h:187
Array< Tracking > TrackingVector
Definition: QuadTree.h:169
void SetName(const char *inName)
Name of the tree for debugging purposes.
Definition: QuadTree.h:176
Definition: Vec3.h:16
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:210
AABox mLeafBounds
Definition: QuadTree.h:212
Data to track location of a Body in the tree.
Definition: QuadTree.h:156
Tracking(const Tracking &inRHS)
Definition: QuadTree.h:159
Tracking()=default
Constructor to satisfy the vector class.
Definition: QuadTree.h:193
NodeID mRootNodeID
This will be the new root node id.
Definition: QuadTree.h:194
Definition: RayCast.h:47