Gfrktyl

Here is a C++ implementation of a bounding volume hierarchy, aimed for fast collision detection (view frustum, rays, other bounding volumes). It is based on the ideas from the book "Real-Time Rendering, Third Edition". Would like to hear some thoughts:

#ifndef BOUNDINGVOLUMEHIERARCHY_H

#define BOUNDINGVOLUMEHIERARCHY_H



#include <Camera.h>

#include <vector>

#include <IntersectionTests.h>

#include <CullResult.h>

#include <Ray.h>

#include <boost/pool/object_pool.hpp>



namespace Log2

{

  template<typename T, typename BV, typename Proxy, typename TNested>

  class BoundingVolumeHierarchy

  {

  public:

    class NodePool;



    static const auto Dim = BV::getDim();



    using Type = typename BV::Type;

    using Ray = Ray<Dim, Type>;

    using Vector = Vector<Dim, Type>;

    using Matrix = Matrix<Dim + 1, Dim + 1, Type>;



    struct ObjectInfo

    {

      T* _object;

      BV _bv;

      Type _bvSize;

    };

  private:

    auto getBV(const T& object)

    {

      return Proxy::GetBoundingVolume()(object);

    }

    auto getSize(const T& object)

    {

      return Proxy::GetLargestBVSize()(object);

    }

    ObjectInfo createInfo(T* object)

    {

      return { object, getBV(*object), getSize(*object) };

    }

  public:

    class Node

    {

    public:

      Node() = default;

      virtual ~Node() = default;

      virtual void cullVisibleObjects(const Camera::CullingParams& cp, CullResult<T>& cull_result) const = 0;

      virtual void cullVisibleObjects(const Camera::CullingParams& cp, const IntersectedPlanes& in, CullResult<T>& cull_result) const = 0;

      virtual void cullAllObjects(const Camera::CullingParams& cp, std::vector<T>& objects) const = 0;

      virtual size_t getSizeInBytes() const = 0;

      virtual void intersectObjects(const BV& bv, std::vector<T>& intersected_objects) const = 0;

      virtual void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T>& intersected_objects) const = 0;

      virtual void intersectObjects(const BV& bv, std::vector<T const *>& intersected_objects) const = 0;

      virtual void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T const *>& intersected_objects) const = 0;

      virtual void intersectFirst(const BV& bv, T const *& first) const = 0;

      virtual void intersectFirst(const BV& bv, const Matrix& transform, T const *& first) const = 0;

      virtual void cullVisibleNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const = 0;

      virtual void cullAllNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const = 0;

      virtual void countNodes(unsigned& internal_nodes, unsigned& leaf_nodes) const = 0;

      virtual void getAllBVs(std::vector<BV>& bvs) const = 0;

      virtual void cullBVs(const Camera::CullingParams& cp, const Matrix& transform, std::vector<BV>& result) const = 0;

