Mercurial > ldcheck / file diff

--- a/geometry.py	Sun Jan 21 15:03:38 2018 +0200
+++ b/geometry.py	Mon Jan 22 12:22:01 2018 +0200
@@ -318,6 +318,16 @@
             sum(self[i, k] * other[k, j] for k in range(3))
             for i, j in matrix_indices()
         ])
+    def __add__(self, other):
+        return Matrix3x3([
+            a + b
+            for a, b in zip(self.values, other.values)
+        ])
+    def __mul__(self, scalar):
+        return Matrix3x3([
+            x * scalar
+            for x in self.values
+        ])
 
 def complete_matrix(matrix, anchor):
     '''
@@ -349,3 +359,58 @@
             + transformation_matrix[10] * vertex.z \
             + transformation_matrix[11],
     )
+
+def transform_to_xy(polygon):
+    '''
+        Transforms the provided polygon into the XY plane. The polygon is
+        assumed to be planar.
+
+        Implementation is based on this StackOverflow answer:
+        https://math.stackexchange.com/a/476311
+    '''
+    v1, v2, v3 = polygon.vertices[:3]
+    a, b = v3 - v2, v1 - v2
+    normal = cross_product(a, b).normalized()
+    v = cross_product(normal, Vector(0, 0, 1))
+    cosine = dot_product(normal, Vector(0, 0, 1))
+    v_matrix = Matrix3x3([
+        0, -v.z, v.y,
+        v.z, 0, -v.x,
+        -v.y, v.x, 0,
+    ])
+    try:
+        rotation_matrix = Matrix3x3()
+        rotation_matrix += v_matrix
+        rotation_matrix += (v_matrix @ v_matrix) * (1 / (1 + cosine))
+    except ZeroDivisionError:
+        rotation_matrix = Matrix3x3()
+    full_matrix = complete_matrix(rotation_matrix, Vertex(0, 0, 0))
+    return Polygon([
+        transform(vertex = vertex, transformation_matrix = full_matrix)
+        for vertex in polygon.vertices
+    ])
+
+def vector_angle(vec_1, vec_2, normalized = False):
+    from math import acos, pi as π
+    cosine = dot_product(vec_1, vec_2)
+    try:
+        cosine /= vec_1.length() * vec_2.length()
+    except ZeroDivisionError:
+        return 0
+    angle = acos(cosine)
+    if normalized and angle > π / 2:
+        angle = π - angle
+    return angle
+
+def split_quadrilateral(polygon):
+    assert(len(polygon.vertices) == 4)
+    vertices = IndexRing(polygon.vertices)
+    for i in (0, 1):
+        a, b = vertices[0 + i], vertices[1 + i]
+        c, d = vertices[2 + i], vertices[3 + i]
+        yield Polygon([a, b, c]), Polygon([b, c, d])
+
+def triangle_plane_normal(polygon):
+    assert(len(polygon.vertices) == 3)
+    a, b, c = polygon.vertices[:3]
+    return cross_product(b - a, b - c)