obstacles.py

import math
import random


class DynamicObstacle:
    def __init__(self, x: float, y: float, radius: float,
                 vx: float, vy: float, bounds: tuple[int, int, int, int]):
        self.x = x
        self.y = y
        self.radius = radius
        self.vx = vx
        self.vy = vy
        self.bounds = bounds  # (x_min, y_min, x_max, y_max)

    def update(self) -> None:
        self.x += self.vx
        self.y += self.vy

        # Bounce off bounds
        if self.x - self.radius < self.bounds[0]:
            self.x = self.bounds[0] + self.radius
            self.vx = abs(self.vx)
        elif self.x + self.radius > self.bounds[2]:
            self.x = self.bounds[2] - self.radius
            self.vx = -abs(self.vx)

        if self.y - self.radius < self.bounds[1]:
            self.y = self.bounds[1] + self.radius
            self.vy = abs(self.vy)
        elif self.y + self.radius > self.bounds[3]:
            self.y = self.bounds[3] - self.radius
            self.vy = -abs(self.vy)

    def collides_with_cell(self, row: int, col: int, cell_size: int) -> bool:
        """Circle-rect intersection test for a grid cell."""
        rect_x = col * cell_size
        rect_y = row * cell_size
        # Find closest point on the rect to the circle center
        closest_x = max(rect_x, min(self.x, rect_x + cell_size))
        closest_y = max(rect_y, min(self.y, rect_y + cell_size))
        dx = self.x - closest_x
        dy = self.y - closest_y
        return (dx * dx + dy * dy) <= (self.radius * self.radius)

    def get_blocked_cells(self, cell_size: int) -> set[tuple[int, int]]:
        """Return set of (row, col) cells this circle currently covers."""
        cells = set()
        min_col = max(0, int((self.x - self.radius) // cell_size))
        max_col = int((self.x + self.radius) // cell_size)
        min_row = max(0, int((self.y - self.radius) // cell_size))
        max_row = int((self.y + self.radius) // cell_size)

        for r in range(min_row, max_row + 1):
            for c in range(min_col, max_col + 1):
                if self.collides_with_cell(r, c, cell_size):
                    cells.add((r, c))
        return cells

    def intersects_path_grid(self, path: list[tuple[int, int]], cell_size: int) -> bool:
        """Check if this obstacle overlaps any cell in a grid path."""
        for (r, c) in path:
            if self.collides_with_cell(r, c, cell_size):
                return True
        return False

    def intersects_path_continuous(self, path: list[tuple[float, float]]) -> bool:
        """Check if this obstacle intersects any segment in an RRT path."""
        for i in range(len(path) - 1):
            p1 = path[i]
            p2 = path[i + 1]
            if self._segment_intersects_circle(p1[0], p1[1], p2[0], p2[1]):
                return True
        return False

    def _segment_intersects_circle(self, p1x, p1y, p2x, p2y) -> bool:
        dx = p2x - p1x
        dy = p2y - p1y
        fx = p1x - self.x
        fy = p1y - self.y

        a = dx * dx + dy * dy
        b = 2 * (fx * dx + fy * dy)
        c = fx * fx + fy * fy - self.radius * self.radius

        if a == 0:
            return c <= 0

        discriminant = b * b - 4 * a * c
        if discriminant < 0:
            return False

        discriminant = math.sqrt(discriminant)
        t1 = (-b - discriminant) / (2 * a)
        t2 = (-b + discriminant) / (2 * a)

        return (0 <= t1 <= 1) or (0 <= t2 <= 1) or (t1 < 0 and t2 > 1)


def create_dynamic_obstacles(count, bounds, radius, speed):
    """Create a list of DynamicObstacle with random positions and velocities."""
    obstacles = []
    for _ in range(count):
        x = random.uniform(bounds[0] + radius + 50, bounds[2] - radius - 50)
        y = random.uniform(bounds[1] + radius + 50, bounds[3] - radius - 50)
        angle = random.uniform(0, 2 * math.pi)
        vx = math.cos(angle) * speed
        vy = math.sin(angle) * speed
        obstacles.append(DynamicObstacle(x, y, radius, vx, vy, bounds))
    return obstacles