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 JPH_EXPORT 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)        { JPH_ASSERT((inIdx & cIsNode) == 0); return NodeID(inIdx | cIsNode); }

        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(std::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();

    AABox                       GetBounds() const;

    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
    uint64                      GetTicks100Pct() const;

    void                        ReportStats(uint64 inTicks100Pct) const;
#endif // JPH_TRACK_BROADPHASE_STATS
 
private:
    static constexpr uint32     cInvalidNodeIndex = 0xffffffff;     
    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 JPH_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

    Allocator *                 mAllocator = nullptr;

    Allocator::Batch            mFreeNodeBatch;

    alignas(JPH_CACHE_LINE_SIZE) atomic<uint32> mNumBodies { 0 };

    RootNode                    mRootNode[2];
    atomic<uint32>              mRootNodeIndex { 0 };

    atomic<bool>                mIsDirty = false;
 
#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>;

    uint64                      GetTicks100Pct(const LayerToStats &inLayer) const;

    void                        ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) 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
};
 
JPH_NAMESPACE_END