Added collision

src/navigation/PathFinder.cpp

@@ -74,8 +74,8 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
 
     Cell startCell;
     Cell endCell;
-    if (!findNearestWalkableCell(start, startCell) ||
-        !findNearestWalkableCell(end, endCell)) {
+    if (!findNearestWalkableCell(start, startCell, walkable) ||
+        !findNearestWalkableCell(end, endCell, walkable)) {
         return {};
     }
 
@@ -121,7 +121,7 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
 
         for (const auto& offset : offsets) {
             Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
-            if (!isCellWalkable(next)) {
+            if (!isCellWalkable(next, walkable)) {
                 continue;
             }
 
@@ -129,7 +129,7 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
             if (diagonal) {
                 Cell horizontal{ currentCell.x + offset[0], currentCell.z };
                 Cell vertical{ currentCell.x, currentCell.z + offset[1] };
-                if (!isCellWalkable(horizontal) || !isCellWalkable(vertical)) {
+                if (!isCellWalkable(horizontal, walkable) || !isCellWalkable(vertical, walkable)) {
                     continue;
                 }
             }
@@ -160,7 +160,161 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
         }
     }
     std::reverse(cells.begin(), cells.end());
-    cells = smoothCells(cells);
+    cells = smoothCells(cells, walkable);
+
+    std::vector<Eigen::Vector3f> path;
+    path.reserve(cells.size());
+    for (const Cell& cell : cells) {
+        path.push_back(cellCenter(cell));
+    }
+
+    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
+        path.erase(path.begin());
+    }
+
+    if (!path.empty()) {
+        Cell requestedEndCell;
+        if (worldToCell(end, requestedEndCell) &&
+            requestedEndCell.x == endCell.x &&
+            requestedEndCell.z == endCell.z) {
+            path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
+        }
+    }
+
+    return path;
+}
+
+std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
+                                                  const Eigen::Vector3f& end,
+                                                  const std::vector<DynamicObstacle>& dynamicObstacles) const
+{
+    if (!ready || walkable.empty()) {
+        return {};
+    }
+
+    std::vector<unsigned char> walkableGrid = walkable;
+    if (!dynamicObstacles.empty()) {
+        for (const DynamicObstacle& obstacle : dynamicObstacles) {
+            const float radius = obstacle.radius + agentRadius;
+            if (radius <= 0.0f) continue;
+
+            const float minWorldX = obstacle.position.x() - radius;
+            const float maxWorldX = obstacle.position.x() + radius;
+            const float minWorldZ = obstacle.position.z() - radius;
+            const float maxWorldZ = obstacle.position.z() + radius;
+
+            const int minCellX = (std::max)(0, static_cast<int>(std::floor((minWorldX - minX) / cellSize)));
+            const int maxCellX = (std::min)(gridWidth - 1, static_cast<int>(std::floor((maxWorldX - minX) / cellSize)));
+            const int minCellZ = (std::max)(0, static_cast<int>(std::floor((minWorldZ - minZ) / cellSize)));
+            const int maxCellZ = (std::min)(gridDepth - 1, static_cast<int>(std::floor((maxWorldZ - minZ) / cellSize)));
+
+            const float radiusSq = radius * radius;
+            for (int z = minCellZ; z <= maxCellZ; ++z) {
+                for (int x = minCellX; x <= maxCellX; ++x) {
+                    const Cell cell{ x, z };
+                    const Eigen::Vector3f center = cellCenter(cell);
+                    const float dx = center.x() - obstacle.position.x();
+                    const float dz = center.z() - obstacle.position.z();
+                    if (dx * dx + dz * dz <= radiusSq) {
+                        const int idx = indexOf(cell);
+                        if (idx >= 0 && idx < static_cast<int>(walkableGrid.size())) {
+                            walkableGrid[static_cast<size_t>(idx)] = 0;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    Cell startCell;
+    Cell endCell;
+    if (!findNearestWalkableCell(start, startCell, walkableGrid) ||
+        !findNearestWalkableCell(end, endCell, walkableGrid)) {
+        return {};
+    }
+
+    if (startCell.x == endCell.x && startCell.z == endCell.z) {
+        return { cellCenter(endCell) };
+    }
+
+    struct QueueNode {
+        int index = 0;
+        float priority = 0.0f;
+
+        bool operator<(const QueueNode& other) const
+        {
+            return priority > other.priority;
+        }
+    };
+
+    const int cellCount = gridWidth * gridDepth;
+    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
+    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
+    std::priority_queue<QueueNode> open;
+
+    const int startIndex = indexOf(startCell);
+    const int endIndex = indexOf(endCell);
+
+    cost[static_cast<size_t>(startIndex)] = 0.0f;
+    open.push({ startIndex, 0.0f });
+
+    static const int offsets[8][2] = {
+        { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
+        { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
+    };
+
+    while (!open.empty()) {
+        const QueueNode current = open.top();
+        open.pop();
+
+        if (current.index == endIndex) {
+            break;
+        }
+
+        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
+
+        for (const auto& offset : offsets) {
+            Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
+            if (!isCellWalkable(next, walkableGrid)) {
+                continue;
+            }
+
+            const bool diagonal = offset[0] != 0 && offset[1] != 0;
+            if (diagonal) {
+                Cell horizontal{ currentCell.x + offset[0], currentCell.z };
+                Cell vertical{ currentCell.x, currentCell.z + offset[1] };
+                if (!isCellWalkable(horizontal, walkableGrid) || !isCellWalkable(vertical, walkableGrid)) {
+                    continue;
+                }
+            }
+
+            const int nextIndex = indexOf(next);
+            const float stepCost = diagonal ? 1.41421356f : 1.0f;
+            const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
+            if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
+                continue;
+            }
+
+            cost[static_cast<size_t>(nextIndex)] = newCost;
+            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
+            const float priority = newCost + distanceCells(next, endCell);
+            open.push({ nextIndex, priority });
+        }
+    }
+
+    if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
+        return {};
+    }
+
+    std::vector<Cell> cells;
+    for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
+        cells.push_back({ current % gridWidth, current / gridWidth });
+        if (current == startIndex) {
+            break;
+        }
+    }
+    std::reverse(cells.begin(), cells.end());
+    cells = smoothCells(cells, walkableGrid);
 
     std::vector<Eigen::Vector3f> path;
     path.reserve(cells.size());
@@ -206,7 +360,7 @@ bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
 bool PathFinder::isWalkable(const Eigen::Vector3f& point) const
 {
     Cell cell;
-    return worldToCell(point, cell) && isCellWalkable(cell);
+    return worldToCell(point, cell) && isCellWalkable(cell, walkable);
 }
 
 void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
@@ -406,7 +560,16 @@ bool PathFinder::isInsideGrid(const Cell& cell) const
 
 bool PathFinder::isCellWalkable(const Cell& cell) const
 {
-    return isInsideGrid(cell) && walkable[static_cast<size_t>(indexOf(cell))] != 0;
+    return isCellWalkable(cell, walkable);
+}
+
+bool PathFinder::isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const
+{
+    const int idx = indexOf(cell);
+    return isInsideGrid(cell) &&
+           idx >= 0 &&
+           idx < static_cast<int>(walkableGrid.size()) &&
+           walkableGrid[static_cast<size_t>(idx)] != 0;
 }
 
 int PathFinder::indexOf(const Cell& cell) const
@@ -415,6 +578,11 @@ int PathFinder::indexOf(const Cell& cell) const
 }
 
 bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const
+{
+    return findNearestWalkableCell(point, out, walkable);
+}
+
+bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const
 {
     Cell origin;
     if (!worldToCell(point, origin)) {
@@ -424,7 +592,7 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out
         origin.z = (std::min)((std::max)(z, 0), gridDepth - 1);
     }
 
-    if (isCellWalkable(origin)) {
+    if (isCellWalkable(origin, walkableGrid)) {
         out = origin;
         return true;
     }
@@ -438,7 +606,7 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out
                 }
 
                 Cell candidate{ origin.x + dx, origin.z + dz };
-                if (isCellWalkable(candidate)) {
+                if (isCellWalkable(candidate, walkableGrid)) {
                     out = candidate;
                     return true;
                 }
@@ -450,6 +618,11 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out
 }
 
 bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
+{
+    return hasLineOfSight(from, to, walkable);
+}
+
+bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const
 {
     const Eigen::Vector3f a = cellCenter(from);
     const Eigen::Vector3f b = cellCenter(to);
@@ -460,7 +633,7 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
         const float t = static_cast<float>(i) / static_cast<float>(steps);
         const Eigen::Vector3f p = a * (1.0f - t) + b * t;
         Cell cell;
-        if (!worldToCell(p, cell) || !isCellWalkable(cell)) {
+        if (!worldToCell(p, cell) || !isCellWalkable(cell, walkableGrid)) {
             return false;
         }
     }
@@ -469,6 +642,11 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
 }
 
 std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells) const
+{
+    return smoothCells(cells, walkable);
+}
+
+std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const
 {
     if (cells.size() <= 2) {
         return cells;
@@ -480,7 +658,7 @@ std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& c
 
     while (anchor < cells.size() - 1) {
         size_t next = cells.size() - 1;
-        while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next])) {
+        while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next], walkableGrid)) {
             --next;
         }
 

src/navigation/PathFinder.h

@@ -14,6 +14,11 @@ public:
         Eigen::Vector3f offset = Eigen::Vector3f::Zero();
     };
 
+	struct DynamicObstacle {
+		Eigen::Vector3f position = Eigen::Vector3f::Zero();
+		float radius = 0.0f;
+	};
+
     struct Cell {
         int x = 0;
         int z = 0;
@@ -32,6 +37,10 @@ public:
     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;
+
     bool setAreaAvailable(const std::string& areaName, bool available);
     bool isReady() const { return ready; }
     bool isWalkable(const Eigen::Vector3f& point) const;
@@ -74,6 +83,11 @@ private:
     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;
 };
 
 } // namespace ZL