Implement the "random expanding sphere" method

[seveibar]

The expanding sphere method is a monte-carlo method that works like this:

---

Start point: Begin at the initial point of your route. End point: Define the destination point. Obstacles: Represent obstacles or forbidden areas in your routing space.

Algorithm steps: a) From the current point, create the largest possible sphere that doesn't intersect any obstacles. b) Choose a random point on the surface of this sphere. c) Move to this new point. d) Repeat until you're within a specified distance of the end point.

[seveibar]

Example python implementation of finding the largest sphere:

import math

def distance(x1, y1, x2, y2):
    return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

def point_to_rectangle_distance(px, py, rect):
    cx, cy, w, h = rect['centerX'], rect['centerY'], rect['width'], rect['height']

    # Find the closest point on the rectangle to the given point
    closest_x = max(cx - w/2, min(px, cx + w/2))
    closest_y = max(cy - h/2, min(py, cy + h/2))

    return distance(px, py, closest_x, closest_y)

def find_largest_sphere(current_point, obstacles, end_point):
    x, y = current_point['x'], current_point['y']

    # Initialize with the distance to the end point
    max_radius = distance(x, y, end_point['x'], end_point['y'])

    # Check each obstacle
    for obstacle in obstacles:
        dist = point_to_rectangle_distance(x, y, obstacle)
        max_radius = min(max_radius, dist)

    return max_radius

# Example usage
current_point = {'x': 0, 'y': 0}
end_point = {'x': 10, 'y': 10}
obstacles = [
    {'centerX': 5, 'centerY': 5, 'width': 2, 'height': 2},
    {'centerX': 8, 'centerY': 3, 'width': 1, 'height': 4}
]

largest_sphere_radius = find_largest_sphere(current_point, obstacles, end_point)
print(f"Largest sphere radius: {largest_sphere_radius}")