diff --git a/grid_map_core/include/grid_map_core/GridMapMath.hpp b/grid_map_core/include/grid_map_core/GridMapMath.hpp
index 6d4832580..b45f44aaa 100644
--- 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
index 6512bb85e..361fe6357 100644
--- 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)