ykcheungab
52 lines
2.0 KiB
Python
52 lines
2.0 KiB
Python
"""Physical-space clearance checks for capsule-shaped laser marks."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
|
|
def point_segment_distance_sq(point, start, end):
|
|
dx, dy = end[0] - start[0], end[1] - start[1]
|
|
length_sq = dx * dx + dy * dy
|
|
if length_sq == 0:
|
|
return (point[0] - start[0]) ** 2 + (point[1] - start[1]) ** 2
|
|
t = max(0.0, min(1.0, ((point[0] - start[0]) * dx + (point[1] - start[1]) * dy) / length_sq))
|
|
return (point[0] - start[0] - t * dx) ** 2 + (point[1] - start[1] - t * dy) ** 2
|
|
|
|
|
|
def segments_distance_sq(a0, a1, b0, b1):
|
|
"""Minimum squared distance, including a proper crossing at zero."""
|
|
def cross(p, q, r):
|
|
return (q[0] - p[0]) * (r[1] - p[1]) - (q[1] - p[1]) * (r[0] - p[0])
|
|
|
|
ab0, ab1 = cross(a0, a1, b0), cross(a0, a1, b1)
|
|
ba0, ba1 = cross(b0, b1, a0), cross(b0, b1, a1)
|
|
if ab0 * ab1 < 0 and ba0 * ba1 < 0:
|
|
return 0.0
|
|
return min(
|
|
point_segment_distance_sq(a0, b0, b1),
|
|
point_segment_distance_sq(a1, b0, b1),
|
|
point_segment_distance_sq(b0, a0, a1),
|
|
point_segment_distance_sq(b1, a0, a1),
|
|
)
|
|
|
|
|
|
def capsule_segment(cx, cy, half_length, angle):
|
|
dx, dy = half_length * math.cos(angle), half_length * math.sin(angle)
|
|
return (cx - dx, cy - dy), (cx + dx, cy + dy)
|
|
|
|
|
|
def has_clearance(candidate, accepted, clearance):
|
|
"""Capsules are (start, end, radius), all in the same physical units."""
|
|
start, end, radius = candidate
|
|
for other_start, other_end, other_radius in accepted:
|
|
limit = radius + other_radius + clearance
|
|
if (max(start[0], end[0]) + limit < min(other_start[0], other_end[0])
|
|
or max(other_start[0], other_end[0]) + limit < min(start[0], end[0])
|
|
or max(start[1], end[1]) + limit < min(other_start[1], other_end[1])
|
|
or max(other_start[1], other_end[1]) + limit < min(start[1], end[1])):
|
|
continue
|
|
if segments_distance_sq(start, end, other_start, other_end) < limit * limit:
|
|
return False
|
|
return True
|