QuadTree.h

Go to the documentation of this file.

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <Jolt/Core/FixedSizeFreeList.h>
#include <Jolt/Core/Atomics.h>
#include <Jolt/Core/NonCopyable.h>
#include <Jolt/Physics/Body/BodyManager.h>
#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
 
//#define JPH_DUMP_BROADPHASE_TREE
 
JPH_NAMESPACE_BEGIN
 
class QuadTree : public NonCopyable
{
public:
    JPH_OVERRIDE_NEW_DELETE
 
private:
    // Forward declare
    class AtomicNodeID;
 
    class NodeID
    {
    public:
        JPH_OVERRIDE_NEW_DELETE
 
        inline                  NodeID() = default;
 
        static inline NodeID    sInvalid()                          { return NodeID(cInvalidNodeIndex); }
        static inline NodeID    sFromBodyID(BodyID inID)            { NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; }
        static inline NodeID    sFromNodeIndex(uint32 inIdx)        { NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; }
 
        inline bool             IsValid() const                     { return mID != cInvalidNodeIndex; }
        inline bool             IsBody() const                      { return (mID & cIsNode) == 0; }
        inline bool             IsNode() const                      { return (mID & cIsNode) != 0; }
 
        inline BodyID           GetBodyID() const                   { JPH_ASSERT(IsBody()); return BodyID(mID); }
        inline uint32           GetNodeIndex() const                { JPH_ASSERT(IsNode()); return mID & ~cIsNode; }
 
        inline bool             operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); }
        inline bool             operator == (const NodeID &inRHS) const { return mID == inRHS.mID; }
 
    private:
        friend class AtomicNodeID;
 
        inline explicit         NodeID(uint32 inID)                 : mID(inID) { }
 
        static const uint32     cIsNode = BodyID::cBroadPhaseBit;   
 
        uint32                  mID;
    };
 
    static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs");
 
    class AtomicNodeID
    {
    public:
                                AtomicNodeID() = default;
        explicit                AtomicNodeID(const NodeID &inRHS)           : mID(inRHS.mID) { }
 
        inline void             operator = (const NodeID &inRHS)            { mID = inRHS.mID; }
 
        inline                  operator NodeID () const                    { return NodeID(mID); }
 
        inline bool             IsValid() const                             { return mID != cInvalidNodeIndex; }
 
        inline bool             operator == (const BodyID &inRHS) const     { return mID == inRHS.GetIndexAndSequenceNumber(); }
        inline bool             operator == (const NodeID &inRHS) const     { return mID == inRHS.mID; }
 
        inline bool             CompareExchange(NodeID inOld, NodeID inNew) { return mID.compare_exchange_strong(inOld.mID, inNew.mID); }
 
    private:
        atomic<uint32>          mID;
    };
 
    class Node
    {
    public:
        explicit                Node(bool inIsChanged);
 
        void                    GetNodeBounds(AABox &outBounds) const;
 
        void                    GetChildBounds(int inChildIndex, AABox &outBounds) const;
 
        void                    SetChildBounds(int inChildIndex, const AABox &inBounds);
 
        void                    InvalidateChildBounds(int inChildIndex);
 
        bool                    EncapsulateChildBounds(int inChildIndex, const AABox &inBounds);
 
        atomic<float>           mBoundsMinX[4];
        atomic<float>           mBoundsMinY[4];
        atomic<float>           mBoundsMinZ[4];
        atomic<float>           mBoundsMaxX[4];
        atomic<float>           mBoundsMaxY[4];
        atomic<float>           mBoundsMaxZ[4];
 
        AtomicNodeID            mChildNodeID[4];
 
        atomic<uint32>          mParentNodeIndex = cInvalidNodeIndex;
 
        atomic<uint32>          mIsChanged;
 
        // Padding to align to 124 bytes
        uint32                  mPadding = 0;
    };
 
    // Maximum size of the stack during tree walk
    static constexpr int        cStackSize = 128;
 
    static_assert(sizeof(atomic<float>) == 4, "Assuming that an atomic doesn't add any additional storage");
    static_assert(sizeof(atomic<uint32>) == 4, "Assuming that an atomic doesn't add any additional storage");
    static_assert(is_trivially_destructible<Node>(), "Assuming that we don't have a destructor");
 
