137 lines
5.2 KiB
C++
137 lines
5.2 KiB
C++
#pragma once
|
|
|
|
#include <Eigen/Core>
|
|
#include <cstdint>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#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;
|
|
|
|
// Reused by synchronous authoritative path queries. Generation stamps avoid
|
|
// copying/clearing the complete map for every dynamic-obstacle search.
|
|
mutable std::vector<std::uint32_t> dynamicBlockedGeneration;
|
|
mutable std::uint32_t dynamicGeneration = 0;
|
|
mutable std::vector<int> searchCost;
|
|
mutable std::vector<int> searchCameFrom;
|
|
mutable std::vector<std::uint32_t> searchVisitedGeneration;
|
|
mutable std::uint32_t searchGeneration = 0;
|
|
mutable std::shared_ptr<std::mutex> queryMutex =
|
|
std::make_shared<std::mutex>();
|
|
|
|
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;
|
|
|
|
std::uint32_t buildDynamicOverlay(
|
|
const std::vector<DynamicObstacle>& dynamicObstacles) const;
|
|
bool isCellWalkableOverlay(const Cell& cell, std::uint32_t generation) const;
|
|
bool findNearestWalkableCellOverlay(
|
|
const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const;
|
|
bool hasLineOfSightOverlay(
|
|
const Cell& from, const Cell& to, std::uint32_t generation) const;
|
|
std::vector<Cell> smoothCellsOverlay(
|
|
const std::vector<Cell>& cells, std::uint32_t generation) const;
|
|
std::vector<Eigen::Vector3f> runSearch(
|
|
const Eigen::Vector3f& start,
|
|
const Eigen::Vector3f& end,
|
|
std::uint32_t obstacleGeneration,
|
|
bool nearestReachable) const;
|
|
};
|
|
|
|
} // namespace FRG
|