Improved pathfinding logic

src/Character.cpp

@@ -7,6 +7,7 @@
 #include "GameConstants.h"
 #include "Environment.h"
 
+
 namespace ZL {
 
 	const float ATTACK_COOLDOWN_TIME = 1.6f; 
@@ -94,6 +95,15 @@ Eigen::Vector3f Character::getCurrentNavigationTarget() const
 	return walkTarget;
 }
 
+void Character::stopInPlace()
+{
+    walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
+    requestedWalkTarget = walkTarget;
+    pathWaypoints.clear();
+    currentWaypointIndex = 0;
+    onArrivedCallback = nullptr;
+}
+
 void Character::forceReplan()
 {
 	if (!pathPlanner) {
@@ -343,6 +353,18 @@ void Character::update(int64_t deltaMs) {
     }
 
 
+    if (!isPlayer && hp > 0.f && battle_state == 0 && maxHomeDrift > 0.f) {
+        homeDriftCheckTimer -= static_cast<float>(deltaMs) / 1000.f;
+        if (homeDriftCheckTimer <= 0.f) {
+            homeDriftCheckTimer = HOME_DRIFT_CHECK_INTERVAL;
+            Eigen::Vector3f toHome = homePosition - position;
+            toHome.y() = 0.f;
+            if (!isMoving() && toHome.norm() > maxHomeDrift) {
+                setTarget(homePosition);
+            }
+        }
+    }
+
     if (hitSparkEmitter.isConfigured()) {
         hitSparkEmitter.update(static_cast<float>(deltaMs));
     }

src/Character.h

@@ -61,6 +61,10 @@ public:
 	const Eigen::Vector3f& getRequestedWalkTarget() const { return requestedWalkTarget; }
 	Eigen::Vector3f getCurrentNavigationTarget() const;
 	void forceReplan();
+	// Cancels any active walk target and clears waypoints so the character
+	// stays at its current position. Used when an external push moves the
+	// character and we don't want it to walk back to where it was.
+	void stopInPlace();
 
 
  	// attackDirection is a world-space horizontal vector pointing from the
@@ -110,6 +114,13 @@ public:
     float hp = 200.f;
     float initialHp = 200.f;
 
+    // The position the NPC should return to if pushed too far away.
+    // Initialised to the spawn position; updated whenever a script walk command is issued.
+    Eigen::Vector3f homePosition = Eigen::Vector3f::Zero();
+    // Maximum allowed drift from homePosition before a return walk is triggered (metres).
+    // Set to 0 to disable drift recovery for this character.
+    float maxHomeDrift = 3.0f;
+
 
     int battle_state = 0;
     bool resetAnim = false;
@@ -163,6 +174,9 @@ private:
 
     static constexpr float WALK_THRESHOLD = 0.05f;
     static constexpr float TARGET_REPLAN_THRESHOLD = 0.25f;
+    static constexpr float HOME_DRIFT_CHECK_INTERVAL = 5.0f;
+
+    float homeDriftCheckTimer = HOME_DRIFT_CHECK_INTERVAL;
 
     // Returns the animation state to actually play/draw, falling back to IDLE
     // if the requested state has no loaded animation.

src/Game.cpp

@@ -233,9 +233,7 @@ namespace ZL
 		};
 
 
-
-
-		uniInteriorParams.teleportsJsonPath = "resources/config2/teleports.json";
+		uniInteriorParams.teleportsJsonPath = "resources/config2/teleports_uni_interior.json";
 		uniInteriorParams.triggerZonesJsonPath = "resources/config2/trigger_zones_uni_interior.json";
 		uniInteriorParams.scriptPath = "resources/start_uni_interior.lua";
 		uniInteriorParams.interactiveObjectsJsonPath = "resources/config2/interactive_objects_uni_interior.json";

src/Location.cpp

@@ -111,6 +111,7 @@ namespace ZL
 		npcs = GameObjectLoader::loadAndCreate_Npcs(params.npcsJsonPath, CONST_ZIP_FILE);
 		for (auto& npc : npcs) {
 			if (!npc) continue;
+			npc->homePosition = Eigen::Vector3f(npc->position.x(), 0.f, npc->position.z());
 			if (npc->canAttack) {
 				npc->setupHitSparks(sparkTexture);
 				npc->attackTarget = player.get();
@@ -297,6 +298,7 @@ namespace ZL
 				const auto addCharacter = [&](const Character* other) {
 					if (!other || other == self) return;
 					if (other->hp <= 0.f || !other->enabled) return;
+					if (other->isMoving()) return;
 
 					if (distancePointToSegmentXZ(other->position, start, end) > kDynamicObstacleInfluenceDist) {
 						return;
@@ -304,7 +306,7 @@ namespace ZL
 
 					PathFinder::DynamicObstacle obs;
 					obs.position = Eigen::Vector3f(other->position.x(), navigation->getFloorY(), other->position.z());
-					obs.radius = (std::max)(0.0f, other->collisionRadius);
+					obs.radius = (std::max)(0.0f, other->collisionRadius * 0.6f);
 					dynamicObstacles.push_back(obs);
 				};
 
@@ -313,7 +315,7 @@ namespace ZL
 					addCharacter(npc.get());
 				}
 
-				return navigation->findPath(start, end, dynamicObstacles);
+				return navigation->findPathToNearest(start, end, dynamicObstacles);
 			};
 		};
 
@@ -981,6 +983,30 @@ namespace ZL
 		return navigation->setAreaAvailable(areaName, available);
 	}
 
