87 lines
2.9 KiB
C++
87 lines
2.9 KiB
C++
#include "navigation/PathFinder.h"
|
|
|
|
#include <Eigen/Core>
|
|
#include <cstdlib>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <stdexcept>
|
|
|
|
namespace {
|
|
int fail(const char* message)
|
|
{
|
|
std::cout << "PathFinderTests: " << message << std::endl;
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
std::filesystem::path writeNavigationFixture()
|
|
{
|
|
const auto path = std::filesystem::temp_directory_path() /
|
|
"shadow_over_bishkek_pathfinder_test.json";
|
|
std::ofstream file(path);
|
|
file << R"json({
|
|
"areas": [{
|
|
"available": true,
|
|
"name": "test_area",
|
|
"polygon": [[-10.0, 10.0], [10.0, 10.0], [10.0, -10.0], [-10.0, -10.0]]
|
|
}],
|
|
"floorY": 2.5,
|
|
"obstacles": [{
|
|
"name": "test_obstacle",
|
|
"polygon": [[-1.0, 1.0], [1.0, 1.0], [1.0, -1.0], [-1.0, -1.0]]
|
|
}]
|
|
})json";
|
|
if (!file)
|
|
throw std::runtime_error("Failed to create navigation test fixture");
|
|
return path;
|
|
}
|
|
}
|
|
|
|
int main()
|
|
{
|
|
const auto configPath = writeNavigationFixture();
|
|
FRG::PathFinder pathFinder;
|
|
pathFinder.build(configPath.string());
|
|
if (!pathFinder.isReady()) return fail("Navmesh was not built");
|
|
|
|
if (pathFinder.getArea().name != "test_area")
|
|
return fail("First navigation area was not loaded");
|
|
if (pathFinder.getArea().polygon.size() != 4)
|
|
return fail("Navigation area polygon was not loaded");
|
|
if (pathFinder.getObstaclePolygons().size() != 1 ||
|
|
pathFinder.getObstaclePolygons().front().name != "test_obstacle")
|
|
return fail("Navigation obstacles were not loaded");
|
|
if (static_cast<float>(pathFinder.getFloorY()) != 2.5f)
|
|
return fail("Navigation floorY was not loaded");
|
|
|
|
const Eigen::Vector3f start(-5.f, 0.f, -5.f);
|
|
const Eigen::Vector3f end(5.f, 0.f, 5.f);
|
|
const auto path = pathFinder.findPathToNearest(start, end);
|
|
if (path.size() < 2) return fail("Path query did not produce a route");
|
|
if (!pathFinder.isWalkable(Eigen::Vector3f(0.f, 0.f, 5.f)))
|
|
return fail("Walkability query failed");
|
|
if (path.back().y() != 2.5f)
|
|
return fail("Path waypoint did not use floorY");
|
|
|
|
const auto clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(50.f, 50.f));
|
|
if (static_cast<float>(clamped.x) > 10.01f ||
|
|
static_cast<float>(clamped.y) > 10.01f)
|
|
return fail("Clamping returned a point outside the navmesh");
|
|
|
|
pathFinder.build(configPath.string());
|
|
if (pathFinder.getObstaclePolygons().size() != 1)
|
|
return fail("Repeated build accumulated navigation obstacles");
|
|
|
|
bool rejectedNonJson = false;
|
|
try {
|
|
pathFinder.build((configPath.parent_path() / "navigation.txt").string());
|
|
} catch (const std::runtime_error&) {
|
|
rejectedNonJson = true;
|
|
}
|
|
if (!rejectedNonJson)
|
|
return fail("Non-JSON navigation config was accepted");
|
|
|
|
std::filesystem::remove(configPath);
|
|
std::cout << "PathFinderTests: OK" << std::endl;
|
|
return EXIT_SUCCESS;
|
|
} |