1# cython: language_level=3
2# distutils: language = c++
3# Copyright (c) 2020, Manfred Moitzi
4# License: MIT License
5from typing import Iterable, TYPE_CHECKING, Sequence, Optional, Tuple
6from libc.math cimport fabs
7from .vector cimport Vec2, v2_isclose, Vec3, v3_from_cpp_vec3, isclose
8from ._cpp_vec3 cimport CppVec3
9
10import cython
11
12if TYPE_CHECKING:
13    from ezdxf.eztypes import Vertex
14
15DEF ABS_TOL = 1e-12
16DEF REL_TOL = 1e-9
17DEF TOLERANCE = 1e-10
18
19def has_clockwise_orientation(vertices: Iterable['Vertex']) -> bool:
20    """ Returns True if 2D `vertices` have clockwise orientation. Ignores
21    z-axis of all vertices.
22
23    Args:
24        vertices: iterable of :class:`Vec2` compatible objects
25
26    Raises:
27        ValueError: less than 3 vertices
28
29    """
30    cdef list _vertices = [Vec2(v) for v in vertices]
31    if len(_vertices) < 3:
32        raise ValueError('At least 3 vertices required.')
33
34    cdef Vec2 p1 = <Vec2> _vertices[0]
35    cdef Vec2 p2 = <Vec2> _vertices[-1]
36    cdef double s = 0.0
37    cdef Py_ssize_t index
38
39    # Using the same tolerance as the Python implementation:
40    if not v2_isclose(p1, p2, REL_TOL, ABS_TOL):
41        _vertices.append(p1)
42
43    for index in range(1, len(_vertices)):
44        p2 = <Vec2> _vertices[index]
45        s += (p2.x - p1.x) * (p2.y + p1.y)
46        p1 = p2
47    return s > 0.0
48
49def intersection_line_line_2d(
50        line1: Sequence[Vec2],
51        line2: Sequence[Vec2],
52        bint virtual=True,
53        double abs_tol=TOLERANCE) -> Optional[Vec2]:
54    """
55    Compute the intersection of two lines in the xy-plane.
56
57    Args:
58        line1: start- and end point of first line to test
59            e.g. ((x1, y1), (x2, y2)).
60        line2: start- and end point of second line to test
61            e.g. ((x3, y3), (x4, y4)).
62        virtual: ``True`` returns any intersection point, ``False`` returns
63            only real intersection points.
64        abs_tol: tolerance for intersection test.
65
66    Returns:
67        ``None`` if there is no intersection point (parallel lines) or
68        intersection point as :class:`Vec2`
69
70    """
71    # Sources:
72    # compas: https://github.com/compas-dev/compas/blob/master/src/compas/geometry/_core/intersections.py (MIT)
73    # wikipedia: https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
74    cdef Vec2 a, b, c, d, res = Vec2()
75    cdef double x1, y1, x2, y2, x3, y3, x4, y4, det
76    cdef double x1_x2, y3_y4, y1_y2, x3_x4, e, f
77    cdef bint in_range
78
79    a = line1[0]
80    b = line1[1]
81    c = line2[0]
82    d = line2[1]
83
84    x1 = a.x
85    y1 = a.y
86    x2 = b.x
87    y2 = b.y
88    x3 = c.x
89    y3 = c.y
90    x4 = d.x
91    y4 = d.y
92
93    x1_x2 = x1 - x2
94    y3_y4 = y3 - y4
95    y1_y2 = y1 - y2
96    x3_x4 = x3 - x4
97
98    det = x1_x2 * y3_y4 - y1_y2 * x3_x4
99
100    if fabs(det) <= abs_tol:
101        return None
102
103    e = x1 * y2 - y1 * x2
104    f = x3 * y4 - y3 * x4
105    # det near zero is checked by if-statement above:
106    with cython.cdivision(True):
107        x = (e * x3_x4 - x1_x2 * f) / det
108        y = (e * y3_y4 - y1_y2 * f) / det
109
110    if not virtual:
111        if x1 > x2:
112            in_range = (x2 - abs_tol) <= x <= (x1 + abs_tol)
113        else:
114            in_range = (x1 - abs_tol) <= x <= (x2 + abs_tol)
115
116        if not in_range:
117            return None
118
119        if x3 > x4:
120            in_range = (x4 - abs_tol) <= x <= (x3 + abs_tol)
121        else:
122            in_range = (x3 - abs_tol) <= x <= (x4 + abs_tol)
123
124        if not in_range:
125            return None
126
127        if y1 > y2:
128            in_range = (y2 - abs_tol) <= y <= (y1 + abs_tol)
129        else:
130            in_range = (y1 - abs_tol) <= y <= (y2 + abs_tol)
131
132        if not in_range:
133            return None
134
135        if y3 > y4:
136            in_range = (y4 - abs_tol) <= y <= (y3 + abs_tol)
137        else:
138            in_range = (y3 - abs_tol) <= y <= (y4 + abs_tol)
139
140        if not in_range:
141            return None
142
143    res.x = x
144    res.y = y
145    return res
146
147cdef double _determinant(CppVec3 v1, CppVec3 v2, CppVec3 v3):
148    return v1.x * v2.y * v3.z + v1.y * v2.z * v3.x + \
149           v1.z * v2.x * v3.y - v1.z * v2.y * v3.x - \
150           v1.x * v2.z * v3.y - v1.y * v2.x * v3.z
151
152def intersection_ray_ray_3d(ray1: Tuple[Vec3, Vec3],
153                            ray2: Tuple[Vec3, Vec3],
154                            double abs_tol=TOLERANCE) -> Sequence[Vec3]:
155    """
156    Calculate intersection of two 3D rays, returns a 0-tuple for parallel rays,
157    a 1-tuple for intersecting rays and a 2-tuple for not intersecting and not
158    parallel rays with points of closest approach on each ray.
159
160    Args:
161        ray1: first ray as tuple of two points as Vec3() objects
162        ray2: second ray as tuple of two points as Vec3() objects
163        abs_tol: absolute tolerance for comparisons
164
165    """
166    # source: http://www.realtimerendering.com/intersections.html#I304
167    cdef CppVec3 o2_o1
168    cdef double det1, det2
169    # Vec3() objects as input are not guaranteed, a hard <Vec3> cast could
170    # crash the interpreter for an invalid input!
171    cdef CppVec3 o1 = Vec3(ray1[0]).to_cpp_vec3()
172    cdef CppVec3 p1 = Vec3(ray1[1]).to_cpp_vec3()
173    cdef CppVec3 o2 = Vec3(ray2[0]).to_cpp_vec3()
174    cdef CppVec3 p2 = Vec3(ray2[1]).to_cpp_vec3()
175
176    cdef CppVec3 d1 = (p1 - o1).normalize(1.0)
177    cdef CppVec3 d2 = (p2 - o2).normalize(1.0)
178    cdef CppVec3 d1xd2 = d1.cross(d2)
179    cdef double denominator = d1xd2.magnitude_sqr()
180    if denominator <= abs_tol:
181        # ray1 is parallel to ray2
182        return tuple()
183    else:
184        o2_o1 = o2 - o1
185        det1 = _determinant(o2_o1, d2, d1xd2)
186        det2 = _determinant(o2_o1, d1, d1xd2)
187        with cython.cdivision(True):  # denominator check is already done
188            p1 = o1 + d1 * (det1 / denominator)
189            p2 = o2 + d2 * (det2 / denominator)
190
191        if p1.isclose(p2, abs_tol):
192            # ray1 and ray2 have an intersection point
193            return v3_from_cpp_vec3(p1),
194        else:
195            # ray1 and ray2 do not have an intersection point,
196            # p1 and p2 are the points of closest approach on each ray
197            return v3_from_cpp_vec3(p1), v3_from_cpp_vec3(p2)
198
199def arc_angle_span_deg(double start, double end) -> float:
200    if isclose(start, end, REL_TOL, ABS_TOL):
201        return 0.0
202
203    start %= 360.0
204    if isclose(start, end % 360.0, REL_TOL, ABS_TOL):
205        return 360.0
206
207    if not isclose(end, 360.0, REL_TOL, ABS_TOL):
208        end %= 360.0
209
210    if end < start:
211        end += 360.0
212    return end - start
213