Fix floating point precision in calculations

Correct length reduction by min range components

nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp

@@ -226,9 +226,9 @@ class VoxelGrid
     double x1, double y1, double z1, unsigned int max_length = UINT_MAX,
     unsigned int min_length = 0)
   {
-    int dx = int(x1) - int(x0);  // NOLINT
-    int dy = int(y1) - int(y0);  // NOLINT
-    int dz = int(z1) - int(z0);  // NOLINT
+    double dx = x1 - x0;
+    double dy = y1 - y0;
+    double dz = z1 - z0;
 
     unsigned int abs_dx = abs(dx);
     unsigned int abs_dy = abs(dy);
@@ -247,9 +247,12 @@ class VoxelGrid
     double scale = std::min(1.0, max_length / dist);
 
     // Updating starting point to the point at distance min_length from the initial point
-    double min_x0 = x0 + dx / dist * min_length;
-    double min_y0 = y0 + dy / dist * min_length;
-    double min_z0 = z0 + dz / dist * min_length;
+    double min_shift_x = dx / dist * min_length;
+    double min_shift_y = dy / dist * min_length;
+    double min_shift_z = dz / dist * min_length;
+    double min_x0 = x0 + min_shift_x;
+    double min_y0 = y0 + min_shift_y;
+    double min_z0 = z0 + min_shift_z;
 
 
     unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)min_z0;
@@ -264,7 +267,7 @@ class VoxelGrid
       int error_y = abs_dx / 2;
       int error_z = abs_dx / 2;
       // Since initial point has been updated above, subtracting min_length from the total length
-      length = (unsigned int)(scale * abs_dx) - min_length;
+      length = (unsigned int)(scale * abs_dx) - abs(min_shift_x);
       bresenham3D(
         at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z,
         offset_dx, offset_dy, offset_dz, offset, z_mask, length);
@@ -276,7 +279,7 @@ class VoxelGrid
       int error_x = abs_dy / 2;
       int error_z = abs_dy / 2;
       // Since initial point has been updated above, subtracting min_length from the total length
-      length = (unsigned int)(scale * abs_dy) - min_length;
+      length = (unsigned int)(scale * abs_dy) - abs(min_shift_y);
       bresenham3D(
         at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z,
         offset_dy, offset_dx, offset_dz, offset, z_mask, length);
@@ -287,7 +290,7 @@ class VoxelGrid
     int error_x = abs_dz / 2;
     int error_y = abs_dz / 2;
     // Since initial point has been updated above, subtracting min_length from the total length
-    length = (unsigned int)(scale * abs_dz) - min_length;
+    length = (unsigned int)(scale * abs_dz) - abs(min_shift_z);
 
     bresenham3D(
       at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz,