Added collision

src/Character.cpp

@@ -78,6 +78,43 @@ void Character::setPathPlanner(PathPlanner planner) {
     pathPlanner = std::move(planner);
 }
 
+bool Character::isMoving() const
+{
+	Eigen::Vector3f toTarget = walkTarget - position;
+	toTarget.y() = 0.f;
+	return !pathWaypoints.empty() || toTarget.norm() > WALK_THRESHOLD;
+}
+
+Eigen::Vector3f Character::getCurrentNavigationTarget() const
+{
+	if (!pathWaypoints.empty() && currentWaypointIndex < pathWaypoints.size()) {
+		return pathWaypoints[currentWaypointIndex];
+	}
+	return walkTarget;
+}
+
+void Character::forceReplan()
+{
+	if (!pathPlanner) {
+		return;
+	}
+
+	const Eigen::Vector3f normalizedTarget(requestedWalkTarget.x(), 0.f, requestedWalkTarget.z());
+	pathWaypoints = pathPlanner(position, normalizedTarget);
+	currentWaypointIndex = 0;
+
+	if (!pathWaypoints.empty()) {
+		for (Eigen::Vector3f& waypoint : pathWaypoints) {
+			waypoint.y() = 0.f;
+		}
+		walkTarget = pathWaypoints.back();
+		return;
+	}
+
+	walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
+	onArrivedCallback = nullptr;
+}
+
 void Character::setTexture(std::shared_ptr<ZL::Texture> texture)
 {
     for (auto& animEntry : animations) {

src/Character.h

@@ -53,11 +53,21 @@ public:
     void drawShadowDepth(Renderer& renderer);
     void drawWithShadow(Renderer& renderer, const Eigen::Matrix4f& lightFromCamera, GLuint shadowMapTex, const Eigen::Vector3f& lightDirCamera);
 
+	// Character-to-character collision (XZ-plane). Used by Location to keep
+	// player/NPCs from walking through each other.
+	float collisionRadius = 0.45f;
 
-	// attackDirection is a world-space horizontal vector pointing from the
+	// Movement/path introspection used for dynamic replanning.
-	// attacker toward this character — i.e. the direction the hit pushes
+	bool isMoving() const;
-	// them. A zero vector disables directional sparks (isotropic burst).
+	const Eigen::Vector3f& getRequestedWalkTarget() const { return requestedWalkTarget; }
-	void applyDamage(float damageAmount, const Eigen::Vector3f& attackDirection = Eigen::Vector3f::Zero());
+	Eigen::Vector3f getCurrentNavigationTarget() const;
+	void forceReplan();
+
+
+ 	// attackDirection is a world-space horizontal vector pointing from the
+ 	// attacker toward this character — i.e. the direction the hit pushes
+ 	// them. A zero vector disables directional sparks (isotropic burst).
+ 	void applyDamage(float damageAmount, const Eigen::Vector3f& attackDirection = Eigen::Vector3f::Zero());
 
 	// Configures the per-character hit-spark emitter with the shared spark
 	// texture. Safe to call once per character after construction.

src/Location.cpp

@@ -23,6 +23,23 @@ namespace ZL
 	// on_npc_interact callback fires and the conversation begins.
 	static constexpr float NPC_TALK_DISTANCE = 1.25f;
 
+	static float distancePointToSegmentXZ(const Eigen::Vector3f& p,
+	                                      const Eigen::Vector3f& a,
+	                                      const Eigen::Vector3f& b)
+	{
+		const Eigen::Vector2f p2(p.x(), p.z());
+		const Eigen::Vector2f a2(a.x(), a.z());
+		const Eigen::Vector2f b2(b.x(), b.z());
+		const Eigen::Vector2f ab = b2 - a2;
+		const float abLenSq = ab.squaredNorm();
+		if (abLenSq <= 1e-8f) {
+			return (p2 - a2).norm();
+		}
+		const float t = std::clamp((p2 - a2).dot(ab) / abLenSq, 0.0f, 1.0f);
+		const Eigen::Vector2f closest = a2 + ab * t;
+		return (p2 - closest).norm();
+	}
+
 	Location::Location(Renderer& iRenderer, Inventory& iInventory)
 		: renderer(iRenderer)
 		, inventory(iInventory)
@@ -201,17 +218,43 @@ namespace ZL
 		buildDebugNavMeshes();
 #endif
 
-		auto planner = [this](const Eigen::Vector3f& start, const Eigen::Vector3f& end) {
+		static constexpr float kDynamicObstacleInfluenceDist = 6.0f;
-			return navigation.findPath(start, end);
+
+		auto makePlanner = [this](const Character* self) {
+			return [this, self](const Eigen::Vector3f& start, const Eigen::Vector3f& end) {
+				std::vector<PathFinder::DynamicObstacle> dynamicObstacles;
+				dynamicObstacles.reserve(npcs.size() + 1);
+
+				const auto addCharacter = [&](const Character* other) {
+					if (!other || other == self) return;
+					if (other->hp <= 0.f) return;
+
+					if (distancePointToSegmentXZ(other->position, start, end) > kDynamicObstacleInfluenceDist) {
+						return;
+					}
+
+					PathFinder::DynamicObstacle obs;
+					obs.position = Eigen::Vector3f(other->position.x(), navigation.getFloorY(), other->position.z());
+					obs.radius = (std::max)(0.0f, other->collisionRadius);
+					dynamicObstacles.push_back(obs);
+				};
+
+				addCharacter(player.get());
+				for (const auto& npc : npcs) {
+					addCharacter(npc.get());
+				}
+
+				return navigation.findPath(start, end, dynamicObstacles);
 			};
+		};
 
 		if (player) {
-			player->setPathPlanner(planner);
+			player->setPathPlanner(makePlanner(player.get()));
 		}
 
 		for (auto& npc : npcs) {
 			if (npc) {
-				npc->setPathPlanner(planner);
+				npc->setPathPlanner(makePlanner(npc.get()));
 			}
 		}
 	}
@@ -640,6 +683,156 @@ namespace ZL
 		return navigation.setAreaAvailable(areaName, available);
 	}
 
+	void Location::resolveCharacterCollisions()
+	{
+		std::vector<Character*> characters;
+		characters.reserve(npcs.size() + 1);
+
+		if (player) {
+			characters.push_back(player.get());
+		}
+		for (auto& npc : npcs) {
+			if (npc) {
+				characters.push_back(npc.get());
+			}
+		}
+
+		if (characters.size() < 2) {
+			return;
+		}
+
+		static constexpr int kIterations = 3;
+		static constexpr float kMinSeparationEps = 1e-4f;
+
+		for (int iter = 0; iter < kIterations; ++iter) {
+			for (size_t i = 0; i < characters.size(); ++i) {
+				for (size_t j = i + 1; j < characters.size(); ++j) {
+					Character* a = characters[i];
+					Character* b = characters[j];
+					if (!a || !b) continue;
+					if (a->hp <= 0.f || b->hp <= 0.f) continue;
+
+					const float minDist = a->collisionRadius + b->collisionRadius;
+					if (minDist <= 0.f) continue;
+
+					const Eigen::Vector2f pa(a->position.x(), a->position.z());
+					const Eigen::Vector2f pb(b->position.x(), b->position.z());
+					const Eigen::Vector2f delta = pb - pa;
+					const float dist = delta.norm();
+
+					if (dist >= minDist) {
+						continue;
+					}
+
+					Eigen::Vector2f normal(1.f, 0.f);
+					if (dist > kMinSeparationEps) {
+						normal = delta / dist;
+					}
+
+					const float penetration = (minDist - dist);
+					const float push = penetration * 0.5f;
+
+					Eigen::Vector3f newA = a->position;
+					Eigen::Vector3f newB = b->position;
+					newA.x() -= normal.x() * push;
+					newA.z() -= normal.y() * push;
+					newA.y() = 0.f;
+					newB.x() += normal.x() * push;
+					newB.z() += normal.y() * push;
+					newB.y() = 0.f;
+
+					if (navigation.isReady()) {
+						const bool aOk = navigation.isWalkable(newA);
+						const bool bOk = navigation.isWalkable(newB);
+
+						if (aOk && bOk) {
+							a->position = newA;
+							b->position = newB;
+						}
+						else if (aOk && !bOk) {
+							a->position = newA;
+						}
+						else if (!aOk && bOk) {
+							b->position = newB;
+						}
+					}
+					else {
+						a->position = newA;
+						b->position = newB;
+					}
+				}
+			}
+		}
+	}
+
+	void Location::updateDynamicReplans(int64_t deltaMs)
+	{
+		static constexpr float kMovedEps = 0.05f;
+		static constexpr float kReplanTriggerDist = 1.1f;
+		static constexpr int64_t kReplanCooldownMs = 300;
+
+		for (auto it = replanCooldownRemainingMs.begin(); it != replanCooldownRemainingMs.end();) {
+			it->second -= deltaMs;
+			if (it->second <= 0) {
+				it = replanCooldownRemainingMs.erase(it);
+			}
+			else {
+				++it;
+			}
+		}
+
+		std::vector<Character*> characters;
+		characters.reserve(npcs.size() + 1);
+		if (player) characters.push_back(player.get());
+		for (auto& npc : npcs) if (npc) characters.push_back(npc.get());
+
+		std::vector<Character*> movers;
+		movers.reserve(characters.size());
+		for (Character* c : characters) {
+			if (!c) continue;
+			auto it = lastCharacterPositions.find(c);
+			if (it == lastCharacterPositions.end()) {
+				lastCharacterPositions[c] = c->position;
+				continue;
+			}
+
+			const Eigen::Vector3f prev = it->second;
+			const float moved = (c->position - prev).norm();
+			it->second = c->position;
+
+			if (moved > kMovedEps) {
+				movers.push_back(c);
+			}
+		}
+
+		if (movers.empty()) {
+			return;
+		}
+
+		for (Character* mover : movers) {
+			if (!mover) continue;
+
+			for (Character* walker : characters) {
+				if (!walker || walker == mover) continue;
+				if (walker->hp <= 0.f) continue;
+				if (!walker->isMoving()) continue;
+
+				if (replanCooldownRemainingMs.find(walker) != replanCooldownRemainingMs.end()) {
+					continue;
+				}
+
+				const Eigen::Vector3f nextTarget = walker->getCurrentNavigationTarget();
+				const float distToSegment = distancePointToSegmentXZ(mover->position, walker->position, nextTarget);
+				if (distToSegment > kReplanTriggerDist) {
+					continue;
+				}
+
+				walker->forceReplan();
+				replanCooldownRemainingMs[walker] = kReplanCooldownMs;
+			}
+		}
+	}
+
 	void Location::update(int64_t delta)
 	{
 		if (player) {
@@ -651,6 +844,9 @@ namespace ZL
 			npc->update(delta);
 		}
 
+		resolveCharacterCollisions();
+		updateDynamicReplans(delta);
+
 		// Check if player reached target interactive object
 		if (targetInteractiveObject && player) {
 			float distToObject = (player->position - targetInteractiveObject->position).norm();
@@ -883,4 +1079,4 @@ namespace ZL
 	}
 
 
-} // namespace ZL
+} // namespace ZL

src/Location.h

@@ -13,6 +13,8 @@
 #include "dialogue/DialogueSystem.h"
 #include "SparkEmitter.h"
 #include <functional>
+#include <cstdint>
+#include <unordered_map>
 namespace ZL
 {
 
@@ -112,6 +114,13 @@ namespace ZL
 	protected:
 		Renderer& renderer;
 		Inventory& inventory;		
+
+	private:
+		void resolveCharacterCollisions();
+		void updateDynamicReplans(int64_t deltaMs);
+
+		std::unordered_map<Character*, Eigen::Vector3f> lastCharacterPositions;
+		std::unordered_map<Character*, int64_t> replanCooldownRemainingMs;
 	};
 
-} // namespace ZL
+} // namespace ZL

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) ||
+    if (!findNearestWalkableCell(start, startCell, walkable) ||
-        !findNearestWalkableCell(end, endCell)) {
+        !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