feat: add raycasts

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