Merge branch 's2025_teamA' of https://github.com/krishauser/GEMstack into s2025_teamA

Author: AadarshHegde123

testing/test_collision_detection.py

@@ -9,212 +9,7 @@
 import matplotlib.patches as patches
 import math
 
-
-################################################################################
-########## Collisiong Detection ################################################
-################################################################################
-class CollisionDetector:
-    """
-    Simulation class to update positions of two rectangles (vehicle and pedestrian)
-    with velocities v1 and v2, performing collision detection at each time step.
-    All functions remain within the class, and variables defined in __init__ remain unchanged;
-    local copies are used during simulation.
-    """
-    def __init__(self, x1, y1, t1, x2, y2, t2, v1, v2, total_time=10.0, desired_speed=2.0, acceleration=0.5):
-
-        self.vehicle_x = x1
-        self.vehicle_y = y1
-        self.pedestrian_x = x2
-        self.pedestrian_y = y2
-
-        # Vehicle parameters with buffer adjustments
-        self.vehicle_size_x = 3.2
-        self.vehicle_size_y = 1.7
-        self.vehicle_buffer_x = 3.0 + 1.0
-        self.vehicle_buffer_y = 2.0
-
-        # Vehicle rectangle
-        self.x1 = self.vehicle_x + (self.vehicle_size_x + self.vehicle_buffer_x)*0.5 # Offset for buffer (remains constant)
-        self.y1 = self.vehicle_y
-        self.w1 = self.vehicle_size_x + self.vehicle_buffer_x  # Increase width with buffer
-        self.h1 = self.vehicle_size_y + self.vehicle_buffer_y  # Increase height with buffer
-        self.t1 = t1
-
-        # Pedestrian rectangle
-        self.x2 = x2
-        self.y2 = y2
-        self.w2 = 0.5
-        self.h2 = 0.5
-        self.t2 = t2
-
-        # Velocities are expected as [vx, vy]
-        self.v1 = v1
-        self.v2 = v2
-
-        self.dt = 0.1  # seconds
-        self.total_time = total_time  # seconds
-
-        self.desired_speed = desired_speed
-        self.acceleration = acceleration
-
-    def set_params(self, speed, acceleration):
-        self.desired_speed = speed
-        self.acceleration = acceleration
-
-    def get_corners(self, x, y, w, h, theta):
-        """
-        Returns the 4 corners of a rotated rectangle.
-        (x, y): center of rectangle
-        w, h: width and height of rectangle
-        theta: rotation angle in radians
-        """
-        cos_t = math.cos(theta)
-        sin_t = math.sin(theta)
-        hw, hh = w / 2.0, h / 2.0
-
-        corners = np.array([
-            [ hw * cos_t - hh * sin_t,  hw * sin_t + hh * cos_t],
-            [-hw * cos_t - hh * sin_t, -hw * sin_t + hh * cos_t],
-            [-hw * cos_t + hh * sin_t, -hw * sin_t - hh * cos_t],
-            [ hw * cos_t + hh * sin_t,  hw * sin_t - hh * cos_t]
-        ])
-
-        corners[:, 0] += x
-        corners[:, 1] += y
-
-        return corners
-
-    def get_axes(self, rect):
-        axes = []
-        for i in range(len(rect)):
-            p1 = rect[i]
-            p2 = rect[(i + 1) % len(rect)]
-            edge = p2 - p1
-            normal = np.array([-edge[1], edge[0]])
-            norm = np.linalg.norm(normal)
-            if norm:
-                normal /= norm
-            axes.append(normal)
-        return axes
-
-    def project(self, rect, axis):
-        dots = np.dot(rect, axis)
-        return np.min(dots), np.max(dots)
-
-    def sat_collision(self, rect1, rect2):
-        """
-        Determines if two convex polygons (rectangles) collide using the
-        Separating Axis Theorem (SAT).
-        rect1 and rect2 are numpy arrays of shape (4,2) representing their corners.
-        """
-        axes1 = self.get_axes(rect1)
-        axes2 = self.get_axes(rect2)
-
-        for axis in axes1 + axes2:
-            min1, max1 = self.project(rect1, axis)
-            min2, max2 = self.project(rect2, axis)
-            if max1 < min2 or max2 < min1:
-                return False
-        return True
-
-    def plot_rectangles(self, rect1, rect2, collision, ax):
-        """
-        Plot two rectangles on a provided axis and indicate collision by color.
-        """
-        color = 'red' if collision else 'blue'
-        for rect in [rect1, rect2]:
-            polygon = patches.Polygon(rect, closed=True, edgecolor=color, fill=False, linewidth=2)
-            ax.add_patch(polygon)
-            ax.scatter(rect[:, 0], rect[:, 1], color=color, zorder=3)
-        ax.set_title(f"Collision: {'Yes' if collision else 'No'}")
-
-    def run(self, is_displayed=False):
-        collision_distance = -1
-        steps = int(self.total_time / self.dt) + 1
-
-        # Create local variables for positions; these will be updated 
-        # without modifying the __init__ attributes.
-        current_x1 = self.x1
-        current_y1 = self.y1
-        current_x2 = self.x2
-        current_y2 = self.y2
-        current_v1 = self.v1[0]
-
-        if is_displayed:
-            plt.ion()  # Turn on interactive mode
-            fig, ax = plt.subplots(figsize=(10,5))
-
-        for i in range(steps):
-            t_sim = i * self.dt
-
-            # Compute rectangle corners using the local positional variables.
-            rect1 = self.get_corners(current_x1, current_y1, self.w1, self.h1, self.t1)
-            rect2 = self.get_corners(current_x2, current_y2, self.w2, self.h2, self.t2)
-
-            # Perform collision detection.
-            collision = self.sat_collision(rect1, rect2)
-
-            if is_displayed:
-                # Plot the current step.
-                ax.clear()
-                self.plot_rectangles(rect1, rect2, collision, ax)
-                ax.set_aspect('equal')
-                ax.grid(True, linestyle='--', alpha=0.5)
-                ax.set_xlim(self.vehicle_x - 5, self.vehicle_x + 20)
-                ax.set_ylim(self.vehicle_y -5, self.vehicle_y +5)
-
-                # Draw an additional rectangle (vehicle body) at the vehicle's center.
-                rect_vehiclebody = patches.Rectangle(
-                    (current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x)*0.5, current_y1 - self.vehicle_size_y * 0.5),
-                    self.w1 - self.vehicle_buffer_x,
-                    self.h1 - self.vehicle_buffer_y,
-                    edgecolor='green',
-                    fill=False,
-                    linewidth=2,
-                    linestyle='--'
-                )
-                ax.add_patch(rect_vehiclebody)
-
-                ax.text(0.01, 0.99, f"t = {t_sim:.1f}s", fontsize=12, transform=ax.transAxes, verticalalignment='top')
-                plt.draw()
-
-                # Pause briefly to simulate real-time updating.
-                plt.pause(self.dt * 0.05)
-
-            # Stop simulation if collision is detected.
-            if collision:
-                current_vehicle_x  = current_x1 - (self.vehicle_size_x + self.vehicle_buffer_x) * 0.5
-                current_vehicle_y  = current_y1
-                collision_distance = current_vehicle_x - self.vehicle_x
-                break
-
-            # Update the vehicle's speed if it is not at the desired speed.
-            next_v = current_v1 + self.acceleration * self.dt
-            # Accelerating: Check if the vehicle is about to exceed the desired speed.
-            if next_v > self.desired_speed and current_v1 <= self.desired_speed:
-                current_v1 = self.desired_speed
-            # Decelerating: Check if the vehicle is about to fall below the desired speed.
-            elif next_v < self.desired_speed and current_v1 >= self.desired_speed:
-                current_v1 = self.desired_speed
-            else:
-                current_v1 = next_v
-
-            current_v1 = 0.0 if current_v1 < 0.0 else current_v1
-
-            # Update local positions based on velocities.
-            current_x1 += current_v1 * self.dt
-            current_y1 += self.v1[1] * self.dt
-            current_x2 += self.v2[0] * self.dt
-            current_y2 += self.v2[1] * self.dt
-
-        if is_displayed:
-            plt.ioff()
-            plt.show(block=True)
-
-        # print("Collision distance:", collision_distance)
-
-        return collision_distance
-
+from GEMstack.onboard.planning.pedestrian_yield_logic import CollisionDetector
 
 if __name__ == "__main__":
     # Vehicle parameters. x, y, theta (angle in radians)