diff --git a/src/fvi.py b/src/fvi.py
index e5cd11a..710c957 100644
--- a/src/fvi.py
+++ b/src/fvi.py
@@ -1,7 +1,7 @@
 import numpy as np
 
 
-def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, list]:
+def fast_voxel_intersect(start_, end_, origin_, step_, shape_) -> tuple[list, list, list]:
     """Compute the voxels intersected by a line segment.
 
     Args:
@@ -17,14 +17,14 @@ def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, l
     """
 
     # Convert to numpy arrays
-    start = np.asarray(start)
-    end = np.asarray(end)
-    origin = np.asarray(origin)
-    step = np.asarray(step)
+    start_ = np.asarray(start_)
+    end_ = np.asarray(end_)
+    origin_ = np.asarray(origin_)
+    step_ = np.asarray(step_)
 
     # Translate line segment to voxel grid
-    start = start - origin
-    end = end - origin
+    start = (start_ - origin_) / step_
+    end = (end_ - origin_) / step_
 
     # Initialize list of intersected voxels
     intersections = []
@@ -45,16 +45,18 @@ def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, l
 
     # Initialize current position to start
     position = start.copy()
-    # print("position: ", position)
+
+    # Initialize early exit
+    is_in = False
 
     # Main loop
     while True:
         # Compute the distance to the next boundaries
-        next_boundaries = np.divide(position + step * direction_signs, step)
+        next_boundaries = position + direction_signs
         errored = np.abs(np.round(next_boundaries) - next_boundaries) < 1e-12
         next_boundaries[errored] = np.round(next_boundaries[errored])
         distances = ((1 - is_negative) * np.floor(next_boundaries) +
-                     is_negative * np.ceil(next_boundaries)) * step - position
+                     is_negative * np.ceil(next_boundaries)) - position
 
         # Determine the nearest boundary to be reached
         boundary_distances = np.abs(distances / direction)
@@ -68,28 +70,24 @@ def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, l
 
         # Update position
         position = position + clothest_boundary_distance * direction
-        # print("position_update: ", position)
 
         # Correct position to be on boundary
-        position[clothest_boundary] = round(
-            position[clothest_boundary] / step[clothest_boundary]) * step[clothest_boundary]
+        position[clothest_boundary] = round(position[clothest_boundary])
 
         # Get corresponding voxel
-        on_boundary = np.mod(position, step) == 0
-        voxel = np.floor_divide(position, step) * step - is_negative * on_boundary * step
+        on_boundary = np.mod(position, np.ones_like(position)) == 0
+        voxel = np.floor(position) - is_negative * on_boundary
 
         # Add voxel to list
-        idx = np.floor_divide(voxel, step).astype(int)
-        if np.any(idx < 0) or np.any(idx >= shape):
+        idx = np.floor(voxel).astype(int)
+        if np.any(idx < 0) or np.any(idx >= shape_):
+            if is_in:
+                break
             continue
-        # print(f"voxel: {voxel}, step: {step}, idx: {idx}")
-        intersections.append(position + origin)
-        voxels.append(voxel + origin)
+        intersections.append(position * step_ + origin_)
+        voxels.append(voxel * step_ + origin_)
         voxels_idx.append(idx)
-
-    # print(f"intersections: {intersections}")
-    # print(f"voxels: {voxels}")
-    # print(f"voxels_idx: {voxels_idx}")
+        is_in = True
 
     return intersections, voxels, voxels_idx
 
@@ -119,12 +117,14 @@ if __name__ == '__main__':
 
         # Plot line segment
         plt.plot([start[0], end[0]], [start[1], end[1]], 'k-')
-        plt.plot(start[0], start[1], 'go')
-        plt.plot(end[0], end[1], 'ro')
 
         # Plot intersection points
         for pos in positions:
             plt.plot(pos[0], pos[1], 'bo')
+        
+        # Plot start and end points
+        plt.plot(start[0], start[1], 'go')
+        plt.plot(end[0], end[1], 'ro')
 
         # Plot voxel grid
         plt.axis('equal')
@@ -144,7 +144,7 @@ if __name__ == '__main__':
 
     # Define voxel grid
     origin = np.array([-5., -5.])
-    step = np.array([0.7, 0.7])
+    step = np.array([1.4, 1.4])
     shape = (10, 10)
 
     # Define segment