Prototype spatial GPS edge weights

red4ext/EdgeWeightGPS/src/Main.cpp

@@ -11,6 +11,7 @@
 #include <algorithm>
 #include <array>
 #include <chrono>
+#include <cmath>
 #include <cstring>
 #include <cstdint>
 #include <filesystem>
@@ -22,6 +23,8 @@
 #include <string>
 #include <string_view>
 
+#include "GeneratedSpatialRoadGrid.hpp"
+
 namespace RED4ext::v1
 {
 struct Logger
@@ -103,10 +106,15 @@ constexpr bool kEnableGpsTraceHooks = true;
 constexpr bool kEnableGpsQueryLifecycleHooks = true;
 constexpr bool kEnableGpsAsyncTrafficProbe = true;
 constexpr bool kEnableGpsTrafficEdgeWeightPatch = false;
+constexpr bool kEnableGpsSpatialEdgeWeightPatch = false;
+constexpr bool kEnableGpsSpatialEdgeWeightPatchTrace = false;
 constexpr bool kEnableGpsRouteJobLifecycleHooks = false;
 constexpr bool kEnableGpsRouteJobStartTrace = false;
 constexpr bool kEnableGpsLocalSearchHooks = false;
 constexpr bool kEnableRouteUiTraceHooks = false;
+constexpr bool kEnableGpsLocalSearchTraceHooks = kEnableGpsTraceHooks && kEnableGpsLocalSearchHooks;
+constexpr bool kEnableGpsRouteProducerHook = kEnableGpsLocalSearchTraceHooks || kEnableGpsSpatialEdgeWeightPatch;
+constexpr bool kEnableGpsEdgeCostHook = kEnableGpsLocalSearchTraceHooks || kEnableGpsSpatialEdgeWeightPatch;
 constexpr uintptr_t kGpsSystemTickRva = 0x0E6B62C;
 constexpr uintptr_t kJournalTrackEntryImplRva = 0x05944FC;
 constexpr uintptr_t kJournalListenerArrayOffset = 0x210;
@@ -197,6 +205,12 @@ constexpr uint16_t kTrafficLaneFlagGpsOnly = 0x0200;
 constexpr uint16_t kTrafficLaneFlagNoAiDriving = 0x2000;
 constexpr uint16_t kTrafficLaneFlagHighway = 0x4000;
 constexpr uint16_t kTrafficLaneFlagNoAutodrive = 0x8000;
+constexpr bool kGpsSpatialPatchRequiresRouteProducer = true;
+constexpr float kGpsSpatialHighwayMultiplier = 0.62f;
+constexpr float kGpsSpatialRoadMultiplier = 1.0f;
+constexpr float kGpsSpatialGpsOnlyMultiplier = 1.20f;
+constexpr float kGpsSpatialPavementMultiplier = 1.35f;
+constexpr float kGpsSpatialUnknownMultiplier = 1.05f;
 
 struct GpsProviderClassOverride
 {
@@ -982,6 +996,122 @@ struct Vector3Value
     float z;
 };
 
+int GpsSpatialRoadClassRank(char aCode)
+{
+    switch (aCode)
+    {
+    case 'H':
+        return 4;
+    case 'R':
+        return 3;
+    case 'G':
+        return 2;
+    case 'P':
+        return 1;
+    default:
+        return 0;
+    }
+}
+
+const char* GpsSpatialRoadClassName(char aCode)
+{
+    switch (aCode)
+    {
+    case 'H':
+        return "highway";
+    case 'R':
+        return "road";
+    case 'G':
+        return "gpsonly";
+    case 'P':
+        return "pavement";
+    default:
+        return "unknown";
+    }
+}
+
+float GpsSpatialRoadClassMultiplier(char aCode)
+{
+    switch (aCode)
+    {
+    case 'H':
+        return kGpsSpatialHighwayMultiplier;
+    case 'R':
+        return kGpsSpatialRoadMultiplier;
+    case 'G':
+        return kGpsSpatialGpsOnlyMultiplier;
+    case 'P':
+        return kGpsSpatialPavementMultiplier;
+    default:
+        return kGpsSpatialUnknownMultiplier;
+    }
+}
+
+char LookupGpsSpatialRoadClass(float aX, float aMapY)
+{
+    using namespace EdgeWeightGPS::Generated;
+
+    if (!std::isfinite(aX) || !std::isfinite(aMapY))
+    {
+        return '.';
+    }
+
+    const auto columnFloat = (aX - kSpatialRoadGridMinX) / kSpatialRoadGridCellSize;
+    const auto rowFloat = (aMapY - kSpatialRoadGridMinY) / kSpatialRoadGridCellSize;
+    if (columnFloat < 0.0f || rowFloat < 0.0f)
+    {
+        return '.';
+    }
+
+    const auto column = static_cast<int64_t>(std::floor(columnFloat));
+    const auto row = static_cast<int64_t>(std::floor(rowFloat));
+    if (column < 0 || row < 0 || column >= static_cast<int64_t>(kSpatialRoadGridWidth) ||
+        row >= static_cast<int64_t>(kSpatialRoadGridHeight))
+    {
+        return '.';
+    }
+
+    const auto& rowData = kSpatialRoadGridRows[static_cast<size_t>(row)];
+    if (static_cast<size_t>(column) >= rowData.size())
+    {
+        return '.';
+    }
+
+    return rowData[static_cast<size_t>(column)];
+}
+
+bool TryLookupGpsSpatialEdgeClass(void* aCurrentState, void* aNeighborState, char& aCode)
+{
+    Vector3Value current{};
+    Vector3Value neighbor{};
+    if (!aCurrentState || !aNeighborState || !TryReadMemory(aCurrentState, current) ||
+        !TryReadMemory(aNeighborState, neighbor))
+    {
+        return false;
+    }
+
+    const auto midpointX = (current.x + neighbor.x) * 0.5f;
+    const auto midpointMapY = (current.z + neighbor.z) * 0.5f;
+
+    // Search states are (world X, height, world Y); route producer query vectors use (X, Y, Z, W).
+    const std::array candidates = {
+        LookupGpsSpatialRoadClass(midpointX, midpointMapY),
+        LookupGpsSpatialRoadClass(current.x, current.z),
+        LookupGpsSpatialRoadClass(neighbor.x, neighbor.z),
+    };
+
+    aCode = '.';
+    for (const auto candidate : candidates)
+    {
+        if (GpsSpatialRoadClassRank(candidate) > GpsSpatialRoadClassRank(aCode))
+        {
+            aCode = candidate;
+        }
+    }
+
+    return true;
+}
+
 void AppendVector3Pointer(std::ostringstream& aLine, const char* aLabel, const void* aVector)
 {
     Vector3Value value{};
@@ -4068,7 +4198,7 @@ int32_t DetourGpsRouteProducer(void* aGraph, void* aQuery, void* aOutResult)
     {
         std::lock_guard lock(gGpsCostLogMutex);
         callCount = gGpsRouteProducerLogCount++;
-        shouldLog = callCount < kMaxCalls;
+        shouldLog = kEnableGpsLocalSearchTraceHooks && callCount < kMaxCalls;
     }
 
     if (shouldLog)
@@ -4331,6 +4461,21 @@ float DetourGpsEdgeCost(void* aCostProvider, void* aCurrentState, void* aNeighbo
 
     const auto sourceClass = ReadGpsPointClass(aCurrentPoint);
     const auto neighborClass = ReadGpsPointClass(aNeighborPoint);
+    const auto originalResult = result;
+
+    char spatialClass = '.';
+    float spatialMultiplier = 1.0f;
+    bool spatialAdjusted = false;
+    const auto spatialPatchAllowed =
+        kEnableGpsSpatialEdgeWeightPatch &&
+        (!kGpsSpatialPatchRequiresRouteProducer || gActiveGpsRouteProducerCall != UINT32_MAX);
+    if (spatialPatchAllowed && result > 0.0f && std::isfinite(result) &&
+        TryLookupGpsSpatialEdgeClass(aCurrentState, aNeighborState, spatialClass))
+    {
+        spatialMultiplier = GpsSpatialRoadClassMultiplier(spatialClass);
+        result *= spatialMultiplier;
+        spatialAdjusted = spatialMultiplier != 1.0f;
+    }
 
     if (gActiveGpsEdgeCostStats)
     {
@@ -4339,9 +4484,9 @@ float DetourGpsEdgeCost(void* aCostProvider, void* aCurrentState, void* aNeighbo
     }
 
     float multiplier = 0.0f;
-    const auto hasMultiplier = sourceClass < 64 &&
-                               TryReadFloatField(aCostProvider, 0x08 + static_cast<uintptr_t>(sourceClass) * sizeof(float),
-                                                 multiplier);
+    const auto hasMultiplier =
+        neighborClass < 64 &&
+        TryReadFloatField(aCostProvider, 0x08 + static_cast<uintptr_t>(neighborClass) * sizeof(float), multiplier);
 
     uint32_t callCount = 0;
     uint32_t classCallCount = 0;
@@ -4349,21 +4494,29 @@ float DetourGpsEdgeCost(void* aCostProvider, void* aCurrentState, void* aNeighbo
     {
         std::lock_guard lock(gGpsCostLogMutex);
         callCount = gGpsEdgeCostLogCount++;
-        if (sourceClass < gGpsEdgeCostClassLogCounts.size())
+        if (neighborClass < gGpsEdgeCostClassLogCounts.size())
         {
-            classCallCount = gGpsEdgeCostClassLogCounts[sourceClass]++;
+            classCallCount = gGpsEdgeCostClassLogCounts[neighborClass]++;
         }
-        shouldLog = callCount < 32 || (sourceClass < 64 && classCallCount < 1);
+        shouldLog =
+            (kEnableGpsLocalSearchTraceHooks && (callCount < 32 || (neighborClass < 64 && classCallCount < 1))) ||
+            (kEnableGpsSpatialEdgeWeightPatchTrace && spatialAdjusted && callCount < 128);
     }
 
     if (shouldLog)
     {
-        const auto baseCost = hasMultiplier && multiplier != 0.0f ? result / multiplier : 0.0f;
+        const auto baseCost = hasMultiplier && multiplier != 0.0f ? originalResult / multiplier : 0.0f;
 
         std::ostringstream line;
         line << "hook GPSEdgeCost 0x44f838 call=" << callCount << " classCall=" << classCallCount
              << " sourceClass=" << static_cast<uint32_t>(sourceClass)
              << " neighborClass=" << static_cast<uint32_t>(neighborClass) << " value=" << result;
+        if (spatialPatchAllowed)
+        {
+            line << " originalValue=" << originalResult << " spatialClass=" << GpsSpatialRoadClassName(spatialClass)
+                 << " spatialMultiplier=" << spatialMultiplier
+                 << " spatialAdjusted=" << static_cast<uint32_t>(spatialAdjusted);
+        }
         if (hasMultiplier)
         {
             line << " multiplier=" << multiplier << " baseCost=" << baseCost;
@@ -4815,21 +4968,30 @@ RED4EXT_C_EXPORT bool RED4EXT_CALL Main(RED4ext::v1::PluginHandle aHandle, RED4e
                             reinterpret_cast<void*>(&DetourGpsRouteJobStart),
                             reinterpret_cast<void**>(&gOriginalGpsRouteJobStart));
         }
-        if (kEnableGpsProviderClassPatch || (kEnableGpsTraceHooks && kEnableGpsLocalSearchHooks))
+        if (kEnableGpsProviderClassPatch || kEnableGpsLocalSearchTraceHooks)
         {
             AttachProbeHook("GPSSearch 0x44f054", kGpsSearchRva, reinterpret_cast<void*>(&DetourGpsSearch),
                             reinterpret_cast<void**>(&gOriginalGpsSearch));
         }
-        if (kEnableGpsTraceHooks && kEnableGpsLocalSearchHooks)
+        if (kEnableGpsRouteProducerHook)
         {
             AttachProbeHook("GPSRouteProducer 0x44cc7c", kGpsRouteProducerRva,
                             reinterpret_cast<void*>(&DetourGpsRouteProducer),
                             reinterpret_cast<void**>(&gOriginalGpsRouteProducer));
+        }
+        if (kEnableGpsLocalSearchTraceHooks)
+        {
             AttachProbeHook("GPSResolveHandle 0x44e1a8", kGpsResolveHandleRva,
                             reinterpret_cast<void*>(&DetourGpsResolveHandle),
                             reinterpret_cast<void**>(&gOriginalGpsResolveHandle));
+        }
+        if (kEnableGpsEdgeCostHook)
+        {
             AttachProbeHook("GPSEdgeCost 0x44f838", kGpsEdgeCostRva, reinterpret_cast<void*>(&DetourGpsEdgeCost),
                             reinterpret_cast<void**>(&gOriginalGpsEdgeCost));
+        }
+        if (kEnableGpsLocalSearchTraceHooks)
+        {
             AttachProbeHook("GPSFilteredProviderFilter 0x44ff68", kGpsFilteredProviderFilterRva,
                             reinterpret_cast<void*>(&DetourGpsFilteredProviderFilter),
                             reinterpret_cast<void**>(&gOriginalGpsFilteredProviderFilter));
@@ -4993,15 +5155,24 @@ RED4EXT_C_EXPORT bool RED4EXT_CALL Main(RED4ext::v1::PluginHandle aHandle, RED4e
         {
             DetachProbeHook("GPSRouteJobStart 0x818928", kGpsRouteJobStartRva);
         }
-        if (kEnableGpsProviderClassPatch || (kEnableGpsTraceHooks && kEnableGpsLocalSearchHooks))
+        if (kEnableGpsProviderClassPatch || kEnableGpsLocalSearchTraceHooks)
         {
             DetachProbeHook("GPSSearch 0x44f054", kGpsSearchRva);
         }
-        if (kEnableGpsTraceHooks && kEnableGpsLocalSearchHooks)
+        if (kEnableGpsRouteProducerHook)
         {
             DetachProbeHook("GPSRouteProducer 0x44cc7c", kGpsRouteProducerRva);
+        }
+        if (kEnableGpsLocalSearchTraceHooks)
+        {
             DetachProbeHook("GPSResolveHandle 0x44e1a8", kGpsResolveHandleRva);
+        }
+        if (kEnableGpsEdgeCostHook)
+        {
             DetachProbeHook("GPSEdgeCost 0x44f838", kGpsEdgeCostRva);
+        }
+        if (kEnableGpsLocalSearchTraceHooks)
+        {
             DetachProbeHook("GPSFilteredProviderFilter 0x44ff68", kGpsFilteredProviderFilterRva);
             DetachProbeHook("GPSBaseProviderFilter 0x450b08", kGpsBaseProviderFilterRva);
         }