Improved pathfinding logic

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;