Working about pathfinding

src/CharacterState.cpp

@@ -6,10 +6,22 @@
 namespace FRG {
 
 namespace {
+// Tick counts are authoritative and therefore independent of render rate.
 constexpr float ATTACK_COOLDOWN_TIME = 1.6f;
 constexpr float HOME_DRIFT_CHECK_INTERVAL = 5.0f;
 constexpr float TARGET_REPLAN_THRESHOLD = 0.25f;
 constexpr float PI = 3.14159265358979323846f;
+constexpr int BLOCKED_REPLAN_TICKS = 3;
+
+std::uint32_t stableIdHash(const std::string& value)
+{
+    std::uint32_t hash = 2166136261u;
+    for (const unsigned char byte : value) {
+        hash ^= byte;
+        hash *= 16777619u;
+    }
+    return hash;
+}
 }
 
 void CharacterState::configureAsPlayer() {
@@ -23,6 +35,36 @@ void CharacterState::configureAsPlayer() {
     isPlayer = true;
 }
 
+void CharacterState::configureNavigationRetryCooldowns(
+    const std::string& entityId, std::vector<int> cooldownTicks)
+{
+    if (cooldownTicks.empty()) {
+        cooldownTicks = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
+    }
+    for (int& ticks : cooldownTicks)
+        ticks = (std::max)(1, (std::min)(600, ticks));
+
+    std::uint32_t state = stableIdHash(entityId);
+    for (std::size_t i = cooldownTicks.size(); i > 1; --i) {
+        state = state * 1664525u + 1013904223u;
+        const std::size_t other = state % i;
+        std::swap(cooldownTicks[i - 1], cooldownTicks[other]);
+    }
+    navigationRetryCooldownTicks = std::move(cooldownTicks);
+    navigationRetryCooldownIndex = 0;
+}
+
+void CharacterState::scheduleNextNavigationRetry()
+{
+    if (navigationRetryCooldownTicks.empty())
+        configureNavigationRetryCooldowns(npcName);
+    navigationRetryTicks = navigationRetryCooldownTicks[
+        navigationRetryCooldownIndex % navigationRetryCooldownTicks.size()];
+    navigationRetryCooldownIndex =
+        (navigationRetryCooldownIndex + 1) %
+        navigationRetryCooldownTicks.size();
+}
+
 bool CharacterState::isMoving() const {
     Eigen::Vector3f toTarget = walkTarget - position;
     toTarget.y() = 0.f;
@@ -42,13 +84,17 @@ void CharacterState::stopInPlace() {
     pathWaypoints.clear();
     currentWaypointIndex = 0;
     onArrivedCallbackName.clear();
+    movementBlockedTicks = 0;
+    navigationRetryTicks = 0;
+    blockingCharacterId.clear();
 }
 
 void CharacterState::setTarget(
     const Eigen::Vector3f& target,
     std::string callbackName,
     const LocationNetworkState& location,
-    const std::string& entityId)
+    const std::string& entityId,
+    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
 {
     const Eigen::Vector3f normalizedTarget(target.x(), 0.f, target.z());
     const bool sameRequestedTarget =
@@ -66,9 +112,13 @@ void CharacterState::setTarget(
 
     requestedWalkTarget = normalizedTarget;
     onArrivedCallbackName = std::move(callbackName);
+    movementBlockedTicks = 0;
+    navigationRetryTicks = 0;
+    blockingCharacterId.clear();
     pathWaypoints = location.navigationMaps.empty()
         ? std::vector<Eigen::Vector3f>{}
-        : location.planPathExceptMyself(position, normalizedTarget, entityId);
+        : location.planPathExceptMyself(
+            position, normalizedTarget, entityId, dynamicObstacles);
     currentWaypointIndex = 0;
     if (!pathWaypoints.empty()) {
         for (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
@@ -77,26 +127,32 @@ void CharacterState::setTarget(
     }
 
     walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
-    onArrivedCallbackName.clear();
+    if (!isPlayer) scheduleNextNavigationRetry();
 }
 
 void CharacterState::forceReplan(
-    const LocationNetworkState& location, const std::string& entityId)
+    const LocationNetworkState& location,
+    const std::string& entityId,
+    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
 {
     const Eigen::Vector3f normalizedTarget(
         requestedWalkTarget.x(), 0.f, requestedWalkTarget.z());
     pathWaypoints = location.navigationMaps.empty()
         ? std::vector<Eigen::Vector3f>{}
-        : location.planPathExceptMyself(position, normalizedTarget, entityId);
+        : location.planPathExceptMyself(
+            position, normalizedTarget, entityId, dynamicObstacles);
     currentWaypointIndex = 0;
+    movementBlockedTicks = 0;
+    navigationRetryTicks = 0;
     if (!pathWaypoints.empty()) {
         for (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
         walkTarget = pathWaypoints.back();
+        blockingCharacterId.clear();
         return;
     }
 
     walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
-    onArrivedCallbackName.clear();
+    if (!isPlayer) scheduleNextNavigationRetry();
 }
 
 void CharacterState::stopAndRotateToTarget(std::string characterId)
@@ -146,7 +202,9 @@ CharacterUpdateEvents CharacterState::update(
     std::int64_t deltaMs, const CharacterUpdateContext& context)
 {
     CharacterUpdateEvents events;
+    movedLastTick = false;
     if (!enabled) return events;
+    bool navigationPausedThisTick = false;
 
     const CharacterObservation* attackTarget = context.attackTarget;
     const CharacterObservation* faceTarget = context.faceTarget;
@@ -185,12 +243,75 @@ CharacterUpdateEvents CharacterState::update(
         if (distance > kWalkThreshold) {
             const Eigen::Vector3f direction = toTarget / distance;
             const float moveAmount = walkSpeed * static_cast<float>(deltaMs) / 1000.f;
-            position = moveAmount >= distance ? activeTarget : position + direction * moveAmount;
-            position.y() = 0.f;
+            Eigen::Vector3f candidate =
+                moveAmount >= distance ? activeTarget : position + direction * moveAmount;
+            candidate.y() = 0.f;
+            const CharacterMovementObstacle* blocker = nullptr;
+            if (context.movementObstacles) {
+                for (const CharacterMovementObstacle& obstacle :
+                     *context.movementObstacles) {
+                    if (obstacle.id == context.characterId) continue;
+                    Eigen::Vector3f startDelta = position - obstacle.position;
+                    Eigen::Vector3f endDelta = candidate - obstacle.position;
+                    startDelta.y() = 0.f;
+                    endDelta.y() = 0.f;
+                    const float minimumDistance =
+                        movementContactRadius + obstacle.radius;
+                    const float minimumDistanceSq =
+                        minimumDistance * minimumDistance;
+                    const Eigen::Vector3f segment = candidate - position;
+                    const float segmentLengthSq =
+                        segment.x() * segment.x() + segment.z() * segment.z();
+                    float t = 0.f;
+                    if (segmentLengthSq > 1e-8f) {
+                        t = -(startDelta.x() * segment.x() +
+                              startDelta.z() * segment.z()) / segmentLengthSq;
+                        t = (std::max)(0.f, (std::min)(1.f, t));
+                    }
+                    Eigen::Vector3f closest = startDelta + segment * t;
+                    closest.y() = 0.f;
+                    const bool movingCloser =
+                        endDelta.squaredNorm() + 1e-6f < startDelta.squaredNorm();
+                    if (movingCloser &&
+                        closest.squaredNorm() < minimumDistanceSq) {
+                        blocker = &obstacle;
+                        break;
+                    }
+                }
+            }
+            if (blocker) {
+                blockingCharacterId = blocker->id;
+                currentState = AnimationState::STAND;
+                const bool sameDirection =
+                    blocker->moving &&
+                    blocker->movementDirection.dot(direction) > 0.5f;
+                if (sameDirection) {
+                    movementBlockedTicks = 0;
+                } else if (++movementBlockedTicks >= BLOCKED_REPLAN_TICKS) {
+                    if (isPlayer) {
+                        stopInPlace();
+                    } else {
+                        pathWaypoints.clear();
+                        currentWaypointIndex = 0;
+                        walkTarget =
+                            Eigen::Vector3f(position.x(), 0.f, position.z());
+                        movementBlockedTicks = 0;
+                        scheduleNextNavigationRetry();
+                        navigationPausedThisTick = true;
+                    }
+                }
+            } else {
+                movedLastTick =
+                    (candidate - position).squaredNorm() > 1e-8f;
+                position = candidate;
+                movementBlockedTicks = 0;
+                blockingCharacterId.clear();
+            }
             targetFacingAngle = std::atan2(direction.x(), -direction.z());
-            currentState = AnimationState::WALK;
+            if (!blocker) currentState = AnimationState::WALK;
         } else {
             currentState = AnimationState::STAND;
+            const bool completedPath = !pathWaypoints.empty();
             const bool hasNextWaypoint =
                 !pathWaypoints.empty() && currentWaypointIndex + 1 < pathWaypoints.size();
             if (hasNextWaypoint) {
@@ -198,7 +319,12 @@ CharacterUpdateEvents CharacterState::update(
             } else {
                 pathWaypoints.clear();
                 currentWaypointIndex = 0;
-                if (!onArrivedCallbackName.empty()) {
+                if (completedPath &&
+                    (requestedWalkTarget - position).norm() >
+                        TARGET_REPLAN_THRESHOLD &&
+                    !isPlayer)
+                    scheduleNextNavigationRetry();
+                if (completedPath && !onArrivedCallbackName.empty()) {
                     events.arrivedCallbackName = onArrivedCallbackName;
                     onArrivedCallbackName.clear();
                 }
@@ -217,6 +343,18 @@ CharacterUpdateEvents CharacterState::update(
         }
     }
 
+    if (!isPlayer && !navigationPausedThisTick && pathWaypoints.empty() &&
+        (requestedWalkTarget - position).norm() > TARGET_REPLAN_THRESHOLD &&
+        !events.navigationRequested) {
+        if (navigationRetryTicks > 0) --navigationRetryTicks;
+        if (navigationRetryTicks == 0) {
+            events.navigationRequested = true;
+            events.forceNavigationReplan = true;
+            events.navigationTarget = requestedWalkTarget;
+            events.blockingCharacterId = blockingCharacterId;
+        }
+    }
+
     if (hp <= 0.f) {
         if (currentState != AnimationState::ACTION_TO_DEATH &&
             currentState != AnimationState::DEATH_IDLE) {
@@ -350,6 +488,13 @@ void CharacterState::save(nlohmann::json& out) const
     out["pathWaypoints"]        = std::move(wpArr);
     out["currentWaypointIndex"] = currentWaypointIndex;
     out["homeDriftCheckTimer"]  = homeDriftCheckTimer;
+    out["movementBlockedTicks"] = movementBlockedTicks;
+    out["navigationRetryTicks"] = navigationRetryTicks;
+    out["movedLastTick"] = movedLastTick;
+    out["navigationRetryCooldownTicks"] = navigationRetryCooldownTicks;
+    out["navigationRetryCooldownIndex"] = navigationRetryCooldownIndex;
+    if (!blockingCharacterId.empty())
+        out["blockingCharacterId"] = blockingCharacterId;
     out["animationFrame"]       = animationFrame;
     out["visibleRange"]         = visibleRange;
     if (!onArrivedCallbackName.empty())
@@ -387,6 +532,17 @@ void CharacterState::load(const nlohmann::json& in)
     }
     currentWaypointIndex = in.value("currentWaypointIndex", currentWaypointIndex);
     homeDriftCheckTimer  = in.value("homeDriftCheckTimer",  homeDriftCheckTimer);
+    movementBlockedTicks = in.value("movementBlockedTicks", movementBlockedTicks);
+    navigationRetryTicks = in.value("navigationRetryTicks", navigationRetryTicks);
+    movedLastTick = in.value("movedLastTick", movedLastTick);
+    navigationRetryCooldownTicks = in.value(
+        "navigationRetryCooldownTicks", navigationRetryCooldownTicks);
+    navigationRetryCooldownIndex = in.value(
+        "navigationRetryCooldownIndex", navigationRetryCooldownIndex);
+    if (!navigationRetryCooldownTicks.empty())
+        navigationRetryCooldownIndex %= navigationRetryCooldownTicks.size();
+    blockingCharacterId =
+        in.value("blockingCharacterId", std::string());
     animationFrame      = in.value("animationFrame", animationFrame);
     onArrivedCallbackName = in.value("onArrivedCallbackName", std::string());
 }