123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504 |
- from __future__ import annotations
- from functools import reduce
- import math
- import operator as op
- import platform
- from mapbox_earcut import triangulate_float32 as earcut
- import numpy as np
- from scipy.spatial.transform import Rotation
- from tqdm.auto import tqdm as ProgressDisplay
- from manimlib.constants import DOWN, OUT, RIGHT, UP
- from manimlib.constants import PI, TAU
- from manimlib.utils.iterables import adjacent_pairs
- from manimlib.utils.simple_functions import clip
- from typing import TYPE_CHECKING
- if TYPE_CHECKING:
- from typing import Callable, Sequence, List, Tuple
- from manimlib.typing import Vect2, Vect3, Vect4, VectN, Matrix3x3, Vect3Array, Vect2Array
- def cross(
- v1: Vect3 | List[float],
- v2: Vect3 | List[float],
- out: np.ndarray | None = None
- ) -> Vect3 | Vect3Array:
- is2d = isinstance(v1, np.ndarray) and len(v1.shape) == 2
- if is2d:
- x1, y1, z1 = v1[:, 0], v1[:, 1], v1[:, 2]
- x2, y2, z2 = v2[:, 0], v2[:, 1], v2[:, 2]
- else:
- x1, y1, z1 = v1
- x2, y2, z2 = v2
- if out is None:
- out = np.empty(np.shape(v1))
- out.T[:] = [
- y1 * z2 - z1 * y2,
- z1 * x2 - x1 * z2,
- x1 * y2 - y1 * x2,
- ]
- return out
- def get_norm(vect: VectN | List[float]) -> float:
- return sum((x**2 for x in vect))**0.5
- def normalize(
- vect: VectN | List[float],
- fall_back: VectN | List[float] | None = None
- ) -> VectN:
- norm = get_norm(vect)
- if norm > 0:
- return np.array(vect) / norm
- elif fall_back is not None:
- return np.array(fall_back)
- else:
- return np.zeros(len(vect))
- def poly_line_length(points):
- """
- Return the sum of the lengths between adjacent points
- """
- diffs = points[1:] - points[:-1]
- return np.sqrt((diffs**2).sum(1)).sum()
- # Operations related to rotation
- def quaternion_mult(*quats: Vect4) -> Vect4:
- """
- Inputs are treated as quaternions, where the real part is the
- last entry, so as to follow the scipy Rotation conventions.
- """
- if len(quats) == 0:
- return np.array([0, 0, 0, 1])
- result = np.array(quats[0])
- for next_quat in quats[1:]:
- x1, y1, z1, w1 = result
- x2, y2, z2, w2 = next_quat
- result[:] = [
- w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
- w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2,
- w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2,
- w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
- ]
- return result
- def quaternion_from_angle_axis(
- angle: float,
- axis: Vect3,
- ) -> Vect4:
- return Rotation.from_rotvec(angle * normalize(axis)).as_quat()
- def angle_axis_from_quaternion(quat: Vect4) -> Tuple[float, Vect3]:
- rot_vec = Rotation.from_quat(quat).as_rotvec()
- norm = get_norm(rot_vec)
- return norm, rot_vec / norm
- def quaternion_conjugate(quaternion: Vect4) -> Vect4:
- result = np.array(quaternion)
- result[:3] *= -1
- return result
- def rotate_vector(
- vector: Vect3,
- angle: float,
- axis: Vect3 = OUT
- ) -> Vect3:
- rot = Rotation.from_rotvec(angle * normalize(axis))
- return np.dot(vector, rot.as_matrix().T)
- def rotate_vector_2d(vector: Vect2, angle: float) -> Vect2:
- # Use complex numbers...because why not
- z = complex(*vector) * np.exp(complex(0, angle))
- return np.array([z.real, z.imag])
- def rotation_matrix_transpose_from_quaternion(quat: Vect4) -> Matrix3x3:
- return Rotation.from_quat(quat).as_matrix()
- def rotation_matrix_from_quaternion(quat: Vect4) -> Matrix3x3:
- return np.transpose(rotation_matrix_transpose_from_quaternion(quat))
- def rotation_matrix(angle: float, axis: Vect3) -> Matrix3x3:
- """
- Rotation in R^3 about a specified axis of rotation.
- """
- return Rotation.from_rotvec(angle * normalize(axis)).as_matrix()
- def rotation_matrix_transpose(angle: float, axis: Vect3) -> Matrix3x3:
- return rotation_matrix(angle, axis).T
- def rotation_about_z(angle: float) -> Matrix3x3:
- cos_a = math.cos(angle)
- sin_a = math.sin(angle)
- return np.array([
- [cos_a, -sin_a, 0],
- [sin_a, cos_a, 0],
- [0, 0, 1]
- ])
- def rotation_between_vectors(v1: Vect3, v2: Vect3) -> Matrix3x3:
- atol = 1e-8
- if get_norm(v1 - v2) < atol:
- return np.identity(3)
- axis = cross(v1, v2)
- if get_norm(axis) < atol:
- # v1 and v2 align
- axis = cross(v1, RIGHT)
- if get_norm(axis) < atol:
- # v1 and v2 _and_ RIGHT all align
- axis = cross(v1, UP)
- return rotation_matrix(
- angle=angle_between_vectors(v1, v2),
- axis=axis,
- )
- def z_to_vector(vector: Vect3) -> Matrix3x3:
- return rotation_between_vectors(OUT, vector)
- def angle_of_vector(vector: Vect2 | Vect3) -> float:
- """
- Returns polar coordinate theta when vector is project on xy plane
- """
- return math.atan2(vector[1], vector[0])
- def angle_between_vectors(v1: VectN, v2: VectN) -> float:
- """
- Returns the angle between two 3D vectors.
- This angle will always be btw 0 and pi
- """
- n1 = get_norm(v1)
- n2 = get_norm(v2)
- if n1 == 0 or n2 == 0:
- return 0
- cos_angle = np.dot(v1, v2) / np.float64(n1 * n2)
- return math.acos(clip(cos_angle, -1, 1))
- def project_along_vector(point: Vect3, vector: Vect3) -> Vect3:
- matrix = np.identity(3) - np.outer(vector, vector)
- return np.dot(point, matrix.T)
- def normalize_along_axis(
- array: np.ndarray,
- axis: int,
- ) -> np.ndarray:
- norms = np.sqrt((array * array).sum(axis))
- norms[norms == 0] = 1
- return array / norms[:, np.newaxis]
- def get_unit_normal(
- v1: Vect3,
- v2: Vect3,
- tol: float = 1e-6
- ) -> Vect3:
- v1 = normalize(v1)
- v2 = normalize(v2)
- cp = cross(v1, v2)
- cp_norm = get_norm(cp)
- if cp_norm < tol:
- # Vectors align, so find a normal to them in the plane shared with the z-axis
- new_cp = cross(cross(v1, OUT), v1)
- new_cp_norm = get_norm(new_cp)
- if new_cp_norm < tol:
- return DOWN
- return new_cp / new_cp_norm
- return cp / cp_norm
- ###
- def thick_diagonal(dim: int, thickness: int = 2) -> np.ndarray:
- row_indices = np.arange(dim).repeat(dim).reshape((dim, dim))
- col_indices = np.transpose(row_indices)
- return (np.abs(row_indices - col_indices) < thickness).astype('uint8')
- def compass_directions(n: int = 4, start_vect: Vect3 = RIGHT) -> Vect3:
- angle = TAU / n
- return np.array([
- rotate_vector(start_vect, k * angle)
- for k in range(n)
- ])
- def complex_to_R3(complex_num: complex) -> Vect3:
- return np.array((complex_num.real, complex_num.imag, 0))
- def R3_to_complex(point: Vect3) -> complex:
- return complex(*point[:2])
- def complex_func_to_R3_func(complex_func: Callable[[complex], complex]) -> Callable[[Vect3], Vect3]:
- def result(p: Vect3):
- return complex_to_R3(complex_func(R3_to_complex(p)))
- return result
- def center_of_mass(points: Sequence[Vect3]) -> Vect3:
- return np.array(points).sum(0) / len(points)
- def midpoint(point1: VectN, point2: VectN) -> VectN:
- return center_of_mass([point1, point2])
- def line_intersection(
- line1: Tuple[Vect3, Vect3],
- line2: Tuple[Vect3, Vect3]
- ) -> Vect3:
- """
- return intersection point of two lines,
- each defined with a pair of vectors determining
- the end points
- """
- x_diff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])
- y_diff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])
- def det(a, b):
- return a[0] * b[1] - a[1] * b[0]
- div = det(x_diff, y_diff)
- if div == 0:
- raise Exception("Lines do not intersect")
- d = (det(*line1), det(*line2))
- x = det(d, x_diff) / div
- y = det(d, y_diff) / div
- return np.array([x, y, 0])
- def find_intersection(
- p0: Vect3 | Vect3Array,
- v0: Vect3 | Vect3Array,
- p1: Vect3 | Vect3Array,
- v1: Vect3 | Vect3Array,
- threshold: float = 1e-5,
- ) -> Vect3:
- """
- Return the intersection of a line passing through p0 in direction v0
- with one passing through p1 in direction v1. (Or array of intersections
- from arrays of such points/directions).
- For 3d values, it returns the point on the ray p0 + v0 * t closest to the
- ray p1 + v1 * t
- """
- d = len(p0.shape)
- if d == 1:
- is_3d = any(arr[2] for arr in (p0, v0, p1, v1))
- else:
- is_3d = any(z for arr in (p0, v0, p1, v1) for z in arr.T[2])
- if not is_3d:
- numer = np.array(cross2d(v1, p1 - p0))
- denom = np.array(cross2d(v1, v0))
- else:
- cp1 = cross(v1, p1 - p0)
- cp2 = cross(v1, v0)
- numer = np.array((cp1 * cp1).sum(d - 1))
- denom = np.array((cp1 * cp2).sum(d - 1))
- denom[abs(denom) < threshold] = np.inf
- ratio = numer / denom
- return p0 + (ratio * v0.T).T
- def line_intersects_path(
- start: Vect2 | Vect3,
- end: Vect2 | Vect3,
- path: Vect2Array | Vect3Array,
- ) -> bool:
- """
- Tests whether the line (start, end) intersects
- a polygonal path defined by its vertices
- """
- n = len(path) - 1
- p1 = np.empty((n, 2))
- q1 = np.empty((n, 2))
- p1[:] = start[:2]
- q1[:] = end[:2]
- p2 = path[:-1, :2]
- q2 = path[1:, :2]
- v1 = q1 - p1
- v2 = q2 - p2
- mis1 = cross2d(v1, p2 - p1) * cross2d(v1, q2 - p1) < 0
- mis2 = cross2d(v2, p1 - p2) * cross2d(v2, q1 - p2) < 0
- return bool((mis1 * mis2).any())
- def get_closest_point_on_line(a: VectN, b: VectN, p: VectN) -> VectN:
- """
- It returns point x such that
- x is on line ab and xp is perpendicular to ab.
- If x lies beyond ab line, then it returns nearest edge(a or b).
- """
- # x = b + t*(a-b) = t*a + (1-t)*b
- t = np.dot(p - b, a - b) / np.dot(a - b, a - b)
- if t < 0:
- t = 0
- if t > 1:
- t = 1
- return ((t * a) + ((1 - t) * b))
- def get_winding_number(points: Sequence[Vect2 | Vect3]) -> float:
- total_angle = 0
- for p1, p2 in adjacent_pairs(points):
- d_angle = angle_of_vector(p2) - angle_of_vector(p1)
- d_angle = ((d_angle + PI) % TAU) - PI
- total_angle += d_angle
- return total_angle / TAU
- ##
- def cross2d(a: Vect2 | Vect2Array, b: Vect2 | Vect2Array) -> Vect2 | Vect2Array:
- if len(a.shape) == 2:
- return a[:, 0] * b[:, 1] - a[:, 1] * b[:, 0]
- else:
- return a[0] * b[1] - b[0] * a[1]
- def tri_area(
- a: Vect2,
- b: Vect2,
- c: Vect2
- ) -> float:
- return 0.5 * abs(
- a[0] * (b[1] - c[1]) +
- b[0] * (c[1] - a[1]) +
- c[0] * (a[1] - b[1])
- )
- def is_inside_triangle(
- p: Vect2,
- a: Vect2,
- b: Vect2,
- c: Vect2
- ) -> bool:
- """
- Test if point p is inside triangle abc
- """
- crosses = np.array([
- cross2d(p - a, b - p),
- cross2d(p - b, c - p),
- cross2d(p - c, a - p),
- ])
- return bool(np.all(crosses > 0) or np.all(crosses < 0))
- def norm_squared(v: VectN | List[float]) -> float:
- return sum(x * x for x in v)
- # TODO, fails for polygons drawn over themselves
- def earclip_triangulation(verts: Vect3Array | Vect2Array, ring_ends: list[int]) -> list[int]:
- """
- Returns a list of indices giving a triangulation
- of a polygon, potentially with holes
- - verts is a numpy array of points
- - ring_ends is a list of indices indicating where
- the ends of new paths are
- """
- rings = [
- list(range(e0, e1))
- for e0, e1 in zip([0, *ring_ends], ring_ends)
- ]
- epsilon = 1e-6
- def is_in(point, ring_id):
- return abs(abs(get_winding_number([i - point for i in verts[rings[ring_id]]])) - 1) < epsilon
- def ring_area(ring_id):
- ring = rings[ring_id]
- s = 0
- for i, j in zip(ring[1:], ring):
- s += cross2d(verts[i], verts[j])
- return abs(s) / 2
- # Points at the same position may cause problems
- for i in rings:
- if len(i) < 2:
- continue
- verts[i[0]] += (verts[i[1]] - verts[i[0]]) * epsilon
- verts[i[-1]] += (verts[i[-2]] - verts[i[-1]]) * epsilon
- # First, we should know which rings are directly contained in it for each ring
- right = [max(verts[rings[i], 0]) for i in range(len(rings))]
- left = [min(verts[rings[i], 0]) for i in range(len(rings))]
- top = [max(verts[rings[i], 1]) for i in range(len(rings))]
- bottom = [min(verts[rings[i], 1]) for i in range(len(rings))]
- area = [ring_area(i) for i in range(len(rings))]
- # The larger ring must be outside
- rings_sorted = list(range(len(rings)))
- rings_sorted.sort(key=lambda x: area[x], reverse=True)
- def is_in_fast(ring_a, ring_b):
- # Whether a is in b
- return reduce(op.and_, (
- left[ring_b] <= left[ring_a] <= right[ring_a] <= right[ring_b],
- bottom[ring_b] <= bottom[ring_a] <= top[ring_a] <= top[ring_b],
- is_in(verts[rings[ring_a][0]], ring_b)
- ))
- chilren = [[] for i in rings]
- ringenum = ProgressDisplay(
- enumerate(rings_sorted),
- total=len(rings),
- leave=False,
- ascii=True if platform.system() == 'Windows' else None,
- dynamic_ncols=True,
- desc="SVG Triangulation",
- delay=3,
- )
- for idx, i in ringenum:
- for j in rings_sorted[:idx][::-1]:
- if is_in_fast(i, j):
- chilren[j].append(i)
- break
- res = []
- # Then, we can use earcut for each part
- used = [False] * len(rings)
- for i in rings_sorted:
- if used[i]:
- continue
- v = rings[i]
- ring_ends = [len(v)]
- for j in chilren[i]:
- used[j] = True
- v += rings[j]
- ring_ends.append(len(v))
- res += [v[i] for i in earcut(verts[v, :2], ring_ends)]
- return res
|