35 lines
1.0 KiB
C++
35 lines
1.0 KiB
C++
#include "navigation/PathFinder.h"
|
|
|
|
#include <Eigen/Core>
|
|
#include <cstdlib>
|
|
#include <iostream>
|
|
|
|
namespace {
|
|
int fail(const char* message)
|
|
{
|
|
std::cout << "PathFinderTests: " << message << std::endl;
|
|
return EXIT_FAILURE;
|
|
}
|
|
}
|
|
|
|
int main()
|
|
{
|
|
FRG::PathFinder pathFinder;
|
|
pathFinder.build();
|
|
if (!pathFinder.isReady()) return fail("Navmesh was not built");
|
|
|
|
const Eigen::Vector3f start(-50.f, 0.f, -50.f);
|
|
const Eigen::Vector3f end(50.f, 0.f, 50.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, 50.f)))
|
|
return fail("Walkability query failed");
|
|
|
|
const auto clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(150.f, 150.f));
|
|
if (static_cast<float>(clamped.x) > 100.01f ||
|
|
static_cast<float>(clamped.y) > 100.01f)
|
|
return fail("Clamping returned a point outside the navmesh");
|
|
|
|
std::cout << "PathFinderTests: OK" << std::endl;
|
|
return EXIT_SUCCESS;
|
|
} |