diff --git a/manim/utils/space_ops.py b/manim/utils/space_ops.py index 345403e621..761e32ea84 100644 --- a/manim/utils/space_ops.py +++ b/manim/utils/space_ops.py @@ -14,7 +14,6 @@ "z_to_vector", "angle_of_vector", "angle_between_vectors", - "project_along_vector", "normalize", "get_unit_normal", "compass_directions", @@ -27,6 +26,8 @@ "find_intersection", "line_intersection", "get_winding_number", + "shoelace", + "shoelace_direction", "cross2d", "earclip_triangulation", "cartesian_to_spherical", @@ -37,14 +38,14 @@ import itertools as it import math -from functools import reduce from typing import Sequence import numpy as np from mapbox_earcut import triangulate_float32 as earcut +from scipy.spatial.transform import Rotation from .. import config -from ..constants import DOWN, OUT, PI, RIGHT, TAU +from ..constants import DOWN, OUT, PI, RIGHT, TAU, UP from ..utils.iterables import adjacent_pairs @@ -167,7 +168,9 @@ def quaternion_conjugate(quaternion: Sequence[float]) -> np.ndarray: return result -def rotate_vector(vector: np.ndarray, angle: int, axis: np.ndarray = OUT) -> np.ndarray: +def rotate_vector( + vector: np.ndarray, angle: float, axis: np.ndarray = OUT +) -> np.ndarray: """Function for rotating a vector. Parameters @@ -190,18 +193,11 @@ def rotate_vector(vector: np.ndarray, angle: int, axis: np.ndarray = OUT) -> np. If vector is not of dimension 2 or 3. """ + if len(vector) > 3: + raise ValueError("Vector must have the correct dimensions.") if len(vector) == 2: - # Use complex numbers...because why not - z = complex(*vector) * np.exp(complex(0, angle)) - return np.array([z.real, z.imag]) - elif len(vector) == 3: - # Use quaternions...because why not - quat = quaternion_from_angle_axis(angle, axis) - quat_inv = quaternion_conjugate(quat) - product = reduce(quaternion_mult, [quat, np.append(0, vector), quat_inv]) - return product[1:] - else: - raise ValueError("vector must be of dimension 2 or 3") + vector = np.append(vector, 0) + return rotation_matrix(angle, axis) @ vector def thick_diagonal(dim: int, thickness=2) -> np.ndarray: @@ -242,19 +238,9 @@ def rotation_matrix_from_quaternion(quat: np.ndarray) -> np.ndarray: def rotation_matrix_transpose(angle: float, axis: np.ndarray) -> np.ndarray: - if axis[0] == 0 and axis[1] == 0: - # axis = [0, 0, z] case is common enough it's worth - # having a shortcut - sgn = 1 if axis[2] > 0 else -1 - cos_a = math.cos(angle) - sin_a = math.sin(angle) * sgn - return [ - [cos_a, sin_a, 0], - [-sin_a, cos_a, 0], - [0, 0, 1], - ] - quat = quaternion_from_angle_axis(angle, axis) - return rotation_matrix_transpose_from_quaternion(quat) + if all(np.array(axis)[:2] == np.zeros(2)): + return rotation_about_z(angle * np.sign(axis[2])).T + return rotation_matrix(angle, axis).T def rotation_matrix( @@ -265,10 +251,9 @@ def rotation_matrix( """ Rotation in R^3 about a specified axis of rotation. """ - about_z = rotation_about_z(angle) - z_to_axis = z_to_vector(axis) - axis_to_z = np.linalg.inv(z_to_axis) - inhomogeneous_rotation_matrix = reduce(np.dot, [z_to_axis, about_z, axis_to_z]) + inhomogeneous_rotation_matrix = Rotation.from_rotvec( + angle * normalize(np.array(axis)) + ).as_matrix() if not homogeneous: return inhomogeneous_rotation_matrix else: @@ -277,7 +262,7 @@ def rotation_matrix( return rotation_matrix -def rotation_about_z(angle: float) -> list[list[float]]: +def rotation_about_z(angle: float) -> np.ndarray: """Returns a rotation matrix for a given angle. Parameters @@ -287,14 +272,17 @@ def rotation_about_z(angle: float) -> list[list[float]]: Returns ------- - List[float] + np.ndarray Gives back the rotated matrix. """ - return [ - [np.cos(angle), -np.sin(angle), 0], - [np.sin(angle), np.cos(angle), 0], - [0, 0, 1], - ] + c, s = math.cos(angle), math.sin(angle) + return np.array( + [ + [c, -s, 0], + [s, c, 0], + [0, 0, 1], + ] + ) def z_to_vector(vector: np.ndarray) -> np.ndarray: @@ -302,23 +290,15 @@ def z_to_vector(vector: np.ndarray) -> np.ndarray: Returns some matrix in SO(3) which takes the z-axis to the (normalized) vector provided as an argument """ - norm = np.linalg.norm(vector) - if norm == 0: - return np.identity(3) - v = np.array(vector) / norm - phi = np.arccos(v[2]) - if any(v[:2]): - # projection of vector to unit circle - axis_proj = v[:2] / np.linalg.norm(v[:2]) - theta = np.arccos(axis_proj[0]) - if axis_proj[1] < 0: - theta = -theta - else: - theta = 0 - phi_down = np.array( - [[np.cos(phi), 0, np.sin(phi)], [0, 1, 0], [-np.sin(phi), 0, np.cos(phi)]], - ) - return np.dot(rotation_about_z(theta), phi_down) + axis_z = normalize(vector) + axis_y = normalize(np.cross(axis_z, RIGHT)) + axis_x = np.cross(axis_y, axis_z) + if np.linalg.norm(axis_y) == 0: + # the vector passed just so happened to be in the x direction. + axis_x = normalize(np.cross(UP, axis_z)) + axis_y = -np.cross(axis_x, axis_z) + + return np.array([axis_x, axis_y, axis_z]).T def angle_of_vector(vector: Sequence[float]) -> float: @@ -334,13 +314,7 @@ def angle_of_vector(vector: Sequence[float]) -> float: float The angle of the vector projected. """ - if config.renderer == "opengl": - return np.angle(complex(*vector[:2])) - else: - z = complex(*vector[:2]) - if z == 0: - return 0 - return np.angle(complex(*vector[:2])) + return np.angle(complex(*vector[:2])) def angle_between_vectors(v1: np.ndarray, v2: np.ndarray) -> np.ndarray: @@ -366,34 +340,12 @@ def angle_between_vectors(v1: np.ndarray, v2: np.ndarray) -> np.ndarray: ) -def project_along_vector(point: float, vector: np.ndarray) -> np.ndarray: - """Projects a vector along a point. - - Parameters - ---------- - point - The point to be project from. - vector - The vector which has to projected. - - Returns - ------- - np.ndarray - A dot product of the point and vector. - """ - matrix = np.identity(3) - np.outer(vector, vector) - return np.dot(point, matrix.T) - - def normalize(vect: np.ndarray | tuple[float], fall_back=None) -> np.ndarray: norm = np.linalg.norm(vect) if norm > 0: return np.array(vect) / norm else: - if fall_back is not None: - return fall_back - else: - return np.zeros(len(vect)) + return fall_back or np.zeros(len(vect)) def normalize_along_axis(array: np.ndarray, axis: np.ndarray) -> np.ndarray: @@ -435,21 +387,16 @@ def get_unit_normal(v1: np.ndarray, v2: np.ndarray, tol: float = 1e-6) -> np.nda np.ndarray The normal of the two vectors. """ - if config.renderer == "opengl": - v1 = normalize(v1) - v2 = normalize(v2) - cp = np.cross(v1, v2) + v1, v2 = (normalize(i) for i in (v1, v2)) + cp = np.cross(v1, v2) + cp_norm = np.linalg.norm(cp) + if cp_norm < tol: + # Vectors align, so find a normal to them in the plane shared with the z-axis + cp = np.cross(np.cross(v1, OUT), v1) cp_norm = np.linalg.norm(cp) if cp_norm < tol: - # Vectors align, so find a normal to them in the plane shared with the z-axis - new_cp = np.cross(np.cross(v1, OUT), v1) - new_cp_norm = np.linalg.norm(new_cp) - if new_cp_norm < tol: - return DOWN - return new_cp / new_cp_norm - return cp / cp_norm - else: - return normalize(np.cross(v1, v2)) + return DOWN + return normalize(cp) ### @@ -536,8 +483,7 @@ def center_of_mass(points: Sequence[float]) -> np.ndarray: np.ndarray The center of mass of the points. """ - points = [np.array(point).astype("float") for point in points] - return sum(points) / len(points) + return np.average(points, 0, np.ones(len(points))) def midpoint( @@ -561,16 +507,18 @@ def midpoint( return center_of_mass([point1, point2]) -def line_intersection(line1: Sequence[float], line2: Sequence[float]) -> np.ndarray: - """Returns intersection point of two lines, each defined with - a pair of vectors determining the end points. +def line_intersection( + line1: Sequence[np.ndarray], line2: Sequence[np.ndarray] +) -> np.ndarray: + """Returns the intersection point of two lines, each defined by + a pair of distinct points lying on the line. Parameters ---------- line1 - The first line. + A list of two points that determine the first line. line2 - The second line. + A list of two points that determine the second line. Returns ------- @@ -581,49 +529,49 @@ def line_intersection(line1: Sequence[float], line2: Sequence[float]) -> np.ndar ------ ValueError Error is produced if the two lines don't intersect with each other + or if the coordinates don't lie on the xy-plane. """ - 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]) + if any(np.array([line1, line2])[:, :, 2].reshape(-1)): + # checks for z coordinates != 0 + raise ValueError("Coords must be in the xy-plane.") + + # algorithm from https://stackoverflow.com/a/42727584 + padded = ( + np.pad(np.array(i)[:, :2], ((0, 0), (0, 1)), constant_values=1) + for i in (line1, line2) + ) + line1, line2 = (np.cross(*i) for i in padded) + x, y, z = np.cross(line1, line2) - def det(a, b): - return a[0] * b[1] - a[1] * b[0] + if z == 0: + raise ValueError( + "The lines are parallel, there is no unique intersection point." + ) - div = det(x_diff, y_diff) - if div == 0: - raise ValueError("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]) + return np.array([x / z, y / z, 0]) -def find_intersection(p0, v0, p1, v1, threshold=1e-5) -> np.ndarray: +def find_intersection( + p0s: Sequence[np.ndarray], + v0s: Sequence[np.ndarray], + p1s: Sequence[np.ndarray], + v1s: Sequence[np.ndarray], + threshold: float = 1e-5, +) -> Sequence[np.ndarray]: """ Return the intersection of a line passing through p0 in direction v0 - with one passing through p1 in direction v1. (Or array of intersections + 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 """ - p0 = np.array(p0, ndmin=2) - v0 = np.array(v0, ndmin=2) - p1 = np.array(p1, ndmin=2) - v1 = np.array(v1, ndmin=2) - m, n = np.shape(p0) - assert n in [2, 3] - - numerator = np.cross(v1, p1 - p0) - denominator = np.cross(v1, v0) - if n == 3: - d = len(np.shape(numerator)) - new_numerator = np.multiply(numerator, numerator).sum(d - 1) - new_denominator = np.multiply(denominator, numerator).sum(d - 1) - numerator, denominator = new_numerator, new_denominator - - denominator[abs(denominator) < threshold] = np.inf # So that ratio goes to 0 there - ratio = numerator / denominator - ratio = np.repeat(ratio, n).reshape((m, n)) - return p0 + ratio * v0 + # algorithm from https://en.wikipedia.org/wiki/Skew_lines#Nearest_points + result = [] + + for p0, v0, p1, v1 in zip(*[p0s, v0s, p1s, v1s]): + normal = np.cross(v1, np.cross(v0, v1)) + result += [p0 + np.dot(p1 - p0, normal) / np.dot(v0, normal) * v0] + return result def get_winding_number(points: Sequence[float]) -> float: @@ -645,8 +593,7 @@ def shoelace(x_y: np.ndarray) -> float: """ x = x_y[:, 0] y = x_y[:, 1] - area = 0.5 * np.array(np.dot(x, np.roll(y, 1)) - np.dot(y, np.roll(x, 1))) - return area + return np.trapz(y, x) def shoelace_direction(x_y: np.ndarray) -> str: diff --git a/tests/test_graphical_units/control_data/threed/AmbientCameraMove.npz b/tests/test_graphical_units/control_data/threed/AmbientCameraMove.npz index 4dff94bbba..f91b970ef8 100644 Binary files a/tests/test_graphical_units/control_data/threed/AmbientCameraMove.npz and b/tests/test_graphical_units/control_data/threed/AmbientCameraMove.npz differ diff --git a/tests/test_graphical_units/control_data/threed/CameraMove.npz b/tests/test_graphical_units/control_data/threed/CameraMove.npz index 1f4226c6f9..a884913567 100644 Binary files a/tests/test_graphical_units/control_data/threed/CameraMove.npz and b/tests/test_graphical_units/control_data/threed/CameraMove.npz differ diff --git a/tests/test_graphical_units/control_data/threed/Cone.npz b/tests/test_graphical_units/control_data/threed/Cone.npz index 828a5ef092..50dc203646 100644 Binary files a/tests/test_graphical_units/control_data/threed/Cone.npz and b/tests/test_graphical_units/control_data/threed/Cone.npz differ diff --git a/tests/test_graphical_units/control_data/threed/Cube.npz b/tests/test_graphical_units/control_data/threed/Cube.npz index a1832bd5ba..a955b37eef 100644 Binary files a/tests/test_graphical_units/control_data/threed/Cube.npz and b/tests/test_graphical_units/control_data/threed/Cube.npz differ diff --git a/tests/test_graphical_units/control_data/threed/Line3D.npz b/tests/test_graphical_units/control_data/threed/Line3D.npz index 173414da57..05d698208f 100644 Binary files a/tests/test_graphical_units/control_data/threed/Line3D.npz and b/tests/test_graphical_units/control_data/threed/Line3D.npz differ diff --git a/tests/test_graphical_units/control_data/threed/Sphere.npz b/tests/test_graphical_units/control_data/threed/Sphere.npz index a0dcfc426c..d45b68e896 100644 Binary files a/tests/test_graphical_units/control_data/threed/Sphere.npz and b/tests/test_graphical_units/control_data/threed/Sphere.npz differ diff --git a/tests/test_graphical_units/test_threed.py b/tests/test_graphical_units/test_threed.py index 650ee13bf6..d70657c652 100644 --- a/tests/test_graphical_units/test_threed.py +++ b/tests/test_graphical_units/test_threed.py @@ -40,10 +40,10 @@ def test_Cylinder(scene): @frames_comparison(base_scene=ThreeDScene) def test_Line3D(scene): - line = Line3D() - perp_line = Line3D.perpendicular_to(line, UP) - parallel_line = Line3D.parallel_to(line, UP) - scene.add(line, perp_line, parallel_line) + line1, line2 = Line3D().shift(LEFT * 2), Line3D().shift(RIGHT * 2) + perp_line = Line3D.perpendicular_to(line1, UP + OUT) + parallel_line = Line3D.parallel_to(line2, DOWN + IN) + scene.add(line1, line2, perp_line, parallel_line) @frames_comparison(base_scene=ThreeDScene) diff --git a/tests/test_space_ops.py b/tests/test_space_ops.py index fef1862d58..36e529f467 100644 --- a/tests/test_space_ops.py +++ b/tests/test_space_ops.py @@ -1,15 +1,113 @@ from __future__ import annotations import numpy as np +import pytest -from manim import cartesian_to_spherical, spherical_to_cartesian +from manim.utils.space_ops import * +from manim.utils.space_ops import shoelace, shoelace_direction + + +def test_rotate_vector(): + vec = np.array([0, 1, 0]) + rotated = rotate_vector(vec, np.pi / 2) + assert np.round(rotated[0], 5) == -1.0 + assert not np.round(rotated[1:], 5).any() + assert np.all(rotate_vector(np.zeros(3), np.pi / 4) == np.zeros(3)) + + +def test_rotation_matrices(): + ang = np.pi / 6 + ax = np.array([1, 1, 1]) + assert np.all( + np.round(rotation_matrix(ang, ax, True), 5) + == np.round( + np.array( + [ + [0.91068, -0.24402, 0.33333, 0.0], + [0.33333, 0.91068, -0.24402, 0.0], + [-0.24402, 0.33333, 0.91068, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ), + 5, + ) + ) + assert np.all( + np.round(rotation_about_z(np.pi / 3), 5) + == np.array( + [ + [0.5, -0.86603, 0.0], + [0.86603, 0.5, 0.0], + [0.0, 0.0, 1.0], + ] + ) + ) + assert np.all( + np.round(z_to_vector(np.array([1, 2, 3])), 5) + == np.array( + [ + [0.96362, 0.0, 0.26726], + [-0.14825, 0.83205, 0.53452], + [-0.22237, -0.5547, 0.80178], + ] + ) + ) + + +def test_angle_of_vector(): + assert angle_of_vector(np.array([1, 1, 1])) == np.pi / 4 + assert ( + np.round(angle_between_vectors(np.array([1, 1, 1]), np.array([-1, 1, 1])), 5) + == 1.23096 + ) + assert angle_of_vector(np.zeros(3)) == 0.0 + + +def test_center_of_mass(): + assert np.all(center_of_mass([[0, 0, 0], [1, 2, 3]]) == np.array([0.5, 1.0, 1.5])) + + +def test_line_intersection(): + assert np.all( + line_intersection( + [[0, 0, 0], [3, 3, 0]], + [[0, 3, 0], [3, 0, 0]], + ) + == np.array([1.5, 1.5, 0.0]) + ) + with pytest.raises(ValueError): + line_intersection( # parallel lines + [[0, 1, 0], [5, 1, 0]], + [[0, 6, 0], [5, 6, 0]], + ) + with pytest.raises(ValueError): + line_intersection( # lines not in xy-plane + [[0, 0, 3], [3, 3, 3]], + [[0, 3, 3], [3, 0, 3]], + ) + with pytest.raises(ValueError): + line_intersection( # lines are equal + [[2, 2, 0], [3, 1, 0]], + [[2, 2, 0], [3, 1, 0]], + ) + assert np.all( + line_intersection( # lines with ends out of bounds + [[0, 0, 0], [1, 1, 0]], + [[0, 4, 0], [1, 3, 0]], + ) + == np.array([2, 2, 0]) + ) + + +def test_shoelace(): + assert shoelace(np.array([[1, 2], [3, 4]])) == 6 def test_polar_coords(): a = np.array([1, 1, 0]) b = (2, np.pi / 2, np.pi / 2) - assert all( + assert np.all( np.round(cartesian_to_spherical(a), 4) == np.round([2 ** 0.5, np.pi / 4, np.pi / 2], 4), ) - assert all(np.round(spherical_to_cartesian(b), 4) == np.array([0, 2, 0])) + assert np.all(np.round(spherical_to_cartesian(b), 4) == np.array([0, 2, 0]))