Added collision

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) {
-			return navigation.findPath(start, end);
+		static constexpr float kDynamicObstacleInfluenceDist = 6.0f;
+
+		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