diff --git a/scripts/HP_SlotRegistry.lua b/scripts/HP_SlotRegistry.lua index 37ee43a..afb60e3 100644 --- a/scripts/HP_SlotRegistry.lua +++ b/scripts/HP_SlotRegistry.lua @@ -187,6 +187,15 @@ end if HP_WorldObstacleTurnEscape == nil and source ~= nil then source((g_currentModDirectory or "") .. "scripts/HP_WorldObstacleTurnEscape.lua") end +if HP_WorldLocalAvoidance == nil and source ~= nil then + source((g_currentModDirectory or "") .. "scripts/HP_WorldLocalAvoidance.lua") +end +if HP_WorldAvoidancePhases == nil and source ~= nil then + source((g_currentModDirectory or "") .. "scripts/HP_WorldAvoidancePhases.lua") +end +if HP_WorldAnticipatorySteering == nil and source ~= nil then + source((g_currentModDirectory or "") .. "scripts/HP_WorldAnticipatorySteering.lua") +end if HP_WorldMovementProbe == nil and source ~= nil then source((g_currentModDirectory or "") .. "scripts/HP_WorldMovementProbe.lua") end diff --git a/scripts/HP_WorldAnticipatorySteering.lua b/scripts/HP_WorldAnticipatorySteering.lua new file mode 100644 index 0000000..bc4c11b --- /dev/null +++ b/scripts/HP_WorldAnticipatorySteering.lua @@ -0,0 +1,395 @@ +-- HP_WorldAnticipatorySteering.lua (FS25_HelperProfiles) +-- Alpha 4 smooth local-avoidance refinement. +-- +-- Normal obstacle avoidance should read as continuous walking rather than a +-- stop/turn/waypoint sequence. This layer therefore adds a longer-range sensor +-- ahead of FOLLOW motion and smoothly biases the existing curved controller's +-- target heading around an obstruction before the short-range safety detector +-- needs to stop the worker. +-- +-- The validated obstacle stop/hold + local waypoint planner remain available +-- as fallback only when no safe anticipatory steering corridor can be found. +-- Once fallback owns an escape episode, anticipatory steering stays suppressed +-- until the worker has returned to sustained, genuinely clear long-range space. + +if HP_WorldObstacleAwareness == nil then return end +if HP_WorldFollow == nil then return end +if HP_WorldLocomotionPrototype == nil then return end +if HP_WorldLocalAvoidance == nil then return end +if HP_WorldAnticipatorySteering ~= nil then return end + +HP_WorldAnticipatorySteering = { + version = "2.2.0.0-alpha4-anticipatory-steering-2", + lookAhead = 3.80, + virtualTargetDistance = 4.50, + engageRateRadPerSec = 0.72, + recoverRateRadPerSec = 0.52, + directClearHoldMs = 300, + fallbackClearReleaseMs = 1500, + logIntervalMs = 700, + candidateOffsetsDeg = {30, -30, 45, -45, 60, -60, 72, -72}, + installed = false +} + +local Steer = HP_WorldAnticipatorySteering +local Awareness = HP_WorldObstacleAwareness +local Follow = HP_WorldFollow +local Loco = HP_WorldLocomotionPrototype +local LOG = "[FS25_HelperProfiles/WorldAvoidance] " +local TWO_PI = math.pi * 2 + +local function log(message, ...) + print(LOG .. string.format(tostring(message), ...)) +end + +local function normalizeAngle(value) + value = tonumber(value) or 0 + while value > math.pi do value = value - TWO_PI end + while value < -math.pi do value = value + TWO_PI end + return value +end + +local function yawFromDirection(dx, dz) + if MathUtil ~= nil and MathUtil.getYRotationFromDirection ~= nil then + local ok, yaw = pcall(MathUtil.getYRotationFromDirection, dx, dz) + if ok and tonumber(yaw) ~= nil then return normalizeAngle(tonumber(yaw)) end + end + return normalizeAngle(math.atan2(dx, dz)) +end + +local function directionFromYaw(yaw) + if MathUtil ~= nil and MathUtil.getDirectionFromYRotation ~= nil then + local ok, dx, dz = pcall(MathUtil.getDirectionFromYRotation, yaw) + if ok and tonumber(dx) ~= nil and tonumber(dz) ~= nil then + return tonumber(dx), tonumber(dz) + end + end + return math.sin(tonumber(yaw) or 0), math.cos(tonumber(yaw) or 0) +end + +local function getSlot(index) + if HP_SlotRegistry ~= nil then return HP_SlotRegistry:indexToSlot(index) end + return tostring(index) +end + +local function moveTowards(value, target, maximumDelta) + value = tonumber(value) or 0 + target = tonumber(target) or 0 + maximumDelta = math.max(0, tonumber(maximumDelta) or 0) + local delta = target - value + if math.abs(delta) <= maximumDelta then return target end + return value + (delta > 0 and maximumDelta or -maximumDelta) +end + +local function walkingSpeed() + return math.max(0.1, tonumber(Loco.walkSpeed) or 1.35) +end + +-- HP_WorldObstacleAwareness intentionally keeps a short, validated emergency +-- stopping corridor. For anticipation we need a longer *read-only* query but +-- must not change the emergency detector's global dimensions. scan() is fully +-- synchronous, so temporarily widening its range for this single call is safe +-- and is restored immediately even if the call fails. +local function scanLong(index, id, x, y, z, yaw, lookAhead) + lookAhead = math.max(2.1, tonumber(lookAhead) or 3.8) + local oldMinimum = Awareness.minimumLookAhead + local oldMaximum = Awareness.maximumLookAhead + local oldFactor = Awareness.speedLookAheadFactor + + Awareness.minimumLookAhead = lookAhead + Awareness.maximumLookAhead = lookAhead + Awareness.speedLookAheadFactor = 0 + + local ok, blocked, result = pcall( + Awareness.scan, Awareness, + index, id, x, y, z, yaw, walkingSpeed()) + + Awareness.minimumLookAhead = oldMinimum + Awareness.maximumLookAhead = oldMaximum + Awareness.speedLookAheadFactor = oldFactor + + if not ok then return false, nil end + return blocked == true, result +end + +local function clearState(state, silent) + if state == nil then return end + local hadSteering = state.hpAnticipatorySteering == true + state.hpAnticipatorySteering = nil + state.hpSteerSide = nil + state.hpSteerOffsetRad = nil + state.hpSteerTargetOffsetRad = nil + state.hpSteerClearMs = nil + state.hpSteerLogMs = nil + state.hpSteerObstacleName = nil + if hadSteering and not silent then + log("STEER END %s direct follow restored", tostring(getSlot(state.index))) + end +end + +function Steer:suppressFallback(state, reason) + if state == nil then return end + local wasSuppressed = state.hpSteerFallbackSuppressed == true + clearState(state, true) + state.hpSteerFallbackSuppressed = true + state.hpSteerFallbackClearMs = 0 + state.hpSteerFallbackReason = tostring(reason or "local-fallback") + if not wasSuppressed then + log("STEER SUPPRESS %s reason=%s; local escape owns navigation until sustained clear space", + tostring(getSlot(state.index)), tostring(state.hpSteerFallbackReason)) + end +end + +function Steer:tryReleaseFallback(state, motion, dt) + if state == nil or state.hpSteerFallbackSuppressed ~= true then return true end + if motion == nil or state.obstacleBlocked == true or state.hpAvoidance ~= nil + or motion.hpAvoidanceWaypoint == true or motion.navigationKind ~= "follow" then + state.hpSteerFallbackClearMs = 0 + return false + end + + local x = tonumber(motion.x) + local y = tonumber(motion.y) + local z = tonumber(motion.z) + local targetX = tonumber(motion.targetX) + local targetZ = tonumber(motion.targetZ) + if x == nil or y == nil or z == nil or targetX == nil or targetZ == nil then + state.hpSteerFallbackClearMs = 0 + return false + end + + local dx = targetX - x + local dz = targetZ - z + local distance = math.sqrt(dx * dx + dz * dz) + if distance <= 0.35 then + state.hpSteerFallbackClearMs = 0 + return false + end + + local directYaw = yawFromDirection(dx, dz) + local blocked = scanLong(state.index, state.id, x, y, z, directYaw, self.lookAhead) + if blocked == true then + state.hpSteerFallbackClearMs = 0 + return false + end + + state.hpSteerFallbackClearMs = (tonumber(state.hpSteerFallbackClearMs) or 0) + + math.max(0, tonumber(dt) or 0) + + -- The local planner deliberately retains its chain state for a period of + -- normal direct following. Respect that ownership too: do not re-enable + -- anticipation merely because one long-range sample happened to be clear. + local chainActive = (tonumber(state.hpAvoidanceSegments) or 0) > 0 + or state.hpAvoidanceSide ~= nil + or state.hpAvoidanceExhausted == true + local requiredMs = math.max(500, tonumber(self.fallbackClearReleaseMs) or 1500) + if chainActive or state.hpSteerFallbackClearMs < requiredMs then + return false + end + + log("STEER RESTORE %s clear long-range corridor sustained for %.2fs", + tostring(getSlot(state.index)), state.hpSteerFallbackClearMs * 0.001) + state.hpSteerFallbackSuppressed = nil + state.hpSteerFallbackClearMs = nil + state.hpSteerFallbackReason = nil + return true +end + +function Steer:chooseOffset(state, x, y, z, directYaw) + local currentSide = state.hpSteerSide + local best = nil + local summary = {} + + for _, offsetDeg in ipairs(self.candidateOffsetsDeg or {}) do + local side = offsetDeg >= 0 and "RIGHT" or "LEFT" + local candidateYaw = normalizeAngle(directYaw + math.rad(offsetDeg)) + local blocked, result = scanLong( + state.index, state.id, x, y, z, candidateYaw, self.lookAhead) + + if blocked then + summary[#summary + 1] = string.format("%s%d=X:%s", + side:sub(1, 1), math.abs(offsetDeg), + tostring(result ~= nil and result.nodeName or "?")) + else + -- Smoothness is the main objective: prefer the smallest heading + -- correction that exposes a full long-range walking corridor. Once + -- a side has been chosen, keep a modest preference for that side so + -- the steering field does not flick left/right between frames. + local score = math.abs(offsetDeg) + if currentSide ~= nil and side == currentSide then score = score - 18 end + if best == nil or score < best.score then + best = { + score = score, + side = side, + offsetDeg = offsetDeg, + yaw = candidateYaw + } + end + summary[#summary + 1] = string.format("%s%d=O", side:sub(1, 1), math.abs(offsetDeg)) + end + end + + return best, table.concat(summary, " ") +end + +function Steer:apply(state, motion, dt) + if state == nil or motion == nil then return end + + -- If the emergency/local planner has taken over, make that ownership + -- sticky for the complete escape episode. Previously the anticipatory + -- controller re-entered immediately after every short bypass waypoint, + -- creating STOP -> AVOID -> STEER -> STOP loops in tight obstacle pockets. + if state.obstacleBlocked == true + or state.hpAvoidance ~= nil + or motion.hpAvoidanceWaypoint == true + or motion.navigationKind ~= "follow" then + if state.obstacleBlocked == true or state.hpAvoidance ~= nil or motion.hpAvoidanceWaypoint == true then + self:suppressFallback(state, state.obstacleBlocked == true and "obstacle-blocked" or "local-avoidance") + else + clearState(state, true) + end + return + end + + if state.hpSteerFallbackSuppressed == true then + clearState(state, true) + if not self:tryReleaseFallback(state, motion, dt) then return end + end + + local x = tonumber(motion.x) + local y = tonumber(motion.y) + local z = tonumber(motion.z) + local directTargetX = tonumber(motion.targetX) + local directTargetZ = tonumber(motion.targetZ) + if x == nil or y == nil or z == nil or directTargetX == nil or directTargetZ == nil then + clearState(state, true) + return + end + + local dx = directTargetX - x + local dz = directTargetZ - z + local distance = math.sqrt(dx * dx + dz * dz) + if distance <= 0.35 then + clearState(state, false) + return + end + + local dtMs = math.max(0, tonumber(dt) or 0) + local dtSeconds = dtMs * 0.001 + local directYaw = yawFromDirection(dx, dz) + local directBlocked, directResult = scanLong( + state.index, state.id, x, y, z, directYaw, self.lookAhead) + + local currentOffset = tonumber(state.hpSteerOffsetRad) or 0 + local targetOffset = 0 + + if directBlocked then + state.hpSteerClearMs = 0 + local candidate, summary = self:chooseOffset(state, x, y, z, directYaw) + if candidate == nil then + -- No long-range alternative is safe. Yield once, then stay yielded + -- until the fallback system has genuinely escaped the obstacle + -- field. Re-entering on each short waypoint was the loop source. + if state.hpAnticipatorySteering == true then + log("STEER FALLBACK %s no clear anticipatory corridor (%s)", + tostring(getSlot(state.index)), tostring(summary)) + end + self:suppressFallback(state, "no-clear-long-range-corridor") + return + end + + targetOffset = math.rad(candidate.offsetDeg) + state.hpSteerTargetOffsetRad = targetOffset + state.hpSteerSide = candidate.side + state.hpSteerObstacleName = directResult ~= nil and directResult.nodeName or "unknown" + + if state.hpAnticipatorySteering ~= true then + state.hpAnticipatorySteering = true + state.hpSteerLogMs = 0 + log("STEER START %s obstacle=%s side=%s offset=%d lookAhead=%.2f probes=[%s]", + tostring(getSlot(state.index)), tostring(state.hpSteerObstacleName), + tostring(candidate.side), candidate.offsetDeg, + tonumber(self.lookAhead) or 3.8, tostring(summary)) + end + elseif state.hpAnticipatorySteering == true then + state.hpSteerClearMs = (tonumber(state.hpSteerClearMs) or 0) + dtMs + if state.hpSteerClearMs < (tonumber(self.directClearHoldMs) or 300) then + targetOffset = tonumber(state.hpSteerTargetOffsetRad) or currentOffset + else + targetOffset = 0 + end + else + return + end + + local rate + if math.abs(targetOffset) > math.abs(currentOffset) then + rate = math.max(0.1, tonumber(self.engageRateRadPerSec) or 0.72) + else + rate = math.max(0.1, tonumber(self.recoverRateRadPerSec) or 0.52) + end + currentOffset = moveTowards(currentOffset, targetOffset, rate * dtSeconds) + state.hpSteerOffsetRad = currentOffset + + -- The target itself is virtual and moves every frame. The existing curved + -- locomotion controller therefore sees a continuously changing desired + -- heading while retaining sole ownership of model yaw, speed and animation. + local steerYaw = normalizeAngle(directYaw + currentOffset) + local steerX, steerZ = directionFromYaw(steerYaw) + local virtualDistance = math.max(2.5, tonumber(self.virtualTargetDistance) or 4.5) + motion.targetX = x + steerX * virtualDistance + motion.targetZ = z + steerZ * virtualDistance + motion.hpAnticipatorySteer = true + + state.hpSteerLogMs = (tonumber(state.hpSteerLogMs) or 0) - dtMs + if state.hpSteerLogMs <= 0 then + state.hpSteerLogMs = tonumber(self.logIntervalMs) or 700 + log("STEER %s side=%s offset=%.1fdeg target=%.1fdeg direct=%s obstacle=%s yaw=%.3f", + tostring(getSlot(state.index)), tostring(state.hpSteerSide or "-"), + math.deg(currentOffset), math.deg(targetOffset), + directBlocked and "BLOCKED" or "CLEAR", + tostring(state.hpSteerObstacleName or "-"), steerYaw) + end + + if not directBlocked + and (tonumber(state.hpSteerClearMs) or 0) >= (tonumber(self.directClearHoldMs) or 300) + and math.abs(currentOffset) <= math.rad(1.0) then + clearState(state, false) + motion.hpAnticipatorySteer = nil + end +end + +function Steer:install() + if self.installed then return true end + + -- Follow.updateFollower executes before locomotion each frame. Call the + -- existing stack first so it establishes the current live player-relative + -- target, then bend that target only for this frame when anticipation sees + -- an obstruction farther ahead. + local originalUpdateFollower = Follow.updateFollower + function Follow:updateFollower(state, dt, ...) + local result = originalUpdateFollower(self, state, dt, ...) + if state ~= nil then + -- Record fallback ownership even on frames where the emergency + -- detector has already stopped locomotion and no motion remains. + if state.obstacleBlocked == true or state.hpAvoidance ~= nil then + Steer:suppressFallback(state, state.obstacleBlocked == true and "obstacle-blocked" or "local-avoidance") + end + + local motion = Loco.motions[state.id] + if motion ~= nil then + Steer:apply(state, motion, dt) + elseif state.hpAnticipatorySteering == true then + clearState(state, true) + end + end + return result + end + + self.installed = true + log("Loaded %s (%.1fm anticipatory sensor; sticky fallback ownership; hard stop safety retained)", + tostring(self.version), tonumber(self.lookAhead) or 3.8) + return true +end + +Steer:install() \ No newline at end of file diff --git a/scripts/HP_WorldAvoidancePhases.lua b/scripts/HP_WorldAvoidancePhases.lua new file mode 100644 index 0000000..eab21df --- /dev/null +++ b/scripts/HP_WorldAvoidancePhases.lua @@ -0,0 +1,163 @@ +-- HP_WorldAvoidancePhases.lua (FS25_HelperProfiles) +-- Alpha 4 visual-execution refinement for local obstacle avoidance. +-- +-- The local planner already chooses safe short bypass waypoints. This layer +-- changes only how those waypoints are executed so the worker reads more like +-- a person making deliberate adjustments: +-- stop/hold -> turn in place -> walk -> settle -> reassess. +-- +-- It reuses the validated obstacle turn-escape implementation for stationary +-- yaw changes and the validated locomotion controller for all forward motion. + +if HP_WorldLocalAvoidance == nil then return end +if HP_WorldObstacleTurnEscape == nil then return end +if HP_WorldLocomotionPrototype == nil then return end +if HP_WorldFollow == nil then return end +if HP_WorldAvoidancePhases ~= nil then return end + +HP_WorldAvoidancePhases = { + version = "2.2.0.0-alpha4-avoidance-phases-1", + waypointSettleMs = 220, + installed = false +} + +local Phases = HP_WorldAvoidancePhases +local Avoid = HP_WorldLocalAvoidance +local Loco = HP_WorldLocomotionPrototype +local Follow = HP_WorldFollow +local LOG = "[FS25_HelperProfiles/WorldAvoidance] " +local TWO_PI = math.pi * 2 + +local function log(message, ...) + print(LOG .. string.format(tostring(message), ...)) +end + +local function normalizeAngle(value) + value = tonumber(value) or 0 + while value > math.pi do value = value - TWO_PI end + while value < -math.pi do value = value + TWO_PI end + return value +end + +local function yawFromDirection(dx, dz) + if MathUtil ~= nil and MathUtil.getYRotationFromDirection ~= nil then + local ok, yaw = pcall(MathUtil.getYRotationFromDirection, dx, dz) + if ok and tonumber(yaw) ~= nil then return normalizeAngle(tonumber(yaw)) end + end + return normalizeAngle(math.atan2(dx, dz)) +end + +local function angleDifference(target, current) + return normalizeAngle((tonumber(target) or 0) - (tonumber(current) or 0)) +end + +local function getSlot(index) + if HP_SlotRegistry ~= nil then return HP_SlotRegistry:indexToSlot(index) end + return tostring(index) +end + +local function setPhase(state, active, phase, detail) + if active == nil or active.hpPhase == phase then return end + active.hpPhase = phase + log("AVOID PHASE %s segment=%d -> %s%s", + tostring(getSlot(state.index)), tonumber(active.segment) or 0, + tostring(phase), detail ~= nil and (" (" .. tostring(detail) .. ")") or "") +end + +function Phases:install() + if self.installed then return true end + + -- A newly planned bypass must align before it advances. Setting the proven + -- turn-escape flag gives that module ownership of yaw with speed held at 0. + -- Once alignment is within its existing tolerance it clears the flag and + -- the normal curved walker takes over without a second steering system. + local originalStartBypass = Avoid.startBypass + function Avoid:startBypass(state, workerX, workerY, workerZ, playerX, playerZ, directYaw, ...) + local ok = originalStartBypass(self, state, workerX, workerY, workerZ, playerX, playerZ, directYaw, ...) + if ok ~= true or state == nil or state.hpAvoidance == nil then return ok end + + local motion = Loco.motions[state.id] + if motion ~= nil then + local targetX = tonumber(motion.targetX) or tonumber(state.hpAvoidance.x) or tonumber(motion.x) or 0 + local targetZ = tonumber(motion.targetZ) or tonumber(state.hpAvoidance.z) or tonumber(motion.z) or 0 + local dx = targetX - (tonumber(motion.x) or 0) + local dz = targetZ - (tonumber(motion.z) or 0) + local targetYaw = yawFromDirection(dx, dz) + local currentYaw = tonumber(motion.yaw) or targetYaw + local delta = angleDifference(targetYaw, currentYaw) + + motion.speed = 0 + motion.hpObstacleTurnEscape = true + motion.hpAvoidanceForcedTurn = true + setPhase(state, state.hpAvoidance, "TURN", + string.format("yaw=%.3f targetYaw=%.3f delta=%.3f", currentYaw, targetYaw, delta)) + log("AVOID TURN START %s side=%s segment=%d yaw=%.3f targetYaw=%.3f delta=%.3f", + tostring(getSlot(state.index)), tostring(state.hpAvoidance.side), + tonumber(state.hpAvoidance.segment) or 0, currentYaw, targetYaw, delta) + end + return ok + end + + local originalUpdateActive = Avoid.updateActive + function Avoid:updateActive(state, dt, ...) + local active = state ~= nil and state.hpAvoidance or nil + if active == nil then return originalUpdateActive(self, state, dt, ...) end + + dt = math.max(0, tonumber(dt) or 0) + local motion = Loco.motions[state.id] + + if motion ~= nil then + if motion.hpAvoidanceForcedTurn == true then + if motion.hpObstacleTurnEscape == true then + setPhase(state, active, "TURN") + else + motion.hpAvoidanceForcedTurn = nil + setPhase(state, active, "MOVE") + log("AVOID MOVE START %s side=%s segment=%d alignedYaw=%.3f waypoint=(%.2f,%.2f)", + tostring(getSlot(state.index)), tostring(active.side), tonumber(active.segment) or 0, + tonumber(motion.yaw) or 0, tonumber(active.x) or 0, tonumber(active.z) or 0) + end + elseif active.hpPhase ~= "MOVE" then + setPhase(state, active, "MOVE") + end + return originalUpdateActive(self, state, dt, ...) + end + + -- Safety stops during a bypass remain authoritative. The original + -- avoidance code clears/replans those immediately into its normal + -- blocked hold, which already gives us the planning delay before the + -- next attempt. + if state.obstacleBlocked == true then + return originalUpdateActive(self, state, dt, ...) + end + + -- A cleanly reached waypoint gets a short idle settle before FOLLOW is + -- allowed to reacquire the player or plan another side-step. This makes + -- chained segments read as distinct human decisions rather than one + -- continuous zig-zagging curve. + if active.hpPhase ~= "SETTLE" then + active.hpSettleRemainingMs = math.max(0, tonumber(self.waypointSettleMs) or 220) + setPhase(state, active, "SETTLE", + string.format("%.0fms", active.hpSettleRemainingMs)) + end + + active.hpSettleRemainingMs = math.max(0, + (tonumber(active.hpSettleRemainingMs) or 0) - dt) + if active.hpSettleRemainingMs > 0 then + if Follow.setMode ~= nil then + Follow:setMode(state, "avoiding", string.format("settling segment=%d", tonumber(active.segment) or 0)) + end + return true + end + + log("AVOID SETTLE COMPLETE %s segment=%d; reassessing route", + tostring(getSlot(state.index)), tonumber(active.segment) or 0) + return originalUpdateActive(self, state, dt, ...) + end + + self.installed = true + log("Loaded %s (phased stop-turn-move-settle avoidance execution)", tostring(self.version)) + return true +end + +Phases:install() diff --git a/scripts/HP_WorldLocalAvoidance.lua b/scripts/HP_WorldLocalAvoidance.lua new file mode 100644 index 0000000..1f84fb3 --- /dev/null +++ b/scripts/HP_WorldLocalAvoidance.lua @@ -0,0 +1,424 @@ +-- HP_WorldLocalAvoidance.lua (FS25_HelperProfiles) +-- Alpha 4 local obstacle-avoidance experiment. +-- +-- Builds on the validated obstacle-awareness/turn-escape stack. When FOLLOW is +-- persistently blocked, probe short candidate corridors to either side of the +-- direct player line, choose a safe local bypass waypoint, walk to it, then +-- reacquire the player. This is deliberately local steering, not navmesh or +-- global pathfinding. + +if HP_WorldObstacleAwareness == nil then return end +if HP_WorldFollow == nil then return end +if HP_WorldTargetNavigation == nil then return end +if HP_WorldLocomotionPrototype == nil then return end +if HP_WorldWorkerManager == nil then return end +if HP_WorldLocalAvoidance ~= nil then return end + +HP_WorldLocalAvoidance = { + version = "2.2.0.0-alpha4-local-avoidance-1", + planDelayMs = 450, + retryDelayMs = 300, + clearResetMs = 2000, + waypointDistance = 1.65, + maximumChainSegments = 10, + statusLogIntervalMs = 1200, + candidateOffsetsDeg = {45, -45, 70, -70, 95, -95}, + installed = false +} + +local Avoid = HP_WorldLocalAvoidance +local Awareness = HP_WorldObstacleAwareness +local Follow = HP_WorldFollow +local Nav = HP_WorldTargetNavigation +local Loco = HP_WorldLocomotionPrototype +local Manager = HP_WorldWorkerManager +local LOG = "[FS25_HelperProfiles/WorldAvoidance] " +local TWO_PI = math.pi * 2 + +local function log(message, ...) + print(LOG .. string.format(tostring(message), ...)) +end + +local function normalizeAngle(value) + value = tonumber(value) or 0 + while value > math.pi do value = value - TWO_PI end + while value < -math.pi do value = value + TWO_PI end + return value +end + +local function yawFromDirection(dx, dz) + if MathUtil ~= nil and MathUtil.getYRotationFromDirection ~= nil then + local ok, yaw = pcall(MathUtil.getYRotationFromDirection, dx, dz) + if ok and tonumber(yaw) ~= nil then return normalizeAngle(tonumber(yaw)) end + end + return normalizeAngle(math.atan2(dx, dz)) +end + +local function directionFromYaw(yaw) + if MathUtil ~= nil and MathUtil.getDirectionFromYRotation ~= nil then + local ok, dx, dz = pcall(MathUtil.getDirectionFromYRotation, yaw) + if ok and tonumber(dx) ~= nil and tonumber(dz) ~= nil then + return tonumber(dx), tonumber(dz) + end + end + return math.sin(tonumber(yaw) or 0), math.cos(tonumber(yaw) or 0) +end + +local function getSlot(index) + if HP_SlotRegistry ~= nil then return HP_SlotRegistry:indexToSlot(index) end + return tostring(index) +end + +local function getPlacement(index) + if HP_WorldState == nil then return nil end + return HP_WorldState:getPlacement(index) +end + +local function findLocalPlayer() + if rawget(_G, "g_localPlayer") ~= nil and g_localPlayer ~= nil then return g_localPlayer end + local mission = g_currentMission + if mission == nil then return nil end + if mission.player ~= nil then return mission.player end + if mission.controlledPlayer ~= nil then return mission.controlledPlayer end + local playerSystem = mission.playerSystem + if playerSystem ~= nil then + for _, player in pairs(playerSystem.players or {}) do + if player ~= nil and (player.isOwner == true or player.isLocallyControlled == true) then return player end + end + for _, player in pairs(playerSystem.players or {}) do + if player ~= nil then return player end + end + end + return nil +end + +local function getPlayerXZ() + local player = findLocalPlayer() + if player == nil then return nil, nil end + if player.getPosition ~= nil then + local ok, x, _, z = pcall(player.getPosition, player) + if ok and tonumber(x) ~= nil and tonumber(z) ~= nil then return tonumber(x), tonumber(z) end + end + if player.getMapPositionAndLookYaw ~= nil then + local ok, x, z = pcall(player.getMapPositionAndLookYaw, player) + if ok and tonumber(x) ~= nil and tonumber(z) ~= nil then return tonumber(x), tonumber(z) end + end + if player.rootNode ~= nil and player.rootNode ~= 0 and getWorldTranslation ~= nil then + local ok, x, _, z = pcall(getWorldTranslation, player.rootNode) + if ok and tonumber(x) ~= nil and tonumber(z) ~= nil then return tonumber(x), tonumber(z) end + end + return nil, nil +end + +local function terrainHeight(x, fallbackY, z) + if g_currentMission ~= nil and g_currentMission.terrainRootNode ~= nil and getTerrainHeightAtWorldPos ~= nil then + local ok, y = pcall(getTerrainHeightAtWorldPos, g_currentMission.terrainRootNode, x, 0, z) + if ok and tonumber(y) ~= nil then return tonumber(y) end + end + return tonumber(fallbackY) or 0 +end + +local function walkingSpeed() + return math.max(0.1, tonumber(Loco.walkSpeed) or 1.35) +end + +local function clearObstacleState(state) + state.obstacleBlocked = nil + state.obstacleNodeId = nil + state.obstacleNodeName = nil + state.obstacleClearMs = nil + state.obstacleLogMs = nil +end + +function Avoid:isDirectRouteBlocked(state, workerX, workerY, workerZ, playerX, playerZ) + local dx = playerX - workerX + local dz = playerZ - workerZ + local distance = math.sqrt(dx * dx + dz * dz) + if distance <= 0.001 then return false, nil, nil end + local yaw = yawFromDirection(dx, dz) + local blocked, result = Awareness:scan(state.index, state.id, + workerX, workerY, workerZ, yaw, walkingSpeed()) + return blocked == true, result, yaw +end + +function Avoid:chooseCandidate(state, workerX, workerY, workerZ, playerX, playerZ, directYaw) + local stepDistance = math.max(0.75, tonumber(self.waypointDistance) or 1.65) + local best = nil + local probeSummary = {} + + for _, offsetDeg in ipairs(self.candidateOffsetsDeg or {}) do + local offsetRad = math.rad(tonumber(offsetDeg) or 0) + local candidateYaw = normalizeAngle(directYaw + offsetRad) + local blocked, result = Awareness:scan(state.index, state.id, + workerX, workerY, workerZ, candidateYaw, walkingSpeed()) + local side = offsetDeg >= 0 and "RIGHT" or "LEFT" + + if blocked == true then + probeSummary[#probeSummary + 1] = string.format("%s%d=BLOCKED:%s", + side:sub(1, 1), math.abs(offsetDeg), + tostring(result ~= nil and result.nodeName or "?")) + else + local dirX, dirZ = directionFromYaw(candidateYaw) + local targetX = workerX + dirX * stepDistance + local targetZ = workerZ + dirZ * stepDistance + local targetY = terrainHeight(targetX, workerY, targetZ) + + local nextDx = playerX - targetX + local nextDz = playerZ - targetZ + local nextDistance = math.sqrt(nextDx * nextDx + nextDz * nextDz) + local futureBlocked = false + local futureResult = nil + if nextDistance > 0.001 then + local futureYaw = yawFromDirection(nextDx, nextDz) + futureBlocked, futureResult = Awareness:scan(state.index, state.id, + targetX, targetY, targetZ, futureYaw, walkingSpeed()) + futureBlocked = futureBlocked == true + end + + -- Prefer a candidate whose next leg toward the player is already + -- clear. If every candidate still sees the obstacle from its end + -- point, prefer stronger lateral progress and remain on the same + -- side as prior bypass segments to avoid left/right oscillation. + local absOffset = math.abs(tonumber(offsetDeg) or 0) + local score + if futureBlocked then + score = 1000 - absOffset * 2 + else + score = absOffset + nextDistance * 0.01 + end + if state.hpAvoidanceSide == side then score = score - 12 end + + probeSummary[#probeSummary + 1] = string.format("%s%d=CLEAR%s", + side:sub(1, 1), absOffset, futureBlocked and "" or "+EXIT") + + if best == nil or score < best.score then + best = { + score = score, + side = side, + offsetDeg = offsetDeg, + yaw = candidateYaw, + x = targetX, + y = targetY, + z = targetZ, + futureBlocked = futureBlocked, + futureObstacle = futureResult ~= nil and futureResult.nodeName or nil + } + end + end + end + + log("AVOID PROBE %s %s", tostring(getSlot(state.index)), table.concat(probeSummary, " ")) + return best +end + +function Avoid:startBypass(state, workerX, workerY, workerZ, playerX, playerZ, directYaw) + local segments = math.max(0, tonumber(state.hpAvoidanceSegments) or 0) + if segments >= math.max(1, tonumber(self.maximumChainSegments) or 10) then + if state.hpAvoidanceExhausted ~= true then + state.hpAvoidanceExhausted = true + log("AVOID HOLD %s maximum local segments reached (%d); waiting for direct route", + tostring(getSlot(state.index)), segments) + end + return false + end + + local candidate = self:chooseCandidate(state, workerX, workerY, workerZ, playerX, playerZ, directYaw) + if candidate == nil then + state.hpAvoidanceRetryMs = tonumber(self.retryDelayMs) or 300 + log("AVOID HOLD %s no clear local candidate; retaining blocked state", tostring(getSlot(state.index))) + return false + end + + local obstacleName = state.obstacleNodeName or "unknown" + local ok, err = Nav:startPoint(state.index, candidate.x, candidate.z, "follow") + if not ok then + state.hpAvoidanceRetryMs = tonumber(self.retryDelayMs) or 300 + log("AVOID WAIT %s unable to start %s bypass: %s", + tostring(getSlot(state.index)), candidate.side, tostring(err)) + return false + end + + local motion = Loco.motions[state.id] + if motion == nil then + state.hpAvoidanceRetryMs = tonumber(self.retryDelayMs) or 300 + log("AVOID WAIT %s bypass motion missing after navigation start", tostring(getSlot(state.index))) + return false + end + + segments = segments + 1 + state.hpAvoidanceSegments = segments + state.hpAvoidanceSide = candidate.side + state.hpAvoidanceWaitMs = 0 + state.hpAvoidanceRetryMs = 0 + state.hpAvoidanceExhausted = nil + state.hpAvoidance = { + x = candidate.x, + z = candidate.z, + side = candidate.side, + offsetDeg = candidate.offsetDeg, + segment = segments, + obstacleName = obstacleName, + futureBlocked = candidate.futureBlocked, + logMs = 0 + } + + motion.navigationKind = "follow" + motion.hpAvoidanceWaypoint = true + motion.hpAvoidanceSide = candidate.side + motion.targetX = candidate.x + motion.targetZ = candidate.z + + clearObstacleState(state) + if Follow.setMode ~= nil then + Follow:setMode(state, "avoiding", string.format("%s %ddeg segment=%d", + candidate.side, math.abs(candidate.offsetDeg), segments)) + end + log("AVOID START %s side=%s offset=%d segment=%d obstacle=%s from=(%.2f,%.2f) waypoint=(%.2f,%.2f) nextLeg=%s", + tostring(getSlot(state.index)), candidate.side, candidate.offsetDeg, segments, + tostring(obstacleName), workerX, workerZ, candidate.x, candidate.z, + candidate.futureBlocked and "blocked" or "clear") + return true +end + +function Avoid:updateActive(state, dt) + local active = state.hpAvoidance + if active == nil then return false end + + local motion = Loco.motions[state.id] + if motion ~= nil then + -- FOLLOW must not retarget this short waypoint to the moving player. + -- The validated locomotion/obstacle/turn stack remains sole owner of + -- movement and may still stop this segment if its corridor becomes blocked. + motion.navigationKind = "follow" + motion.hpAvoidanceWaypoint = true + motion.targetX = active.x + motion.targetZ = active.z + if Follow.setMode ~= nil then Follow:setMode(state, "avoiding") end + + active.logMs = (tonumber(active.logMs) or 0) - math.max(0, tonumber(dt) or 0) + if active.logMs <= 0 then + active.logMs = tonumber(self.statusLogIntervalMs) or 1200 + log("AVOID MOVE %s side=%s segment=%d pos=(%.2f,%.2f) waypoint=(%.2f,%.2f)", + tostring(getSlot(state.index)), tostring(active.side), tonumber(active.segment) or 0, + tonumber(motion.x) or 0, tonumber(motion.z) or 0, active.x, active.z) + end + return true + end + + state.hpAvoidance = nil + if state.obstacleBlocked == true then + state.hpAvoidanceRetryMs = tonumber(self.retryDelayMs) or 300 + log("AVOID SEGMENT BLOCKED %s side=%s segment=%d obstacle=%s; replanning", + tostring(getSlot(state.index)), tostring(active.side), tonumber(active.segment) or 0, + tostring(state.obstacleNodeName or "unknown")) + return false + end + + log("AVOID WAYPOINT REACHED %s side=%s segment=%d waypoint=(%.2f,%.2f); reacquiring player", + tostring(getSlot(state.index)), tostring(active.side), tonumber(active.segment) or 0, + active.x, active.z) + state.hpAvoidanceWaitMs = 0 + state.hpAvoidanceRetryMs = 0 + return false +end + +function Avoid:handleFollower(state, dt, originalUpdateFollower, followSelf, ...) + if state == nil then return originalUpdateFollower(followSelf, state, dt, ...) end + dt = math.max(0, tonumber(dt) or 0) + + if state.hpAvoidance ~= nil then + if self:updateActive(state, dt) then return end + -- If a waypoint completed cleanly, fall through and let normal FOLLOW + -- immediately reacquire the live player. If the segment was blocked, + -- the obstacle-aware wrapper below will keep the worker safely held. + end + + if state.obstacleBlocked == true then + state.hpAvoidanceClearMs = 0 + state.hpAvoidanceRetryMs = math.max(0, (tonumber(state.hpAvoidanceRetryMs) or 0) - dt) + + local placement = getPlacement(state.index) + local playerX, playerZ = getPlayerXZ() + if placement ~= nil and playerX ~= nil and playerZ ~= nil then + local workerX = tonumber(placement.x) or 0 + local workerY = tonumber(placement.y) or 0 + local workerZ = tonumber(placement.z) or 0 + local directBlocked, _, directYaw = self:isDirectRouteBlocked( + state, workerX, workerY, workerZ, playerX, playerZ) + + if directBlocked then + state.hpAvoidanceWaitMs = (tonumber(state.hpAvoidanceWaitMs) or 0) + dt + if state.hpAvoidanceWaitMs >= (tonumber(self.planDelayMs) or 450) + and state.hpAvoidanceRetryMs <= 0 then + if self:startBypass(state, workerX, workerY, workerZ, playerX, playerZ, directYaw) then + return + end + end + else + -- Preserve the already validated direct-clear behaviour. The + -- obstacle-awareness layer confirms its clear-hold interval and + -- turn-escape rotates the stationary worker before walking. + state.hpAvoidanceWaitMs = 0 + state.hpAvoidanceRetryMs = 0 + end + end + + return originalUpdateFollower(followSelf, state, dt, ...) + end + + state.hpAvoidanceWaitMs = 0 + state.hpAvoidanceRetryMs = 0 + + -- A sustained period of normal direct following closes the current local + -- avoidance chain. Short re-blocks retain the previous side preference so + -- a worker skirting a long obstacle does not oscillate left/right. + local motion = Loco.motions[state.id] + if motion ~= nil and motion.navigationKind == "follow" and motion.hpAvoidanceWaypoint ~= true then + state.hpAvoidanceClearMs = (tonumber(state.hpAvoidanceClearMs) or 0) + dt + if state.hpAvoidanceClearMs >= (tonumber(self.clearResetMs) or 2000) then + state.hpAvoidanceSegments = 0 + state.hpAvoidanceSide = nil + state.hpAvoidanceExhausted = nil + end + else + state.hpAvoidanceClearMs = 0 + end + + return originalUpdateFollower(followSelf, state, dt, ...) +end + +function Avoid:install() + if self.installed then return true end + + -- Load after obstacle awareness and turn-escape. This wrapper sits outside + -- the obstacle-aware FOLLOW wrapper: it gets first chance to create a safe + -- local bypass, otherwise delegates unchanged to the proven blocked/clear + -- state machine. + local originalUpdateFollower = Follow.updateFollower + function Follow:updateFollower(state, dt, ...) + return Avoid:handleFollower(state, dt, originalUpdateFollower, self, ...) + end + + local originalConsole = Manager.consoleCommandWorld + function Manager:consoleCommandWorld(...) + local args = {...} + local clean = {} + for _, value in ipairs(args) do + if value ~= nil and tostring(value) ~= "" and tostring(value) ~= "hpWorld" then clean[#clean + 1] = tostring(value) end + end + local sub = string.lower(tostring(clean[1] or "status")) + if sub == "help" then + originalConsole(self, ...) + print("[HP] Alpha4 local avoidance: FOLLOW automatically samples short left/right bypass corridors when the direct walking route stays blocked.") + print("[HP] Local avoidance is experimental only: no navmesh/global pathfinding and maximum 10 chained bypass segments.") + return + end + return originalConsole(self, ...) + end + + self.installed = true + log("Loaded %s (local left/right bypass waypoints; no navmesh/global pathfinding)", tostring(self.version)) + return true +end + +Avoid:install()