      virtual void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects) const = 0;

      virtual void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects) const = 0;

      virtual void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects, std::vector<BV>& bvs) const = 0;

      virtual void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects, std::vector<BV>& bvs) const = 0;

      virtual void findNearest(const Ray& ray, const Matrix& transform, T const *& nearest, Type& nearest_t, Matrix& nearest_transform) const = 0;

      virtual void findNearest(const Ray& ray, const Matrix& transform, T const *& nearest, Type& nearest_t, Matrix& nearest_transform, std::vector<BV>& bvs) const = 0;

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const = 0;

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) = 0;

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform, std::vector<BV>& bvs) const = 0;

      virtual void findNearestNested(const Ray& ray, TNested const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const = 0;

      virtual void findNearestNested(const Ray& ray, TNested *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) = 0;

      virtual void queryRange(const BV& bv, std::vector<T>& intersected_objects) const = 0;

      virtual void queryAll(std::vector<T>& intersected_objects) const = 0;

      virtual void queryRange(const BV& bv, const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const = 0;

      virtual void queryAll(const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const = 0;

      virtual bool isLeaf() const = 0;

      virtual Node* getLeft() const = 0;

      virtual Node* getRight() const = 0;

      virtual Node*& getLeft() = 0;

      virtual Node*& getRight() = 0;

      virtual T * getObjectPtr() = 0;

      virtual BV getBV() const = 0;

      virtual BV const * getBVPtr() const = 0;

      virtual BV * getBVPtr() = 0;

      virtual Type distToPoint2(const Vector& point) const = 0;

      virtual void destroy(NodePool& pool) = 0;

      virtual Type* getLargestBVSize() = 0;

      virtual Type cost(const BV& bv) const = 0;

    };

    class LeafNode : public Node

    {

    public:

      explicit LeafNode(T object)

        : Node(),

        _object(object)

      {

      }

      virtual ~LeafNode() = default;

      virtual void cullVisibleObjects(const Camera::CullingParams& cp, CullResult<T>& cull_result) const override

      {

        cull_result._probablyVisibleObjects.push_back(_object);

      }

      virtual void cullVisibleObjects(const Camera::CullingParams& cp, const IntersectedPlanes& in, CullResult<T>& cull_result) const override

      {

        cull_result._probablyVisibleObjects.push_back(_object);

      }

      virtual void cullAllObjects(const Camera::CullingParams& cp, std::vector<T>& objects) const override

      {

        objects.push_back(_object);

      }

      virtual size_t getSizeInBytes() const override

      {

        return sizeof *this;

      }

      virtual void intersectObjects(const BV& bv, std::vector<T>& intersected_objects) const override

      {

        if (bv.intersects(getBV())) {

          intersected_objects.push_back(_object);

        }

      }

      virtual void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T>& intersected_objects) const override

      {

        if (bv.intersects(BV(getBV(), transform))) {

          intersected_objects.push_back(_object);

        }

      }

      virtual void intersectObjects(const BV& bv, std::vector<T const *>& intersected_objects) const override

      {

        if (bv.intersects(getBV())) {

          intersected_objects.push_back(&_object);

        }

      }

      virtual void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T const *>& intersected_objects) const override

      {

        if (bv.intersects(BV(getBV(), transform))) {

          intersected_objects.push_back(&_object);

        }

      }

      virtual void intersectFirst(const BV& bv, T const *& first) const override

      {

        if (!first && bv.intersects(getBV())) {

          first = &_object;

        }

      }

      virtual void intersectFirst(const BV& bv, const Matrix& transform, T const *& first) const override

      {

        if (!first && bv.intersects(BV(getBV(), transform))) {

          first = &_object;

        }

      }

      virtual void cullVisibleNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const override

      {

        if (contributes(cp) && intersectFrustum(cp) != IntersectionResult::OUTSIDE) {

          nodes.push_back(this);

        }

      }

      virtual void cullAllNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const override

      {

        if (contributes(cp)) {

          nodes.push_back(this);

        }

      }

      virtual void countNodes(unsigned& internal_nodes, unsigned& leaf_nodes) const override

      {

        leaf_nodes++;

      }

      virtual void getAllBVs(std::vector<BV>& bvs) const override

      {

        bvs.push_back(getBV());

      }

      virtual void cullBVs(const Camera::CullingParams& cp, const Matrix& transform, std::vector<BV>& result) const override

      {

        BV bv(getBV(), transform);

        if (bv.contributes(cp._camPos, cp._thresh)) {

          result.push_back(bv);

        }

      }

      virtual void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(getBV(), ray))) {

          intersected_objects.push_back(_object);

        }

      }

      virtual void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray))) {

          intersected_objects.push_back(_object);

        }

      }

      virtual void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects, std::vector<BV>& bvs) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(getBV(), ray))) {

          intersected_objects.push_back(_object);

          bvs.push_back(getBV());

        }

      }

      virtual void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects, std::vector<BV>& bvs) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray))) {

          intersected_objects.push_back(_object);

          bvs.push_back(BV(getBV(), transform));

        }

      }

      virtual void findNearest(const Ray& ray, const Matrix& transform, T const *& nearest, Type& nearest_t, Matrix& nearest_transform) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < nearest_t) {

          nearest = &_object;

          nearest_t = std::get<1>(result);

          nearest_transform = transform;

        }

      }

      virtual void findNearest(const Ray& ray, const Matrix& transform, T const *& nearest, Type& nearest_t, Matrix& nearest_transform, std::vector<BV>& bvs) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < nearest_t) {

          nearest = &_object;

          nearest_t = std::get<1>(result);

          nearest_transform = transform;

          bvs.push_back(BV(getBV(), transform));

        }

      }

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray))) {

          auto result = Proxy::IntersectRay()(_object, ray, transform);

          if (std::get<0>(result) && std::get<3>(result) < t) {

            nearest = &_object;

            nearest_transform = transform;

            u = std::get<1>(result);

            v = std::get<2>(result);

            t = std::get<3>(result);

          }

        }

      }

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray))) {

          auto result = Proxy::IntersectRay()(_object, ray, transform);

          if (std::get<0>(result) && std::get<3>(result) < t) {

            nearest = &_object;

            nearest_transform = transform;

            u = std::get<1>(result);

            v = std::get<2>(result);

            t = std::get<3>(result);

          }

        }

      }

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform, std::vector<BV>& bvs) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(getBV(), transform), ray))) {

          auto result = Proxy::IntersectRay()(_object, ray, transform);

          if (std::get<0>(result) && std::get<3>(result) < t) {

            nearest = &_object;

            nearest_transform = transform;

            u = std::get<1>(result);

            v = std::get<2>(result);

            t = std::get<3>(result);

            bvs.push_back(BV(getBV(), transform));

          }

        }

      }

      virtual void findNearestNested(const Ray& ray, TNested const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const override

      {

        Proxy::FindNearestNestedConst()(_object, ray, nearest, u, v, t, nearest_transform);

      }

      virtual void findNearestNested(const Ray& ray, TNested *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) override

      {

        Proxy::FindNearestNested()(_object, ray, nearest, u, v, t, nearest_transform);

      }

      virtual void queryRange(const BV& bv, std::vector<T>& intersected_objects) const override

      {

        if (bv.intersects(getBV())) {

          intersected_objects.push_back(_object);

        }

      }

      virtual void queryAll(std::vector<T>& intersected_objects) const override

      {

        intersected_objects.push_back(_object);

      }

      virtual void queryRange(const BV& bv, const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const override

      {

        if (contributes(cp) && bv.intersects(getBV())) {

          intersected_objects.push_back(_object);

        }

      }

      virtual void queryAll(const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const override

      {

        if (contributes(cp)) {

          intersected_objects.push_back(_object);

        }

      }

      virtual bool isLeaf() const override

      {

        return true;

      }

      virtual Node* getLeft() const override

      {

        throw std::exception("Don't call this method");

        return nullptr;

      }

      virtual Node* getRight() const override

      {

        throw std::exception("Don't call this method");

        return nullptr;

      }

      virtual Node*& getLeft() override

      {

        throw std::exception("Don't call this method");

      }

      virtual Node*& getRight() override

      {

        throw std::exception("Don't call this method");

      }

      virtual T * getObjectPtr() override

      {

        return &_object;

      }

      virtual BV getBV() const override

      {

        return Proxy::GetBoundingVolume()(_object);

      }

      virtual BV const * getBVPtr() const override

      {

        throw std::exception("Don't call this method");

        return nullptr;

      }

      virtual BV * getBVPtr() override

      {

        throw std::exception("Don't call this method");

        return nullptr;

      }

      virtual Type distToPoint2(const Vector& point) const override

      {

        return Proxy::GetBoundingVolume()(_object).distToPoint2(point);

      }

      virtual void destroy(NodePool& pool) override

      {

        pool.destroy(this);

      }

      virtual Type* getLargestBVSize() override

      {

        throw std::exception("Don't call this method");

        return nullptr;

      }

      virtual Type cost(const BV& bv) const override

      {

        return bv.getUnion(getBV()).size2();

      }

    private:

      T _object;

      auto contributes(const Camera::CullingParams& cp) const

      {

        return getBV().contributes(cp._camPos, cp._thresh, Proxy::GetLargestBVSize()(_object));

      }

      auto intersectFrustum(const Camera::CullingParams& cp) const

      {

        return IntersectionTests::frustumIntersectsBoundingVolume(getBV(), cp._frustumPlanes);

      }

    };

    using NodePtr = Node * ;

    class InternalNode : public Node

    {

    public:

      InternalNode(ObjectInfo* begin, ObjectInfo* end, BoundingVolumeHierarchy& bvh)

        : Node(),

        _bv(begin->_bv),

        _largestBVSize(begin->_bvSize)

      {

        for (auto const* ptr = begin + 1; ptr < end; ptr++) {

          _bv.unify(ptr->_bv);

          maximize(ptr->_bvSize, _largestBVSize);

        }

        auto axis = _bv.getLongestAxis();

        auto bv_center = _bv.center(axis);

        auto mid = std::partition(begin, end, [&axis, &bv_center](const ObjectInfo& o) {

          return o._bv.center(axis) < bv_center;

        });

        if (mid == begin || mid == end) {

          mid = begin + (end - begin) / 2u;

        }

        _left = bvh.createNode(begin, mid);

        _right = bvh.createNode(mid, end);

      }

      virtual ~InternalNode() = default;

      virtual void cullVisibleObjects(const Camera::CullingParams& cp, CullResult<T>& cull_result) const override

      {

        if (contributes(cp)) {

          auto result = intersectFrustum(cp);

          if (result == IntersectionResult::INTERSECTING) {

            _left->cullVisibleObjects(cp, cull_result);

            _right->cullVisibleObjects(cp, cull_result);

          }

          else if (result == IntersectionResult::INSIDE) {

            _left->cullAllObjects(cp, cull_result._fullyVisibleObjects);

            _right->cullAllObjects(cp, cull_result._fullyVisibleObjects);

          }

        }

      }

      virtual void cullVisibleObjects(const Camera::CullingParams& cp, const IntersectedPlanes& in, CullResult<T>& cull_result) const override

      {

        if (contributes(cp)) {

          IntersectedPlanes out;

          auto result = intersectFrustum(cp, in, out);

          if (result == IntersectionResult::INTERSECTING) {

            _left->cullVisibleObjects(cp, out, cull_result);

            _right->cullVisibleObjects(cp, out, cull_result);

          }

          else if (result == IntersectionResult::INSIDE) {

            _left->cullAllObjects(cp, cull_result._fullyVisibleObjects);

            _right->cullAllObjects(cp, cull_result._fullyVisibleObjects);

          }

        }

      }

      virtual void cullAllObjects(const Camera::CullingParams& cp, std::vector<T>& objects) const override

      {

        if (contributes(cp)) {

          _left->cullAllObjects(cp, objects);

          _right->cullAllObjects(cp, objects);

        }

      }

      virtual size_t getSizeInBytes() const override

      {

        return sizeof *this + _left->getSizeInBytes() + _right->getSizeInBytes();

      }

      virtual void intersectObjects(const BV& bv, std::vector<T>& intersected_objects) const override

      {

        if (bv.intersects(_bv)) {

          _left->intersectObjects(bv, intersected_objects);

          _right->intersectObjects(bv, intersected_objects);

        }

      }

      virtual void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T>& intersected_objects) const override

      {

        if (bv.intersects(BV(_bv, transform))) {

          _left->intersectObjects(bv, transform, intersected_objects);

          _right->intersectObjects(bv, transform, intersected_objects);

        }

      }

      virtual void intersectObjects(const BV& bv, std::vector<T const *>& intersected_objects) const override

      {

        if (bv.intersects(_bv)) {

          _left->intersectObjects(bv, intersected_objects);

          _right->intersectObjects(bv, intersected_objects);

        }

      }

      virtual void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T const *>& intersected_objects) const override

      {

        if (bv.intersects(BV(_bv, transform))) {

          _left->intersectObjects(bv, transform, intersected_objects);

          _right->intersectObjects(bv, transform, intersected_objects);

        }

      }

      virtual void intersectFirst(const BV& bv, T const *& first) const override

      {

        if (!first && bv.intersects(_bv)) {

          _left->intersectFirst(bv, first);

          _right->intersectFirst(bv, first);

        }

      }

      virtual void intersectFirst(const BV& bv, const Matrix& transform, T const *& first) const override

      {

        if (!first && bv.intersects(BV(_bv, transform))) {

          _left->intersectFirst(bv, transform, first);

          _right->intersectFirst(bv, transform, first);

        }

      }

      virtual void cullVisibleNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const override

      {

        if (contributes(cp)) {

          auto result = intersectFrustum(cp);

          if (result == IntersectionResult::INSIDE) {

            nodes.push_back(this);

            _left->cullAllNodes(cp, nodes);

            _right->cullAllNodes(cp, nodes);

          }

          else if (result == IntersectionResult::INTERSECTING) {

            nodes.push_back(this);

            _left->cullVisibleNodes(cp, nodes);

            _right->cullVisibleNodes(cp, nodes);

          }

        }

      }

      virtual void cullAllNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const override

      {

        if (contributes(cp)) {

          nodes.push_back(this);

          _left->cullAllNodes(cp, nodes);

          _right->cullAllNodes(cp, nodes);

        }

      }

      virtual void countNodes(unsigned& internal_nodes, unsigned& leaf_nodes) const override

      {

        internal_nodes++;

        _left->countNodes(internal_nodes, leaf_nodes);

        _right->countNodes(internal_nodes, leaf_nodes);

      }

      virtual void getAllBVs(std::vector<BV>& bvs) const override

      {

        bvs.push_back(_bv);

        _left->getAllBVs(bvs);

        _right->getAllBVs(bvs);

      }

      virtual void cullBVs(const Camera::CullingParams& cp, const Matrix& transform, std::vector<BV>& result) const override

      {

        BV bv(_bv, transform);

        if (bv.contributes(cp._camPos, cp._thresh)) {

          result.push_back(bv);

          _left->cullBVs(cp, transform, result);

          _right->cullBVs(cp, transform, result);

        }

      }

      virtual void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(_bv, ray))) {

          _left->intersectObjects(ray, intersected_objects);

          _right->intersectObjects(ray, intersected_objects);

        }

      }

      virtual void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray))) {

          _left->intersectObjects(ray, transform, intersected_objects);

          _right->intersectObjects(ray, transform, intersected_objects);

        }

      }

      virtual void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects, std::vector<BV>& bvs) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(_bv, ray))) {

          _left->intersectObjects(ray, intersected_objects, bvs);

          _right->intersectObjects(ray, intersected_objects, bvs);

          bvs.push_back(_bv);

        }

      }

      virtual void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects, std::vector<BV>& bvs) const override

      {

        if (std::get<0>(IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray))) {

          _left->intersectObjects(ray, transform, intersected_objects, bvs);

          _right->intersectObjects(ray, transform, intersected_objects, bvs);

          bvs.push_back(BV(_bv, transform));

        }

      }

      virtual void findNearest(const Ray& ray, const Matrix& transform, T const *& nearest, Type& nearest_t, Matrix& nearest_transform) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < nearest_t) {

          _left->findNearest(ray, transform, nearest, nearest_t, nearest_transform);

          _right->findNearest(ray, transform, nearest, nearest_t, nearest_transform);

        }

      }

      virtual void findNearest(const Ray& ray, const Matrix& transform, T const *& nearest, Type& nearest_t, Matrix& nearest_transform, std::vector<BV>& bvs) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < nearest_t) {

          bvs.push_back(BV(_bv, transform));

          _left->findNearest(ray, transform, nearest, nearest_t, nearest_transform, bvs);

          _right->findNearest(ray, transform, nearest, nearest_t, nearest_transform, bvs);

        }

      }

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < t) {

          _left->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform);

          _right->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform);

        }

      }

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < t) {

          _left->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform);

          _right->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform);

        }

      }

      virtual void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform, std::vector<BV>& bvs) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(BV(_bv, transform), ray);

        if (std::get<0>(result) && std::get<1>(result) < t) {

          bvs.push_back(BV(_bv, transform));

          _left->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform, bvs);

          _right->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform, bvs);

        }

      }

      virtual void findNearestNested(const Ray& ray, TNested const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(_bv, ray);

        if (std::get<0>(result) && std::get<1>(result) < t) {

          Node* nodes[2] = { _left, _right };

          auto result = _left->distToPoint2(ray.getOrigin()) < _right->distToPoint2(ray.getOrigin());

          nodes[!result]->findNearestNested(ray, nearest, u, v, t, nearest_transform);

          nodes[result]->findNearestNested(ray, nearest, u, v, t, nearest_transform);

        }

      }

      virtual void findNearestNested(const Ray& ray, TNested *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) override

      {

        auto result = IntersectionTests::rayIntersectsBoundingVolume(_bv, ray);

        if (std::get<0>(result) && std::get<1>(result) < t) {

          Node* nodes[2] = { _left, _right };

          auto result = _left->distToPoint2(ray.getOrigin()) < _right->distToPoint2(ray.getOrigin());

          nodes[!result]->findNearestNested(ray, nearest, u, v, t, nearest_transform);

          nodes[result]->findNearestNested(ray, nearest, u, v, t, nearest_transform);

        }

      }

      virtual void queryRange(const BV& bv, std::vector<T>& intersected_objects) const override

      {

        if (bv.contains(_bv)) {

          _left->queryAll(intersected_objects);

          _right->queryAll(intersected_objects);

        }

        else if (bv.intersects(_bv)) {

          _left->queryRange(bv, intersected_objects);

          _right->queryRange(bv, intersected_objects);

        }

      }

      virtual void queryAll(std::vector<T>& intersected_objects) const override

      {

        _left->queryAll(intersected_objects);

        _right->queryAll(intersected_objects);

      }

      virtual void queryRange(const BV& bv, const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const override

      {

        if (contributes(cp)) {

          if (bv.contains(_bv)) {

            _left->queryAll(cp, intersected_objects);

            _right->queryAll(cp, intersected_objects);

          }

          else if (bv.intersects(_bv)) {

            _left->queryRange(bv, cp, intersected_objects);

            _right->queryRange(bv, cp, intersected_objects);

          }

        }

      }

      virtual void queryAll(const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const override

      {

        if (contributes(cp)) {

          _left->queryAll(cp, intersected_objects);

          _right->queryAll(cp, intersected_objects);

        }

      }

      virtual bool isLeaf() const override

      {

        return false;

      }

      virtual Node* getLeft() const override

      {

        return _left;

      }

      virtual Node* getRight() const override

      {

        return _right;

      }

      virtual Node*& getLeft() override

      {

        return _left;

      }

      virtual Node*& getRight() override

      {

        return _right;

      }

      virtual T * getObjectPtr() override

      {

        throw std::exception("Don't call this method");

        return nullptr;

      }

      virtual BV getBV() const override

      {

        return _bv;

      }

      virtual BV const * getBVPtr() const override

      {

        return &_bv;

      }

      virtual BV * getBVPtr() override

      {

        return &_bv;

      }

      virtual Type distToPoint2(const Vector& point) const override

      {

        return _bv.distToPoint2(point);

      }

      virtual void destroy(NodePool& pool) override

      {

        pool.destroy(this);

      }

      virtual Type* getLargestBVSize() override

      {

        return &_largestBVSize;

      }

      virtual Type cost(const BV& bv) const override

      {

        return bv.getUnion(_bv).size2();

      }

    private:

      NodePtr _left, _right;

      BV _bv;

      Type _largestBVSize;

      auto contributes(const Camera::CullingParams& cp) const

      {

        return _bv.contributes(cp._camPos, cp._thresh, _largestBVSize);

      }

      auto intersectFrustum(const Camera::CullingParams& cp) const

      {

        return IntersectionTests::frustumIntersectsBoundingVolume(_bv, cp._frustumPlanes);

      }

      auto intersectFrustum(const Camera::CullingParams& cp, const IntersectedPlanes& in, IntersectedPlanes& out) const

      {

        return IntersectionTests::frustumIntersectsBoundingVolume(_bv, cp._frustumPlanes, in, out);

      }

    };

    BoundingVolumeHierarchy() = delete;

    BoundingVolumeHierarchy(T* objects, unsigned count) :

      _nodePool(count)

    {

      std::vector<ObjectInfo> infos;

      infos.reserve(count);

      for (unsigned i = 0; i < count; i++) {

        infos.push_back(createInfo(objects + i));

      }

      _root = createNode(infos.data(), infos.data() + count);

    }

    BoundingVolumeHierarchy(const BoundingVolumeHierarchy& other) = delete;

    BoundingVolumeHierarchy& operator=(const BoundingVolumeHierarchy& other) = delete;

    BoundingVolumeHierarchy(BoundingVolumeHierarchy&& other) = default;

    BoundingVolumeHierarchy& operator=(BoundingVolumeHierarchy&& other) = default;

    void cullVisibleObjects(const Camera::CullingParams& cp, CullResult<T>& cull_result) const

    {

      _root->cullVisibleObjects(cp, cull_result);

    }

    void cullVisibleObjectsWithPlaneMasking(const Camera::CullingParams& cp, CullResult<T>& cull_result) const

    {

      IntersectedPlanes out = { { 0, 1, 2, 3, 4, 5 }, 6 };

      _root->cullVisibleObjects(cp, out, cull_result);

    }

    void intersectObjects(const BV& bv, std::vector<T>& intersected_objects) const

    {

      _root->intersectObjects(bv, intersected_objects);

    }

    void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T>& intersected_objects) const

    {

      _root->intersectObjects(bv, transform, intersected_objects);

    }

    void intersectObjects(const BV& bv, std::vector<T const *>& intersected_objects) const

    {

      _root->intersectObjects(bv, intersected_objects);

    }

    void intersectObjects(const BV& bv, const Matrix& transform, std::vector<T const *>& intersected_objects) const

    {

      _root->intersectObjects(bv, transform, intersected_objects);

    }

    void intersectFirst(const BV& bv, T const *& first) const

    {

      _root->intersectFirst(bv, first);

    }

    void intersectFirst(const BV& bv, const Matrix& transform, T const *& first) const

    {

      _root->intersectFirst(bv, transform, first);

    }

    void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects) const

    {

      _root->intersectObjects(ray, intersected_objects);

    }

    void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects) const

    {

      _root->intersectObjects(ray, transform, intersected_objects);

    }

    void intersectObjects(const Ray& ray, std::vector<T>& intersected_objects, std::vector<BV>& intersected_bvs) const

    {

      _root->intersectObjects(ray, intersected_objects, intersected_bvs);

    }

    void intersectObjects(const Ray& ray, const Matrix& transform, std::vector<T>& intersected_objects, std::vector<BV>& intersected_bvs) const

    {

      _root->intersectObjects(ray, transform, intersected_objects, intersected_bvs);

    }

    void findNearest(const Ray& ray, const Matrix& transform, T*& nearest, Type& nearest_t, Matrix& nearest_transform) const

    {

      _root->findNearest(ray, transform, nearest, nearest_t, nearest_transform);

    }

    void findNearest(const Ray& ray, const Matrix& transform, T*& nearest, Type& nearest_t, Matrix& nearest_transform, std::vector<BV>& bvs) const

    {

      _root->findNearest(ray, transform, nearest, nearest_t, nearest_transform, bvs);

    }

    void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const

    {

      _root->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform);

    }

    void findNearestPrecise(const Ray& ray, const Matrix& transform, T *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const

    {

      _root->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform);

    }

    void findNearestPrecise(const Ray& ray, const Matrix& transform, T const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform, std::vector<BV>& bvs) const

    {

      _root->findNearestPrecise(ray, transform, nearest, u, v, t, nearest_transform, bvs);

    }

    void cullVisibleNodes(const Camera::CullingParams& cp, std::vector<Node const *>& nodes) const

    {

      _root->cullVisibleNodes(cp, nodes);

    }

    void findNearestNested(const Ray& ray, TNested const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const

    {

      _root->findNearestNested(ray, nearest, u, v, t, nearest_transform);

    }

    void findNearestNested(const Ray& ray, TNested *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const

    {

      _root->findNearestNested(ray, nearest, u, v, t, nearest_transform);

    }

    void queryRange(const BV& bv, std::vector<T>& intersected_objects) const

    {

      _root->queryRange(bv, intersected_objects);

    }

    void queryRange(const BV& bv, const Camera::CullingParams& cp, std::vector<T>& intersected_objects) const

    {

      _root->queryRange(bv, cp, intersected_objects);

    }

    template<typename OtherNodePtr, typename OtherT, typename OtherProxy>

    void findOverlappingPairs(NodePtr a, OtherNodePtr b, const Matrix& a_transform, std::vector<std::tuple<T*, OtherT*>>& overlapping_pairs) const

    {

      if (!a->isLeaf() && !b->isLeaf()) {

        auto a_bv = BV(*a->getBVPtr(), a_transform);

        if (a_bv.intersects(*b->getBVPtr())) {

          if (a_bv.size2() > b->getBVPtr()->size2()) {

            findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a->getLeft(), b, a_transform, overlapping_pairs);

            findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a->getRight(), b, a_transform, overlapping_pairs);

          }

          else {

            findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a, b->getLeft(), a_transform, overlapping_pairs);

            findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a, b->getRight(), a_transform, overlapping_pairs);

          }

        }

      }

      else if (a->isLeaf() && b->isLeaf()) {

        auto a_bv = BV(Proxy::GetBoundingVolume()(*a->getObjectPtr()), a_transform);

        auto b_bv = OtherProxy::GetBoundingVolume()(*b->getObjectPtr());

        if (a_bv.intersects(b_bv)) {

          overlapping_pairs.push_back(std::tuple<T*, OtherT*>(a->getObjectPtr(), b->getObjectPtr()));

        }

      }

      else if (a->isLeaf() && !b->isLeaf()) {

        if (BV(a->getBV(), a_transform).intersects(*b->getBVPtr())) {

          findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a, b->getLeft(), a_transform, overlapping_pairs);

          findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a, b->getRight(), a_transform, overlapping_pairs);

        }

      }

      else { // a is internal and b is leaf node

        if (BV(*a->getBVPtr(), a_transform).intersects(b->getBV())) {

          findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a->getLeft(), b, a_transform, overlapping_pairs);

          findOverlappingPairs<OtherNodePtr, OtherT, OtherProxy>(a->getRight(), b, a_transform, overlapping_pairs);

        }

      }

    }

    void insert(T object)

    {

      insert(object, _root);

    }

    auto getSizeInBytes() const

    {

      return _root->getSizeInBytes();

    }

    const auto& getBV() const

    {

      return _root->getBV();

    }

    const auto& getRoot() const

    {

      return _root;

    }

    auto getAllBVs() const

    {

      std::vector<BV> bvs;

      _root->getAllBVs(bvs);

      return bvs;

    }

    auto cullBVs(const Camera::CullingParams& cp, const Matrix& transform, std::vector<BV>& bvs) const

    {

      _root->cullBVs(cp, transform, bvs);

    }

    auto countNodes(unsigned& internal_nodes, unsigned& leaf_nodes) const

    {

      internal_nodes = 0;

      leaf_nodes = 0;

      _root->countNodes(internal_nodes, leaf_nodes);

    }

  private:

    void insert(T object, NodePtr& node)

    {

      if (!node->isLeaf()) {

        node->getBVPtr()->unify(getBV(object));

        maximize(getSize(object), *node->getLargestBVSize());

        insert(object, node->getLeft()->cost(getBV(object)) < node->getRight()->cost(getBV(object)) ? node->getLeft() : node->getRight());

      }

      else {

        ObjectInfo objects[2] = { createInfo(&object), createInfo(node->getObjectPtr()) };

        node->destroy(_nodePool);

        node = createNode(objects, objects + 2);

      }

    }

    class NodePool

    {

    public:

      NodePool(unsigned num_objects)

      {

        auto height = static_cast<unsigned>(std::floor(log2(static_cast<double>(num_objects))));

        auto num_nodes = static_cast<unsigned>(std::pow(2.0, static_cast<double>(height) + 1.0) - 1.0);

        auto num_leaf_nodes = static_cast<unsigned>(std::pow(2.0, static_cast<double>(height)));

        auto num_internal_nodes = num_nodes - num_leaf_nodes;

        maximize(32u, num_leaf_nodes);

        maximize(32u, num_internal_nodes);

        _internalNodePool.set_next_size(num_internal_nodes);

        _leafNodePool.set_next_size(num_leaf_nodes);

      }

      NodePool(const NodePool& other) = delete;

      NodePool& operator=(const NodePool& other) = delete;

      NodePool(NodePool&& other) = delete;

      NodePool& operator=(NodePool&& other) = delete;

      NodePtr createNode(ObjectInfo* begin, ObjectInfo* end, BoundingVolumeHierarchy& bvh)

      {

        if (end - begin > 1) {

          return new (_internalNodePool.malloc()) InternalNode(begin, end, bvh);

        }

        return new (_leafNodePool.malloc()) LeafNode(*begin->_object);

      }

      auto destroy(LeafNode* node)

      {

        _leafNodePool.destroy(node);

      }

      auto destroy(InternalNode* node)

      {

        _internalNodePool.destroy(node);

      }

    private:

      boost::object_pool<InternalNode> _internalNodePool;

      boost::object_pool<LeafNode> _leafNodePool;

    };

    NodePool _nodePool;

    auto* createNode(ObjectInfo* begin, ObjectInfo* end)

    {

      return _nodePool.createNode(begin, end, *this);

    }

    NodePtr _root;

  };

}



