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