Improved pathfinding logic

src/navigation/PathFinder.h

@@ -41,6 +41,14 @@ public:
 	                                      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;
@@ -95,6 +103,12 @@ private:
 	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;
 };
 
 } // namespace ZL