1# Copyright (c) 2020-2021, Manfred Moitzi
2# License: MIT License
3from typing import Sequence, List, Iterable, Union, TYPE_CHECKING, Tuple
4from enum import IntEnum
5import math
6from ezdxf.math import Vec3, Vec2, Matrix44
7
8if TYPE_CHECKING:
9    from ezdxf.eztypes import Vertex
10
11__all__ = [
12    "is_planar_face", "subdivide_face", "subdivide_ngons", "Plane",
13    "LocationState", "normal_vector_3p", "distance_point_line_3d",
14    "basic_transformation", "best_fit_normal", "BarycentricCoordinates"
15]
16
17
18class LocationState(IntEnum):
19    COPLANAR = 0  # all the vertices are within the plane
20    FRONT = 1  # all the vertices are in front of the plane
21    BACK = 2  # all the vertices are at the back of the plane
22    SPANNING = 3  # some vertices are in front, some in the back
23
24
25def is_planar_face(face: Sequence[Vec3], abs_tol=1e-9) -> bool:
26    """ Returns ``True`` if sequence of vectors is a planar face.
27
28    Args:
29         face: sequence of :class:`~ezdxf.math.Vec3` objects
30         abs_tol: tolerance for normals check
31
32    """
33    if len(face) < 3:
34        return False
35    if len(face) == 3:
36        return True
37    first_normal = None
38    for index in range(len(face) - 2):
39        a, b, c = face[index:index + 3]
40        normal = (b - a).cross(c - b).normalize()
41        if first_normal is None:
42            first_normal = normal
43        elif not first_normal.isclose(normal, abs_tol=abs_tol):
44            return False
45    return True
46
47
48def subdivide_face(face: Sequence[Union[Vec3, Vec2]], quads=True) -> Iterable[
49    List[Vec3]]:
50    """ Yields new subdivided faces. Creates new faces from subdivided edges and the face midpoint by linear
51    interpolation.
52
53    Args:
54        face: a sequence of vertices, :class:`Vec2` and :class:`Vec3` objects supported.
55        quads: create quad faces if ``True`` else create triangles
56
57    """
58    if len(face) < 3:
59        raise ValueError('3 or more vertices required.')
60    len_face = len(face)
61    mid_pos = Vec3.sum(face) / len_face
62    subdiv_location = [face[i].lerp(face[(i + 1) % len_face]) for i in
63                       range(len_face)]
64
65    for index, vertex in enumerate(face):
66        if quads:
67            yield vertex, subdiv_location[index], mid_pos, subdiv_location[
68                index - 1]
69        else:
70            yield subdiv_location[index - 1], vertex, mid_pos
71            yield vertex, subdiv_location[index], mid_pos
72
73
74def subdivide_ngons(faces: Iterable[Sequence[Union[Vec3, Vec2]]]) -> Iterable[
75    List[Vec3]]:
76    """ Yields only triangles or quad faces, subdivides ngons into triangles.
77
78    Args:
79        faces: iterable of faces as sequence of :class:`Vec2` and
80            :class:`Vec3` objects
81
82    """
83    for face in faces:
84        if len(face) < 5:
85            yield face
86        else:
87            mid_pos = Vec3.sum(face) / len(face)
88            for index, vertex in enumerate(face):
89                yield face[index - 1], vertex, mid_pos
90
91
92def normal_vector_3p(a: Vec3, b: Vec3, c: Vec3) -> Vec3:
93    """ Returns normal vector for 3 points, which is the normalized cross
94    product for: :code:`a->b x a->c`.
95    """
96    return (b - a).cross(c - a).normalize()
97
98
99def best_fit_normal(vertices: Iterable['Vertex']) -> Vec3:
100    """ Returns the "best fit" normal for a plane defined by three or more
101    vertices. This function tolerates imperfect plane vertices. Safe function
102    to detect the extrusion vector of flat arbitrary polygons.
103
104    """
105    # Source: https://gamemath.com/book/geomprims.html#plane_best_fit (9.5.3)
106    vertices = Vec3.list(vertices)
107    if len(vertices) < 3:
108        raise ValueError("3 or more vertices required")
109    first = vertices[0]
110    if not first.isclose(vertices[-1]):
111        vertices.append(first)  # close polygon
112    prev_x, prev_y, prev_z = first.xyz
113    nx = 0.0
114    ny = 0.0
115    nz = 0.0
116    for v in vertices[1:]:
117        x, y, z = v.xyz
118        nx += (prev_z + z) * (prev_y - y)
119        ny += (prev_x + x) * (prev_z - z)
120        nz += (prev_y + y) * (prev_x - x)
121        prev_x = x
122        prev_y = y
123        prev_z = z
124    return Vec3(nx, ny, nz).normalize()
125
126
127def distance_point_line_3d(point: Vec3, start: Vec3, end: Vec3) -> float:
128    """ Returns the normal distance from `point` to 3D line defined by `start-`
129    and `end` point.
130    """
131    if start.isclose(end):
132        raise ZeroDivisionError('Not a line.')
133    v1 = point - start
134    # point projected onto line start to end:
135    v2 = (end - start).project(v1)
136    # Pythagoras:
137    diff = v1.magnitude_square - v2.magnitude_square
138    if diff <= 0.0:
139        # This should not happen (abs(v1) > abs(v2)), but floating point
140        # imprecision at very small values makes it possible!
141        return 0.0
142    else:
143        return math.sqrt(diff)
144
145
146def basic_transformation(
147        move: 'Vertex' = (0, 0, 0),
148        scale: 'Vertex' = (1, 1, 1),
149        z_rotation: float = 0) -> Matrix44:
150    """ Returns a combined transformation matrix for translation, scaling and
151    rotation about the z-axis.
152
153    Args:
154        move: translation vector
155        scale: x-, y- and z-axis scaling as float triplet, e.g. (2, 2, 1)
156        z_rotation: rotation angle about the z-axis in radians
157
158    """
159    sx, sy, sz = Vec3(scale)
160    m = Matrix44.scale(sx, sy, sz)
161    if z_rotation:
162        m *= Matrix44.z_rotate(z_rotation)
163    translate = Vec3(move)
164    if not translate.is_null:
165        m *= Matrix44.translate(translate.x, translate.y, translate.z)
166    return m
167
168
169class Plane:
170    """ Represents a plane in 3D space as normal vector and the perpendicular
171    distance from origin.
172    """
173    __slots__ = ('_normal', '_distance_from_origin')
174
175    def __init__(self, normal: Vec3, distance: float):
176        self._normal = normal
177        # the (perpendicular) distance of the plane from (0, 0, 0)
178        self._distance_from_origin = distance
179
180    @property
181    def normal(self) -> Vec3:
182        """ Normal vector of the plane. """
183        return self._normal
184
185    @property
186    def distance_from_origin(self) -> float:
187        """ The (perpendicular) distance of the plane from origin (0, 0, 0). """
188        return self._distance_from_origin
189
190    @property
191    def vector(self) -> Vec3:
192        """ Returns the location vector. """
193        return self._normal * self._distance_from_origin
194
195    @classmethod
196    def from_3p(cls, a: Vec3, b: Vec3, c: Vec3) -> 'Plane':
197        """ Returns a new plane from 3 points in space. """
198        n = (b - a).cross(c - a).normalize()
199        return Plane(n, n.dot(a))
200
201    @classmethod
202    def from_vector(cls, vector) -> 'Plane':
203        """ Returns a new plane from a location vector. """
204        v = Vec3(vector)
205        return Plane(v.normalize(), v.magnitude)
206
207    def __copy__(self) -> 'Plane':
208        """ Returns a copy of the plane. """
209        return self.__class__(self._normal, self._distance_from_origin)
210
211    copy = __copy__
212
213    def __repr__(self):
214        return f'Plane({repr(self._normal)}, {self._distance_from_origin})'
215
216    def __eq__(self, other: 'Plane'):
217        if isinstance(other, Plane):
218            return self.vector == other.vector
219        else:
220            raise TypeError
221
222    def signed_distance_to(self, v: Vec3) -> float:
223        """ Returns signed distance of vertex `v` to plane, if distance is > 0, `v` is in 'front' of plane, in direction
224        of the normal vector, if distance is < 0, `v` is at the 'back' of the plane, in the opposite direction of
225        the normal vector.
226
227        """
228        return self._normal.dot(v) - self._distance_from_origin
229
230    def distance_to(self, v: Vec3) -> float:
231        """ Returns absolute (unsigned) distance of vertex `v` to plane. """
232        return math.fabs(self.signed_distance_to(v))
233
234    def is_coplanar_vertex(self, v: Vec3, abs_tol=1e-9) -> bool:
235        """ Returns ``True`` if vertex `v` is coplanar, distance from plane to vertex `v` is 0. """
236        return self.distance_to(v) < abs_tol
237
238    def is_coplanar_plane(self, p: 'Plane', abs_tol=1e-9) -> bool:
239        """ Returns ``True`` if plane `p` is coplanar, normal vectors in same or opposite direction. """
240        n_is_close = self._normal.isclose
241        return n_is_close(
242            p._normal, abs_tol=abs_tol) or n_is_close(
243            -p._normal, abs_tol=abs_tol)
244
245
246class BarycentricCoordinates:
247    """ Barycentric coordinate calculation.
248
249    The arguments `a`, `b` and `c` are the cartesian coordinates of an arbitrary
250    triangle in 3D space. The barycentric coordinates (b1, b2, b3) define the
251    linear combination of `a`, `b` and `c` to represent the point `p`::
252
253        p = a * b1 + b * b2 + c * b3
254
255    This implementation returns the barycentric coordinates of the normal
256    projection of `p` onto the plane defined by (a, b, c).
257
258    These barycentric coordinates have some useful properties:
259
260    - if all barycentric coordinates (b1, b2, b3) are in the range [0, 1], then
261      the point `p` is inside the triangle (a, b, c)
262    - if one of the coordinates is negative, the point `p` is outside the
263      triangle
264    - the sum of b1, b2 and b3 is always 1
265    - the center of "mass" has the barycentric coordinates (1/3, 1/3, 1/3) =
266      (a + b + c)/3
267
268    """
269
270    # Source: https://gamemath.com/book/geomprims.html#triangle_barycentric_space
271
272    def __init__(self, a: 'Vertex', b: 'Vertex', c: 'Vertex'):
273        self.a = Vec3(a)
274        self.b = Vec3(b)
275        self.c = Vec3(c)
276        self._e1 = self.c - self.b
277        self._e2 = self.a - self.c
278        self._e3 = self.b - self.a
279        e1xe2 = self._e1.cross(self._e2)
280        self._n = e1xe2.normalize()
281        self._denom = e1xe2.dot(self._n)
282        if abs(self._denom) < 1e-9:
283            raise ValueError('invalid triangle')
284
285    def from_cartesian(self, p: 'Vertex') -> Vec3:
286        p = Vec3(p)
287        n = self._n
288        denom = self._denom
289        d1 = p - self.a
290        d2 = p - self.b
291        d3 = p - self.c
292        b1 = self._e1.cross(d3).dot(n) / denom
293        b2 = self._e2.cross(d1).dot(n) / denom
294        b3 = self._e3.cross(d2).dot(n) / denom
295        return Vec3(b1, b2, b3)
296
297    def to_cartesian(self, b: 'Vertex') -> Vec3:
298        b1, b2, b3 = Vec3(b).xyz
299        return self.a * b1 + self.b * b2 + self.c * b3
300