From ad1f8c1e8445acdb2f40f7fcee767b587e5df11c Mon Sep 17 00:00:00 2001
From: elsid <elsid.mail@gmail.com>
Date: Thu, 4 Feb 2021 00:14:29 +0100
Subject: [PATCH] Sort water and off mesh connections for recast mesh

Inconsisten order of these objects in navmesh cache key leads to cache misses
due to key inequality.
---
 components/bullethelpers/operators.hpp            | 15 +++++++++++++++
 components/detournavigator/offmeshconnection.hpp  |  7 +++++++
 .../detournavigator/offmeshconnectionsmanager.hpp |  2 ++
 components/detournavigator/recastmesh.hpp         |  8 ++++++++
 components/detournavigator/recastmeshbuilder.cpp  |  1 +
 5 files changed, 33 insertions(+)

diff --git a/components/bullethelpers/operators.hpp b/components/bullethelpers/operators.hpp
index ea88deddf..dd2ec8017 100644
--- a/components/bullethelpers/operators.hpp
+++ b/components/bullethelpers/operators.hpp
@@ -68,4 +68,19 @@ inline std::ostream& operator <<(std::ostream& stream, BroadphaseNativeTypes val
     }
 }
 
+inline bool operator <(const btVector3& lhs, const btVector3& rhs)
+{
+    return std::tie(lhs.x(), lhs.y(), lhs.z()) < std::tie(rhs.x(), rhs.y(), rhs.z());
+}
+
+inline bool operator <(const btMatrix3x3& lhs, const btMatrix3x3& rhs)
+{
+    return std::tie(lhs[0], lhs[1], lhs[2]) < std::tie(rhs[0], rhs[1], rhs[2]);
+}
+
+inline bool operator <(const btTransform& lhs, const btTransform& rhs)
+{
+    return std::tie(lhs.getBasis(), lhs.getOrigin()) < std::tie(rhs.getBasis(), rhs.getOrigin());
+}
+
 #endif
diff --git a/components/detournavigator/offmeshconnection.hpp b/components/detournavigator/offmeshconnection.hpp
index ca999dbdb..01bae0273 100644
--- a/components/detournavigator/offmeshconnection.hpp
+++ b/components/detournavigator/offmeshconnection.hpp
@@ -5,6 +5,8 @@
 
 #include <osg/Vec3f>
 
+#include <tuple>
+
 namespace DetourNavigator
 {
     struct OffMeshConnection
@@ -13,6 +15,11 @@ namespace DetourNavigator
         osg::Vec3f mEnd;
         AreaType mAreaType;
     };
+
+    inline bool operator<(const OffMeshConnection& lhs, const OffMeshConnection& rhs)
+    {
+        return std::tie(lhs.mStart, lhs.mEnd, lhs.mAreaType) < std::tie(rhs.mStart, rhs.mEnd, rhs.mAreaType);
+    }
 }
 
 #endif
diff --git a/components/detournavigator/offmeshconnectionsmanager.hpp b/components/detournavigator/offmeshconnectionsmanager.hpp
index de707f3a8..1ad96e3b9 100644
--- a/components/detournavigator/offmeshconnectionsmanager.hpp
+++ b/components/detournavigator/offmeshconnectionsmanager.hpp
@@ -85,6 +85,8 @@ namespace DetourNavigator
                     std::for_each(byId.first, byId.second, [&] (const auto& v) { result.push_back(v.second); });
                 });
 
+            std::sort(result.begin(), result.end());
+
             return result;
         }
 
diff --git a/components/detournavigator/recastmesh.hpp b/components/detournavigator/recastmesh.hpp
index f3259903f..29f37822e 100644
--- a/components/detournavigator/recastmesh.hpp
+++ b/components/detournavigator/recastmesh.hpp
@@ -5,9 +5,12 @@
 #include "chunkytrimesh.hpp"
 #include "bounds.hpp"
 
+#include <components/bullethelpers/operators.hpp>
+
 #include <memory>
 #include <string>
 #include <vector>
+#include <tuple>
 
 #include <osg/Vec3f>
 
@@ -87,6 +90,11 @@ namespace DetourNavigator
         ChunkyTriMesh mChunkyTriMesh;
         Bounds mBounds;
     };
+
+    inline bool operator<(const RecastMesh::Water& lhs, const RecastMesh::Water& rhs)
+    {
+        return std::tie(lhs.mCellSize, lhs.mTransform) < std::tie(rhs.mCellSize, rhs.mTransform);
+    }
 }
 
 #endif
diff --git a/components/detournavigator/recastmeshbuilder.cpp b/components/detournavigator/recastmeshbuilder.cpp
index ee014b932..f8456acf0 100644
--- a/components/detournavigator/recastmeshbuilder.cpp
+++ b/components/detournavigator/recastmeshbuilder.cpp
@@ -155,6 +155,7 @@ namespace DetourNavigator
     std::shared_ptr<RecastMesh> RecastMeshBuilder::create(std::size_t generation, std::size_t revision)
     {
         optimizeRecastMesh(mIndices, mVertices);
+        std::sort(mWater.begin(), mWater.end());
         return std::make_shared<RecastMesh>(generation, revision, mIndices, mVertices, mAreaTypes,
             mWater, mSettings.get().mTrianglesPerChunk);
     }