mirror of
https://github.com/orange-cpp/omath.git
synced 2026-04-19 10:23:27 +00:00
Compare commits
2 Commits
feature/bv
...
28e86fc355
| Author | SHA1 | Date | |
|---|---|---|---|
|
28e86fc355 | ||
|
|
93e7a9457a |
@@ -1,412 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by Orange on 04/08/2026.
|
|
||||||
//
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
#include "omath/3d_primitives/aabb.hpp"
|
|
||||||
#include "omath/collision/line_tracer.hpp"
|
|
||||||
#include <algorithm>
|
|
||||||
#include <cstdint>
|
|
||||||
#include <numeric>
|
|
||||||
#include <span>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace omath::collision
|
|
||||||
{
|
|
||||||
template<class Type = float>
|
|
||||||
class BvhTree final
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
using AabbType = primitives::Aabb<Type>;
|
|
||||||
|
|
||||||
struct HitResult
|
|
||||||
{
|
|
||||||
std::size_t object_index;
|
|
||||||
Type distance_sqr;
|
|
||||||
};
|
|
||||||
|
|
||||||
BvhTree() = default;
|
|
||||||
|
|
||||||
explicit BvhTree(std::span<const AabbType> aabbs)
|
|
||||||
: m_aabbs(aabbs.begin(), aabbs.end())
|
|
||||||
{
|
|
||||||
if (aabbs.empty())
|
|
||||||
return;
|
|
||||||
|
|
||||||
m_indices.resize(aabbs.size());
|
|
||||||
std::iota(m_indices.begin(), m_indices.end(), std::size_t{0});
|
|
||||||
|
|
||||||
m_nodes.reserve(aabbs.size() * 2);
|
|
||||||
|
|
||||||
build(m_aabbs, 0, aabbs.size());
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
std::vector<std::size_t> query_overlaps(const AabbType& query_aabb) const
|
|
||||||
{
|
|
||||||
std::vector<std::size_t> results;
|
|
||||||
|
|
||||||
if (m_nodes.empty())
|
|
||||||
return results;
|
|
||||||
|
|
||||||
query_overlaps_impl(0, query_aabb, results);
|
|
||||||
return results;
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class RayType = Ray<>>
|
|
||||||
[[nodiscard]]
|
|
||||||
std::vector<HitResult> query_ray(const RayType& ray) const
|
|
||||||
{
|
|
||||||
std::vector<HitResult> results;
|
|
||||||
|
|
||||||
if (m_nodes.empty())
|
|
||||||
return results;
|
|
||||||
|
|
||||||
query_ray_impl(0, ray, results);
|
|
||||||
|
|
||||||
std::ranges::sort(results, [](const HitResult& a, const HitResult& b)
|
|
||||||
{ return a.distance_sqr < b.distance_sqr; });
|
|
||||||
return results;
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
std::size_t node_count() const noexcept
|
|
||||||
{
|
|
||||||
return m_nodes.size();
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
bool empty() const noexcept
|
|
||||||
{
|
|
||||||
return m_nodes.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
static constexpr std::size_t k_sah_bucket_count = 12;
|
|
||||||
static constexpr std::size_t k_leaf_threshold = 1;
|
|
||||||
static constexpr std::size_t k_null_index = std::numeric_limits<std::size_t>::max();
|
|
||||||
|
|
||||||
struct Node
|
|
||||||
{
|
|
||||||
AabbType bounds;
|
|
||||||
std::size_t left = k_null_index;
|
|
||||||
std::size_t right = k_null_index;
|
|
||||||
|
|
||||||
// For leaf nodes: index range into m_indices
|
|
||||||
std::size_t first_index = 0;
|
|
||||||
std::size_t index_count = 0;
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
bool is_leaf() const noexcept
|
|
||||||
{
|
|
||||||
return left == k_null_index;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
struct SahBucket
|
|
||||||
{
|
|
||||||
AabbType bounds = {
|
|
||||||
{std::numeric_limits<Type>::max(), std::numeric_limits<Type>::max(),
|
|
||||||
std::numeric_limits<Type>::max()},
|
|
||||||
{std::numeric_limits<Type>::lowest(), std::numeric_limits<Type>::lowest(),
|
|
||||||
std::numeric_limits<Type>::lowest()}
|
|
||||||
};
|
|
||||||
std::size_t count = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
static constexpr Type surface_area(const AabbType& aabb) noexcept
|
|
||||||
{
|
|
||||||
const auto d = aabb.max - aabb.min;
|
|
||||||
return static_cast<Type>(2) * (d.x * d.y + d.y * d.z + d.z * d.x);
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
static constexpr AabbType merge(const AabbType& a, const AabbType& b) noexcept
|
|
||||||
{
|
|
||||||
return {
|
|
||||||
{std::min(a.min.x, b.min.x), std::min(a.min.y, b.min.y), std::min(a.min.z, b.min.z)},
|
|
||||||
{std::max(a.max.x, b.max.x), std::max(a.max.y, b.max.y), std::max(a.max.z, b.max.z)}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
static constexpr bool overlaps(const AabbType& a, const AabbType& b) noexcept
|
|
||||||
{
|
|
||||||
return a.min.x <= b.max.x && a.max.x >= b.min.x
|
|
||||||
&& a.min.y <= b.max.y && a.max.y >= b.min.y
|
|
||||||
&& a.min.z <= b.max.z && a.max.z >= b.min.z;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::size_t build(std::span<const AabbType> aabbs, std::size_t begin, std::size_t end)
|
|
||||||
{
|
|
||||||
const auto node_idx = m_nodes.size();
|
|
||||||
m_nodes.emplace_back();
|
|
||||||
|
|
||||||
auto& node = m_nodes[node_idx];
|
|
||||||
node.bounds = compute_bounds(aabbs, begin, end);
|
|
||||||
|
|
||||||
const auto count = end - begin;
|
|
||||||
|
|
||||||
if (count <= k_leaf_threshold)
|
|
||||||
{
|
|
||||||
node.first_index = begin;
|
|
||||||
node.index_count = count;
|
|
||||||
return node_idx;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Find best SAH split
|
|
||||||
const auto centroid_bounds = compute_centroid_bounds(aabbs, begin, end);
|
|
||||||
const auto split = find_best_split(aabbs, begin, end, node.bounds, centroid_bounds);
|
|
||||||
|
|
||||||
// If SAH says don't split, make a leaf
|
|
||||||
if (!split.has_value())
|
|
||||||
{
|
|
||||||
node.first_index = begin;
|
|
||||||
node.index_count = count;
|
|
||||||
return node_idx;
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto [axis, split_pos] = split.value();
|
|
||||||
|
|
||||||
// Partition indices around the split
|
|
||||||
const auto mid = partition_indices(aabbs, begin, end, axis, split_pos);
|
|
||||||
|
|
||||||
// Degenerate partition fallback: split in the middle
|
|
||||||
const auto actual_mid = (mid == begin || mid == end) ? begin + count / 2 : mid;
|
|
||||||
|
|
||||||
// Build children — careful: m_nodes may reallocate, so don't hold references across build calls
|
|
||||||
const auto left_idx = build(aabbs, begin, actual_mid);
|
|
||||||
const auto right_idx = build(aabbs, actual_mid, end);
|
|
||||||
|
|
||||||
m_nodes[node_idx].left = left_idx;
|
|
||||||
m_nodes[node_idx].right = right_idx;
|
|
||||||
m_nodes[node_idx].index_count = 0;
|
|
||||||
|
|
||||||
return node_idx;
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
AabbType compute_bounds(std::span<const AabbType> aabbs, std::size_t begin, std::size_t end) const
|
|
||||||
{
|
|
||||||
AabbType bounds = {
|
|
||||||
{std::numeric_limits<Type>::max(), std::numeric_limits<Type>::max(),
|
|
||||||
std::numeric_limits<Type>::max()},
|
|
||||||
{std::numeric_limits<Type>::lowest(), std::numeric_limits<Type>::lowest(),
|
|
||||||
std::numeric_limits<Type>::lowest()}
|
|
||||||
};
|
|
||||||
|
|
||||||
for (auto i = begin; i < end; ++i)
|
|
||||||
bounds = merge(bounds, aabbs[m_indices[i]]);
|
|
||||||
|
|
||||||
return bounds;
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
AabbType compute_centroid_bounds(std::span<const AabbType> aabbs, std::size_t begin, std::size_t end) const
|
|
||||||
{
|
|
||||||
AabbType bounds = {
|
|
||||||
{std::numeric_limits<Type>::max(), std::numeric_limits<Type>::max(),
|
|
||||||
std::numeric_limits<Type>::max()},
|
|
||||||
{std::numeric_limits<Type>::lowest(), std::numeric_limits<Type>::lowest(),
|
|
||||||
std::numeric_limits<Type>::lowest()}
|
|
||||||
};
|
|
||||||
|
|
||||||
for (auto i = begin; i < end; ++i)
|
|
||||||
{
|
|
||||||
const auto c = aabbs[m_indices[i]].center();
|
|
||||||
bounds.min.x = std::min(bounds.min.x, c.x);
|
|
||||||
bounds.min.y = std::min(bounds.min.y, c.y);
|
|
||||||
bounds.min.z = std::min(bounds.min.z, c.z);
|
|
||||||
bounds.max.x = std::max(bounds.max.x, c.x);
|
|
||||||
bounds.max.y = std::max(bounds.max.y, c.y);
|
|
||||||
bounds.max.z = std::max(bounds.max.z, c.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
return bounds;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct SplitResult
|
|
||||||
{
|
|
||||||
int axis;
|
|
||||||
Type position;
|
|
||||||
};
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
std::optional<SplitResult> find_best_split(std::span<const AabbType> aabbs, std::size_t begin,
|
|
||||||
std::size_t end, const AabbType& node_bounds,
|
|
||||||
const AabbType& centroid_bounds) const
|
|
||||||
{
|
|
||||||
const auto count = end - begin;
|
|
||||||
const auto leaf_cost = static_cast<Type>(count);
|
|
||||||
auto best_cost = leaf_cost;
|
|
||||||
std::optional<SplitResult> best_split;
|
|
||||||
|
|
||||||
for (int axis = 0; axis < 3; ++axis)
|
|
||||||
{
|
|
||||||
const auto axis_min = get_component(centroid_bounds.min, axis);
|
|
||||||
const auto axis_max = get_component(centroid_bounds.max, axis);
|
|
||||||
|
|
||||||
if (axis_max - axis_min < std::numeric_limits<Type>::epsilon())
|
|
||||||
continue;
|
|
||||||
|
|
||||||
SahBucket buckets[k_sah_bucket_count] = {};
|
|
||||||
|
|
||||||
const auto inv_extent = static_cast<Type>(k_sah_bucket_count) / (axis_max - axis_min);
|
|
||||||
|
|
||||||
// Fill buckets
|
|
||||||
for (auto i = begin; i < end; ++i)
|
|
||||||
{
|
|
||||||
const auto centroid = get_component(aabbs[m_indices[i]].center(), axis);
|
|
||||||
auto bucket_idx = static_cast<std::size_t>((centroid - axis_min) * inv_extent);
|
|
||||||
bucket_idx = std::min(bucket_idx, k_sah_bucket_count - 1);
|
|
||||||
|
|
||||||
buckets[bucket_idx].count++;
|
|
||||||
if (buckets[bucket_idx].count == 1)
|
|
||||||
buckets[bucket_idx].bounds = aabbs[m_indices[i]];
|
|
||||||
else
|
|
||||||
buckets[bucket_idx].bounds = merge(buckets[bucket_idx].bounds, aabbs[m_indices[i]]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Evaluate split costs using prefix/suffix sweeps
|
|
||||||
AabbType prefix_bounds[k_sah_bucket_count - 1];
|
|
||||||
std::size_t prefix_count[k_sah_bucket_count - 1];
|
|
||||||
|
|
||||||
prefix_bounds[0] = buckets[0].bounds;
|
|
||||||
prefix_count[0] = buckets[0].count;
|
|
||||||
for (std::size_t i = 1; i < k_sah_bucket_count - 1; ++i)
|
|
||||||
{
|
|
||||||
prefix_bounds[i] = (buckets[i].count > 0)
|
|
||||||
? merge(prefix_bounds[i - 1], buckets[i].bounds)
|
|
||||||
: prefix_bounds[i - 1];
|
|
||||||
prefix_count[i] = prefix_count[i - 1] + buckets[i].count;
|
|
||||||
}
|
|
||||||
|
|
||||||
AabbType suffix_bounds = buckets[k_sah_bucket_count - 1].bounds;
|
|
||||||
std::size_t suffix_count = buckets[k_sah_bucket_count - 1].count;
|
|
||||||
|
|
||||||
const auto parent_area = surface_area(node_bounds);
|
|
||||||
const auto inv_parent_area = static_cast<Type>(1) / parent_area;
|
|
||||||
|
|
||||||
for (auto i = static_cast<int>(k_sah_bucket_count) - 2; i >= 0; --i)
|
|
||||||
{
|
|
||||||
const auto left_count = prefix_count[i];
|
|
||||||
const auto right_count = suffix_count;
|
|
||||||
|
|
||||||
if (left_count == 0 || right_count == 0)
|
|
||||||
{
|
|
||||||
if (i > 0)
|
|
||||||
{
|
|
||||||
suffix_bounds = (buckets[i].count > 0)
|
|
||||||
? merge(suffix_bounds, buckets[i].bounds)
|
|
||||||
: suffix_bounds;
|
|
||||||
suffix_count += buckets[i].count;
|
|
||||||
}
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto cost = static_cast<Type>(1)
|
|
||||||
+ (static_cast<Type>(left_count) * surface_area(prefix_bounds[i])
|
|
||||||
+ static_cast<Type>(right_count) * surface_area(suffix_bounds))
|
|
||||||
* inv_parent_area;
|
|
||||||
|
|
||||||
if (cost < best_cost)
|
|
||||||
{
|
|
||||||
best_cost = cost;
|
|
||||||
best_split = SplitResult{
|
|
||||||
axis,
|
|
||||||
axis_min + static_cast<Type>(i + 1) * (axis_max - axis_min)
|
|
||||||
/ static_cast<Type>(k_sah_bucket_count)
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
suffix_bounds = (buckets[i].count > 0)
|
|
||||||
? merge(suffix_bounds, buckets[i].bounds)
|
|
||||||
: suffix_bounds;
|
|
||||||
suffix_count += buckets[i].count;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return best_split;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::size_t partition_indices(std::span<const AabbType> aabbs, std::size_t begin, std::size_t end,
|
|
||||||
int axis, Type split_pos)
|
|
||||||
{
|
|
||||||
auto it = std::partition(m_indices.begin() + static_cast<std::ptrdiff_t>(begin),
|
|
||||||
m_indices.begin() + static_cast<std::ptrdiff_t>(end),
|
|
||||||
[&](std::size_t idx)
|
|
||||||
{ return get_component(aabbs[idx].center(), axis) < split_pos; });
|
|
||||||
|
|
||||||
return static_cast<std::size_t>(std::distance(m_indices.begin(), it));
|
|
||||||
}
|
|
||||||
|
|
||||||
[[nodiscard]]
|
|
||||||
static constexpr Type get_component(const Vector3<Type>& v, int axis) noexcept
|
|
||||||
{
|
|
||||||
switch (axis)
|
|
||||||
{
|
|
||||||
case 0:
|
|
||||||
return v.x;
|
|
||||||
case 1:
|
|
||||||
return v.y;
|
|
||||||
default:
|
|
||||||
return v.z;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void query_overlaps_impl(std::size_t node_idx, const AabbType& query_aabb,
|
|
||||||
std::vector<std::size_t>& results) const
|
|
||||||
{
|
|
||||||
const auto& node = m_nodes[node_idx];
|
|
||||||
|
|
||||||
if (!overlaps(node.bounds, query_aabb))
|
|
||||||
return;
|
|
||||||
|
|
||||||
if (node.is_leaf())
|
|
||||||
{
|
|
||||||
for (auto i = node.first_index; i < node.first_index + node.index_count; ++i)
|
|
||||||
if (overlaps(query_aabb, m_aabbs[m_indices[i]]))
|
|
||||||
results.push_back(m_indices[i]);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
query_overlaps_impl(node.left, query_aabb, results);
|
|
||||||
query_overlaps_impl(node.right, query_aabb, results);
|
|
||||||
}
|
|
||||||
|
|
||||||
template<class RayType>
|
|
||||||
void query_ray_impl(std::size_t node_idx, const RayType& ray,
|
|
||||||
std::vector<HitResult>& results) const
|
|
||||||
{
|
|
||||||
const auto& node = m_nodes[node_idx];
|
|
||||||
|
|
||||||
// Quick AABB-ray rejection using the slab method
|
|
||||||
const auto hit = LineTracer<RayType>::get_ray_hit_point(ray, node.bounds);
|
|
||||||
if (hit == ray.end)
|
|
||||||
return;
|
|
||||||
|
|
||||||
if (node.is_leaf())
|
|
||||||
{
|
|
||||||
for (auto i = node.first_index; i < node.first_index + node.index_count; ++i)
|
|
||||||
{
|
|
||||||
const auto leaf_hit = LineTracer<RayType>::get_ray_hit_point(
|
|
||||||
ray, m_aabbs[m_indices[i]]);
|
|
||||||
if (leaf_hit != ray.end)
|
|
||||||
{
|
|
||||||
const auto diff = leaf_hit - ray.start;
|
|
||||||
results.push_back({m_indices[i], diff.dot(diff)});
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
query_ray_impl(node.left, ray, results);
|
|
||||||
query_ray_impl(node.right, ray, results);
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<Node> m_nodes;
|
|
||||||
std::vector<std::size_t> m_indices;
|
|
||||||
std::vector<AabbType> m_aabbs;
|
|
||||||
};
|
|
||||||
} // namespace omath::collision
|
|
||||||
@@ -35,7 +35,6 @@
|
|||||||
#include "omath/collision/line_tracer.hpp"
|
#include "omath/collision/line_tracer.hpp"
|
||||||
#include "omath/collision/gjk_algorithm.hpp"
|
#include "omath/collision/gjk_algorithm.hpp"
|
||||||
#include "omath/collision/epa_algorithm.hpp"
|
#include "omath/collision/epa_algorithm.hpp"
|
||||||
#include "omath/collision/bvh_tree.hpp"
|
|
||||||
// Pathfinding algorithms
|
// Pathfinding algorithms
|
||||||
#include "omath/pathfinding/a_star.hpp"
|
#include "omath/pathfinding/a_star.hpp"
|
||||||
#include "omath/pathfinding/navigation_mesh.hpp"
|
#include "omath/pathfinding/navigation_mesh.hpp"
|
||||||
|
|||||||
@@ -87,11 +87,11 @@ namespace omath::pathfinding
|
|||||||
|
|
||||||
const auto current_node = current_node_it->second;
|
const auto current_node = current_node_it->second;
|
||||||
|
|
||||||
|
closed_list.emplace(current, current_node);
|
||||||
|
|
||||||
if (current == end_vertex)
|
if (current == end_vertex)
|
||||||
return reconstruct_final_path(closed_list, current);
|
return reconstruct_final_path(closed_list, current);
|
||||||
|
|
||||||
closed_list.emplace(current, current_node);
|
|
||||||
|
|
||||||
for (const auto& neighbor: nav_mesh.get_neighbors(current))
|
for (const auto& neighbor: nav_mesh.get_neighbors(current))
|
||||||
{
|
{
|
||||||
if (closed_list.contains(neighbor))
|
if (closed_list.contains(neighbor))
|
||||||
|
|||||||
@@ -40,8 +40,9 @@ TEST(AStarExtra, TrivialNeighbor)
|
|||||||
nav.m_vertex_map[v2] = {v1};
|
nav.m_vertex_map[v2] = {v1};
|
||||||
|
|
||||||
const auto path = Astar::find_path(v1, v2, nav);
|
const auto path = Astar::find_path(v1, v2, nav);
|
||||||
ASSERT_EQ(path.size(), 1u);
|
ASSERT_EQ(path.size(), 2u);
|
||||||
EXPECT_EQ(path.front(), v2);
|
EXPECT_EQ(path.front(), v1);
|
||||||
|
EXPECT_EQ(path.back(), v2);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(AStarExtra, StartEqualsGoal)
|
TEST(AStarExtra, StartEqualsGoal)
|
||||||
@@ -101,7 +102,7 @@ TEST(AStarExtra, LongerPathAvoidsBlock)
|
|||||||
constexpr Vector3<float> goal = idx(2, 1);
|
constexpr Vector3<float> goal = idx(2, 1);
|
||||||
const auto path = Astar::find_path(start, goal, nav);
|
const auto path = Astar::find_path(start, goal, nav);
|
||||||
ASSERT_FALSE(path.empty());
|
ASSERT_FALSE(path.empty());
|
||||||
EXPECT_EQ(path.front(), goal);
|
EXPECT_EQ(path.back(), goal);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(AstarTests, TrivialDirectNeighborPath)
|
TEST(AstarTests, TrivialDirectNeighborPath)
|
||||||
@@ -114,8 +115,9 @@ TEST(AstarTests, TrivialDirectNeighborPath)
|
|||||||
nav.m_vertex_map.emplace(v2, std::vector<Vector3<float>>{v1});
|
nav.m_vertex_map.emplace(v2, std::vector<Vector3<float>>{v1});
|
||||||
|
|
||||||
const auto path = Astar::find_path(v1, v2, nav);
|
const auto path = Astar::find_path(v1, v2, nav);
|
||||||
ASSERT_EQ(path.size(), 1u);
|
ASSERT_EQ(path.size(), 2u);
|
||||||
EXPECT_EQ(path.front(), v2);
|
EXPECT_EQ(path.front(), v1);
|
||||||
|
EXPECT_EQ(path.back(), v2);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(AstarTests, NoPathWhenDisconnected)
|
TEST(AstarTests, NoPathWhenDisconnected)
|
||||||
|
|||||||
@@ -1,876 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by Orange on 04/08/2026.
|
|
||||||
//
|
|
||||||
#include <gtest/gtest.h>
|
|
||||||
#include <omath/collision/bvh_tree.hpp>
|
|
||||||
#include <algorithm>
|
|
||||||
#include <random>
|
|
||||||
#include <set>
|
|
||||||
|
|
||||||
using Aabb = omath::primitives::Aabb<float>;
|
|
||||||
using BvhTree = omath::collision::BvhTree<float>;
|
|
||||||
using Ray = omath::collision::Ray<>;
|
|
||||||
using HitResult = BvhTree::HitResult;
|
|
||||||
|
|
||||||
using AabbD = omath::primitives::Aabb<double>;
|
|
||||||
using BvhTreeD = omath::collision::BvhTree<double>;
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Helper: brute-force overlap query for verification
|
|
||||||
// ============================================================================
|
|
||||||
static std::set<std::size_t> brute_force_overlaps(const std::vector<Aabb>& aabbs, const Aabb& query)
|
|
||||||
{
|
|
||||||
std::set<std::size_t> result;
|
|
||||||
for (std::size_t i = 0; i < aabbs.size(); ++i)
|
|
||||||
{
|
|
||||||
if (query.min.x <= aabbs[i].max.x && query.max.x >= aabbs[i].min.x
|
|
||||||
&& query.min.y <= aabbs[i].max.y && query.max.y >= aabbs[i].min.y
|
|
||||||
&& query.min.z <= aabbs[i].max.z && query.max.z >= aabbs[i].min.z)
|
|
||||||
result.insert(i);
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Construction tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, EmptyTree)
|
|
||||||
{
|
|
||||||
const BvhTree tree;
|
|
||||||
EXPECT_TRUE(tree.empty());
|
|
||||||
EXPECT_EQ(tree.node_count(), 0);
|
|
||||||
EXPECT_TRUE(tree.query_overlaps({}).empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, EmptySpan)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> empty;
|
|
||||||
const BvhTree tree(empty);
|
|
||||||
EXPECT_TRUE(tree.empty());
|
|
||||||
EXPECT_EQ(tree.node_count(), 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, SingleElement)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
EXPECT_FALSE(tree.empty());
|
|
||||||
EXPECT_EQ(tree.node_count(), 1);
|
|
||||||
|
|
||||||
const auto results = tree.query_overlaps({{0.5f, 0.5f, 0.5f}, {1.5f, 1.5f, 1.5f}});
|
|
||||||
ASSERT_EQ(results.size(), 1);
|
|
||||||
EXPECT_EQ(results[0], 0);
|
|
||||||
|
|
||||||
const auto miss = tree.query_overlaps({{5.f, 5.f, 5.f}, {6.f, 6.f, 6.f}});
|
|
||||||
EXPECT_TRUE(miss.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, TwoElements)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{5.f, 5.f, 5.f}, {6.f, 6.f, 6.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
EXPECT_FALSE(tree.empty());
|
|
||||||
|
|
||||||
// Hit first only
|
|
||||||
auto r = tree.query_overlaps({{-0.5f, -0.5f, -0.5f}, {0.5f, 0.5f, 0.5f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
|
|
||||||
// Hit second only
|
|
||||||
r = tree.query_overlaps({{5.5f, 5.5f, 5.5f}, {7.f, 7.f, 7.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 1);
|
|
||||||
|
|
||||||
// Hit both
|
|
||||||
r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {10.f, 10.f, 10.f}});
|
|
||||||
EXPECT_EQ(r.size(), 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ThreeElements)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{2.f, 2.f, 2.f}, {3.f, 3.f, 3.f}},
|
|
||||||
{{10.f, 10.f, 10.f}, {11.f, 11.f, 11.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto results = tree.query_overlaps({{0.5f, 0.5f, 0.5f}, {2.5f, 2.5f, 2.5f}});
|
|
||||||
EXPECT_EQ(results.size(), 2);
|
|
||||||
|
|
||||||
const auto far = tree.query_overlaps({{9.5f, 9.5f, 9.5f}, {10.5f, 10.5f, 10.5f}});
|
|
||||||
ASSERT_EQ(far.size(), 1);
|
|
||||||
EXPECT_EQ(far[0], 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, NodeCountGrowsSublinearly)
|
|
||||||
{
|
|
||||||
// For N objects, node count should be at most 2N-1
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 100; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 3.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
EXPECT_LE(tree.node_count(), 2 * aabbs.size());
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Overlap query tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapExactTouch)
|
|
||||||
{
|
|
||||||
// Two boxes share exactly one face
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{1.f, 0.f, 0.f}, {2.f, 1.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Query exactly at the shared face — should overlap both
|
|
||||||
const auto r = tree.query_overlaps({{0.5f, 0.f, 0.f}, {1.5f, 1.f, 1.f}});
|
|
||||||
EXPECT_EQ(r.size(), 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapEdgeTouch)
|
|
||||||
{
|
|
||||||
// Query AABB edge-touches an object AABB
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Touching at corner point (1,1,1)
|
|
||||||
const auto r = tree.query_overlaps({{1.f, 1.f, 1.f}, {2.f, 2.f, 2.f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapQueryInsideObject)
|
|
||||||
{
|
|
||||||
// Query is fully inside an object
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{-10.f, -10.f, -10.f}, {10.f, 10.f, 10.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
const auto r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {1.f, 1.f, 1.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapObjectInsideQuery)
|
|
||||||
{
|
|
||||||
// Object is fully inside the query
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{4.f, 4.f, 4.f}, {5.f, 5.f, 5.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {10.f, 10.f, 10.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapMissOnSingleAxis)
|
|
||||||
{
|
|
||||||
// Overlap on X and Y but not Z
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 5.f}, {1.f, 1.f, 6.f}});
|
|
||||||
EXPECT_TRUE(r.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapNegativeCoordinates)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{-5.f, -5.f, -5.f}, {-3.f, -3.f, -3.f}},
|
|
||||||
{{-2.f, -2.f, -2.f}, {0.f, 0.f, 0.f}},
|
|
||||||
{{1.f, 1.f, 1.f}, {3.f, 3.f, 3.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{-6.f, -6.f, -6.f}, {-4.f, -4.f, -4.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapMixedNegativePositive)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{-1.f, -1.f, -1.f}, {1.f, 1.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Query spans negative and positive
|
|
||||||
const auto r = tree.query_overlaps({{-0.5f, -0.5f, -0.5f}, {0.5f, 0.5f, 0.5f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapNoHitsAmongMany)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 50; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 5.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Query far from all objects
|
|
||||||
const auto r = tree.query_overlaps({{-100.f, -100.f, -100.f}, {-90.f, -90.f, -90.f}});
|
|
||||||
EXPECT_TRUE(r.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapAllObjects)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 64; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i);
|
|
||||||
aabbs.push_back({{f, f, f}, {f + 0.5f, f + 0.5f, f + 0.5f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {100.f, 100.f, 100.f}});
|
|
||||||
EXPECT_EQ(r.size(), 64);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlapReturnsCorrectIndices)
|
|
||||||
{
|
|
||||||
// Specific index verification
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}}, // 0
|
|
||||||
{{10.f, 0.f, 0.f}, {11.f, 1.f, 1.f}}, // 1
|
|
||||||
{{20.f, 0.f, 0.f}, {21.f, 1.f, 1.f}}, // 2
|
|
||||||
{{30.f, 0.f, 0.f}, {31.f, 1.f, 1.f}}, // 3
|
|
||||||
{{40.f, 0.f, 0.f}, {41.f, 1.f, 1.f}}, // 4
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Hit only index 2
|
|
||||||
auto r = tree.query_overlaps({{19.5f, -1.f, -1.f}, {20.5f, 2.f, 2.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 2);
|
|
||||||
|
|
||||||
// Hit only index 4
|
|
||||||
r = tree.query_overlaps({{39.5f, -1.f, -1.f}, {40.5f, 2.f, 2.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 4);
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Spatial distribution tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ObjectsAlongXAxis)
|
|
||||||
{
|
|
||||||
// All objects on a line along X
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 20; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 4.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
|
|
||||||
const auto mid = tree.query_overlaps({{7.5f, -1.f, -1.f}, {8.5f, 2.f, 2.f}});
|
|
||||||
EXPECT_EQ(mid.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ObjectsAlongYAxis)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 20; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 4.f;
|
|
||||||
aabbs.push_back({{0.f, f, 0.f}, {1.f, f + 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{-1.f, 38.f, -1.f}, {2.f, 40.f, 2.f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ObjectsAlongZAxis)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 20; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 4.f;
|
|
||||||
aabbs.push_back({{0.f, 0.f, f}, {1.f, 1.f, f + 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{-1.f, -1.f, 38.f}, {2.f, 2.f, 40.f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ObjectsInPlaneXY)
|
|
||||||
{
|
|
||||||
// Grid in the XY plane
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int x = 0; x < 10; ++x)
|
|
||||||
for (int y = 0; y < 10; ++y)
|
|
||||||
{
|
|
||||||
const auto fx = static_cast<float>(x) * 3.f;
|
|
||||||
const auto fy = static_cast<float>(y) * 3.f;
|
|
||||||
aabbs.push_back({{fx, fy, 0.f}, {fx + 1.f, fy + 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
EXPECT_EQ(tree.query_overlaps({{-1.f, -1.f, -1.f}, {100.f, 100.f, 2.f}}).size(), 100);
|
|
||||||
|
|
||||||
// Single cell query
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {0.5f, 0.5f, 0.5f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ClusteredObjects)
|
|
||||||
{
|
|
||||||
// Two clusters far apart
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 25; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 0.5f;
|
|
||||||
aabbs.push_back({{f, f, f}, {f + 0.4f, f + 0.4f, f + 0.4f}});
|
|
||||||
}
|
|
||||||
for (int i = 0; i < 25; ++i)
|
|
||||||
{
|
|
||||||
const auto f = 100.f + static_cast<float>(i) * 0.5f;
|
|
||||||
aabbs.push_back({{f, f, f}, {f + 0.4f, f + 0.4f, f + 0.4f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Query near first cluster
|
|
||||||
const auto r1 = tree.query_overlaps({{-1.f, -1.f, -1.f}, {15.f, 15.f, 15.f}});
|
|
||||||
EXPECT_EQ(r1.size(), 25);
|
|
||||||
|
|
||||||
// Query near second cluster
|
|
||||||
const auto r2 = tree.query_overlaps({{99.f, 99.f, 99.f}, {115.f, 115.f, 115.f}});
|
|
||||||
EXPECT_EQ(r2.size(), 25);
|
|
||||||
|
|
||||||
// Query between clusters — should find nothing
|
|
||||||
const auto gap = tree.query_overlaps({{50.f, 50.f, 50.f}, {60.f, 60.f, 60.f}});
|
|
||||||
EXPECT_TRUE(gap.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, OverlappingObjects)
|
|
||||||
{
|
|
||||||
// Objects that overlap each other
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {2.f, 2.f, 2.f}},
|
|
||||||
{{1.f, 1.f, 1.f}, {3.f, 3.f, 3.f}},
|
|
||||||
{{1.5f, 1.5f, 1.5f}, {4.f, 4.f, 4.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Query at the overlap region of all three
|
|
||||||
const auto r = tree.query_overlaps({{1.5f, 1.5f, 1.5f}, {2.f, 2.f, 2.f}});
|
|
||||||
EXPECT_EQ(r.size(), 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, IdenticalObjects)
|
|
||||||
{
|
|
||||||
// All objects at the same position
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 10; ++i)
|
|
||||||
aabbs.push_back({{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}});
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}});
|
|
||||||
EXPECT_EQ(r.size(), 10);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, DegenerateThickPlanes)
|
|
||||||
{
|
|
||||||
// Very flat AABBs (thickness ~0 in one axis)
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {10.f, 10.f, 0.001f}},
|
|
||||||
{{0.f, 0.f, 5.f}, {10.f, 10.f, 5.001f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, -0.01f}, {10.f, 10.f, 0.01f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, VaryingSizes)
|
|
||||||
{
|
|
||||||
// Objects of wildly different sizes
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {0.01f, 0.01f, 0.01f}}, // tiny
|
|
||||||
{{-500.f, -500.f, -500.f}, {500.f, 500.f, 500.f}}, // huge
|
|
||||||
{{10.f, 10.f, 10.f}, {11.f, 11.f, 11.f}}, // normal
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// The huge box should overlap almost any query
|
|
||||||
auto r = tree.query_overlaps({{200.f, 200.f, 200.f}, {201.f, 201.f, 201.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 1);
|
|
||||||
|
|
||||||
// Query at origin hits the tiny and the huge
|
|
||||||
r = tree.query_overlaps({{-0.1f, -0.1f, -0.1f}, {0.1f, 0.1f, 0.1f}});
|
|
||||||
EXPECT_EQ(r.size(), 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Ray query tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQueryBasic)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{5.f, 0.f, 0.f}, {6.f, 1.f, 1.f}},
|
|
||||||
{{0.f, 5.f, 0.f}, {1.f, 6.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {-1.f, 0.5f, 0.5f};
|
|
||||||
ray.end = {10.f, 0.5f, 0.5f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_GE(hits.size(), 2);
|
|
||||||
|
|
||||||
if (hits.size() >= 2)
|
|
||||||
EXPECT_LE(hits[0].distance_sqr, hits[1].distance_sqr);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQueryMissesAll)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{5.f, 0.f, 0.f}, {6.f, 1.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Ray above everything
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {-1.f, 100.f, 0.5f};
|
|
||||||
ray.end = {10.f, 100.f, 0.5f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_TRUE(hits.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQueryAlongY)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{0.f, 5.f, 0.f}, {1.f, 6.f, 1.f}},
|
|
||||||
{{0.f, 10.f, 0.f}, {1.f, 11.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {0.5f, -1.f, 0.5f};
|
|
||||||
ray.end = {0.5f, 20.f, 0.5f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_EQ(hits.size(), 3);
|
|
||||||
|
|
||||||
// Verify sorted by distance
|
|
||||||
for (std::size_t i = 1; i < hits.size(); ++i)
|
|
||||||
EXPECT_LE(hits[i - 1].distance_sqr, hits[i].distance_sqr);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQueryAlongZ)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{0.f, 0.f, 10.f}, {1.f, 1.f, 11.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {0.5f, 0.5f, -5.f};
|
|
||||||
ray.end = {0.5f, 0.5f, 20.f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_EQ(hits.size(), 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQueryDiagonal)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{5.f, 5.f, 5.f}, {6.f, 6.f, 6.f}},
|
|
||||||
{{10.f, 10.f, 10.f}, {11.f, 11.f, 11.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Diagonal ray through all three
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {-1.f, -1.f, -1.f};
|
|
||||||
ray.end = {15.f, 15.f, 15.f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_EQ(hits.size(), 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQueryOnEmptyTree)
|
|
||||||
{
|
|
||||||
const BvhTree tree;
|
|
||||||
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {0.f, 0.f, 0.f};
|
|
||||||
ray.end = {10.f, 0.f, 0.f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_TRUE(hits.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, RayQuerySortedByDistance)
|
|
||||||
{
|
|
||||||
// Many boxes along a line
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 20; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 3.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
Ray ray;
|
|
||||||
ray.start = {-1.f, 0.5f, 0.5f};
|
|
||||||
ray.end = {100.f, 0.5f, 0.5f};
|
|
||||||
ray.infinite_length = true;
|
|
||||||
|
|
||||||
const auto hits = tree.query_ray(ray);
|
|
||||||
EXPECT_EQ(hits.size(), 20);
|
|
||||||
|
|
||||||
for (std::size_t i = 1; i < hits.size(); ++i)
|
|
||||||
EXPECT_LE(hits[i - 1].distance_sqr, hits[i].distance_sqr);
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Brute-force verification tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, BruteForceVerificationGrid)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int x = 0; x < 10; ++x)
|
|
||||||
for (int y = 0; y < 10; ++y)
|
|
||||||
for (int z = 0; z < 10; ++z)
|
|
||||||
{
|
|
||||||
const auto fx = static_cast<float>(x) * 3.f;
|
|
||||||
const auto fy = static_cast<float>(y) * 3.f;
|
|
||||||
const auto fz = static_cast<float>(z) * 3.f;
|
|
||||||
aabbs.push_back({{fx, fy, fz}, {fx + 1.f, fy + 1.f, fz + 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Test several queries and compare to brute force
|
|
||||||
const std::vector<Aabb> queries = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.5f, 1.5f, 1.5f}},
|
|
||||||
{{-1.f, -1.f, -1.f}, {100.f, 100.f, 100.f}},
|
|
||||||
{{13.f, 13.f, 13.f}, {14.f, 14.f, 14.f}},
|
|
||||||
{{-50.f, -50.f, -50.f}, {-40.f, -40.f, -40.f}},
|
|
||||||
{{5.5f, 5.5f, 5.5f}, {7.5f, 7.5f, 7.5f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
for (const auto& q : queries)
|
|
||||||
{
|
|
||||||
const auto bvh_results = tree.query_overlaps(q);
|
|
||||||
const auto brute_results = brute_force_overlaps(aabbs, q);
|
|
||||||
|
|
||||||
const std::set<std::size_t> bvh_set(bvh_results.begin(), bvh_results.end());
|
|
||||||
EXPECT_EQ(bvh_set, brute_results)
|
|
||||||
<< "Mismatch for query [(" << q.min.x << "," << q.min.y << "," << q.min.z
|
|
||||||
<< ") -> (" << q.max.x << "," << q.max.y << "," << q.max.z << ")]";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, BruteForceVerificationRandom)
|
|
||||||
{
|
|
||||||
std::mt19937 rng(42);
|
|
||||||
std::uniform_real_distribution<float> pos_dist(-50.f, 50.f);
|
|
||||||
std::uniform_real_distribution<float> size_dist(0.5f, 3.f);
|
|
||||||
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 200; ++i)
|
|
||||||
{
|
|
||||||
const auto x = pos_dist(rng);
|
|
||||||
const auto y = pos_dist(rng);
|
|
||||||
const auto z = pos_dist(rng);
|
|
||||||
const auto sx = size_dist(rng);
|
|
||||||
const auto sy = size_dist(rng);
|
|
||||||
const auto sz = size_dist(rng);
|
|
||||||
aabbs.push_back({{x, y, z}, {x + sx, y + sy, z + sz}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Run 50 random queries
|
|
||||||
for (int i = 0; i < 50; ++i)
|
|
||||||
{
|
|
||||||
const auto qx = pos_dist(rng);
|
|
||||||
const auto qy = pos_dist(rng);
|
|
||||||
const auto qz = pos_dist(rng);
|
|
||||||
const auto qsx = size_dist(rng);
|
|
||||||
const auto qsy = size_dist(rng);
|
|
||||||
const auto qsz = size_dist(rng);
|
|
||||||
const Aabb query = {{qx, qy, qz}, {qx + qsx, qy + qsy, qz + qsz}};
|
|
||||||
|
|
||||||
const auto bvh_results = tree.query_overlaps(query);
|
|
||||||
const auto brute_results = brute_force_overlaps(aabbs, query);
|
|
||||||
|
|
||||||
const std::set<std::size_t> bvh_set(bvh_results.begin(), bvh_results.end());
|
|
||||||
EXPECT_EQ(bvh_set, brute_results) << "Mismatch on random query iteration " << i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Large dataset tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, LargeGridDataset)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int x = 0; x < 10; ++x)
|
|
||||||
for (int y = 0; y < 10; ++y)
|
|
||||||
for (int z = 0; z < 10; ++z)
|
|
||||||
{
|
|
||||||
const auto fx = static_cast<float>(x) * 3.f;
|
|
||||||
const auto fy = static_cast<float>(y) * 3.f;
|
|
||||||
const auto fz = static_cast<float>(z) * 3.f;
|
|
||||||
aabbs.push_back({{fx, fy, fz}, {fx + 1.f, fy + 1.f, fz + 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
EXPECT_FALSE(tree.empty());
|
|
||||||
|
|
||||||
const auto results = tree.query_overlaps({{0.f, 0.f, 0.f}, {1.5f, 1.5f, 1.5f}});
|
|
||||||
EXPECT_EQ(results.size(), 1);
|
|
||||||
|
|
||||||
const auto all_results = tree.query_overlaps({{-1.f, -1.f, -1.f}, {100.f, 100.f, 100.f}});
|
|
||||||
EXPECT_EQ(all_results.size(), 1000);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, FiveThousandObjects)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 5000; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 2.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
EXPECT_FALSE(tree.empty());
|
|
||||||
|
|
||||||
// Query that should hit exactly 1
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {0.5f, 0.5f, 0.5f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
|
|
||||||
// Query that misses
|
|
||||||
const auto miss = tree.query_overlaps({{-100.f, -100.f, -100.f}, {-90.f, -90.f, -90.f}});
|
|
||||||
EXPECT_TRUE(miss.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Double precision tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, DoublePrecision)
|
|
||||||
{
|
|
||||||
const std::vector<AabbD> aabbs = {
|
|
||||||
{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}},
|
|
||||||
{{5.0, 5.0, 5.0}, {6.0, 6.0, 6.0}},
|
|
||||||
{{10.0, 10.0, 10.0}, {11.0, 11.0, 11.0}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTreeD tree(aabbs);
|
|
||||||
EXPECT_FALSE(tree.empty());
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{0.5, 0.5, 0.5}, {1.5, 1.5, 1.5}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
|
|
||||||
const auto r2 = tree.query_overlaps({{4.5, 4.5, 4.5}, {5.5, 5.5, 5.5}});
|
|
||||||
ASSERT_EQ(r2.size(), 1);
|
|
||||||
EXPECT_EQ(r2[0], 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, DoublePrecisionLargeCoordinates)
|
|
||||||
{
|
|
||||||
const std::vector<AabbD> aabbs = {
|
|
||||||
{{1e10, 1e10, 1e10}, {1e10 + 1.0, 1e10 + 1.0, 1e10 + 1.0}},
|
|
||||||
{{-1e10, -1e10, -1e10}, {-1e10 + 1.0, -1e10 + 1.0, -1e10 + 1.0}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTreeD tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{1e10 - 0.5, 1e10 - 0.5, 1e10 - 0.5},
|
|
||||||
{1e10 + 0.5, 1e10 + 0.5, 1e10 + 0.5}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// Edge case tests
|
|
||||||
// ============================================================================
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ZeroSizeQuery)
|
|
||||||
{
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Point query inside the box
|
|
||||||
const auto r = tree.query_overlaps({{0.5f, 0.5f, 0.5f}, {0.5f, 0.5f, 0.5f}});
|
|
||||||
EXPECT_EQ(r.size(), 1);
|
|
||||||
|
|
||||||
// Point query outside the box
|
|
||||||
const auto miss = tree.query_overlaps({{5.f, 5.f, 5.f}, {5.f, 5.f, 5.f}});
|
|
||||||
EXPECT_TRUE(miss.empty());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, ZeroSizeObjects)
|
|
||||||
{
|
|
||||||
// Point-like AABBs
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{1.f, 1.f, 1.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{5.f, 5.f, 5.f}, {5.f, 5.f, 5.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{0.f, 0.f, 0.f}, {2.f, 2.f, 2.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, NoDuplicateResults)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 50; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 2.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
const auto r = tree.query_overlaps({{-1.f, -1.f, -1.f}, {200.f, 2.f, 2.f}});
|
|
||||||
|
|
||||||
// Check for duplicates
|
|
||||||
const std::set<std::size_t> unique_results(r.begin(), r.end());
|
|
||||||
EXPECT_EQ(unique_results.size(), r.size());
|
|
||||||
EXPECT_EQ(r.size(), 50);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, LargeSpread)
|
|
||||||
{
|
|
||||||
// Objects with huge gaps between them
|
|
||||||
const std::vector<Aabb> aabbs = {
|
|
||||||
{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}},
|
|
||||||
{{1000.f, 0.f, 0.f}, {1001.f, 1.f, 1.f}},
|
|
||||||
{{-1000.f, 0.f, 0.f}, {-999.f, 1.f, 1.f}},
|
|
||||||
{{0.f, 1000.f, 0.f}, {1.f, 1001.f, 1.f}},
|
|
||||||
{{0.f, -1000.f, 0.f}, {1.f, -999.f, 1.f}},
|
|
||||||
};
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
auto r = tree.query_overlaps({{999.f, -1.f, -1.f}, {1002.f, 2.f, 2.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 1);
|
|
||||||
|
|
||||||
r = tree.query_overlaps({{-1001.f, -1.f, -1.f}, {-998.f, 2.f, 2.f}});
|
|
||||||
ASSERT_EQ(r.size(), 1);
|
|
||||||
EXPECT_EQ(r[0], 2);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, AllObjectsSameCenter)
|
|
||||||
{
|
|
||||||
// All AABBs centered at origin but different sizes
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 1; i <= 10; ++i)
|
|
||||||
{
|
|
||||||
const auto s = static_cast<float>(i);
|
|
||||||
aabbs.push_back({{-s, -s, -s}, {s, s, s}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Small query at origin should hit all
|
|
||||||
const auto r = tree.query_overlaps({{-0.1f, -0.1f, -0.1f}, {0.1f, 0.1f, 0.1f}});
|
|
||||||
EXPECT_EQ(r.size(), 10);
|
|
||||||
|
|
||||||
// Query touching only the largest box
|
|
||||||
const auto r2 = tree.query_overlaps({{9.5f, 9.5f, 9.5f}, {10.5f, 10.5f, 10.5f}});
|
|
||||||
ASSERT_EQ(r2.size(), 1);
|
|
||||||
EXPECT_EQ(r2[0], 9);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(UnitTestBvhTree, MultipleQueriesSameTree)
|
|
||||||
{
|
|
||||||
std::vector<Aabb> aabbs;
|
|
||||||
for (int i = 0; i < 100; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 2.f;
|
|
||||||
aabbs.push_back({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
}
|
|
||||||
|
|
||||||
const BvhTree tree(aabbs);
|
|
||||||
|
|
||||||
// Run many queries on the same tree
|
|
||||||
for (int i = 0; i < 100; ++i)
|
|
||||||
{
|
|
||||||
const auto f = static_cast<float>(i) * 2.f;
|
|
||||||
const auto r = tree.query_overlaps({{f, 0.f, 0.f}, {f + 1.f, 1.f, 1.f}});
|
|
||||||
ASSERT_GE(r.size(), 1) << "Query for object " << i << " should find at least itself";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user