Add live-tunable GPS solver highway weight

2026-06-26 23:33:20 -05:00
parent 3fa723c3cc
commit 92287bd8e1
12 changed files with 363 additions and 43 deletions
@@ -102,7 +102,7 @@ constexpr bool kEnableVerboseMappinRouteState = false;
 constexpr bool kEnableGpsMultiplierHooks = false;
 constexpr bool kEnableGpsAuxMultiplierHook = false;
 constexpr bool kEnableGpsMultiplierSignatureTrace = false;
-constexpr bool kEnableGpsNodeMultiplierInlinePatch = false;
+constexpr bool kEnableGpsNodeMultiplierInlinePatch = true;
 constexpr bool kEnableGpsCostTablePatch = false;
 constexpr bool kEnableGpsProviderClassPatch = false;
 constexpr bool kEnableGpsProviderClassPatchTrace = false;
@@ -213,7 +213,7 @@ constexpr uint16_t kTrafficLaneFlagNoAiDriving = 0x2000;
 constexpr uint16_t kTrafficLaneFlagHighway = 0x4000;
 constexpr uint16_t kTrafficLaneFlagNoAutodrive = 0x8000;
 constexpr uintptr_t kMinimumReadableAddress = 0x10000;
-constexpr float kGpsSolverHighwayMultiplier = 0.35f;
+constexpr float kGpsSolverHighwayMultiplierDefault = 0.80f;
 constexpr float kGpsSolverGpsOnlyMultiplier = 1.10f;
 constexpr float kGpsSolverPavementMultiplier = 1.25f;
 constexpr size_t kGpsNodeMultiplierRoadTailPatchSize = 14;
@@ -226,6 +226,7 @@ constexpr float kGpsSpatialRoadMultiplier = 1.0f;
 constexpr float kGpsSpatialGpsOnlyMultiplier = 1.20f;
 constexpr float kGpsSpatialPavementMultiplier = 1.35f;
 constexpr float kGpsSpatialUnknownMultiplier = 1.05f;
+constexpr uint64_t kGpsSolverWeightsReloadIntervalMs = 500;
 constexpr uint64_t kGpsSpatialWeightsReloadIntervalMs = 500;

 struct GpsProviderClassOverride
@@ -327,10 +328,12 @@ uint32_t gGpsResolveTraceWindowSerial = 0;
 std::array<uint32_t, 64> gGpsEdgeCostClassLogCounts{};
 std::array<uint8_t, kGpsNodeMultiplierRoadTailPatchSize> gGpsNodeMultiplierRoadTailOriginal{};
 void* gGpsNodeMultiplierRoadTailCave = nullptr;
+uint8_t* gGpsNodeMultiplierRoadTailHighwayConstant = nullptr;
 bool gGpsNodeMultiplierRoadTailPatchApplied = false;
 std::mutex gGpsCostLogMutex;
 std::mutex gGpsAsyncLogMutex;
 std::mutex gGpsCostTablePatchMutex;
+std::mutex gGpsSolverWeightsMutex;
 uint64_t gLogStartTick = 0;
 uint64_t gLogFrequency = 0;
 void* gMappinSetTrackedTarget = nullptr;
@@ -569,9 +572,13 @@ std::array<std::atomic<float>, 5> gGpsSpatialMultipliers = {
    kGpsSpatialPavementMultiplier,
    kGpsSpatialUnknownMultiplier,
 };
+std::atomic<float> gGpsSolverHighwayMultiplier{kGpsSolverHighwayMultiplierDefault};
 std::mutex gGpsSpatialWeightsMutex;
+std::atomic<uint64_t> gGpsSolverWeightsNextReloadCheckMs = 0;
 std::atomic<uint64_t> gGpsSpatialWeightsNextReloadCheckMs = 0;
+std::filesystem::file_time_type gGpsSolverWeightsLastWriteTime{};
 std::filesystem::file_time_type gGpsSpatialWeightsLastWriteTime{};
+bool gGpsSolverWeightsFileKnown = false;
 bool gGpsSpatialWeightsFileKnown = false;

 uintptr_t GetImageBase();
@@ -598,6 +605,11 @@ std::filesystem::path GetSpatialWeightsPath()
    return GetPluginDirectory() / L"spatial_weights.bin";
 }

+std::filesystem::path GetSolverWeightsPath()
+{
+    return GetPluginDirectory() / L"solver_weights.bin";
+}
+
 uint64_t GetElapsedMs()
 {
    if (!gLogStartTick || !gLogFrequency)
@@ -652,6 +664,103 @@ void WriteFloat(uint8_t* aTarget, float aValue)
    std::memcpy(aTarget, &aValue, sizeof(aValue));
 }

