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