62 lines
1.6 KiB
Python
62 lines
1.6 KiB
Python
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)))
|
|
|
|
|
|
def orientation(a: Vec, b: Vec, c: Vec) -> float:
|
|
return (b - a).cross(c - a)
|
|
|
|
|
|
def segments_intersect(a1: Vec, a2: Vec, b1: Vec, b2: Vec) -> bool:
|
|
o1 = orientation(a1, a2, b1)
|
|
o2 = orientation(a1, a2, b2)
|
|
o3 = orientation(b1, b2, a1)
|
|
o4 = orientation(b1, b2, a2)
|
|
|
|
# General case: segments straddle each other
|
|
if (o1 * o2 < 0) and (o3 * o4 < 0):
|
|
return True
|
|
|
|
# Special cases: Collinear overlaps
|
|
if o1 == 0 and b1.within(a1, a2):
|
|
return True
|
|
if o2 == 0 and b2.within(a1, a2):
|
|
return True
|
|
if o3 == 0 and a1.within(b1, b2):
|
|
return True
|
|
if o4 == 0 and a2.within(b1, b2):
|
|
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
|