+	void Location::nudgeCharacterAside(Character* standing, const Eigen::Vector3f& awayFrom)
+	{
+		if (!standing || standing->isPlayer) return;
+		if (!navigation || !navigation->isReady()) return;
+
+		static constexpr float kNudgeDist = 1.2f;
+
+		Eigen::Vector3f dir = standing->position - awayFrom;
+		dir.y() = 0.f;
+		if (dir.norm() < 1e-3f) dir = Eigen::Vector3f(1.f, 0.f, 0.f);
+		else dir.normalize();
+
+		const float angles[] = { 0.f, static_cast<float>(M_PI * 0.5), static_cast<float>(-M_PI * 0.5), static_cast<float>(M_PI) };
+		for (float angle : angles) {
+			Eigen::Vector3f rotated = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY()) * dir;
+			Eigen::Vector3f candidate = standing->position + rotated * kNudgeDist;
+			candidate.y() = 0.f;
+			if (navigation->isWalkable(candidate)) {
+				standing->setTarget(candidate);
+				return;
+			}
+		}
+	}
+
 	void Location::resolveCharacterCollisions()
 	{
 		std::vector<Character*> characters;
@@ -1039,6 +1065,9 @@ namespace ZL
 					newB.z() += normal.y() * push;
 					newB.y() = 0.f;
 
+					const bool aWasMoving = a->isMoving();
+					const bool bWasMoving = b->isMoving();
+
 					if (navigation && navigation->isReady()) {
 						const bool aOk = navigation->isWalkable(newA);
 						const bool bOk = navigation->isWalkable(newB);
@@ -1058,6 +1087,15 @@ namespace ZL
 						a->position = newA;
 						b->position = newB;
 					}
+
+					if (a->isPlayer && !aWasMoving) a->stopInPlace();
+					if (b->isPlayer && !bWasMoving) b->stopInPlace();
+
+					if (aWasMoving && !bWasMoving) {
+						nudgeCharacterAside(b, a->position);
+					} else if (bWasMoving && !aWasMoving) {
+						nudgeCharacterAside(a, b->position);
+					}
 				}
 			}
 		}
