mirror of
https://github.com/orange-cpp/omath.git
synced 2026-04-18 18:43:27 +00:00
Compare commits
12 Commits
feature/bv
...
f3f454b02e
| Author | SHA1 | Date | |
|---|---|---|---|
| f3f454b02e | |||
| 0419043720 | |||
| 79f64d9679 | |||
| dbe29926dc | |||
| 9d30446c55 | |||
| ba80aebfae | |||
| 9c1b6d0ba3 | |||
| ea07d17dbb | |||
| bb974da0e2 | |||
| fde764c1fa | |||
|
28e86fc355 | ||
|
|
93e7a9457a |
6
.idea/editor.xml
generated
6
.idea/editor.xml
generated
@@ -103,7 +103,7 @@
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppImplicitDefaultConstructorNotAvailable/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIncompatiblePointerConversion/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIncompleteSwitchStatement/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppInconsistentNaming/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppInconsistentNaming/@EntryIndexedValue" value="HINT" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppIntegralToPointerConversion/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppInvalidLineContinuation/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppJoinDeclarationAndAssignment/@EntryIndexedValue" value="SUGGESTION" type="string" />
|
||||
@@ -202,7 +202,7 @@
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStaticDataMemberInUnnamedStruct/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStaticSpecifierOnAnonymousNamespaceMember/@EntryIndexedValue" value="SUGGESTION" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppStringLiteralToCharPointerConversion/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTabsAreDisallowed/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTabsAreDisallowed/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTemplateArgumentsCanBeDeduced/@EntryIndexedValue" value="HINT" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTemplateParameterNeverUsed/@EntryIndexedValue" value="HINT" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppTemplateParameterShadowing/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
@@ -216,7 +216,7 @@
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnmatchedPragmaEndRegionDirective/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnmatchedPragmaRegionDirective/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnnamedNamespaceInHeaderFile/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnnecessaryWhitespace/@EntryIndexedValue" value="DO_NOT_SHOW" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnnecessaryWhitespace/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnsignedZeroComparison/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUnusedIncludeDirective/@EntryIndexedValue" value="WARNING" type="string" />
|
||||
<option name="/Default/CodeInspection/Highlighting/InspectionSeverities/=CppUseAlgorithmWithCount/@EntryIndexedValue" value="SUGGESTION" type="string" />
|
||||
|
||||
@@ -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
|
||||
@@ -9,5 +9,5 @@
|
||||
|
||||
namespace omath::cry_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, false, NDCDepthRange::ZERO_TO_ONE>;
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::ZERO_TO_ONE>;
|
||||
} // namespace omath::cry_engine
|
||||
@@ -9,5 +9,5 @@
|
||||
|
||||
namespace omath::frostbite_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, false, NDCDepthRange::ZERO_TO_ONE>;
|
||||
} // namespace omath::unity_engine
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::ZERO_TO_ONE>;
|
||||
} // namespace omath::frostbite_engine
|
||||
@@ -9,5 +9,5 @@
|
||||
|
||||
namespace omath::iw_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, false, NDCDepthRange::ZERO_TO_ONE>;
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::ZERO_TO_ONE>;
|
||||
} // namespace omath::iw_engine
|
||||
@@ -8,5 +8,5 @@
|
||||
|
||||
namespace omath::opengl_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, true, NDCDepthRange::NEGATIVE_ONE_TO_ONE>;
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::NEGATIVE_ONE_TO_ONE, {.inverted_forward = true}>;
|
||||
} // namespace omath::opengl_engine
|
||||
@@ -7,5 +7,5 @@
|
||||
#include "traits/camera_trait.hpp"
|
||||
namespace omath::source_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, false, NDCDepthRange::ZERO_TO_ONE>;
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::ZERO_TO_ONE>;
|
||||
} // namespace omath::source_engine
|
||||
@@ -9,5 +9,5 @@
|
||||
|
||||
namespace omath::unity_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, false, NDCDepthRange::ZERO_TO_ONE>;
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::ZERO_TO_ONE, {.inverted_forward = true}>;
|
||||
} // namespace omath::unity_engine
|
||||
@@ -9,5 +9,5 @@
|
||||
|
||||
namespace omath::unreal_engine
|
||||
{
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, false, NDCDepthRange::ZERO_TO_ONE>;
|
||||
using Camera = projection::Camera<Mat4X4, ViewAngles, CameraTrait, NDCDepthRange::ZERO_TO_ONE, {.inverted_right = true}>;
|
||||
} // namespace omath::unreal_engine
|
||||
@@ -35,7 +35,6 @@
|
||||
#include "omath/collision/line_tracer.hpp"
|
||||
#include "omath/collision/gjk_algorithm.hpp"
|
||||
#include "omath/collision/epa_algorithm.hpp"
|
||||
#include "omath/collision/bvh_tree.hpp"
|
||||
// Pathfinding algorithms
|
||||
#include "omath/pathfinding/a_star.hpp"
|
||||
#include "omath/pathfinding/navigation_mesh.hpp"
|
||||
|
||||
46
include/omath/pathfinding/walk_bot.hpp
Normal file
46
include/omath/pathfinding/walk_bot.hpp
Normal file
@@ -0,0 +1,46 @@
|
||||
//
|
||||
// Created by orange on 4/12/2026.
|
||||
//
|
||||
#pragma once
|
||||
#include "navigation_mesh.hpp"
|
||||
#include "omath/linear_algebra/vector3.hpp"
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
namespace omath::pathfinding
|
||||
{
|
||||
enum class WalkBotStatus
|
||||
{
|
||||
IDLE,
|
||||
PATHING,
|
||||
FINISHED
|
||||
};
|
||||
class WalkBot
|
||||
{
|
||||
public:
|
||||
WalkBot() = default;
|
||||
explicit WalkBot(const std::shared_ptr<NavigationMesh>& mesh, float min_node_distance = 1.f);
|
||||
|
||||
void set_nav_mesh(const std::shared_ptr<NavigationMesh>& mesh);
|
||||
void set_min_node_distance(float distance);
|
||||
|
||||
void set_target(const Vector3<float>& target);
|
||||
|
||||
// Clear navigation state so the bot can be re-routed without stale
|
||||
// visited-node memory.
|
||||
void reset();
|
||||
|
||||
// Call every game tick with the current bot world position.
|
||||
void update(const Vector3<float>& bot_position);
|
||||
|
||||
void on_path(const std::function<void(const Vector3<float>&)>& callback);
|
||||
void on_status(const std::function<void(WalkBotStatus)>& callback);
|
||||
|
||||
private:
|
||||
std::weak_ptr<NavigationMesh> m_nav_mesh;
|
||||
std::optional<std::function<void(const Vector3<float>&)>> m_on_next_path_node;
|
||||
std::optional<std::function<void(WalkBotStatus)>> m_on_status_update;
|
||||
std::optional<Vector3<float>> m_last_visited;
|
||||
std::optional<Vector3<float>> m_target;
|
||||
float m_min_node_distance{1.f};
|
||||
};
|
||||
} // namespace omath::pathfinding
|
||||
@@ -42,6 +42,12 @@ namespace omath::projection
|
||||
AUTO,
|
||||
MANUAL,
|
||||
};
|
||||
struct CameraAxes
|
||||
{
|
||||
bool inverted_forward = false;
|
||||
bool inverted_right = false;
|
||||
};
|
||||
|
||||
template<class T, class MatType, class ViewAnglesType>
|
||||
concept CameraEngineConcept =
|
||||
requires(const Vector3<float>& cam_origin, const Vector3<float>& look_at, const ViewAnglesType& angles,
|
||||
@@ -58,8 +64,9 @@ namespace omath::projection
|
||||
requires noexcept(T::calc_projection_matrix(fov, viewport, znear, zfar, ndc_depth_range));
|
||||
};
|
||||
|
||||
template<class Mat4X4Type, class ViewAnglesType, class TraitClass, bool inverted_z = false,
|
||||
NDCDepthRange depth_range = NDCDepthRange::NEGATIVE_ONE_TO_ONE>
|
||||
template<class Mat4X4Type, class ViewAnglesType, class TraitClass,
|
||||
NDCDepthRange depth_range = NDCDepthRange::NEGATIVE_ONE_TO_ONE,
|
||||
CameraAxes axes = {}>
|
||||
requires CameraEngineConcept<TraitClass, Mat4X4Type, ViewAnglesType>
|
||||
class Camera final
|
||||
{
|
||||
@@ -83,6 +90,27 @@ namespace omath::projection
|
||||
{
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
static ViewAnglesType calc_view_angles_from_view_matrix(const Mat4X4Type& view_matrix) noexcept
|
||||
{
|
||||
Vector3<float> forward_vector = {view_matrix[2, 0], view_matrix[2, 1], view_matrix[2, 2]};
|
||||
if constexpr (axes.inverted_forward)
|
||||
forward_vector = -forward_vector;
|
||||
return TraitClass::calc_look_at_angle({}, forward_vector);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
static Vector3<float> calc_origin_from_view_matrix(const Mat4X4Type& view_matrix) noexcept
|
||||
{
|
||||
// The view matrix is R * T(-origin), so the last column stores t = -R * origin.
|
||||
// Recovering origin: origin = -R^T * t
|
||||
return {
|
||||
-(view_matrix[0, 0] * view_matrix[0, 3] + view_matrix[1, 0] * view_matrix[1, 3] + view_matrix[2, 0] * view_matrix[2, 3]),
|
||||
-(view_matrix[0, 1] * view_matrix[0, 3] + view_matrix[1, 1] * view_matrix[1, 3] + view_matrix[2, 1] * view_matrix[2, 3]),
|
||||
-(view_matrix[0, 2] * view_matrix[0, 3] + view_matrix[1, 2] * view_matrix[1, 3] + view_matrix[2, 2] * view_matrix[2, 3]),
|
||||
};
|
||||
}
|
||||
|
||||
void look_at(const Vector3<float>& target)
|
||||
{
|
||||
m_view_angles = TraitClass::calc_look_at_angle(m_origin, target);
|
||||
@@ -100,7 +128,7 @@ namespace omath::projection
|
||||
{
|
||||
const auto& view_matrix = get_view_matrix();
|
||||
|
||||
if constexpr (inverted_z)
|
||||
if constexpr (axes.inverted_forward)
|
||||
return -Vector3<float>{view_matrix[2, 0], view_matrix[2, 1], view_matrix[2, 2]};
|
||||
return {view_matrix[2, 0], view_matrix[2, 1], view_matrix[2, 2]};
|
||||
}
|
||||
@@ -109,6 +137,8 @@ namespace omath::projection
|
||||
Vector3<float> get_right() const noexcept
|
||||
{
|
||||
const auto& view_matrix = get_view_matrix();
|
||||
if constexpr (axes.inverted_right)
|
||||
return -Vector3<float>{view_matrix[0, 0], view_matrix[0, 1], view_matrix[0, 2]};
|
||||
return {view_matrix[0, 0], view_matrix[0, 1], view_matrix[0, 2]};
|
||||
}
|
||||
|
||||
|
||||
@@ -87,11 +87,11 @@ namespace omath::pathfinding
|
||||
|
||||
const auto current_node = current_node_it->second;
|
||||
|
||||
closed_list.emplace(current, current_node);
|
||||
|
||||
if (current == end_vertex)
|
||||
return reconstruct_final_path(closed_list, current);
|
||||
|
||||
closed_list.emplace(current, current_node);
|
||||
|
||||
for (const auto& neighbor: nav_mesh.get_neighbors(current))
|
||||
{
|
||||
if (closed_list.contains(neighbor))
|
||||
|
||||
92
source/pathfinding/walk_bot.cpp
Normal file
92
source/pathfinding/walk_bot.cpp
Normal file
@@ -0,0 +1,92 @@
|
||||
//
|
||||
// Created by orange on 4/12/2026.
|
||||
//
|
||||
#include "omath/pathfinding/walk_bot.hpp"
|
||||
#include "omath/pathfinding/a_star.hpp"
|
||||
|
||||
namespace omath::pathfinding
|
||||
{
|
||||
|
||||
WalkBot::WalkBot(const std::shared_ptr<NavigationMesh>& mesh, const float min_node_distance)
|
||||
: m_nav_mesh(mesh), m_min_node_distance(min_node_distance) {}
|
||||
|
||||
void WalkBot::set_nav_mesh(const std::shared_ptr<NavigationMesh>& mesh)
|
||||
{
|
||||
m_nav_mesh = mesh;
|
||||
}
|
||||
|
||||
void WalkBot::set_min_node_distance(const float distance)
|
||||
{
|
||||
m_min_node_distance = distance;
|
||||
}
|
||||
|
||||
void WalkBot::set_target(const Vector3<float>& target)
|
||||
{
|
||||
m_target = target;
|
||||
}
|
||||
|
||||
void WalkBot::reset()
|
||||
{
|
||||
m_last_visited.reset();
|
||||
}
|
||||
|
||||
void WalkBot::update(const Vector3<float>& bot_position)
|
||||
{
|
||||
if (!m_target.has_value())
|
||||
return;
|
||||
|
||||
if (m_target->distance_to(bot_position) <= m_min_node_distance)
|
||||
{
|
||||
if (m_on_status_update.has_value())
|
||||
m_on_status_update->operator()(WalkBotStatus::FINISHED);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!m_on_next_path_node.has_value())
|
||||
return;
|
||||
|
||||
const auto nav_mesh = m_nav_mesh.lock();
|
||||
if (!nav_mesh)
|
||||
{
|
||||
if (m_on_status_update.has_value())
|
||||
m_on_status_update->operator()(WalkBotStatus::IDLE);
|
||||
return;
|
||||
}
|
||||
|
||||
const auto path = Astar::find_path(bot_position, *m_target, *nav_mesh);
|
||||
if (path.empty())
|
||||
{
|
||||
if (m_on_status_update.has_value())
|
||||
m_on_status_update->operator()(WalkBotStatus::IDLE);
|
||||
return;
|
||||
}
|
||||
|
||||
const auto& nearest = path.front();
|
||||
|
||||
// Record the nearest node as visited once we are close enough to it.
|
||||
if (nearest.distance_to(bot_position) <= m_min_node_distance)
|
||||
m_last_visited = nearest;
|
||||
|
||||
// If the nearest node was already visited, advance to the next one so
|
||||
// we never oscillate back to a node we just left.
|
||||
// If the bot was displaced (blown back), nearest will be an unvisited
|
||||
// node, so we route to it first before continuing forward.
|
||||
if (m_last_visited.has_value() && *m_last_visited == nearest && path.size() > 1)
|
||||
m_on_next_path_node->operator()(path[1]);
|
||||
else
|
||||
m_on_next_path_node->operator()(nearest);
|
||||
|
||||
if (m_on_status_update.has_value())
|
||||
m_on_status_update->operator()(WalkBotStatus::PATHING);
|
||||
}
|
||||
|
||||
void WalkBot::on_path(const std::function<void(const Vector3<float>&)>& callback)
|
||||
{
|
||||
m_on_next_path_node = callback;
|
||||
}
|
||||
|
||||
void WalkBot::on_status(const std::function<void(WalkBotStatus)>& callback)
|
||||
{
|
||||
m_on_status_update = callback;
|
||||
}
|
||||
} // namespace omath::pathfinding
|
||||
@@ -40,8 +40,9 @@ TEST(AStarExtra, TrivialNeighbor)
|
||||
nav.m_vertex_map[v2] = {v1};
|
||||
|
||||
const auto path = Astar::find_path(v1, v2, nav);
|
||||
ASSERT_EQ(path.size(), 1u);
|
||||
EXPECT_EQ(path.front(), v2);
|
||||
ASSERT_EQ(path.size(), 2u);
|
||||
EXPECT_EQ(path.front(), v1);
|
||||
EXPECT_EQ(path.back(), v2);
|
||||
}
|
||||
|
||||
TEST(AStarExtra, StartEqualsGoal)
|
||||
@@ -101,7 +102,7 @@ TEST(AStarExtra, LongerPathAvoidsBlock)
|
||||
constexpr Vector3<float> goal = idx(2, 1);
|
||||
const auto path = Astar::find_path(start, goal, nav);
|
||||
ASSERT_FALSE(path.empty());
|
||||
EXPECT_EQ(path.front(), goal);
|
||||
EXPECT_EQ(path.back(), goal);
|
||||
}
|
||||
|
||||
TEST(AstarTests, TrivialDirectNeighborPath)
|
||||
@@ -114,8 +115,9 @@ TEST(AstarTests, TrivialDirectNeighborPath)
|
||||
nav.m_vertex_map.emplace(v2, std::vector<Vector3<float>>{v1});
|
||||
|
||||
const auto path = Astar::find_path(v1, v2, nav);
|
||||
ASSERT_EQ(path.size(), 1u);
|
||||
EXPECT_EQ(path.front(), v2);
|
||||
ASSERT_EQ(path.size(), 2u);
|
||||
EXPECT_EQ(path.front(), v1);
|
||||
EXPECT_EQ(path.back(), v2);
|
||||
}
|
||||
|
||||
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";
|
||||
}
|
||||
}
|
||||
@@ -5,8 +5,12 @@
|
||||
#include <complex>
|
||||
#include <gtest/gtest.h>
|
||||
#include <omath/3d_primitives/aabb.hpp>
|
||||
#include <omath/engines/cry_engine/camera.hpp>
|
||||
#include <omath/engines/frostbite_engine/camera.hpp>
|
||||
#include <omath/engines/iw_engine/camera.hpp>
|
||||
#include <omath/engines/opengl_engine/camera.hpp>
|
||||
#include <omath/engines/source_engine/camera.hpp>
|
||||
#include <omath/engines/unreal_engine/camera.hpp>
|
||||
#include <omath/projection/camera.hpp>
|
||||
#include <print>
|
||||
#include <random>
|
||||
@@ -510,4 +514,430 @@ TEST(UnitTestProjection, AabbUnityEngineStraddlesNearNotCulled)
|
||||
// Box straddles near plane (Unity: +Z forward)
|
||||
const omath::primitives::Aabb<float> aabb{{-1.f, -1.f, -5.f}, {1.f, 1.f, 5.f}};
|
||||
EXPECT_FALSE(cam.is_aabb_culled_by_frustum(aabb));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcViewAnglesFromViewMatrix_LookingForward)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(0.f),
|
||||
omath::source_engine::YawAngle::from_degrees(0.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({0, 0, 0}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto result = omath::source_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), 0.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), 0.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcViewAnglesFromViewMatrix_PositivePitchAndYaw)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(30.f),
|
||||
omath::source_engine::YawAngle::from_degrees(45.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({0, 0, 0}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto result = omath::source_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), 30.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), 45.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcViewAnglesFromViewMatrix_NegativePitchAndYaw)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(-45.f),
|
||||
omath::source_engine::YawAngle::from_degrees(-90.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({0, 0, 0}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto result = omath::source_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), -45.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), -90.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcViewAnglesFromViewMatrix_OffOriginCameraIgnored)
|
||||
{
|
||||
// The forward vector from the view matrix does not depend on camera origin,
|
||||
// so the same angles should be recovered regardless of position.
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(20.f),
|
||||
omath::source_engine::YawAngle::from_degrees(60.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({100.f, 200.f, -50.f}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto result = omath::source_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), 20.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), 60.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcViewAnglesFromViewMatrix_RollAlwaysZero)
|
||||
{
|
||||
// Roll cannot be encoded in the forward vector, so it is always 0 in the result.
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(10.f),
|
||||
omath::source_engine::YawAngle::from_degrees(30.f),
|
||||
omath::source_engine::RollAngle::from_degrees(15.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({0, 0, 0}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto result = omath::source_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_FLOAT_EQ(result.roll.as_degrees(), 0.f);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcOriginFromViewMatrix_AtOrigin)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const auto cam = omath::source_engine::Camera({0, 0, 0}, {}, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto origin = omath::source_engine::Camera::calc_origin_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin.x, 0.f, k_eps);
|
||||
EXPECT_NEAR(origin.y, 0.f, k_eps);
|
||||
EXPECT_NEAR(origin.z, 0.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcOriginFromViewMatrix_ArbitraryPosition)
|
||||
{
|
||||
constexpr float k_eps = 1e-3f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(0.f),
|
||||
omath::source_engine::YawAngle::from_degrees(0.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({100.f, 200.f, -50.f}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto origin = omath::source_engine::Camera::calc_origin_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin.x, 100.f, k_eps);
|
||||
EXPECT_NEAR(origin.y, 200.f, k_eps);
|
||||
EXPECT_NEAR(origin.z, -50.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcOriginFromViewMatrix_WithRotation)
|
||||
{
|
||||
// Origin recovery must work even when the camera is rotated.
|
||||
constexpr float k_eps = 1e-3f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
const omath::source_engine::ViewAngles angles{
|
||||
omath::source_engine::PitchAngle::from_degrees(30.f),
|
||||
omath::source_engine::YawAngle::from_degrees(45.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::source_engine::Camera({300.f, -100.f, 75.f}, angles, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto origin = omath::source_engine::Camera::calc_origin_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin.x, 300.f, k_eps);
|
||||
EXPECT_NEAR(origin.y, -100.f, k_eps);
|
||||
EXPECT_NEAR(origin.z, 75.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CalcOriginFromViewMatrix_IndependentOfAngles)
|
||||
{
|
||||
// Same position, different orientations — should always recover the same origin.
|
||||
constexpr float k_eps = 1e-3f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(90.f);
|
||||
constexpr omath::Vector3<float> expected_origin{50.f, 50.f, 50.f};
|
||||
|
||||
const omath::source_engine::ViewAngles angles_a{
|
||||
omath::source_engine::PitchAngle::from_degrees(0.f),
|
||||
omath::source_engine::YawAngle::from_degrees(0.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const omath::source_engine::ViewAngles angles_b{
|
||||
omath::source_engine::PitchAngle::from_degrees(-60.f),
|
||||
omath::source_engine::YawAngle::from_degrees(135.f),
|
||||
omath::source_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
|
||||
const auto cam_a = omath::source_engine::Camera(expected_origin, angles_a, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
const auto cam_b = omath::source_engine::Camera(expected_origin, angles_b, {1920.f, 1080.f}, fov, 0.01f, 1000.f);
|
||||
|
||||
const auto origin_a = omath::source_engine::Camera::calc_origin_from_view_matrix(cam_a.get_view_matrix());
|
||||
const auto origin_b = omath::source_engine::Camera::calc_origin_from_view_matrix(cam_b.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin_a.x, expected_origin.x, k_eps);
|
||||
EXPECT_NEAR(origin_a.y, expected_origin.y, k_eps);
|
||||
EXPECT_NEAR(origin_a.z, expected_origin.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(origin_b.x, expected_origin.x, k_eps);
|
||||
EXPECT_NEAR(origin_b.y, expected_origin.y, k_eps);
|
||||
EXPECT_NEAR(origin_b.z, expected_origin.z, k_eps);
|
||||
}
|
||||
|
||||
// ---- Unity engine camera tests ----
|
||||
|
||||
TEST(UnitTestProjection, Unity_CalcViewAnglesFromViewMatrix_LookingForward)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(60.f);
|
||||
const omath::unity_engine::ViewAngles angles{
|
||||
omath::unity_engine::PitchAngle::from_degrees(0.f),
|
||||
omath::unity_engine::YawAngle::from_degrees(0.f),
|
||||
omath::unity_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::unity_engine::Camera({0, 0, 0}, angles, {1280.f, 720.f}, fov, 0.03f, 1000.f);
|
||||
|
||||
const auto result = omath::unity_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), 0.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), 0.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, Unity_CalcViewAnglesFromViewMatrix_PositivePitchAndYaw)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(60.f);
|
||||
const omath::unity_engine::ViewAngles angles{
|
||||
omath::unity_engine::PitchAngle::from_degrees(30.f),
|
||||
omath::unity_engine::YawAngle::from_degrees(45.f),
|
||||
omath::unity_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::unity_engine::Camera({0, 0, 0}, angles, {1280.f, 720.f}, fov, 0.03f, 1000.f);
|
||||
|
||||
const auto result = omath::unity_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), 30.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), 45.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, Unity_CalcViewAnglesFromViewMatrix_NegativePitchAndYaw)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(60.f);
|
||||
const omath::unity_engine::ViewAngles angles{
|
||||
omath::unity_engine::PitchAngle::from_degrees(-45.f),
|
||||
omath::unity_engine::YawAngle::from_degrees(-90.f),
|
||||
omath::unity_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::unity_engine::Camera({0, 0, 0}, angles, {1280.f, 720.f}, fov, 0.03f, 1000.f);
|
||||
|
||||
const auto result = omath::unity_engine::Camera::calc_view_angles_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(result.pitch.as_degrees(), -45.f, k_eps);
|
||||
EXPECT_NEAR(result.yaw.as_degrees(), -90.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, Unity_CalcOriginFromViewMatrix_AtOrigin)
|
||||
{
|
||||
constexpr float k_eps = 1e-4f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(60.f);
|
||||
const auto cam = omath::unity_engine::Camera({0, 0, 0}, {}, {1280.f, 720.f}, fov, 0.03f, 1000.f);
|
||||
|
||||
const auto origin = omath::unity_engine::Camera::calc_origin_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin.x, 0.f, k_eps);
|
||||
EXPECT_NEAR(origin.y, 0.f, k_eps);
|
||||
EXPECT_NEAR(origin.z, 0.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, Unity_CalcOriginFromViewMatrix_ArbitraryPosition)
|
||||
{
|
||||
constexpr float k_eps = 1e-3f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(60.f);
|
||||
const omath::unity_engine::ViewAngles angles{
|
||||
omath::unity_engine::PitchAngle::from_degrees(0.f),
|
||||
omath::unity_engine::YawAngle::from_degrees(0.f),
|
||||
omath::unity_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::unity_engine::Camera({100.f, 200.f, -50.f}, angles, {1280.f, 720.f}, fov, 0.03f, 1000.f);
|
||||
|
||||
const auto origin = omath::unity_engine::Camera::calc_origin_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin.x, 100.f, k_eps);
|
||||
EXPECT_NEAR(origin.y, 200.f, k_eps);
|
||||
EXPECT_NEAR(origin.z, -50.f, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, Unity_CalcOriginFromViewMatrix_WithRotation)
|
||||
{
|
||||
constexpr float k_eps = 1e-3f;
|
||||
constexpr auto fov = omath::projection::FieldOfView::from_degrees(60.f);
|
||||
const omath::unity_engine::ViewAngles angles{
|
||||
omath::unity_engine::PitchAngle::from_degrees(30.f),
|
||||
omath::unity_engine::YawAngle::from_degrees(45.f),
|
||||
omath::unity_engine::RollAngle::from_degrees(0.f)
|
||||
};
|
||||
const auto cam = omath::unity_engine::Camera({300.f, -100.f, 75.f}, angles, {1280.f, 720.f}, fov, 0.03f, 1000.f);
|
||||
|
||||
const auto origin = omath::unity_engine::Camera::calc_origin_from_view_matrix(cam.get_view_matrix());
|
||||
|
||||
EXPECT_NEAR(origin.x, 300.f, k_eps);
|
||||
EXPECT_NEAR(origin.y, -100.f, k_eps);
|
||||
EXPECT_NEAR(origin.z, 75.f, k_eps);
|
||||
}
|
||||
// ---- Camera basis vectors at zero angles ----
|
||||
|
||||
TEST(UnitTestProjection, SourceEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::source_engine::Camera({}, {}, {1920.f, 1080.f},
|
||||
omath::projection::FieldOfView::from_degrees(90.f), 0.01f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::source_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::source_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::source_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::source_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::source_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::source_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::source_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::source_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::source_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, UnityEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::unity_engine::Camera({}, {}, {1280.f, 720.f},
|
||||
omath::projection::FieldOfView::from_degrees(60.f), 0.03f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::unity_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::unity_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::unity_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::unity_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::unity_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::unity_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::unity_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::unity_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::unity_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, OpenGLEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::opengl_engine::Camera({}, {}, {1920.f, 1080.f},
|
||||
omath::projection::FieldOfView::from_degrees(90.f), 0.01f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::opengl_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::opengl_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::opengl_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::opengl_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::opengl_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::opengl_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::opengl_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::opengl_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::opengl_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, UnrealEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::unreal_engine::Camera({}, {}, {1920.f, 1080.f},
|
||||
omath::projection::FieldOfView::from_degrees(90.f), 0.01f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::unreal_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::unreal_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::unreal_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::unreal_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::unreal_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::unreal_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::unreal_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::unreal_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::unreal_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, FrostbiteEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::frostbite_engine::Camera({}, {}, {1920.f, 1080.f},
|
||||
omath::projection::FieldOfView::from_degrees(90.f), 0.01f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::frostbite_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::frostbite_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::frostbite_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::frostbite_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::frostbite_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::frostbite_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::frostbite_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::frostbite_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::frostbite_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, CryEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::cry_engine::Camera({}, {}, {1920.f, 1080.f},
|
||||
omath::projection::FieldOfView::from_degrees(90.f), 0.01f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::cry_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::cry_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::cry_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::cry_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::cry_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::cry_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::cry_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::cry_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::cry_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
TEST(UnitTestProjection, IWEngine_ZeroAngles_BasisVectors)
|
||||
{
|
||||
constexpr float k_eps = 1e-5f;
|
||||
const auto cam = omath::iw_engine::Camera({}, {}, {1920.f, 1080.f},
|
||||
omath::projection::FieldOfView::from_degrees(90.f), 0.01f, 1000.f);
|
||||
const auto fwd = cam.get_forward();
|
||||
const auto right = cam.get_right();
|
||||
const auto up = cam.get_up();
|
||||
|
||||
EXPECT_NEAR(fwd.x, omath::iw_engine::k_abs_forward.x, k_eps);
|
||||
EXPECT_NEAR(fwd.y, omath::iw_engine::k_abs_forward.y, k_eps);
|
||||
EXPECT_NEAR(fwd.z, omath::iw_engine::k_abs_forward.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(right.x, omath::iw_engine::k_abs_right.x, k_eps);
|
||||
EXPECT_NEAR(right.y, omath::iw_engine::k_abs_right.y, k_eps);
|
||||
EXPECT_NEAR(right.z, omath::iw_engine::k_abs_right.z, k_eps);
|
||||
|
||||
EXPECT_NEAR(up.x, omath::iw_engine::k_abs_up.x, k_eps);
|
||||
EXPECT_NEAR(up.y, omath::iw_engine::k_abs_up.y, k_eps);
|
||||
EXPECT_NEAR(up.z, omath::iw_engine::k_abs_up.z, k_eps);
|
||||
}
|
||||
|
||||
640
tests/general/unit_test_walk_bot.cpp
Normal file
640
tests/general/unit_test_walk_bot.cpp
Normal file
@@ -0,0 +1,640 @@
|
||||
// Unit tests for omath::pathfinding::WalkBot
|
||||
// Covers all status transitions, callback behaviour, and a full walk simulation.
|
||||
#include <gtest/gtest.h>
|
||||
#include "omath/pathfinding/walk_bot.hpp"
|
||||
#include "omath/pathfinding/navigation_mesh.hpp"
|
||||
|
||||
using namespace omath;
|
||||
using namespace omath::pathfinding;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Helpers
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
// Build a simple bidirectional linear chain:
|
||||
// (0,0,0) <-> (1,0,0) <-> (2,0,0) <-> ... <-> (n-1,0,0)
|
||||
static std::shared_ptr<NavigationMesh> make_linear_mesh(int n)
|
||||
{
|
||||
auto mesh = std::make_shared<NavigationMesh>();
|
||||
for (int i = 0; i < n; ++i)
|
||||
{
|
||||
const Vector3<float> v{static_cast<float>(i), 0.f, 0.f};
|
||||
std::vector<Vector3<float>> neighbors;
|
||||
if (i > 0)
|
||||
neighbors.push_back(Vector3<float>{static_cast<float>(i - 1), 0.f, 0.f});
|
||||
if (i + 1 < n)
|
||||
neighbors.push_back(Vector3<float>{static_cast<float>(i + 1), 0.f, 0.f});
|
||||
mesh->m_vertex_map[v] = neighbors;
|
||||
}
|
||||
return mesh;
|
||||
}
|
||||
|
||||
// Collect every status update fired during one update() call.
|
||||
static auto make_status_recorder(std::vector<WalkBotStatus>& out)
|
||||
{
|
||||
return [&out](WalkBotStatus s) { out.push_back(s); };
|
||||
}
|
||||
|
||||
// Collect every "next node" hint fired during one update() call.
|
||||
static auto make_node_recorder(std::vector<Vector3<float>>& out)
|
||||
{
|
||||
return [&out](const Vector3<float>& v) { out.push_back(v); };
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Construction
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, DefaultConstructedBotIsInert)
|
||||
{
|
||||
// A default-constructed bot with no mesh, no target, and no callbacks must
|
||||
// not crash.
|
||||
WalkBot bot;
|
||||
EXPECT_NO_THROW(bot.update({0.f, 0.f, 0.f}));
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, ConstructWithMeshAndDistance)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
EXPECT_NO_THROW(WalkBot bot(mesh, 0.5f));
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Status: FINISHED
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, FiresFinishedWhenBotIsAtTarget)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({2.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
|
||||
// bot_position == target_position -> distance == 0, well within threshold
|
||||
bot.update({2.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.front(), WalkBotStatus::FINISHED);
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, FiresFinishedWhenBotIsWithinMinDistance)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({0.4f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
|
||||
// distance between (0,0,0) and (0.4,0,0) is 0.4 < 0.5 threshold
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.front(), WalkBotStatus::FINISHED);
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, NoFinishedWhenOutsideMinDistance)
|
||||
{
|
||||
auto mesh = make_linear_mesh(5);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({4.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
|
||||
// Attach path callback so we get further status updates
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
// FINISHED must NOT appear in the status list
|
||||
for (auto s : statuses)
|
||||
EXPECT_NE(s, WalkBotStatus::FINISHED);
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, FinishedFiredEvenWithoutPathCallback)
|
||||
{
|
||||
// FINISHED is emitted before the on_path guard, so it fires regardless.
|
||||
auto mesh = make_linear_mesh(2);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({1.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
// Intentionally do NOT register on_path callback.
|
||||
|
||||
bot.update({1.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_EQ(statuses.size(), 1u);
|
||||
EXPECT_EQ(statuses[0], WalkBotStatus::FINISHED);
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, FinishedDoesNotFallThroughToPathing)
|
||||
{
|
||||
// update() must return after FINISHED — PATHING must not fire on the same tick.
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({1.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({1.f, 0.f, 0.f}); // bot is at target
|
||||
|
||||
ASSERT_EQ(statuses.size(), 1u);
|
||||
EXPECT_EQ(statuses[0], WalkBotStatus::FINISHED);
|
||||
EXPECT_TRUE(nodes.empty());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// No target set
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, NoUpdateWithoutTarget)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
// Intentionally do NOT call set_target()
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
EXPECT_TRUE(statuses.empty());
|
||||
EXPECT_TRUE(nodes.empty());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Status: IDLE — no path callback registered
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, NoPathCallbackMeansNoPathingStatus)
|
||||
{
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({3.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
// No on_path registered -> update returns early after FINISHED check
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
EXPECT_TRUE(statuses.empty());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Status: IDLE — null/expired navigation mesh
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, FiresIdleWhenNavMeshIsNull)
|
||||
{
|
||||
WalkBot bot; // no mesh assigned
|
||||
bot.set_target({5.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.back(), WalkBotStatus::IDLE);
|
||||
EXPECT_TRUE(nodes.empty());
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, FiresIdleWhenNavMeshExpires)
|
||||
{
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({3.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
// Let the shared_ptr expire — WalkBot holds only a weak_ptr.
|
||||
mesh.reset();
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.back(), WalkBotStatus::IDLE);
|
||||
EXPECT_TRUE(nodes.empty());
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, SetNavMeshRestoresPathing)
|
||||
{
|
||||
WalkBot bot; // starts with no mesh
|
||||
bot.set_target({3.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
// First call — no mesh -> IDLE
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
ASSERT_EQ(statuses.back(), WalkBotStatus::IDLE);
|
||||
|
||||
// Assign a mesh and call again. Keep the shared_ptr alive so the
|
||||
// weak_ptr inside WalkBot does not expire before update() is called.
|
||||
statuses.clear();
|
||||
nodes.clear();
|
||||
auto new_mesh = make_linear_mesh(4);
|
||||
bot.set_nav_mesh(new_mesh);
|
||||
bot.set_min_node_distance(0.5f);
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.back(), WalkBotStatus::PATHING);
|
||||
EXPECT_FALSE(nodes.empty());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Status: IDLE — A* finds no path
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, FiresIdleWhenNoPathExists)
|
||||
{
|
||||
// Disconnected graph: two isolated vertices
|
||||
auto mesh = std::make_shared<NavigationMesh>();
|
||||
mesh->m_vertex_map[{0.f, 0.f, 0.f}] = {};
|
||||
mesh->m_vertex_map[{10.f, 0.f, 0.f}] = {};
|
||||
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({10.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.back(), WalkBotStatus::IDLE);
|
||||
EXPECT_TRUE(nodes.empty());
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Status: PATHING — normal routing
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, FiresPathingAndProvidesNextNode)
|
||||
{
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({3.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.back(), WalkBotStatus::PATHING);
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, NextNodeIsOnThePath)
|
||||
{
|
||||
auto mesh = make_linear_mesh(5);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({4.f, 0.f, 0.f});
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
// The suggested node must be a mesh vertex (x in [0..4], y=0, z=0)
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
const auto& hint = nodes.front();
|
||||
EXPECT_GE(hint.x, 0.f);
|
||||
EXPECT_LE(hint.x, 4.f);
|
||||
EXPECT_FLOAT_EQ(hint.y, 0.f);
|
||||
EXPECT_FLOAT_EQ(hint.z, 0.f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// set_min_node_distance
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, MinNodeDistanceAffectsFinishedThreshold)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.1f); // very tight threshold
|
||||
bot.set_target({1.f, 0.f, 0.f});
|
||||
|
||||
std::vector<WalkBotStatus> statuses;
|
||||
bot.on_status(make_status_recorder(statuses));
|
||||
|
||||
// distance 0.4 — outside 0.1 threshold
|
||||
bot.update({0.6f, 0.f, 0.f});
|
||||
EXPECT_TRUE(statuses.empty() ||
|
||||
std::find(statuses.begin(), statuses.end(), WalkBotStatus::FINISHED) == statuses.end());
|
||||
|
||||
statuses.clear();
|
||||
bot.set_min_node_distance(0.5f); // widen threshold
|
||||
bot.update({0.6f, 0.f, 0.f}); // now 0.4 < 0.5 -> FINISHED
|
||||
|
||||
ASSERT_FALSE(statuses.empty());
|
||||
EXPECT_EQ(statuses.front(), WalkBotStatus::FINISHED);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// reset()
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, ResetClearsLastVisited)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({2.f, 0.f, 0.f});
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
// Tick 1: mark node 0 visited -> hint is node 1
|
||||
bot.update({0.05f, 0.f, 0.f});
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
EXPECT_FLOAT_EQ(nodes.back().x, 1.f);
|
||||
|
||||
// Without reset, a second tick from the same position also gives node 1.
|
||||
nodes.clear();
|
||||
bot.reset();
|
||||
|
||||
// After reset, m_last_visited is cleared. The nearest node is 0 again,
|
||||
// it is within threshold so it gets marked visited and we advance to 1.
|
||||
// The hint should still be node 1 (same outcome, but state was cleanly reset).
|
||||
bot.update({0.05f, 0.f, 0.f});
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
// Confirm the bot still routes correctly after reset.
|
||||
EXPECT_GE(nodes.back().x, 0.f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Node advancement — visited node causes skip to next
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, AdvancesWhenNearestNodeAlreadyVisited)
|
||||
{
|
||||
// Chain: (0,0,0) -> (1,0,0) -> (2,0,0)
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({2.f, 0.f, 0.f});
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
// Tick 1: bot is very close to node 0 -> node 0 marked visited -> hint is node 1.
|
||||
bot.update({0.05f, 0.f, 0.f});
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
EXPECT_FLOAT_EQ(nodes.back().x, 1.f);
|
||||
|
||||
nodes.clear();
|
||||
|
||||
// Tick 2: bot has moved to near node 1 -> node 1 marked visited -> hint advances to node 2.
|
||||
bot.update({1.05f, 0.f, 0.f});
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
EXPECT_GT(nodes.back().x, 1.f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Displacement recovery — bot knocked back to unvisited node
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, RecoverAfterDisplacementToUnvisitedNode)
|
||||
{
|
||||
// Chain: 0 -> 1 -> 2 -> 3 -> 4
|
||||
auto mesh = make_linear_mesh(5);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({4.f, 0.f, 0.f});
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
// Walk forward through nodes 0-3 to build visited state.
|
||||
for (int i = 0; i <= 3; ++i)
|
||||
{
|
||||
nodes.clear();
|
||||
bot.update(Vector3<float>{static_cast<float>(i) + 0.1f, 0.f, 0.f});
|
||||
}
|
||||
|
||||
// Displace the bot back to near node 1. The bot should route toward node 1
|
||||
// first rather than skipping ahead to node 4.
|
||||
nodes.clear();
|
||||
bot.update({1.1f, 0.f, 0.f});
|
||||
|
||||
ASSERT_FALSE(nodes.empty());
|
||||
EXPECT_LE(nodes.back().x, 3.f);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Callback wiring
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotTests, ReplacingPathCallbackTakesEffect)
|
||||
{
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({3.f, 0.f, 0.f});
|
||||
|
||||
int first_cb_calls = 0;
|
||||
int second_cb_calls = 0;
|
||||
|
||||
bot.on_path([&](const Vector3<float>&) { ++first_cb_calls; });
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
bot.on_path([&](const Vector3<float>&) { ++second_cb_calls; });
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
EXPECT_EQ(first_cb_calls, 1);
|
||||
EXPECT_EQ(second_cb_calls, 1);
|
||||
}
|
||||
|
||||
TEST(WalkBotTests, ReplacingStatusCallbackTakesEffect)
|
||||
{
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkBot bot(mesh, 0.5f);
|
||||
bot.set_target({3.f, 0.f, 0.f});
|
||||
|
||||
std::vector<Vector3<float>> nodes;
|
||||
bot.on_path(make_node_recorder(nodes));
|
||||
|
||||
int cb1 = 0, cb2 = 0;
|
||||
bot.on_status([&](WalkBotStatus) { ++cb1; });
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
bot.on_status([&](WalkBotStatus) { ++cb2; });
|
||||
bot.update({0.f, 0.f, 0.f});
|
||||
|
||||
EXPECT_EQ(cb1, 1);
|
||||
EXPECT_EQ(cb2, 1);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Full walk simulation — bot traverses a linear mesh step by step
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
// Simulates one game-loop tick and immediately "teleports" the bot to the
|
||||
// suggested node so the next tick starts from there.
|
||||
struct WalkSimulator
|
||||
{
|
||||
WalkBot bot;
|
||||
Vector3<float> position;
|
||||
std::vector<Vector3<float>> visited_nodes;
|
||||
std::vector<WalkBotStatus> status_history;
|
||||
bool finished{false};
|
||||
|
||||
WalkSimulator(const std::shared_ptr<NavigationMesh>& mesh,
|
||||
const Vector3<float>& start,
|
||||
const Vector3<float>& goal,
|
||||
float threshold)
|
||||
: position(start)
|
||||
{
|
||||
bot = WalkBot(mesh, threshold);
|
||||
bot.set_target(goal);
|
||||
bot.on_path([this](const Vector3<float>& next) {
|
||||
visited_nodes.push_back(next);
|
||||
position = next; // teleport to the suggested node
|
||||
});
|
||||
bot.on_status([this](WalkBotStatus s) {
|
||||
status_history.push_back(s);
|
||||
if (s == WalkBotStatus::FINISHED)
|
||||
finished = true;
|
||||
});
|
||||
}
|
||||
|
||||
void run(int max_ticks = 100)
|
||||
{
|
||||
for (int tick = 0; tick < max_ticks && !finished; ++tick)
|
||||
bot.update(position);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(WalkBotSimulation, BotReachesTargetOnLinearMesh)
|
||||
{
|
||||
auto mesh = make_linear_mesh(5);
|
||||
WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {4.f, 0.f, 0.f}, 0.5f);
|
||||
sim.run(50);
|
||||
|
||||
EXPECT_TRUE(sim.finished);
|
||||
}
|
||||
|
||||
TEST(WalkBotSimulation, StatusTransitionSequenceIsCorrect)
|
||||
{
|
||||
// Expect: one or more PATHING updates, then exactly FINISHED at the end.
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {3.f, 0.f, 0.f}, 0.5f);
|
||||
sim.run(50);
|
||||
|
||||
ASSERT_TRUE(sim.finished);
|
||||
ASSERT_FALSE(sim.status_history.empty());
|
||||
|
||||
// All entries before the last must be PATHING
|
||||
for (std::size_t i = 0; i + 1 < sim.status_history.size(); ++i)
|
||||
EXPECT_EQ(sim.status_history[i], WalkBotStatus::PATHING);
|
||||
|
||||
EXPECT_EQ(sim.status_history.back(), WalkBotStatus::FINISHED);
|
||||
}
|
||||
|
||||
TEST(WalkBotSimulation, BotVisitsNodesInForwardOrder)
|
||||
{
|
||||
auto mesh = make_linear_mesh(5);
|
||||
WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {4.f, 0.f, 0.f}, 0.5f);
|
||||
sim.run(50);
|
||||
|
||||
ASSERT_FALSE(sim.visited_nodes.empty());
|
||||
|
||||
// Verify that x-coordinates are non-decreasing (forward progress only).
|
||||
for (std::size_t i = 1; i < sim.visited_nodes.size(); ++i)
|
||||
EXPECT_GE(sim.visited_nodes[i].x, sim.visited_nodes[i - 1].x - 1e-3f);
|
||||
}
|
||||
|
||||
TEST(WalkBotSimulation, TwoNodePathReachesGoal)
|
||||
{
|
||||
auto mesh = make_linear_mesh(2); // (0,0,0) <-> (1,0,0)
|
||||
WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {1.f, 0.f, 0.f}, 0.5f);
|
||||
sim.run(10);
|
||||
|
||||
EXPECT_TRUE(sim.finished);
|
||||
}
|
||||
|
||||
TEST(WalkBotSimulation, LongChainEventuallyFinishes)
|
||||
{
|
||||
constexpr int kLength = 20;
|
||||
auto mesh = make_linear_mesh(kLength);
|
||||
WalkSimulator sim(mesh,
|
||||
{0.f, 0.f, 0.f},
|
||||
{static_cast<float>(kLength - 1), 0.f, 0.f},
|
||||
0.5f);
|
||||
sim.run(200);
|
||||
|
||||
EXPECT_TRUE(sim.finished);
|
||||
}
|
||||
|
||||
TEST(WalkBotSimulation, StartAlreadyAtTargetFinishesImmediately)
|
||||
{
|
||||
auto mesh = make_linear_mesh(3);
|
||||
WalkSimulator sim(mesh, {1.f, 0.f, 0.f}, {1.f, 0.f, 0.f}, 0.5f);
|
||||
sim.run(5);
|
||||
|
||||
EXPECT_TRUE(sim.finished);
|
||||
EXPECT_EQ(sim.status_history.front(), WalkBotStatus::FINISHED);
|
||||
EXPECT_EQ(sim.status_history.size(), 1u);
|
||||
}
|
||||
|
||||
TEST(WalkBotSimulation, NoIdleEmittedDuringSuccessfulWalk)
|
||||
{
|
||||
auto mesh = make_linear_mesh(4);
|
||||
WalkSimulator sim(mesh, {0.f, 0.f, 0.f}, {3.f, 0.f, 0.f}, 0.5f);
|
||||
sim.run(50);
|
||||
|
||||
for (auto s : sim.status_history)
|
||||
EXPECT_NE(s, WalkBotStatus::IDLE);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Walk simulation on a branching mesh
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
TEST(WalkBotSimulation, BotNavigatesBranchingMesh)
|
||||
{
|
||||
// Diamond topology:
|
||||
// (1,1,0)
|
||||
// / \
|
||||
// (0,0,0) (2,0,0)
|
||||
// \ /
|
||||
// (1,-1,0)
|
||||
auto mesh = std::make_shared<NavigationMesh>();
|
||||
const Vector3<float> start{0.f, 0.f, 0.f};
|
||||
const Vector3<float> top{1.f, 1.f, 0.f};
|
||||
const Vector3<float> bot_node{1.f, -1.f, 0.f};
|
||||
const Vector3<float> goal{2.f, 0.f, 0.f};
|
||||
|
||||
mesh->m_vertex_map[start] = {top, bot_node};
|
||||
mesh->m_vertex_map[top] = {start, goal};
|
||||
mesh->m_vertex_map[bot_node] = {start, goal};
|
||||
mesh->m_vertex_map[goal] = {top, bot_node};
|
||||
|
||||
WalkSimulator sim(mesh, start, goal, 0.5f);
|
||||
sim.run(20);
|
||||
|
||||
EXPECT_TRUE(sim.finished);
|
||||
}
|
||||
Reference in New Issue
Block a user