+bool IsUsableSolverMultiplier(float aValue)
+{
+    return std::isfinite(aValue) && aValue > 0.0f && aValue < 20.0f;
+}
+
+void StoreGpsSolverHighwayMultiplier(float aValue)
+{
+    gGpsSolverHighwayMultiplier.store(aValue, std::memory_order_relaxed);
+
+    if (gGpsNodeMultiplierRoadTailHighwayConstant)
+    {
+        WriteFloat(gGpsNodeMultiplierRoadTailHighwayConstant, aValue);
+        FlushInstructionCache(GetCurrentProcess(), gGpsNodeMultiplierRoadTailHighwayConstant, sizeof(aValue));
+    }
+}
+
+void LogGpsSolverHighwayMultiplier(std::string_view aPrefix, float aValue)
+{
+    std::ostringstream line;
+    line << aPrefix << " highway=" << aValue;
+    LogRed4ext(line.str());
+}
+
+void MaybeReloadGpsSolverWeights(bool aForce = false)
+{
+    if (!kEnableGpsNodeMultiplierInlinePatch)
+    {
+        return;
+    }
+
+    const auto nowMs = GetElapsedMs();
+    if (!aForce && nowMs < gGpsSolverWeightsNextReloadCheckMs.load(std::memory_order_relaxed))
+    {
+        return;
+    }
+
+    std::lock_guard lock(gGpsSolverWeightsMutex);
+    if (!aForce && nowMs < gGpsSolverWeightsNextReloadCheckMs.load(std::memory_order_relaxed))
+    {
+        return;
+    }
+    gGpsSolverWeightsNextReloadCheckMs.store(nowMs + kGpsSolverWeightsReloadIntervalMs, std::memory_order_relaxed);
+
+    const auto path = GetSolverWeightsPath();
+    std::error_code error;
+    const auto writeTime = std::filesystem::last_write_time(path, error);
+    if (error)
+    {
+        if (aForce || gGpsSolverWeightsFileKnown)
+        {
+            gGpsSolverWeightsFileKnown = false;
+            std::ostringstream line;
+            line << "solver weights file unavailable path=" << path.string()
+                 << " using highway=" << gGpsSolverHighwayMultiplier.load(std::memory_order_relaxed);
+            LogRed4ext(line.str());
+        }
+        return;
+    }
+
+    if (!aForce && gGpsSolverWeightsFileKnown && writeTime == gGpsSolverWeightsLastWriteTime)
+    {
+        return;
+    }
+
+    float highwayMultiplier = 0.0f;
+    std::ifstream input(path, std::ios::binary);
+    input.read(reinterpret_cast<char*>(&highwayMultiplier), static_cast<std::streamsize>(sizeof(highwayMultiplier)));
+    if (input.gcount() != static_cast<std::streamsize>(sizeof(highwayMultiplier)))
+    {
+        std::ostringstream line;
+        line << "solver weights reload skipped path=" << path.string() << " bytes=" << input.gcount()
+             << " expected=" << sizeof(highwayMultiplier);
+        LogRed4ext(line.str());
+        gGpsSolverWeightsFileKnown = true;
+        gGpsSolverWeightsLastWriteTime = writeTime;
+        return;
+    }
+
+    if (!IsUsableSolverMultiplier(highwayMultiplier))
+    {
+        std::ostringstream line;
+        line << "solver weights reload skipped path=" << path.string() << " invalidHighway=" << highwayMultiplier;
+        LogRed4ext(line.str());
+        gGpsSolverWeightsFileKnown = true;
+        gGpsSolverWeightsLastWriteTime = writeTime;
+        return;
+    }
+
+    StoreGpsSolverHighwayMultiplier(highwayMultiplier);
+    gGpsSolverWeightsFileKnown = true;
+    gGpsSolverWeightsLastWriteTime = writeTime;
+
+    std::ostringstream line;
+    line << "solver weights reloaded path=" << path.string() << " highway=" << highwayMultiplier;
+    LogRed4ext(line.str());
+}
+
 bool InstallGpsNodeMultiplierInlinePatch()
 {
    if (!kEnableGpsNodeMultiplierInlinePatch || gGpsNodeMultiplierRoadTailPatchApplied)
@@ -659,7 +768,7 @@ bool InstallGpsNodeMultiplierInlinePatch()
        return true;
    }

-    constexpr size_t kCaveSize = 64;
+    constexpr size_t kCaveSize = 80;
    auto* cave = static_cast<uint8_t*>(
        VirtualAlloc(nullptr, kCaveSize, MEM_COMMIT | MEM_RESERVE, PAGE_EXECUTE_READWRITE));
    if (!cave)
@@ -668,8 +777,9 @@ bool InstallGpsNodeMultiplierInlinePatch()
        return false;
    }

