perf(motion_utils): improve performance of findNearestIndex (autoware…

…foundation#3082) (#344) Signed-off-by: Taiga Takano <[email protected]> Co-authored-by: Taiga TAKANO <[email protected]>
tier4 · Apr 3, 2023 · b6ae4cf · b6ae4cf
1 parent c4ceede
commit b6ae4cf
Showing 1 changed file with 1 addition and 5 deletions.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -198,7 +198,7 @@ boost::optional<size_t> findNearestIndex(
 
   for (size_t i = 0; i < points.size(); ++i) {
     const auto squared_dist = tier4_autoware_utils::calcSquaredDistance2d(points.at(i), pose);
-    if (squared_dist > max_squared_dist) {
+    if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) {
       continue;
     }
 
@@ -208,10 +208,6 @@ boost::optional<size_t> findNearestIndex(
       continue;
     }
 
-    if (squared_dist >= min_squared_dist) {
-      continue;
-    }
-
     min_squared_dist = squared_dist;
     min_idx = i;
     is_nearest_found = true;