public:
    using Allocator = FixedSizeFreeList<Node>;
 
    static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes");
 
    struct Tracking
    {
                                Tracking() = default;
                                Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { }
 
        static const uint32     cInvalidBodyLocation = 0xffffffff;
 
        atomic<BroadPhaseLayer::Type> mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
        atomic<ObjectLayer>     mObjectLayer = cObjectLayerInvalid;
        atomic<uint32>          mBodyLocation { cInvalidBodyLocation };
    };
 
    using TrackingVector = Array<Tracking>;
 
                                ~QuadTree();
 
#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
    void                        SetName(const char *inName)         { mName = inName; }
    inline const char *         GetName() const                     { return mName; }
#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
 
    inline bool                 HasBodies() const                   { return mNumBodies != 0; }
 
    inline bool                 IsDirty() const                     { return mIsDirty; }
 
    inline bool                 CanBeUpdated() const                { return mFreeNodeBatch.mNumObjects == 0; }
 
    void                        Init(Allocator &inAllocator);
 
    struct UpdateState
    {
        NodeID                  mRootNodeID;                        
    };
 
    void                        DiscardOldTree();
 
    void                        UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
    void                        UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
 
    struct AddState
    {
        NodeID                  mLeafID = NodeID::sInvalid();
        AABox                   mLeafBounds;
    };
 
    void                        AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState);
 
    void                        AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState);
 
    void                        AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState);
 
    void                        RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber);
 
    void                        NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber);
 
    void                        CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
 
    void                        CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
 
    void                        CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
 
    void                        CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
    
    void                        CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
 
    void                        CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
 
    void                        FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const;
 
#ifdef JPH_TRACK_BROADPHASE_STATS
    void                        ReportStats() const;
#endif // JPH_TRACK_BROADPHASE_STATS
 
private:
    static const uint32         cInvalidNodeIndex = 0xffffffff;     
    static const float          cLargeFloat;                        
    static const AABox          cInvalidBounds;                     
 
    struct RootNode
    {
        inline NodeID           GetNodeID() const                   { return NodeID::sFromNodeIndex(mIndex); }
 
        atomic<uint32>          mIndex { cInvalidNodeIndex };
    };
 
    void                        GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const;
    void                        SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const;
    static void                 sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
 
    JPH_INLINE const RootNode & GetCurrentRoot() const              { return mRootNode[mRootNodeIndex]; }
    JPH_INLINE RootNode &       GetCurrentRoot()                    { return mRootNode[mRootNodeIndex]; }
 
    inline AABox                GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const;
 
    inline void                 MarkNodeAndParentsChanged(uint32 inNodeIndex);
 
    inline void                 WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds);
 
    inline uint32               AllocateNode(bool inIsChanged);
 
    inline bool                 TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
 
    inline bool                 TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
 
    NodeID                      BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds);
 
    static void                 sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint);
 
    static void                 sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit);
 
#ifdef _DEBUG
    void                        ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
#endif
 
#ifdef JPH_DUMP_BROADPHASE_TREE
    void                        DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
#endif
 
#ifdef JPH_TRACK_BROADPHASE_STATS
    mutable Mutex               mStatsMutex;
 
    struct Stat
    {
        uint64                  mNumQueries = 0;
        uint64                  mNodesVisited = 0;
        uint64                  mBodiesVisited = 0;
        uint64                  mHitsReported = 0;
        uint64                  mTotalTicks = 0;
        uint64                  mCollectorTicks = 0;
    };
    
    using LayerToStats = UnorderedMap<String, Stat>;
 
    void                        ReportStats(const char *inName, const LayerToStats &inLayer) const;
    
    mutable LayerToStats        mCastRayStats;
    mutable LayerToStats        mCollideAABoxStats;
    mutable LayerToStats        mCollideSphereStats;
    mutable LayerToStats        mCollidePointStats;
    mutable LayerToStats        mCollideOrientedBoxStats;
    mutable LayerToStats        mCastAABoxStats;
#endif // JPH_TRACK_BROADPHASE_STATS
 
    uint                        GetMaxTreeDepth(const NodeID &inNodeID) const;
 
    template <class Visitor>
    JPH_INLINE void             WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const;
 
#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
    const char *                mName = "Layer";
#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
 
    atomic<uint32>              mNumBodies { 0 };
 
    RootNode                    mRootNode[2];
    atomic<uint32>              mRootNodeIndex { 0 };
 
    Allocator *                 mAllocator = nullptr;
 
    Allocator::Batch            mFreeNodeBatch;
 
    atomic<bool>                mIsDirty = false;
};
 
JPH_NAMESPACE_END