-    // Replaces the road-tail at 0x40bb98. This preserves the vanilla node93
-    // multiplier, then applies the highway multiplier before returning.
+    // Replaces the road-tail at 0x40bb98. Reaching this block already means
+    // vanilla selected one of the road-style solver modes (0/2/4), so keep
+    // the change broad enough for manual GPS and autodrive-like callers.
    size_t offset = 0;
    const auto emit = [&](std::initializer_list<uint8_t> aBytes) {
        for (auto byte : aBytes)
@@ -691,7 +801,7 @@ bool InstallGpsNodeMultiplierInlinePatch()
    emit({0xF3, 0x0F, 0x59, 0x05, 0x00, 0x00, 0x00, 0x00}); // mulss xmm0, [rip+disp32]
    emit({0xC3});                                           // ret

-    while (offset < 40)
+    while ((offset % alignof(float)) != 0)
    {
        cave[offset++] = 0x90;
    }
@@ -700,7 +810,8 @@ bool InstallGpsNodeMultiplierInlinePatch()
    WriteFloat(cave + offset, 1.1f);
    offset += sizeof(float);
    const auto highwayConstantOffset = offset;
-    WriteFloat(cave + offset, kGpsSolverHighwayMultiplier);
+    const auto highwayMultiplier = gGpsSolverHighwayMultiplier.load(std::memory_order_relaxed);
+    WriteFloat(cave + offset, highwayMultiplier);
    offset += sizeof(float);

    const auto caveAddress = reinterpret_cast<uintptr_t>(cave);
@@ -743,12 +854,13 @@ bool InstallGpsNodeMultiplierInlinePatch()
    VirtualProtect(target, sizeof(patch), oldProtect, &ignoredProtect);

    gGpsNodeMultiplierRoadTailCave = cave;
+    gGpsNodeMultiplierRoadTailHighwayConstant = cave + highwayConstantOffset;
    gGpsNodeMultiplierRoadTailPatchApplied = true;

    std::ostringstream line;
    line << "gps-node-multiplier-inline-patch applied target_rva=0x" << std::hex
         << kGpsNodeMultiplierRoadTailRva << " cave=" << static_cast<void*>(cave)
-         << " highwayMultiplier=" << std::dec << kGpsSolverHighwayMultiplier;
+         << " highwayMultiplier=" << std::dec << highwayMultiplier << " modes=road-tail";
    LogRed4ext(line.str());
    return true;
 }
@@ -782,6 +894,7 @@ void RemoveGpsNodeMultiplierInlinePatch()
        gGpsNodeMultiplierRoadTailCave = nullptr;
    }

+    gGpsNodeMultiplierRoadTailHighwayConstant = nullptr;
    gGpsNodeMultiplierRoadTailPatchApplied = false;
 }

@@ -2338,16 +2451,21 @@ bool TryReadUint32FieldValue(void* aObject, uintptr_t aOffset, uint32_t& aValue)
                                    aValue);
 }

+bool IsGpsRoadStyleMode(uint32_t aMode)
+{
+    return aMode == 0 || aMode == 2 || aMode == 4;
+}
+
 bool IsDrivingRouteJob(void* aRouteSystem, void* aActiveEntry)
 {
    uint32_t mode = 0;
-    if (TryReadUint32FieldValue(aActiveEntry, 0x80, mode) && mode == 2)
+    if (TryReadUint32FieldValue(aActiveEntry, 0x80, mode) && IsGpsRoadStyleMode(mode))
    {
        return true;
    }

    auto* job = ReadPointerField(aRouteSystem, 0x90D0);
-    return TryReadUint32FieldValue(job, 0xC4, mode) && mode == 2;
+    return TryReadUint32FieldValue(job, 0xC4, mode) && IsGpsRoadStyleMode(mode);
 }

 void AppendFloatArray(std::ostringstream& aLine, const char* aLabel, const float* aValues, size_t aCount)
@@ -4339,7 +4457,7 @@ float GpsSolverNodeWeightMultiplier(uint16_t aFlags)
 {
    if ((aFlags & kTrafficLaneFlagHighway) != 0)
    {
-        return kGpsSolverHighwayMultiplier;
+        return gGpsSolverHighwayMultiplier.load(std::memory_order_relaxed);
    }
    if ((aFlags & kTrafficLaneFlagGpsOnly) != 0)
    {
@@ -5084,7 +5202,7 @@ float DetourGpsNodeMultiplier(void* aJob, void* aNode)
        result = ReadGpsSolverNonRoadMultiplier(retry);
    }

-    if (mode == 2 && std::isfinite(result))
+    if (IsGpsRoadStyleMode(mode) && std::isfinite(result))
    {
        result *= GpsSolverNodeWeightMultiplier(nodeFlags);
    }
@@ -5352,6 +5470,8 @@ bool OnRunningUpdate(void* aApp)
        ++gUpdateLogCount;
    }

+    MaybeReloadGpsSolverWeights();
+
    return false;
 }

@@ -5420,6 +5540,16 @@ RED4EXT_C_EXPORT bool RED4EXT_CALL Main(RED4ext::v1::PluginHandle aHandle, RED4e
            LogGpsSpatialMultipliers("spatial weights defaults", values);
        }

+        if (kEnableGpsNodeMultiplierInlinePatch)
+        {
+            std::ostringstream line;
+            line << "solver weights path=" << GetSolverWeightsPath().string();
+            LogRed4ext(line.str());
+            MaybeReloadGpsSolverWeights(true);
+            LogGpsSolverHighwayMultiplier("solver weights active",
+                                          gGpsSolverHighwayMultiplier.load(std::memory_order_relaxed));
+        }
+
        InstallGpsNodeMultiplierInlinePatch();

        if (kEnableGpsQueryLifecycleHooks)