Add doc for Euclidean distance calculation and line segment intersect…

…ion distance calculation

robot_sf/sensor/range_sensor.py

@@ -21,27 +21,58 @@
 
 @numba.njit(fastmath=True)
 def euclid_dist(vec_1: Vec2D, vec_2: Vec2D) -> float:
+    """
+    Calculate Euclidean distance between two 2D vectors.
+
+    Parameters:
+    vec_1 (Vec2D): First 2D vector.
+    vec_2 (Vec2D): Second 2D vector.
+
+    Returns:
+    float: Euclidean distance between vec_1 and vec_2.
+    """
+    # Subtract corresponding elements of vectors
+    # Square the results, sum them, and take square root
     return ((vec_1[0] - vec_2[0])**2 + (vec_1[1] - vec_2[1])**2)**0.5
 
 
 @numba.njit(fastmath=True)
-def lineseg_line_intersection_distance(segment: Line2D, sensor_pos: Vec2D, ray_vec: Vec2D) -> float:
+def lineseg_line_intersection_distance(
+        segment: Line2D, sensor_pos: Vec2D, ray_vec: Vec2D) -> float:
+    """
+    Calculate the distance from the sensor position to the intersection point
+    between a line segment and a ray vector.
+
+    Parameters:
+    segment (Line2D): The line segment.
+    sensor_pos (Vec2D): The sensor position.
+    ray_vec (Vec2D): The ray vector.
+
+    Returns:
+    float: The distance to the intersection point, or infinity if no intersection.
+    """
+    # Unpack segment endpoints, sensor position, and ray vector
     (x_1, y_1), (x_2, y_2) = segment
     x_sensor, y_sensor = sensor_pos
+    x_ray, y_ray = ray_vec
+
+    # Calculate differences for intersection formula
     x_diff, y_diff = x_1 - x_sensor, y_1 - y_sensor
     x_seg, y_seg = x_2 - x_1, y_2 - y_1
-    x_ray, y_ray = ray_vec
 
+    # Calculate numerator and denominator of intersection formula
     num = x_ray * y_diff - y_ray * x_diff
     den = x_seg * y_ray - x_ray * y_seg
 
-    # edge case: line segment has same orientation as ray vector
+    # Edge case: line segment has same orientation as ray vector
     if den == 0:
         return np.inf
 
+    # Calculate intersection parameters
     mu = num / den
     tau = (mu * x_seg + x_diff) / x_ray
 
+    # If intersection is within segment and in ray direction, calculate distance
     if 0 <= mu <= 1 and tau >= 0:
         cross_x, cross_y = x_1 + mu * (x_2 - x_1), y_1 + mu * (y_2 - y_1)
         return euclid_dist(sensor_pos, (cross_x, cross_y))