Merge branch 'anymal_research/rsl/feature/fasterindexwrapping' into '…

…master' [grid_map] refactor index wrapping Function is used a lot inside grid_map and popped up during profiling. New implementation is 1.4x faster http://quick-bench.com/uBwp32u3L9aaxl_7LAlRe7JbwKQ @perception/localization-and-mapping Squashed commit: Import change set of branch rsl/feature/fasterindexwrapping. Commits: -- Change 1 of 1 by rgrandia <[email protected]>: refactored index wrapping Authors: Co-authored-by: rgrandia <[email protected]> **Related ANYmal Research MR:** https://code.anymal.com/anymal-research/anymal_research/merge_requests/148 Authored by: Ruben Grandia *Note: The MR in ANYmal Research has to be closed manually.* ANYmal Research issues closed GitOrigin-RevId: 145455e1523f0110f5ff9c1ce9cf5aa30e8dad73
Calm-wy · Feb 5, 2020 · de13236 · de13236
1 parent 1ecd7b0
commit de13236
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git a/grid_map_core/include/grid_map_core/GridMapMath.hpp b/grid_map_core/include/grid_map_core/GridMapMath.hpp
@@ -142,7 +142,7 @@ void wrapIndexToRange(Index& index, const Size& bufferSize);
  * @param[in/out] index the index that will be wrapped into the valid region of the buffer.
  * @param[in] bufferSize the size of the buffer.
  */
-void wrapIndexToRange(int& index, const int& bufferSize);
+void wrapIndexToRange(int& index, int bufferSize);
 
 /*!
  * Bound (cuts off) the position to lie inside the map.

diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp
@@ -218,10 +218,12 @@ void wrapIndexToRange(Index& index, const Size& bufferSize)
   }
 }
 
-void wrapIndexToRange(int& index, const int& bufferSize)
+void wrapIndexToRange(int& index, int bufferSize)
 {
-  if (index < 0) index += ((-index / bufferSize) + 1) * bufferSize;
   index = index % bufferSize;
+  if (index < 0) {
+    index += bufferSize;
+  }
 }
 
 void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition)