feat: add raycasts

src/car.py

@@ -1,9 +1,10 @@
 from math import radians
+from typing import Optional
 
 import pygame
 
 from src.camera import Camera
-from src.utils import segments_intersect
+from src.utils import get_segments_intersection, segments_intersect
 from src.vec import Vec
 
 
@@ -12,14 +13,17 @@ sign = lambda x: 0 if x == 0 else (-1 if x < 0 else 1)
 
 class Car:
     MAX_SPEED = 5
-    MAX_BACK_SPEED = -2
+    MAX_BACK_SPEED = -3
     ROTATE_SPEED = 1
     COLOR = (230, 150, 80)
     WIDTH = 0.4
     LENGTH = 0.6
     COLLISION_MARGIN = 0.4
     ACCELERATION = 2
-    FRICTION = 3
+    FRICTION = 2.5
+    N_RAYS = 15
+    RAYS_FOV = 180
+    RAYS_MAX_DIST = 100
 
     def __init__(self, pos: Vec, direction: Vec) -> None:
         self.pos: Vec = pos
@@ -31,6 +35,9 @@ class Car:
         self.right: bool = False
         self.colliding: bool = False
 
+        self.rays: list[float] = [0] * self.N_RAYS
+        self.rays_end: list[Vec] = [Vec() for _ in range(self.N_RAYS)]
+
     def update(self, dt: float):
         if self.forward:
             self.speed += self.ACCELERATION * dt
@@ -53,15 +60,21 @@ class Car:
             self.direction = self.direction.rotate(rotate_angle)
 
         if not self.forward and not self.backward:
+            fn = max if self.speed >= 0 else min
             self.speed -= sign(self.speed) * self.FRICTION * dt
-            self.speed = max(0, self.speed)
+            self.speed = fn(0, self.speed)
 
         if abs(self.speed) < 1e-4:
             self.speed = 0
 
         self.pos += self.direction * self.speed * dt
 
-    def render(self, surf: pygame.Surface, camera: Camera):
+    def render(self, surf: pygame.Surface, camera: Camera, show_raycasts: bool = False):
+        if show_raycasts:
+            pos: Vec = camera.world2screen(self.pos)
+            for p in self.rays_end:
+                pygame.draw.line(surf, (255, 0, 0), pos, camera.world2screen(p), 2)
+
         pts: list[Vec] = self.get_corners()
         pts = [camera.world2screen(p) for p in pts]
         pygame.draw.polygon(surf, self.COLOR, pts)
@@ -77,6 +90,8 @@ class Car:
         return [p1, p2, p3, p4]
 
     def check_collisions(self, polygons: list[list[Vec]]):
+        self.cast_rays(polygons)
+
         self.colliding = False
         corners: list[Vec] = self.get_corners()
         sides: list[tuple[Vec, Vec]] = [
@@ -102,3 +117,35 @@ class Car:
                         self.speed = 0
                         self.pos = self.pos + n * (self.COLLISION_MARGIN - dist)
                         return
+
+    def cast_rays(self, polygons: list[list[Vec]]):
+        for i in range(self.N_RAYS):
+            angle: float = radians((i / (self.N_RAYS - 1) - 0.5) * self.RAYS_FOV)
+            p: Optional[Vec] = self.cast_ray(angle, polygons)
+            self.rays[i] = self.RAYS_MAX_DIST if p is None else (p - self.pos).mag()
+            self.rays_end[i] = self.pos if p is None else p
+
+    def cast_ray(self, angle: float, polygons: list[list[Vec]]) -> Optional[Vec]:
+        v: Vec = self.direction.normalized.rotate(angle)
+
+        segments: list[tuple[Vec, Vec]] = []
+        for polygon in polygons:
+            n_pts: int = len(polygon)
+            for i in range(n_pts):
+                pt1: Vec = polygon[i]
+                pt2: Vec = polygon[(i + 1) % n_pts]
+                segments.append((pt1, pt2))
+
+        p1: Vec = self.pos
+        p2: Vec = p1 + v * self.RAYS_MAX_DIST
+        dist: float = self.RAYS_MAX_DIST
+        closest: Optional[Vec] = None
+
+        for q1, q2 in segments:
+            p: Optional[Vec] = get_segments_intersection(p1, p2, q1, q2)
+            if p is not None:
+                d: float = (p - p1).mag()
+                if d < dist:
+                    dist = d
+                    closest = p
+        return closest

src/game.py

@@ -31,6 +31,7 @@ class Game:
         )
         self.show_fps: bool = True
         self.show_speed: bool = True
+        self.show_raycasts: bool = True
 
     def mainloop(self):
         while self.running:
@@ -64,7 +65,7 @@ class Game:
     def render(self):
         self.win.fill(self.BACKGROUND_COLOR)
         self.track.render(self.win, self.camera)
-        self.car.render(self.win, self.camera)
+        self.car.render(self.win, self.camera, self.show_raycasts)
         if self.show_fps:
             self.render_fps()
         if self.show_speed:
@@ -85,6 +86,8 @@ class Game:
             self.show_fps = not self.show_fps
         elif event.key == pygame.K_v:
             self.show_speed = not self.show_speed
+        elif event.key == pygame.K_c:
+            self.show_raycasts = not self.show_raycasts
 
     def on_key_up(self, event: pygame.event.Event):
         if event.key == pygame.K_w:

src/utils.py

@@ -1,9 +1,9 @@
 import os
 from pathlib import Path
+from typing import Optional
 
 from src.vec import Vec
 
-
 ROOT = Path(os.path.abspath(os.path.join(os.path.dirname(__file__), os.pardir)))
 
 
@@ -32,3 +32,30 @@ def segments_intersect(a1: Vec, a2: Vec, b1: Vec, b2: Vec) -> bool:
         return True
 
     return False
+
+
+def get_segments_intersection(a1: Vec, a2: Vec, b1: Vec, b2: Vec) -> Optional[Vec]:
+    da: Vec = a2 - a1
+    db: Vec = b2 - b1
+    dp: Vec = a1 - b1
+    dap: Vec = da.perp
+    denom: float = dap.dot(db)
+
+    if abs(denom) < 1e-9:
+        o1: float = da.cross(-dp)
+        if abs(o1) < 1e-9:
+            for p in [b1, b2]:
+                if p.within(a1, a2):
+                    return p
+            for p in [a1, a2]:
+                if p.within(b1, b2):
+                    return p
+            return None
+        return None
+
+    num: float = dap.dot(dp)
+    t: float = num / denom
+    intersection: Vec = b1 + db * t
+    if intersection.within(a1, a2) and intersection.within(b1, b2):
+        return intersection
+    return None

src/vec.py

@@ -24,6 +24,9 @@ class Vec:
     def __truediv__(self, value: float) -> Vec:
         return Vec(self.x / value, self.y / value)
 
+    def __neg__(self) -> Vec:
+        return Vec(-self.x, -self.y)
+
     def dot(self, other: Vec) -> float:
         return self.x * other.x + self.y * other.y