BroadphaseTree.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "ecs/physics/SweptCollision.hpp"
 
#include <cstdint>
#include <entt/entt.hpp>
#include <functional>
#include <glm/vec3.hpp>
#include <vector>
 
namespace physics::broadphase
{

struct Proxy
{
    int32_t id = -1;
    [[nodiscard]] bool valid() const noexcept { return id >= 0; }
};

class Tree
{
public:
    static constexpr float k_fatAabbMargin = 4.0f;

    static constexpr float k_motionPredict = 2.0f;
 
    Tree() = default;

    Proxy insertProxy(const WorldAABB& aabb, entt::entity entity);

    bool updateProxy(Proxy p, const WorldAABB& newAabb, glm::vec3 displacement);

    void removeProxy(Proxy p);

    void queryAABB(const WorldAABB& aabb, const std::function<bool(entt::entity)>& visit) const;

    void raycast(glm::vec3 origin, glm::vec3 dir, float maxT, const std::function<bool(entt::entity)>& visit) const;

    void sweptAABB(glm::vec3 halfExtents,
                   glm::vec3 start,
                   glm::vec3 end,
                   const std::function<bool(entt::entity)>& visit) const;

    void clear();

    [[nodiscard]] int proxyCount() const noexcept { return proxyCount_; }

    [[nodiscard]] int32_t rootIndex() const noexcept { return root_; }

    [[nodiscard]] WorldAABB nodeAabb(int32_t nodeId) const noexcept;

    [[nodiscard]] bool isLeaf(int32_t nodeId) const noexcept;

    [[nodiscard]] int32_t leftChild(int32_t nodeId) const noexcept;
    [[nodiscard]] int32_t rightChild(int32_t nodeId) const noexcept;
 
private:
    struct Node
    {
        WorldAABB aabb{glm::vec3{0.0f}, glm::vec3{0.0f}};
        entt::entity entity{entt::null}; 
        int32_t parent = -1;
        int32_t left = -1;
        int32_t right = -1;
        int32_t height = 0; 
    };
 
    std::vector<Node> nodes_;
    int32_t root_ = -1;
    int32_t freeList_ = -1;
    int proxyCount_ = 0;
 
    int32_t allocNode();
    void freeNode(int32_t id);
    void insertLeaf(int32_t leaf);
    void removeLeaf(int32_t leaf);
    static WorldAABB combine(const WorldAABB& a, const WorldAABB& b);
    static float aabbArea(const WorldAABB& a);
    static bool aabbOverlap(const WorldAABB& a, const WorldAABB& b);
    static bool aabbContains(const WorldAABB& container, const WorldAABB& inside);
    int32_t balance(int32_t a);
};
 
} // namespace physics::broadphase