Working on minor fixing, working on pathfinding

src/Location.cpp

@@ -450,7 +450,16 @@ namespace FRG
 				const auto addCharacter = [&](const Character* other) {
 					if (!other || other == self) return;
 					if (other->getHp() <= 0.f || !other->state.enabled) return;
-					if (other->isMoving()) return;
+
+					// Проверяем, находятся ли персонажи в состоянии фактического столкновения
+					const float dist = (other->state.position - self->state.position).norm();
+					const float collisionDist = self->state.collisionRadius + other->state.collisionRadius;
+
+					// Добавляем epsilon (0.1f) для учета погрешностей при входе в зону коллизии
+					const bool isColliding = dist <= (collisionDist + 0.1f);
+
+					// Игнорируем движущегося персонажа, только если мы с ним НЕ сталкиваемся в данный момент
+					if (other->isMoving() && !isColliding) return;
 
 					if (distancePointToSegmentXZ(other->state.position, start, end) > kDynamicObstacleInfluenceDist) {
 						return;
@@ -460,7 +469,7 @@ namespace FRG
 					obs.position = Eigen::Vector3f(other->state.position.x(), navigation->getFloorY(), other->state.position.z());
 					obs.radius = (std::max)(0.0f, other->state.collisionRadius * 0.6f);
 					dynamicObstacles.push_back(obs);
-				};
+					};
 
 				addCharacter(player.get());
 				for (const auto& npc : npcs) {
@@ -468,8 +477,8 @@ namespace FRG
 				}
 
 				return navigation->findPathToNearest(start, end, dynamicObstacles);
+				};
 			};
-		};
 
 		if (player) {
 			player->setPathPlanner(makePlanner(player.get()));
@@ -591,6 +600,9 @@ namespace FRG
 	}
 
 	Character* Location::raycastNpcs(const Eigen::Vector3f& rayOrigin, const Eigen::Vector3f& rayDir, float maxDistance) {
+
+		//return nullptr;
+		
 		// Every NPC is treated as a vertical cylinder: radius 1.0m, height 1.85m,
 		// base at npc->position (the model's foot). Intersection = circle hit in the
 		// XZ plane, then clip the entry/exit t against the [yFoot, yFoot+height] slab.
@@ -654,7 +666,6 @@ namespace FRG
 		else {
 			//logger() << "[RAYCAST_NPC] No NPC hit" << std::endl;
 		}
-
 		return closestNpc;
 	}
 
@@ -1413,53 +1424,83 @@ namespace FRG
 						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->state.position;
-					Eigen::Vector3f newB = b->state.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;
-
+					// 1. ПЕРЕСТРОЕНИЕ ПУТИ (выполняется при пересечении обычного радиуса коллизии)
 					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);
+					auto tryReplan = [&](Character* c) {
+						if (replanCooldownRemainingMs.find(c) != replanCooldownRemainingMs.end()) {
+							return; // Уже находится в откате
+						}
+						if (c->forceReplan()) {
+							replanCooldownRemainingMs[c] = 500; // таймаут на спам
+						}
+						};
 
-						if (aOk && bOk) {
+					if (aWasMoving || bWasMoving) {
+						if (aWasMoving && bWasMoving) {
+							tryReplan(a);
+							tryReplan(b);
+						}
+						else if (aWasMoving) {
+							tryReplan(a);
+						}
+						else if (bWasMoving) {
+							tryReplan(b);
+						}
+					}
+
+					// 2. ФИЗИЧЕСКОЕ ВЫТАЛКИВАНИЕ (только если дистанция меньше 0.5 * minDist)
+					const float hardMinDist = minDist * 0.5f;
+					if (dist < hardMinDist) {
+						Eigen::Vector2f normal(1.f, 0.f);
+						if (dist > kMinSeparationEps) {
+							normal = delta / dist;
+						}
+
+						// Вычисляем выталкивание относительно hardMinDist, а не minDist
+						const float penetration = (hardMinDist - dist);
+						const float push = penetration * 0.5f;
+
+						Eigen::Vector3f newA = a->state.position;
+						Eigen::Vector3f newB = b->state.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 && navigation->isReady()) {
+							const bool aOk = navigation->isWalkable(newA);
+							const bool bOk = navigation->isWalkable(newB);
+
+							if (aOk && bOk) {
+								a->state.position = newA;
+								b->state.position = newB;
+							}
+							else if (aOk && !bOk) {
+								a->state.position = newA;
+							}
+							else if (!aOk && bOk) {
+								b->state.position = newB;
+							}
+						}
+						else {
 							a->state.position = newA;
 							b->state.position = newB;
 						}
-						else if (aOk && !bOk) {
-							a->state.position = newA;
-						}
-						else if (!aOk && bOk) {
-							b->state.position = newB;
-						}
-					}
-					else {
-						a->state.position = newA;
-						b->state.position = newB;
 					}
 
+					// 3. ОСТАНОВКА И КОЛЛБЭКИ
 					if (a->state.isPlayer && !aWasMoving) a->stopInPlace();
 					if (b->state.isPlayer && !bWasMoving) b->stopInPlace();
 
 					if (aWasMoving && !bWasMoving) {
 						nudgeCharacterAside(b, a->state.position);
 						fireBumpCallbacks(a, b);
-					} else if (bWasMoving && !aWasMoving) {
+					}
+					else if (bWasMoving && !aWasMoving) {
 						nudgeCharacterAside(a, b->state.position);
 						fireBumpCallbacks(b, a);
 					}