Speedup the navcel location

navmap_core/src/navmap_core/NavMap.cpp

@@ -546,7 +546,7 @@ bool NavMap::locate_by_walking(
   Vec3 * hit_pt,
   float planar_eps) const
 {
-  const int kMaxSteps = 64;
+  const int kMaxSteps = 16;
   NavCelId cid = start_cid;
 
   for (int step = 0; step < kMaxSteps; ++step) {
@@ -599,25 +599,70 @@ bool NavMap::locate_navcel_core(
   Vec3 * hit_pt,
   const LocateOpts & opts) const
 {
-  // 1) Try walking if there is a valid hint.
+  // 0) Fast path: directly test the hinted triangle, if any.
   if (opts.hint_cid.has_value()) {
-    if (locate_by_walking(opts.hint_cid.value(),
-                          p_world,
-                          cid,
-                          bary,
-                          hit_pt,
-                          opts.planar_eps))
-    {
-      for (size_t s = 0; s < surfaces.size(); ++s) {
-        const auto & surf = surfaces[s];
-        if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) !=
-          surf.navcels.end())
+    const NavCelId hint = *opts.hint_cid;
+    if (hint < navcels.size()) {
+      const auto & c = navcels[hint];
+      const Vec3 a = positions.at(c.v[0]);
+      const Vec3 b = positions.at(c.v[1]);
+      const Vec3 d = positions.at(c.v[2]);
+      const Vec3 & n = c.normal;
+
+      const float dist = n.dot(p_world - a);
+      const Vec3 q = p_world - dist * n;
+
+      Vec3 bary_hint;
+      if (point_in_triangle_bary(q, a, b, d, bary_hint, opts.planar_eps) &&
+        std::fabs(dist) <= opts.height_eps)
+      {
+        cid = hint;
+        bary = bary_hint;
+        if (hit_pt) {
+          *hit_pt = q;
+        }
+
+        // Find the surface that owns this navcel.
+        for (size_t s = 0; s < surfaces.size(); ++s) {
+          const auto & surf = surfaces[s];
+          if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) {
+            surface_idx = s;
+            return true;
+          }
+        }
+        // If no surface owns the hinted navcel, fall through to the generic search.
+      }
+    }
+  }
+
+  // 1) Try walking if there is a valid hint and we are not far from its plane.
+  if (opts.hint_cid.has_value()) {
+    const NavCelId start = *opts.hint_cid;
+    if (start < navcels.size()) {
+      const auto & c0 = navcels[start];
+      const Vec3 a0 = positions.at(c0.v[0]);
+      const Vec3 & n0 = c0.normal;
+      const float dist0 = n0.dot(p_world - a0);
+
+      // Do not walk if the query point is clearly off the hinted plane.
+      if (std::fabs(dist0) <= opts.height_eps) {
+        if (locate_by_walking(start,
+                              p_world,
+                              cid,
+                              bary,
+                              hit_pt,
+                              opts.planar_eps))
         {
-          surface_idx = s;
-          return true;
+          for (size_t s = 0; s < surfaces.size(); ++s) {
+            const auto & surf = surfaces[s];
+            if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) {
+              surface_idx = s;
+              return true;
+            }
+          }
+          // Fall through if surface not found.
         }
       }
-      // Fall through if surface not found.
     }
   }