@@ -1066,8 +1104,8 @@ namespace ZL
 	void Location::updateDynamicReplans(int64_t deltaMs)
 	{
 		static constexpr float kMovedEps = 0.05f;
-		static constexpr float kReplanTriggerDist = 1.1f;
-		static constexpr int64_t kReplanCooldownMs = 300;
+		static constexpr float kReplanTriggerDist = 1.8f;
+		static constexpr int64_t kReplanCooldownMs = 500;
 
 		for (auto it = replanCooldownRemainingMs.begin(); it != replanCooldownRemainingMs.end();) {
 			it->second -= deltaMs;

src/Location.h

@@ -151,6 +151,7 @@ namespace ZL
 		void loadTeleportZones(const std::string& jsonPath, const char* zipFile);
 		void loadTriggerZones(const std::string& jsonPath, const char* zipFile);
 		void updateTriggerZones(const Eigen::Vector3f& playerPos);
+		void nudgeCharacterAside(Character* standing, const Eigen::Vector3f& awayFrom);
 
 		std::unordered_map<Character*, Eigen::Vector3f> lastCharacterPositions;
 		std::unordered_map<Character*, int64_t> replanCooldownRemainingMs;

src/ScriptEngine.cpp

@@ -57,6 +57,7 @@ namespace ZL {
                         }
                         };
                 }
+                npcs[index]->homePosition = Eigen::Vector3f(x, 0.f, z);
                 npcs[index]->setTarget(Eigen::Vector3f(x, y, z), std::move(cb));
             });
 
