diff --git a/src/tools/coordinate.py b/src/tools/coordinate.py index f552e22..1ebbc8e 100644 --- a/src/tools/coordinate.py +++ b/src/tools/coordinate.py @@ -1,6 +1,6 @@ from __future__ import annotations from enum import Enum -from math import gcd, sqrt, inf, atan2, degrees +from math import gcd, sqrt, inf, atan2, degrees, isclose from .math import round_half_up from typing import Union, List @@ -468,3 +468,48 @@ class Cube(Shape): if top_left.z is None or bottom_right.z is None: raise ValueError("Both Coordinates need to be 3D") super(Cube, self).__init__(top_left, bottom_right) + + +class Line: + def __init__(self, start: Coordinate, end: Coordinate): + if start[2] is not None or end[2] is not None: + raise NotImplementedError("3D Lines are hard(er)") + self.start = start + self.end = end + + def contains(self, point: Coordinate | tuple) -> bool: + return isclose( + self.start.getDistanceTo(self.end), + self.start.getDistanceTo(point) + self.end.getDistanceTo(point), + ) + + def intersects(self, other: Line, strict: bool = True) -> bool: + try: + self.get_intersection(other, strict=strict) + return True + except ValueError: + return False + + def get_intersection(self, other: Line, strict: bool = True) -> Coordinate: + xdiff = (self.start[0] - self.end[0], other.start[0] - other.end[0]) + ydiff = (self.start[1] - self.end[1], other.start[1] - other.end[1]) + + def det(a, b): + return a[0] * b[1] - a[1] * b[0] + + div = det(xdiff, ydiff) + if div == 0: + raise ValueError("lines do not intersect") + + d = (det(self.start, self.end), det(other.start, other.end)) + x = det(d, xdiff) / div + y = det(d, ydiff) / div + ret = Coordinate(x, y) + + if not strict: + return ret + else: + if self.contains(ret) and other.contains(ret): + return ret + else: + raise ValueError("intersection out of bounds")