#endif

Edit:
Here is a sample use, in my Renderer class, I have this:

  struct Proxy

    {

      struct GetBoundingVolume

      {

        const BV& operator()(const MeshRenderablePtr& ptr) const

        {

          return ptr->getBV();

        }

      };

      struct GetLargestBVSize

      {

        auto operator()(const MeshRenderablePtr& ptr) const

        {

          return ptr->getLargestObjectBVSize();

        }

      };

      struct IntersectRay

      {

        auto operator()(const MeshRenderablePtr& ptr, const Ray& ray, const Matrix& transform) const

        {

          throw std::exception("Don't call this method");

          return std::tuple<bool, float, float, float>();

        }

      };

      struct FindNearestNestedConst

      {

        auto operator()(const MeshRenderablePtr& ptr, const Ray& ray, Triangle const *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform) const

        {

          ptr->getGPUMeshData()->getMesh().getBVH().findNearestPrecise(ray, *ptr->getModelMatrix(), nearest, u, v, t, nearest_transform);

        }

      };

      struct FindNearestNested

      {

        auto operator()(const MeshRenderablePtr& ptr, const Ray& ray, Triangle *& nearest, Type& u, Type& v, Type& t, Matrix& nearest_transform)

        {

          ptr->getGPUMeshData()->getMesh().getBVH().findNearestPrecise(ray, *ptr->getModelMatrix(), nearest, u, v, t, nearest_transform);

        }

      };

    };

    using BVH = BoundingVolumeHierarchy<MeshRenderable*, BV, Proxy, Triangle>;

Usage:

auto ray = _renderer.getRay(_mousePos);

Triangle const * nearest = nullptr;

Mat4f nearest_transform;

float u, v;

float nearest_t = std::numeric_limits<float>::max();

_renderer.getStaticBVH()->findNearestNested(ray, nearest, u, v, nearest_t, nearest_transform);

if (nearest) {

  // Do something

}