@@ -104,7 +105,7 @@ namespace ZL {
         api.set_function("set_object_rotation", [game](const std::string& objectName, float value) {
             for (auto& intObj : game->interactiveObjects) {
                 if (intObj.loadedObject.name == objectName) {
-                    intObj.rotationY = value;
+                    intObj.rotationY = value * static_cast<float>(M_PI) / 180.f;
                     std::cout << "[script] set_object_rotation: " << objectName << " " << value << std::endl;
                     return;
                 }
@@ -316,7 +317,7 @@ namespace ZL {
                 npcs[index]->setTarget(pos);
             });
 
-        // npc_set_rotation(index, angle) — sets NPC facing angle around Y axis (radians).
+        // npc_set_rotation(index, angle) — sets NPC facing angle around Y axis (degrees).
         api.set_function("npc_set_rotation",
             [game](int index, float angle) {
                 auto& npcs = game->npcs;
@@ -324,8 +325,9 @@ namespace ZL {
                     std::cerr << "[script] npc_set_rotation: index " << index << " out of range\n";
                     return;
                 }
-                npcs[index]->facingAngle = angle;
-                npcs[index]->targetFacingAngle = angle;
+                const float rad = angle * static_cast<float>(M_PI) / 180.f;
+                npcs[index]->facingAngle = rad;
+                npcs[index]->targetFacingAngle = rad;
             });
 
         // set_npc_enabled(index, enabled)

src/items/GameObjectLoader.cpp

@@ -119,13 +119,14 @@ namespace ZL {
 		else
 			obj.mesh.data = LoadFromTextFile02(data.meshPath, zipPath);
 
+		constexpr float kDeg2Rad = static_cast<float>(M_PI) / 180.0f;
 		Eigen::Quaternionf rot = Eigen::Quaternionf::Identity();
 		if (data.rotationX != 0.0f)
-			rot = Eigen::Quaternionf(Eigen::AngleAxisf(data.rotationX, Eigen::Vector3f::UnitX())) * rot;
+			rot = Eigen::Quaternionf(Eigen::AngleAxisf(data.rotationX * kDeg2Rad, Eigen::Vector3f::UnitX())) * rot;
 		if (data.rotationY != 0.0f)
-			rot = Eigen::Quaternionf(Eigen::AngleAxisf(data.rotationY, Eigen::Vector3f::UnitY())) * rot;
+			rot = Eigen::Quaternionf(Eigen::AngleAxisf(data.rotationY * kDeg2Rad, Eigen::Vector3f::UnitY())) * rot;
 		if (data.rotationZ != 0.0f)
-			rot = Eigen::Quaternionf(Eigen::AngleAxisf(data.rotationZ, Eigen::Vector3f::UnitZ())) * rot;
+			rot = Eigen::Quaternionf(Eigen::AngleAxisf(data.rotationZ * kDeg2Rad, Eigen::Vector3f::UnitZ())) * rot;
 		obj.mesh.data.RotateByMatrix(rot.toRotationMatrix());
 
 		if (data.scale != 1.0f)
@@ -262,7 +263,7 @@ namespace ZL {
 				data.modelCorrectionRotX = item.value("modelCorrectionRotX", 0.0f) * static_cast<float>(M_PI) / 180.0f;
 				data.modelCorrectionRotY = item.value("modelCorrectionRotY", 0.0f) * static_cast<float>(M_PI) / 180.0f;
 				data.modelCorrectionRotZ = item.value("modelCorrectionRotZ", 0.0f) * static_cast<float>(M_PI) / 180.0f;
-				data.facingAngle    = item.value("facingAngle", 0.0f);
+				data.facingAngle    = item.value("facingAngle", 0.0f) * static_cast<float>(M_PI) / 180.0f;
 				data.hp             = item.value("hp", 100.0f);
 				data.canAttack      = item.value("canAttack", false);
 				data.enabled        = item.value("enabled", true);

src/navigation/PathFinder.cpp

@@ -397,6 +397,124 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
     return path;
 }
 
+std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
+    const Eigen::Vector3f& start,
+    const Eigen::Vector3f& end,
+    const std::vector<unsigned char>& walkableGrid) const
+{
+    if (!ready || walkableGrid.empty()) return {};
+
+    Cell startCell;
+    Cell endCell;
+    if (!findNearestWalkableCell(start, startCell, walkableGrid)) return {};
+    if (!findNearestWalkableCell(end,   endCell,   walkableGrid)) return {};
+
+    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.f;
+    open.push({ startIndex, 0.f });
+
+    int   bestIndex = startIndex;
+    float bestDist  = distanceCells(startCell, endCell);
+
+    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) {
+            bestIndex = endIndex;
+            break;
+        }
+
+        if (cost[static_cast<size_t>(current.index)] < current.priority - distanceCells({ current.index % gridWidth, current.index / gridWidth }, endCell)) {
+            continue; // stale entry
+        }
+
+        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
+        const float d = distanceCells(currentCell, endCell);
+        if (d < bestDist) {
+            bestDist  = d;
+            bestIndex = current.index;
+        }
+
+        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 h{ currentCell.x + offset[0], currentCell.z };
+                Cell v{ currentCell.x, currentCell.z + offset[1] };
+                if (!isCellWalkable(h, walkableGrid) || !isCellWalkable(v, 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;
+            open.push({ nextIndex, newCost + distanceCells(next, endCell) });
+        }
+    }
+
+    if (bestIndex == startIndex) return {};
+
+    std::vector<Cell> cells;
+    for (int cur = bestIndex; cur != -1; cur = cameFrom[static_cast<size_t>(cur)]) {
+        cells.push_back({ cur % gridWidth, cur / gridWidth });
+        if (cur == startIndex) break;
+    }
+    std::reverse(cells.begin(), cells.end());
+    cells = smoothCells(cells, walkableGrid);
+
+    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());
+
+    return path;
+}
+
+std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
+    const Eigen::Vector3f& start, const Eigen::Vector3f& end) const
+{
+    auto path = findPath(start, end);
+    if (!path.empty()) return path;
+    return findNearestReachableImpl(start, end, walkable);
+}
+
+std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
+    const Eigen::Vector3f& start, const Eigen::Vector3f& end,
+    const std::vector<DynamicObstacle>& dynamicObstacles) const
+{
+    auto path = findPath(start, end, dynamicObstacles);
+    if (!path.empty()) return path;
+    path = findPath(start, end);
+    if (!path.empty()) return path;
+    return findNearestReachableImpl(start, end, walkable);
+}
+
 bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
 {
     bool found = false;

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