Mercurial > ldcheck / file revision

def degree_rep(angle):
    from math import degrees
    try:
        return '%.2f°' % degrees(angle)
    except TypeError:
        return angle

def position_vector(vertex):
    assert isinstance(vertex, Vertex)
    return Vector(*vertex.coordinates)

def angle_magnitude_key(angle):
    '''
        Returns how great an angle is.
    '''
    from math import pi as π
    normalized_angle = abs(angle) % (2 * π)
    if normalized_angle > π:
        normalized_angle = (2 * π) - normalized_angle
    return normalized_angle

class Vertex:
    def __init__(self, x, y, z):
        if not all(is_real(coordinate) for coordinate in (x, y, z)):
            raise ValueError(str.format('Bad vertex coordinates: {!r}',
                (x, y, z),
            ))
        self.x, self.y, self.z = x, y, z
    def __repr__(self):
        return str.format('Vertex({!r}, {!r}, {!r})', self.x, self.y, self.z)
    def distance_to(self, other):
        # can't use hypot because of 3 arguments
        from math import sqrt
        return sqrt(
            (self.x - other.x) ** 2 +
            (self.y - other.y) ** 2 +
            (self.z - other.z) ** 2
        )
    @property
    def coordinates(self):
        return self.x, self.y, self.z
    def __add__(self, other):
        assert not (type(self) == type(other) == Vertex)
        if type(self) == Vector and type(other) == Vector:
            result_type = Vector
        else:
            result_type = Vertex
        return result_type(self.x + other.x, self.y + other.y, self.z + other.z)
    def __radd__(self, other):
        return self + other
    def __neg__(self):
        return type(self)(-self.x, -self.y, -self.z)
    def __sub__(self, other):
        result = self + (-position_vector(other))
        if isinstance(other, Vertex):
            return Vector(*result.coordinates)
        else:
            return result
    def __mul__(self, scalar):
        assert is_real(scalar)
        return type(self)(self.x * scalar, self.y * scalar, self.z * scalar)
    def __rmul__(self, other):
        return self * other
    def __truediv__(self, scalar):
        assert is_real(scalar)
        return type(self)(self.x / scalar, self.y / scalar, self.z / scalar)
    def __floordiv__(self, scalar):
        assert is_real(scalar)
        return type(self)(self.x // scalar, self.y // scalar, self.z // scalar)
    def __matmul__(self, transformation_matrix):
        return transform(self, transformation_matrix)
    def __eq__(self, other):
        return self.is_close(other, threshold = 1.0e-10)
    def __ne__(self, other):
        return not self.__eq__(other)
    def __lt__(self, other):
        return self.coordinates < other.coordinates
    def __hash__(self):
        return hash(self.coordinates)
    def is_close(self, other, *, threshold = 1.0e-05):
        return self.distance_to(other) < threshold
        #return all(
        #    abs(a - b) < threshold
        #    for a, b in zip(self.coordinates, other.coordinates)
        #)

class LineSegment:
    def __init__(self, v1, v2):
        self.v1, self.v2 = v1, v2
    def __repr__(self):
        return str.format('LineSegment({!r}, {!r})', self.v1, self.v2)
    @property
    def length(self):
        return self.v1.distance_to(self.v2)
    @property
    def vertices(self):
        return self.v1, self.v2
    def __len__(self):
        return 2
    def __getitem__(self, index):
        return self.vertices[index]

class IndexRing:
    def __init__(self, container):
        self.container = container
    def __getitem__(self, index):
        return self.container[index % len(self.container)]

class Vector(Vertex):
    def __init__(self, x, y, z):
        super().__init__(x, y, z)
    def __repr__(self):
        return str.format('Vector({!r}, {!r}, {!r})', self.x, self.y, self.z)
    def length(self):
        from math import sqrt
        return sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
    def normalized(self):
        length = self.length()
        if length > 0:
            return Vector(self.x / length, self.y / length, self.z / length)
        else:
            return Vector(0, 0, 0)

def dot_product(v1, v2):
    ''' Returns the dot product v1 · v2. '''
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z

def cross_product(v1, v2):
    ''' Returns the cross product v1 × v2. '''
    return Vector(
        x = v1.y * v2.z - v1.z * v2.y,
        y = v1.z * v2.x - v1.x * v2.z,
        z = v1.x * v2.y - v1.y * v2.x,
    )

def unit_normal(a, b, c):
    x = Matrix3x3([
        1, a.y, a.z,
        1, b.y, b.z,
        1, c.y, c.z,
    ]).determinant()
    y = Matrix3x3([
        a.x, 1, a.z,
        b.x, 1, b.z,
        c.x, 1, c.z,
    ]).determinant()
    z = Matrix3x3([
        a.x, a.y, 1,
        b.x, b.y, 1,
        c.x, c.y, 1,
    ]).determinant()
    return Vector(x, y, z).normalized()

class NoIntersection(Exception):
    pass

def pairs(iterable, *, count = 2):
    '''
        Iterates the given iterable and returns tuples containing `count`
        sequential values in the iterable.

        Formally, for a vector A with indices 0…n and pair count k, this
        function yields:
            (A₀, A₁, …, Aₖ),
            (A₁, A₂, …, Aₖ₊₁),
            …,
            (Aₙ₋₂, Aₙ₋₁, …, Aₖ₋₂),
            (Aₙ₋₁, A₀, …, Aₖ₋₁).
    '''
    iterable_count = len(iterable)
    for index in range(iterable_count):
        yield tuple(
            iterable[(index + offset) % iterable_count]
            for offset in range(count)
        )

class Polygon:
    def __init__(self, vertices):
        self.vertices = vertices
    def __repr__(self):
        return str.format('Polygon({!r})', self.vertices)
    def area(self):
        total = Vector(0, 0, 0)
        for v1, v2 in pairs(self.vertices):
            total += cross_product(v1, v2)
        return dot_product(total, unit_normal(self.vertices[0], self.vertices[1], self.vertices[2]))
    def centroid(self):
        ...
        raise NotImplementedError
    def __getitem__(self, index):
        return self.vertices[index % len(self.vertices)]
    def __len__(self):
        return len(self.vertices)
    @property
    def perimeter_lines(self):
        for v1, v2 in pairs(self.vertices):
            yield LineSegment(v1, v2)
    @property
    def angles(self):
        from math import acos
        for v1, v2, v3 in pairs(self.vertices, count = 3):
            vec1 = (position_vector(v3) - position_vector(v2)).normalized()
            vec2 = (position_vector(v1) - position_vector(v2)).normalized()
            cosine = dot_product(vec1, vec2) / vec1.length() / vec2.length()
            yield acos(cosine)
    @property
    def smallest_angle(self):
        return min(
            angle_magnitude_key(angle)
            for angle in self.angles)
    @property
    def hairline_ratio(self):
        lengths = [line.length for line in self.perimeter_lines]
        return max(lengths) / min(lengths)

def is_real(number):
    return isinstance(number, int) or isinstance(number, float)

def matrix_indices():
    '''
        Yields index pairs for matrix iteration
    '''
    from itertools import product
    yield from product(range(3), range(3))

class Matrix3x3:
    '''
        A 3×3 matrix forming the top-left portion of a full 4×4 transformation
        matrix.
    '''
    class Row:
        def __init__(self, matrix_ref, i1, i2, i3):
            self.matrix_ref = matrix_ref
            self.matrix_indices = i1, i2, i3
        def __getitem__(self, row_index):
            return self.matrix_ref[self.matrix_indices[row_index]]
        def __setitem__(self, row_index, value):
            self.matrix_ref[self.matrix_indices[row_index]] = value
    def __init__(self, values = None):
        if values is not None:
            assert(all(is_real(x) for x in values))
            assert len(values) == 9
            self.values = values
        else:
            self.values = [1, 0, 0, 0, 1, 0, 0, 0, 1]
    def __repr__(self):
        if self != Matrix3x3():
            return str.format('Matrix3x3({!r})', self.values)
        else:
            return 'Matrix3x3()'
    def __getitem__(self, index):
        if isinstance(index, tuple):
            assert all(k in [0, 1, 2] for k in index)
            return self.values[index[0] * 3 + index[1]]
        else:
            return Matrix3x3.Row(self, index * 3, index * 3 + 1, index * 3 + 2)
    def determinant(self):
        return sum([
            +(self[0, 0] * self[1, 1] * self[2, 2]),
            +(self[0, 1] * self[1, 2] * self[2, 0]),
            +(self[0, 2] * self[1, 0] * self[2, 1]),
            -(self[0, 2] * self[1, 1] * self[2, 0]),
            -(self[0, 1] * self[1, 0] * self[2, 2]),
            -(self[0, 0] * self[1, 2] * self[2, 1]),
        ])
    def scaling_vector(self):
        '''
            Extracts scaling factors from this transformation matrix.
        '''
        from math import sqrt
        return Vector(
            x = sqrt(self[0, 0] ** 2 + self[1, 0] ** 2 + self[2, 0] ** 2),
            y = sqrt(self[0, 1] ** 2 + self[1, 1] ** 2 + self[2, 1] ** 2),
            z = sqrt(self[0, 2] ** 2 + self[1, 2] ** 2 + self[2, 2] ** 2),
        )
    def rotation_component(self):
        '''
            Extracts rotation from this matrix.
        '''
        return self.split()[0]
    def split(self):
        '''
            Extracts the rotation matrix and scaling vector.
        '''
        vec = self.scaling_vector()
        return Matrix3x3([
            self[i, j] / vec.coordinates[j]
            for i, j in matrix_indices()
        ]), vec
    def contains_scaling(self):
        '''
            Returns whether this matrix contains scaling factors.
        '''
        vec = self.scaling_vector()
        return abs((vec.x * vec.y * vec.z) - 1) >= 1e-10
    def contains_rotation(self):
        '''
            Returns whether this matrix contains rotation factors.
        '''
        return self.rotation_component() != Matrix3x3()
    def __eq__(self, other):
        '''
            Returns whether two matrices are equivalent.
        '''
        return all(
            abs(self[(i, j)] - other[(i, j)]) < 1e-10
            for i, j in matrix_indices()
        )
    def __matmul__(self, other):
        '''
            Computes the matrix multiplication self × other.
        '''
        return Matrix3x3([
            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):
    '''
        Combines a 3×3 matrix and an anchor vertex into a full 4×4
        transformation matrix.
    '''
    return [
        matrix[0, 0], matrix[0, 1], matrix[0, 2], anchor.x,
        matrix[1, 0], matrix[1, 1], matrix[1, 2], anchor.y,
        matrix[2, 0], matrix[2, 1], matrix[2, 2], anchor.z,
        0, 0, 0, 1,
    ]

def transform(vertex, transformation_matrix):
    '''
        Transforms a vertex by a 4×4 transformation matrix.
    '''
    return Vertex(
        x = transformation_matrix[0] * vertex.x \
            + transformation_matrix[1] * vertex.y \
            + transformation_matrix[2] * vertex.z \
            + transformation_matrix[3],
        y = transformation_matrix[4] * vertex.x \
            + transformation_matrix[5] * vertex.y \
            + transformation_matrix[6] * vertex.z \
            + transformation_matrix[7],
        z = transformation_matrix[8] * vertex.x \
            + transformation_matrix[9] * vertex.y \
            + 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_matrix = Matrix3x3([
        0, 0, -normal.x,
        0, 0, -normal.y,
        normal.x, normal.y, 0,
    ])
    try:
        rotation_matrix = Matrix3x3()
        rotation_matrix += v_matrix
        rotation_matrix += (v_matrix @ v_matrix) * (1 / (1 + normal.z))
    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
    # Fix the cosine, it can go outside bounds due to rounding errors,
    # e.g. 1.0000000000000002, which then causes a math domain error.
    cosine = min(max(-1, cosine), 1)
    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)

def line_intersection_xy(line_1, line_2):
    '''
        Computes 2D line intersection. Can return a point outside the given
        segments. Z is ignored.
    '''
    a, b = line_1.vertices
    c, d = line_2.vertices
    denominator = (a.x - b.x) * (c.y - d.y) - (a.y - b.y) * (c.x - d.x)
    x = (c.x - d.x) * (a.x * b.y - a.y * b.x)
    x -= (a.x - b.x) * (c.x * d.y - c.y * d.x)
    y = (c.y - d.y) * (a.x * b.y - a.y * b.x)
    y -= (a.y - b.y) * (c.x * d.y - c.y * d.x)
    try:
        x /= denominator
        y /= denominator
    except ZeroDivisionError:
        return None
    else:
        return Vertex(x, y, 0)

def line_segment_intersection_xy(line_1, line_2):
    '''
        Computes 2D line intersection. Only returns a result if it falls
        inside the line segments. Z is ignored.
    '''
    from math import inf
    intersection = line_intersection_xy(line_1, line_2)
    if intersection:
        a, b = line_1.vertices
        c, d = line_2.vertices
        try:
            t1 = (intersection.x - a.x) / (b.x - a.x)
        except ZeroDivisionError:
            t1 = inf
        try:
            t2 = (intersection.x - c.x) / (d.x - c.x)
        except ZeroDivisionError:
            t2 = inf
        try:
            u1 = (intersection.y - a.y) / (b.y - a.y)
        except ZeroDivisionError:
            u1 = inf
        try:
            u2 = (intersection.y - c.y) / (d.y - c.y)
        except ZeroDivisionError:
            u2 = inf
        if (0 < t1 < 1 and 0 < t2 < 1) or (0 < u1 < 1 and 0 < u2 < 1):
            return intersection
        else:
            return None
    else:
        return None