Skip to content

Commit

Permalink
Merge branch 's2025_teamA' of https://github.com/krishauser/GEMstack …
Browse files Browse the repository at this point in the history 
…into s2025_teamA
  • Loading branch information
@AadarshHegde123
AadarshHegde123 committed Feb 27, 2025
2 parents a5af802 + 868325e commit aa02bb0
Showing 1 changed file with 1 addition and 206 deletions.
207 changes: 1 addition & 206 deletions testing/test_collision_detection.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit aa02bb0

Please sign in to comment.