120 lines
4.5 KiB
C++
120 lines
4.5 KiB
C++
#pragma once
|
|
|
|
#include "render/Renderer.h"
|
|
#include <Eigen/Core>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
namespace FRG {
|
|
|
|
class PathFinder {
|
|
public:
|
|
|
|
struct ObstaclePolygon {
|
|
std::string name;
|
|
std::vector<Eigen::Vector2f> polygon;
|
|
};
|
|
|
|
struct DynamicObstacle {
|
|
Eigen::Vector3f position = Eigen::Vector3f::Zero();
|
|
float radius = 0.0f;
|
|
};
|
|
|
|
struct Cell {
|
|
int x = 0;
|
|
int z = 0;
|
|
};
|
|
|
|
struct NavigationArea {
|
|
std::string name;
|
|
bool available = true;
|
|
std::vector<Eigen::Vector2f> polygon;
|
|
};
|
|
|
|
void build(const std::string& configPath,
|
|
const std::string& zipPath = "");
|
|
|
|
std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end) const;
|
|
|
|
std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end,
|
|
const std::vector<DynamicObstacle>& dynamicObstacles) const;
|
|
|
|
// Like findPath, but when the destination is unreachable the character
|
|
// walks as close as possible instead of not moving at all.
|
|
std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end) const;
|
|
std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end,
|
|
const std::vector<DynamicObstacle>& dynamicObstacles) const;
|
|
|
|
bool setAreaAvailable(const std::string& areaName, bool available);
|
|
bool isReady() const { return ready; }
|
|
bool isWalkable(const Eigen::Vector3f& point) const;
|
|
const std::vector<NavigationArea>& getAreas() const { return areas; }
|
|
const std::vector<ObstaclePolygon>& getObstaclePolygons() const { return obstaclePolygons; }
|
|
float getFloorY() const { return floorY; }
|
|
|
|
void addObstaclePolygon(const ObstaclePolygon& polygon);
|
|
bool saveConfig(const std::string& path) const;
|
|
bool saveGrid(const std::string& path) const;
|
|
|
|
private:
|
|
|
|
float cellSize = 0.4f;
|
|
float agentRadius = 0.45f;
|
|
float floorY = 0.0f;
|
|
float objectPadding = 0.25f;
|
|
float boundaryPadding = 0.0f;
|
|
|
|
float minX = 0.0f;
|
|
float minZ = 0.0f;
|
|
int gridWidth = 0;
|
|
int gridDepth = 0;
|
|
bool ready = false;
|
|
|
|
std::string loadedConfigPath;
|
|
std::string loadedZipPath;
|
|
std::vector<ObstaclePolygon> obstaclePolygons;
|
|
std::vector<unsigned char> walkable;
|
|
std::vector<NavigationArea> areas;
|
|
|
|
void loadConfig(const std::string& configPath, const std::string& zipPath);
|
|
bool loadGrid(const std::string& path, const std::string& zipPath);
|
|
void resetGridBounds();
|
|
void rebuildWalkableGrid();
|
|
void markAvailableAreasWalkable();
|
|
void markObstaclePolygonsBlocked();
|
|
|
|
bool worldToCell(const Eigen::Vector3f& point, Cell& out) const;
|
|
Eigen::Vector3f cellCenter(const Cell& cell) const;
|
|
bool isInsideGrid(const Cell& cell) const;
|
|
bool isCellWalkable(const Cell& cell) const;
|
|
int indexOf(const Cell& cell) const;
|
|
|
|
bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const;
|
|
bool hasLineOfSight(const Cell& from, const Cell& to) const;
|
|
std::vector<Cell> smoothCells(const std::vector<Cell>& cells) const;
|
|
|
|
static bool pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon);
|
|
|
|
bool isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const;
|
|
bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const;
|
|
bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
|
|
std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;
|
|
|
|
// A* that runs to exhaustion and returns a path to the reachable cell
|
|
// geometrically closest to end when end itself is unreachable.
|
|
std::vector<Eigen::Vector3f> findNearestReachableImpl(const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end,
|
|
const std::vector<unsigned char>& walkableGrid) const;
|
|
|
|
std::vector<Eigen::Vector3f> runAStar(const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end,
|
|
const std::vector<unsigned char>& grid,
|
|
bool returnNearestOnFail) const;
|
|
};
|
|
|
|
} // namespace FRG
|