Coverage for trimesh/transformations.py: 93%
808 statements
« prev ^ index » next coverage.py v7.14.1, created at 2026-10-24 01:00 +0000
1# transformations.py
3# Modified for inclusion in the `trimesh` library
4# https://github.com/mikedh/trimesh
5# -----------------------------------------------------------------------
6#
7# Copyright (c) 2006-2017, Christoph Gohlke
8# Copyright (c) 2006-2017, The Regents of the University of California
9# Produced at the Laboratory for Fluorescence Dynamics
10# All rights reserved.
11#
12# Redistribution and use in source and binary forms, with or without
13# modification, are permitted provided that the following conditions are met:
14#
15# * Redistributions of source code must retain the above copyright
16# notice, this list of conditions and the following disclaimer.
17# * Redistributions in binary form must reproduce the above copyright
18# notice, this list of conditions and the following disclaimer in the
19# documentation and/or other materials provided with the distribution.
20# * Neither the name of the copyright holders nor the names of any
21# contributors may be used to endorse or promote products derived
22# from this software without specific prior written permission.
23#
24# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
25# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
26# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
27# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
28# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
29# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
30# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
31# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
32# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
33# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34# POSSIBILITY OF SUCH DAMAGE.
36"""Homogeneous Transformation Matrices and Quaternions.
38A library for calculating 4x4 matrices for translating, rotating, reflecting,
39scaling, shearing, projecting, orthogonalizing, and superimposing arrays of
403D homogeneous coordinates as well as for converting between rotation matrices,
41Euler angles, and quaternions. Also includes an Arcball control object and
42functions to decompose transformation matrices.
44:Author:
45 `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`_
47:Organization:
48 Laboratory for Fluorescence Dynamics, University of California, Irvine
50:Version: 2017.02.17
52Requirements
53------------
54* `CPython 2.7 or 3.4 <http://www.python.org>`_
55* `numpy 1.9 <http://www.np.org>`_
56* `Transformations.c 2015.03.19 <http://www.lfd.uci.edu/~gohlke/>`_
57 (recommended for speedup of some functions)
59Notes
60-----
61The API is not stable yet and is expected to change between revisions.
63This Python code is not optimized for speed. Refer to the transformations.c
64module for a faster implementation of some functions.
66Documentation in HTML format can be generated with epydoc.
68Matrices (M) can be inverted using np.linalg.inv(M), be concatenated using
69np.dot(M0, M1), or transform homogeneous coordinate arrays (v) using
70np.dot(M, v) for shape (4, *) column vectors, respectively
71np.dot(v, M.T) for shape (*, 4) row vectors ("array of points").
73This module follows the "column vectors on the right" and "row major storage"
74(C contiguous) conventions. The translation components are in the right column
75of the transformation matrix, i.e. M[:3, 3].
76The transpose of the transformation matrices may have to be used to interface
77with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16].
79Calculations are carried out with np.float64 precision.
81Vector, point, quaternion, and matrix function arguments are expected to be
82"array like", i.e. tuple, list, or numpy arrays.
84Return types are numpy arrays unless specified otherwise.
86Angles are in radians unless specified otherwise.
88Quaternions w+ix+jy+kz are represented as [w, x, y, z].
90A triple of Euler angles can be applied/interpreted in 24 ways, which can
91be specified using a 4 character string or encoded 4-tuple:
93 *Axes 4-string*: e.g. 'sxyz' or 'ryxy'
95 - first character : rotations are applied to 's'tatic or 'r'otating frame
96 - remaining characters : successive rotation axis 'x', 'y', or 'z'
98 *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
100 - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
101 - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed
102 by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
103 - repetition : first and last axis are same (1) or different (0).
104 - frame : rotations are applied to static (0) or rotating (1) frame.
106Other Python packages and modules for 3D transformations and quaternions:
108* `Transforms3d <https://pypi.python.org/pypi/transforms3d>`_
109 includes most code of this module.
110* `Blender.mathutils <http://www.blender.org/api/blender_python_api>`_
111* `numpy-dtypes <https://github.com/numpy/numpy-dtypes>`_
113References
114----------
115(1) Matrices and transformations. Ronald Goldman.
116 In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
117(2) More matrices and transformations: shear and pseudo-perspective.
118 Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
119(3) Decomposing a matrix into simple transformations. Spencer Thomas.
120 In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
121(4) Recovering the data from the transformation matrix. Ronald Goldman.
122 In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991.
123(5) Euler angle conversion. Ken Shoemake.
124 In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
125(6) Arcball rotation control. Ken Shoemake.
126 In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
127(7) Representing attitude: Euler angles, unit quaternions, and rotation
128 vectors. James Diebel. 2006.
129(8) A discussion of the solution for the best rotation to relate two sets
130 of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.
131(9) Closed-form solution of absolute orientation using unit quaternions.
132 BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642.
133(10) Quaternions. Ken Shoemake.
134 http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf
135(11) From quaternion to matrix and back. JMP van Waveren. 2005.
136 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
137(12) Uniform random rotations. Ken Shoemake.
138 In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992.
139(13) Quaternion in molecular modeling. CFF Karney.
140 J Mol Graph Mod, 25(5):595-604
141(14) New method for extracting the quaternion from a rotation matrix.
142 Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087.
143(15) Multiple View Geometry in Computer Vision. Hartley and Zissermann.
144 Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130.
145(16) Column Vectors vs. Row Vectors.
146 http://steve.hollasch.net/cgindex/math/matrix/column-vec.html
148Examples
149--------
150>>> alpha, beta, gamma = 0.123, -1.234, 2.345
151>>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
152>>> I = identity_matrix()
153>>> Rx = rotation_matrix(alpha, xaxis)
154>>> Ry = rotation_matrix(beta, yaxis)
155>>> Rz = rotation_matrix(gamma, zaxis)
156>>> R = concatenate_matrices(Rx, Ry, Rz)
157>>> euler = euler_from_matrix(R, 'rxyz')
158>>> np.allclose([alpha, beta, gamma], euler)
159True
160>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
161>>> is_same_transform(R, Re)
162True
163>>> al, be, ga = euler_from_matrix(Re, 'rxyz')
164>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
165True
166>>> qx = quaternion_about_axis(alpha, xaxis)
167>>> qy = quaternion_about_axis(beta, yaxis)
168>>> qz = quaternion_about_axis(gamma, zaxis)
169>>> q = quaternion_multiply(qx, qy)
170>>> q = quaternion_multiply(q, qz)
171>>> Rq = quaternion_matrix(q)
172>>> is_same_transform(R, Rq)
173True
174>>> S = scale_matrix(1.23, origin)
175>>> T = translation_matrix([1, 2, 3])
176>>> Z = shear_matrix(beta, xaxis, origin, zaxis)
177>>> R = random_rotation_matrix(np.random.rand(3))
178>>> M = concatenate_matrices(T, R, Z, S)
179>>> scale, shear, angles, trans, persp = decompose_matrix(M)
180>>> np.allclose(scale, 1.23)
181True
182>>> np.allclose(trans, [1, 2, 3])
183True
184>>> np.allclose(shear, [0, np.tan(beta), 0])
185True
186>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
187True
188>>> M1 = compose_matrix(scale, shear, angles, trans, persp)
189>>> is_same_transform(M, M1)
190True
191>>> v0, v1 = random_vector(3), random_vector(3)
192>>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1))
193>>> v2 = np.dot(v0, M[:3,:3].T)
194>>> np.allclose(unit_vector(v1), unit_vector(v2))
195True
197"""
199import numpy as np
200from numpy.typing import ArrayLike, NDArray
202from .typed import Integer, Number, Seed
203from .util import diagonal_dot, random_generator
205# a cached immutable identity matrix provides a speedup sometimes
206_IDENTITY = np.eye(4)
207_IDENTITY.flags["WRITEABLE"] = False
208# a flattened 3x3 identity for comparing against raveled rotations
209_IDENTITY9 = np.eye(3).ravel()
210_IDENTITY9.flags["WRITEABLE"] = False
213def identity_matrix():
214 """Return 4x4 identity/unit matrix.
216 >>> I = identity_matrix()
217 >>> np.allclose(I, np.dot(I, I))
218 True
219 >>> float(np.sum(I)), float(np.trace(I))
220 (4.0, 4.0)
221 >>> np.allclose(I, np.identity(4))
222 True
224 """
225 return np.identity(4)
228def translation_matrix(direction):
229 """
230 Return matrix to translate by direction vector.
232 >>> v = np.random.random(3) - 0.5
233 >>> np.allclose(v, translation_matrix(v)[:3, 3])
234 True
236 """
237 # are we 2D or 3D
238 dim = len(direction)
240 # start with identity matrix
242 if any("sympy" in str(type(v)) for v in direction):
243 # if we have been passed input values as sympy.Symbol
244 from sympy import eye
246 M = eye(dim + 1)
247 else:
248 M = np.eye(dim + 1)
250 # apply the offset
251 M[:dim, dim] = direction[:dim]
253 return M
256def translation_from_matrix(matrix):
257 """Return translation vector from translation matrix.
259 >>> v0 = np.random.random(3) - 0.5
260 >>> v1 = translation_from_matrix(translation_matrix(v0))
261 >>> np.allclose(v0, v1)
262 True
264 """
265 return np.asarray(matrix)[:3, 3].copy()
268def reflection_matrix(point, normal):
269 """Return matrix to mirror at plane defined by point and normal vector.
271 >>> v0 = np.random.random(4) - 0.5
272 >>> v0[3] = 1.
273 >>> v1 = np.random.random(3) - 0.5
274 >>> R = reflection_matrix(v0, v1)
275 >>> np.allclose(2, np.trace(R))
276 True
277 >>> np.allclose(v0, np.dot(R, v0))
278 True
279 >>> v2 = v0.copy()
280 >>> v2[:3] += v1
281 >>> v3 = v0.copy()
282 >>> v2[:3] -= v1
283 >>> np.allclose(v2, np.dot(R, v3))
284 True
286 """
287 normal = unit_vector(normal[:3])
288 M = np.identity(4)
289 M[:3, :3] -= 2.0 * np.outer(normal, normal)
290 M[:3, 3] = (2.0 * np.dot(point[:3], normal)) * normal
291 return M
294def reflection_from_matrix(matrix):
295 """Return mirror plane point and normal vector from reflection matrix.
297 >>> v0 = np.random.random(3) - 0.5
298 >>> v1 = np.random.random(3) - 0.5
299 >>> M0 = reflection_matrix(v0, v1)
300 >>> point, normal = reflection_from_matrix(M0)
301 >>> M1 = reflection_matrix(point, normal)
302 >>> is_same_transform(M0, M1)
303 True
305 """
306 M = np.asarray(matrix, dtype=np.float64)
307 # normal: unit eigenvector corresponding to eigenvalue -1
308 w, V = np.linalg.eig(M[:3, :3])
309 i = np.where(abs(np.real(w) + 1.0) < 1e-8)[0]
310 if not len(i):
311 raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
312 normal = np.real(V[:, i[0]]).squeeze()
313 # point: any unit eigenvector corresponding to eigenvalue 1
314 w, V = np.linalg.eig(M)
315 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
316 if not len(i):
317 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
318 point = np.real(V[:, i[-1]]).squeeze()
319 point /= point[3]
320 return point, normal
323def rotation_matrix(angle, direction, point=None):
324 """
325 Return matrix to rotate about axis defined by point and
326 direction.
328 Parameters
329 -------------
330 angle : float, or sympy.Symbol
331 Angle, in radians or symbolic angle
332 direction : (3,) float
333 Any vector along rotation axis
334 point : (3, ) float, or None
335 Origin point of rotation axis
337 Returns
338 -------------
339 matrix : (4, 4) float, or (4, 4) sympy.Matrix
340 Homogeneous transformation matrix
342 Examples
343 -------------
344 >>> R = rotation_matrix(np.pi/2, [0, 0, 1], [1, 0, 0])
345 >>> np.allclose(np.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])
346 True
347 >>> angle = (random.random() - 0.5) * (2*np.pi)
348 >>> direc = np.random.random(3) - 0.5
349 >>> point = np.random.random(3) - 0.5
350 >>> R0 = rotation_matrix(angle, direc, point)
351 >>> R1 = rotation_matrix(angle-2*np.pi, direc, point)
352 >>> is_same_transform(R0, R1)
353 True
354 >>> R0 = rotation_matrix(angle, direc, point)
355 >>> R1 = rotation_matrix(-angle, -direc, point)
356 >>> is_same_transform(R0, R1)
357 True
358 >>> I = np.identity(4, np.float64)
359 >>> np.allclose(I, rotation_matrix(np.pi*2, direc))
360 True
361 >>> np.allclose(2, np.trace(rotation_matrix(np.pi/2,direc,point)))
362 True
364 """
365 if "sympy" in str(type(angle)):
366 # special case sympy symbolic angles
367 import sympy as sp
369 symbolic = True
370 sina = sp.sin(angle)
371 cosa = sp.cos(angle)
372 else:
373 symbolic = False
374 sina = np.sin(angle)
375 cosa = np.cos(angle)
377 direction = unit_vector(direction[:3])
379 # rotation matrix around unit vector
380 M = np.diag([cosa, cosa, cosa, 1.0])
381 M[:3, :3] += np.outer(direction, direction) * (1.0 - cosa)
383 direction = direction * sina
384 M[:3, :3] += np.array(
385 [
386 [0.0, -direction[2], direction[1]],
387 [direction[2], 0.0, -direction[0]],
388 [-direction[1], direction[0], 0.0],
389 ]
390 )
392 # if point is specified, rotation is not around origin
393 if point is not None:
394 point = np.asarray(point[:3], dtype=np.float64)
395 M[:3, 3] = point - np.dot(M[:3, :3], point)
397 # return symbolic angles as sympy Matrix objects
398 if symbolic:
399 return sp.Matrix(M)
401 return M
404def rotation_from_matrix(matrix):
405 """Return rotation angle and axis from rotation matrix.
407 >>> angle = (random.random() - 0.5) * (2*np.pi)
408 >>> direc = np.random.random(3) - 0.5
409 >>> point = np.random.random(3) - 0.5
410 >>> R0 = rotation_matrix(angle, direc, point)
411 >>> angle, direc, point = rotation_from_matrix(R0)
412 >>> R1 = rotation_matrix(angle, direc, point)
413 >>> is_same_transform(R0, R1)
414 True
416 """
417 R = np.asarray(matrix, dtype=np.float64)
418 R33 = R[:3, :3]
419 # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
420 w, W = np.linalg.eig(R33.T)
421 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
422 if not len(i):
423 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
424 direction = np.real(W[:, i[-1]]).squeeze()
425 # point: unit eigenvector of R33 corresponding to eigenvalue of 1
426 w, Q = np.linalg.eig(R)
427 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
428 if not len(i):
429 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
430 point = np.real(Q[:, i[-1]]).squeeze()
431 point /= point[3]
432 # rotation angle depending on direction
433 cosa = (np.trace(R33) - 1.0) / 2.0
434 if abs(direction[2]) > 1e-8:
435 sina = (R[1, 0] + (cosa - 1.0) * direction[0] * direction[1]) / direction[2]
436 elif abs(direction[1]) > 1e-8:
437 sina = (R[0, 2] + (cosa - 1.0) * direction[0] * direction[2]) / direction[1]
438 else:
439 sina = (R[2, 1] + (cosa - 1.0) * direction[1] * direction[2]) / direction[0]
440 angle = np.arctan2(sina, cosa)
441 return angle, direction, point
444def scale_matrix(factor, origin=None, direction=None):
445 """Return matrix to scale by factor around origin in direction.
447 Use factor -1 for point symmetry.
449 >>> v = (np.random.rand(4, 5) - 0.5) * 20
450 >>> v[3] = 1
451 >>> S = scale_matrix(-1.234)
452 >>> np.allclose(np.dot(S, v)[:3], -1.234*v[:3])
453 True
454 >>> factor = random.random() * 10 - 5
455 >>> origin = np.random.random(3) - 0.5
456 >>> direct = np.random.random(3) - 0.5
457 >>> S = scale_matrix(factor, origin)
458 >>> S = scale_matrix(factor, origin, direct)
460 """
461 if direction is None:
462 # uniform scaling
463 M = np.diag([factor, factor, factor, 1.0])
464 if origin is not None:
465 M[:3, 3] = origin[:3]
466 M[:3, 3] *= 1.0 - factor
467 else:
468 # nonuniform scaling
469 direction = unit_vector(direction[:3])
470 factor = 1.0 - factor
471 M = np.identity(4)
472 M[:3, :3] -= factor * np.outer(direction, direction)
473 if origin is not None:
474 M[:3, 3] = (factor * np.dot(origin[:3], direction)) * direction
475 return M
478def scale_from_matrix(matrix):
479 """Return scaling factor, origin and direction from scaling matrix.
481 >>> factor = random.random() * 10 - 5
482 >>> origin = np.random.random(3) - 0.5
483 >>> direct = np.random.random(3) - 0.5
484 >>> S0 = scale_matrix(factor, origin)
485 >>> factor, origin, direction = scale_from_matrix(S0)
486 >>> S1 = scale_matrix(factor, origin, direction)
487 >>> is_same_transform(S0, S1)
488 True
489 >>> S0 = scale_matrix(factor, origin, direct)
490 >>> factor, origin, direction = scale_from_matrix(S0)
491 >>> S1 = scale_matrix(factor, origin, direction)
492 >>> is_same_transform(S0, S1)
493 True
495 """
496 M = np.asarray(matrix, dtype=np.float64)
497 M33 = M[:3, :3]
498 factor = np.trace(M33) - 2.0
499 try:
500 # direction: unit eigenvector corresponding to eigenvalue factor
501 w, V = np.linalg.eig(M33)
502 i = np.where(abs(np.real(w) - factor) < 1e-8)[0][0]
503 direction = np.real(V[:, i]).squeeze()
504 direction /= vector_norm(direction)
505 except IndexError:
506 # uniform scaling
507 factor = (factor + 2.0) / 3.0
508 direction = None
509 # origin: any eigenvector corresponding to eigenvalue 1
510 w, V = np.linalg.eig(M)
511 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
512 if not len(i):
513 raise ValueError("no eigenvector corresponding to eigenvalue 1")
514 origin = np.real(V[:, i[-1]]).squeeze()
515 origin /= origin[3]
516 return factor, origin, direction
519def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False):
520 """Return matrix to project onto plane defined by point and normal.
522 Using either perspective point, projection direction, or none of both.
524 If pseudo is True, perspective projections will preserve relative depth
525 such that Perspective = dot(Orthogonal, PseudoPerspective).
527 >>> P = projection_matrix([0, 0, 0], [1, 0, 0])
528 >>> np.allclose(P[1:, 1:], np.identity(4)[1:, 1:])
529 True
530 >>> point = np.random.random(3) - 0.5
531 >>> normal = np.random.random(3) - 0.5
532 >>> direct = np.random.random(3) - 0.5
533 >>> persp = np.random.random(3) - 0.5
534 >>> P0 = projection_matrix(point, normal)
535 >>> P1 = projection_matrix(point, normal, direction=direct)
536 >>> P2 = projection_matrix(point, normal, perspective=persp)
537 >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
538 >>> is_same_transform(P2, np.dot(P0, P3))
539 True
540 >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0])
541 >>> v0 = (np.random.rand(4, 5) - 0.5) * 20
542 >>> v0[3] = 1
543 >>> v1 = np.dot(P, v0)
544 >>> np.allclose(v1[1], v0[1])
545 True
546 >>> np.allclose(v1[0], 3-v1[1])
547 True
549 """
550 M = np.identity(4)
551 point = np.asarray(point[:3], dtype=np.float64)
552 normal = unit_vector(normal[:3])
553 if perspective is not None:
554 # perspective projection
555 perspective = np.asarray(perspective[:3], dtype=np.float64)
556 M[0, 0] = M[1, 1] = M[2, 2] = np.dot(perspective - point, normal)
557 M[:3, :3] -= np.outer(perspective, normal)
558 if pseudo:
559 # preserve relative depth
560 M[:3, :3] -= np.outer(normal, normal)
561 M[:3, 3] = np.dot(point, normal) * (perspective + normal)
562 else:
563 M[:3, 3] = np.dot(point, normal) * perspective
564 M[3, :3] = -normal
565 M[3, 3] = np.dot(perspective, normal)
566 elif direction is not None:
567 # parallel projection
568 direction = np.asarray(direction[:3], dtype=np.float64)
569 scale = np.dot(direction, normal)
570 M[:3, :3] -= np.outer(direction, normal) / scale
571 M[:3, 3] = direction * (np.dot(point, normal) / scale)
572 else:
573 # orthogonal projection
574 M[:3, :3] -= np.outer(normal, normal)
575 M[:3, 3] = np.dot(point, normal) * normal
576 return M
579def projection_from_matrix(matrix, pseudo=False):
580 """Return projection plane and perspective point from projection matrix.
582 Return values are same as arguments for projection_matrix function:
583 point, normal, direction, perspective, and pseudo.
585 >>> point = np.random.random(3) - 0.5
586 >>> normal = np.random.random(3) - 0.5
587 >>> direct = np.random.random(3) - 0.5
588 >>> persp = np.random.random(3) - 0.5
589 >>> P0 = projection_matrix(point, normal)
590 >>> result = projection_from_matrix(P0)
591 >>> P1 = projection_matrix(*result)
592 >>> is_same_transform(P0, P1)
593 True
594 >>> P0 = projection_matrix(point, normal, direct)
595 >>> result = projection_from_matrix(P0)
596 >>> P1 = projection_matrix(*result)
597 >>> is_same_transform(P0, P1)
598 True
599 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
600 >>> result = projection_from_matrix(P0, pseudo=False)
601 >>> P1 = projection_matrix(*result)
602 >>> is_same_transform(P0, P1)
603 True
604 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
605 >>> result = projection_from_matrix(P0, pseudo=True)
606 >>> P1 = projection_matrix(*result)
607 >>> is_same_transform(P0, P1)
608 True
610 """
611 M = np.asarray(matrix, dtype=np.float64)
612 M33 = M[:3, :3]
613 w, V = np.linalg.eig(M)
614 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
615 if not pseudo and len(i):
616 # point: any eigenvector corresponding to eigenvalue 1
617 point = np.real(V[:, i[-1]]).squeeze()
618 point /= point[3]
619 # direction: unit eigenvector corresponding to eigenvalue 0
620 w, V = np.linalg.eig(M33)
621 i = np.where(abs(np.real(w)) < 1e-8)[0]
622 if not len(i):
623 raise ValueError("no eigenvector corresponding to eigenvalue 0")
624 direction = np.real(V[:, i[0]]).squeeze()
625 direction /= vector_norm(direction)
626 # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
627 w, V = np.linalg.eig(M33.T)
628 i = np.where(abs(np.real(w)) < 1e-8)[0]
629 if len(i):
630 # parallel projection
631 normal = np.real(V[:, i[0]]).squeeze()
632 normal /= vector_norm(normal)
633 return point, normal, direction, None, False
634 else:
635 # orthogonal projection, where normal equals direction vector
636 return point, direction, None, None, False
637 else:
638 # perspective projection
639 i = np.where(abs(np.real(w)) > 1e-8)[0]
640 if not len(i):
641 raise ValueError("no eigenvector not corresponding to eigenvalue 0")
642 point = np.real(V[:, i[-1]]).squeeze()
643 point /= point[3]
644 normal = -M[3, :3]
645 perspective = M[:3, 3] / np.dot(point[:3], normal)
646 if pseudo:
647 perspective -= normal
648 return point, normal, None, perspective, pseudo
651def clip_matrix(left, right, bottom, top, near, far, perspective=False):
652 """Return matrix to obtain normalized device coordinates from frustum.
654 The frustum bounds are axis-aligned along x (left, right),
655 y (bottom, top) and z (near, far).
657 Normalized device coordinates are in range [-1, 1] if coordinates are
658 inside the frustum.
660 If perspective is True the frustum is a truncated pyramid with the
661 perspective point at origin and direction along z axis, otherwise an
662 orthographic canonical view volume (a box).
664 Homogeneous coordinates transformed by the perspective clip matrix
665 need to be dehomogenized (divided by w coordinate).
667 >>> frustum = np.random.rand(6)
668 >>> frustum[1] += frustum[0]
669 >>> frustum[3] += frustum[2]
670 >>> frustum[5] += frustum[4]
671 >>> M = clip_matrix(perspective=False, *frustum)
672 >>> a = np.dot(M, [frustum[0], frustum[2], frustum[4], 1])
673 >>> np.allclose(a, [-1., -1., -1., 1.])
674 True
675 >>> b = np.dot(M, [frustum[1], frustum[3], frustum[5], 1])
676 >>> np.allclose(b, [ 1., 1., 1., 1.])
677 True
678 >>> M = clip_matrix(perspective=True, *frustum)
679 >>> v = np.dot(M, [frustum[0], frustum[2], frustum[4], 1])
680 >>> c = v / v[3]
681 >>> np.allclose(c, [-1., -1., -1., 1.])
682 True
683 >>> v = np.dot(M, [frustum[1], frustum[3], frustum[4], 1])
684 >>> d = v / v[3]
685 >>> np.allclose(d, [ 1., 1., -1., 1.])
686 True
688 """
689 if left >= right or bottom >= top or near >= far:
690 raise ValueError("invalid frustum")
691 if perspective:
692 if near <= _EPS:
693 raise ValueError("invalid frustum: near <= 0")
694 t = 2.0 * near
695 M = [
696 [t / (left - right), 0.0, (right + left) / (right - left), 0.0],
697 [0.0, t / (bottom - top), (top + bottom) / (top - bottom), 0.0],
698 [0.0, 0.0, (far + near) / (near - far), t * far / (far - near)],
699 [0.0, 0.0, -1.0, 0.0],
700 ]
701 else:
702 M = [
703 [2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)],
704 [0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)],
705 [0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)],
706 [0.0, 0.0, 0.0, 1.0],
707 ]
708 return np.array(M)
711def shear_matrix(angle, direction, point, normal):
712 """Return matrix to shear by angle along direction vector on shear plane.
714 The shear plane is defined by a point and normal vector. The direction
715 vector must be orthogonal to the plane's normal vector.
717 A point P is transformed by the shear matrix into P" such that
718 the vector P-P" is parallel to the direction vector and its extent is
719 given by the angle of P-P'-P", where P' is the orthogonal projection
720 of P onto the shear plane.
722 >>> angle = (random.random() - 0.5) * 4*np.pi
723 >>> direct = np.random.random(3) - 0.5
724 >>> point = np.random.random(3) - 0.5
725 >>> normal = np.cross(direct, np.random.random(3))
726 >>> S = shear_matrix(angle, direct, point, normal)
727 >>> np.allclose(1, np.linalg.det(S))
728 True
730 """
731 normal = unit_vector(normal[:3])
732 direction = unit_vector(direction[:3])
733 if abs(np.dot(normal, direction)) > 1e-6:
734 raise ValueError("direction and normal vectors are not orthogonal")
735 angle = np.tan(angle)
736 M = np.identity(4)
737 M[:3, :3] += angle * np.outer(direction, normal)
738 M[:3, 3] = -angle * np.dot(point[:3], normal) * direction
739 return M
742def shear_from_matrix(matrix):
743 """Return shear angle, direction and plane from shear matrix.
745 >>> angle = np.pi / 2.0
746 >>> direct = [0.0, 1.0, 0.0]
747 >>> point = [0.0, 0.0, 0.0]
748 >>> normal = np.cross(direct, np.roll(direct,1))
749 >>> S0 = shear_matrix(angle, direct, point, normal)
750 >>> angle, direct, point, normal = shear_from_matrix(S0)
751 >>> S1 = shear_matrix(angle, direct, point, normal)
752 >>> is_same_transform(S0, S1)
753 True
755 """
756 M = np.asarray(matrix, dtype=np.float64)
757 M33 = M[:3, :3]
758 # normal: cross independent eigenvectors corresponding to the eigenvalue 1
759 w, V = np.linalg.eig(M33)
761 i = np.where(abs(np.real(w) - 1.0) < 1e-4)[0]
762 if len(i) < 2:
763 raise ValueError(f"no two linear independent eigenvectors found {w}")
764 V = np.real(V[:, i]).squeeze().T
765 lenorm = -1.0
766 for i0, i1 in ((0, 1), (0, 2), (1, 2)):
767 n = np.cross(V[i0], V[i1])
768 w = vector_norm(n)
769 if w > lenorm:
770 lenorm = w
771 normal = n
772 normal /= lenorm
773 # direction and angle
774 direction = np.dot(M33 - np.identity(3), normal)
775 angle = vector_norm(direction)
776 direction /= angle
777 angle = np.arctan(angle)
778 # point: eigenvector corresponding to eigenvalue 1
779 w, V = np.linalg.eig(M)
781 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
782 if not len(i):
783 raise ValueError("no eigenvector corresponding to eigenvalue 1")
784 point = np.real(V[:, i[-1]]).squeeze()
785 point /= point[3]
786 return angle, direction, point, normal
789def decompose_matrix(matrix):
790 """Return sequence of transformations from transformation matrix.
792 matrix : array_like
793 Non-degenerative homogeneous transformation matrix
795 Return tuple of:
796 scale : vector of 3 scaling factors
797 shear : list of shear factors for x-y, x-z, y-z axes
798 angles : list of Euler angles about static x, y, z axes
799 translate : translation vector along x, y, z axes
800 perspective : perspective partition of matrix
802 Raise ValueError if matrix is of wrong type or degenerative.
804 >>> T0 = translation_matrix([1, 2, 3])
805 >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
806 >>> T1 = translation_matrix(trans)
807 >>> np.allclose(T0, T1)
808 True
809 >>> S = scale_matrix(0.123)
810 >>> scale, shear, angles, trans, persp = decompose_matrix(S)
811 >>> bool(np.isclose(scale[0], 0.123))
812 True
813 >>> R0 = euler_matrix(1, 2, 3)
814 >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
815 >>> R1 = euler_matrix(*angles)
816 >>> np.allclose(R0, R1)
817 True
819 """
820 M = np.array(matrix, dtype=np.float64).T
821 if abs(M[3, 3]) < _EPS:
822 raise ValueError("M[3, 3] is zero")
823 M /= M[3, 3]
824 P = M.copy()
825 P[:, 3] = 0.0, 0.0, 0.0, 1.0
826 if not np.linalg.det(P):
827 raise ValueError("matrix is singular")
829 scale = np.zeros((3,))
830 shear = [0.0, 0.0, 0.0]
831 angles = [0.0, 0.0, 0.0]
833 if any(abs(M[:3, 3]) > _EPS):
834 perspective = np.dot(M[:, 3], np.linalg.inv(P.T))
835 M[:, 3] = 0.0, 0.0, 0.0, 1.0
836 else:
837 perspective = np.array([0.0, 0.0, 0.0, 1.0])
839 translate = M[3, :3].copy()
840 M[3, :3] = 0.0
842 row = M[:3, :3].copy()
843 scale[0] = vector_norm(row[0])
844 row[0] /= scale[0]
845 shear[0] = np.dot(row[0], row[1])
846 row[1] -= row[0] * shear[0]
847 scale[1] = vector_norm(row[1])
848 row[1] /= scale[1]
849 shear[0] /= scale[1]
850 shear[1] = np.dot(row[0], row[2])
851 row[2] -= row[0] * shear[1]
852 shear[2] = np.dot(row[1], row[2])
853 row[2] -= row[1] * shear[2]
854 scale[2] = vector_norm(row[2])
855 row[2] /= scale[2]
856 shear[1:] /= scale[2]
858 if np.dot(row[0], np.cross(row[1], row[2])) < 0:
859 np.negative(scale, scale)
860 np.negative(row, row)
862 angles[1] = np.arcsin(-row[0, 2])
863 if np.cos(angles[1]):
864 angles[0] = np.arctan2(row[1, 2], row[2, 2])
865 angles[2] = np.arctan2(row[0, 1], row[0, 0])
866 else:
867 angles[0] = np.arctan2(-row[2, 1], row[1, 1])
868 angles[2] = 0.0
870 return scale, shear, angles, translate, perspective
873def compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None):
874 """Return transformation matrix from sequence of transformations.
876 This is the inverse of the decompose_matrix function.
878 Sequence of transformations:
879 scale : vector of 3 scaling factors
880 shear : list of shear factors for x-y, x-z, y-z axes
881 angles : list of Euler angles about static x, y, z axes
882 translate : translation vector along x, y, z axes
883 perspective : perspective partition of matrix
885 >>> scale = np.random.random(3) - 0.5
886 >>> shear = np.random.random(3) - 0.5
887 >>> angles = (np.random.random(3) - 0.5) * (2*np.pi)
888 >>> trans = np.random.random(3) - 0.5
889 >>> persp = np.random.random(4) - 0.5
890 >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
891 >>> result = decompose_matrix(M0)
892 >>> M1 = compose_matrix(*result)
893 >>> is_same_transform(M0, M1)
894 True
896 """
897 M = np.identity(4)
898 if perspective is not None:
899 P = np.identity(4)
900 P[3, :] = perspective[:4]
901 M = np.dot(M, P)
902 if translate is not None:
903 T = np.identity(4)
904 T[:3, 3] = translate[:3]
905 M = np.dot(M, T)
906 if angles is not None:
907 R = euler_matrix(angles[0], angles[1], angles[2], "sxyz")
908 M = np.dot(M, R)
909 if shear is not None:
910 Z = np.identity(4)
911 Z[1, 2] = shear[2]
912 Z[0, 2] = shear[1]
913 Z[0, 1] = shear[0]
914 M = np.dot(M, Z)
915 if scale is not None:
916 S = np.identity(4)
917 S[0, 0] = scale[0]
918 S[1, 1] = scale[1]
919 S[2, 2] = scale[2]
920 M = np.dot(M, S)
921 M /= M[3, 3]
922 return M
925def orthogonalization_matrix(lengths, angles):
926 """Return orthogonalization matrix for crystallographic cell coordinates.
928 Angles are expected in degrees.
930 The de-orthogonalization matrix is the inverse.
932 >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
933 >>> np.allclose(O[:3, :3], np.identity(3, float) * 10)
934 True
935 >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
936 >>> np.allclose(np.sum(O), 43.063229)
937 True
939 """
940 a, b, c = lengths
941 angles = np.radians(angles)
942 sina, sinb, _ = np.sin(angles)
943 cosa, cosb, cosg = np.cos(angles)
944 co = (cosa * cosb - cosg) / (sina * sinb)
945 return np.array(
946 [
947 [a * sinb * np.sqrt(1.0 - co * co), 0.0, 0.0, 0.0],
948 [-a * sinb * co, b * sina, 0.0, 0.0],
949 [a * cosb, b * cosa, c, 0.0],
950 [0.0, 0.0, 0.0, 1.0],
951 ]
952 )
955def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
956 """Return affine transform matrix to register two point sets.
958 v0 and v1 are shape (ndims, *) arrays of at least ndims non-homogeneous
959 coordinates, where ndims is the dimensionality of the coordinate space.
961 If shear is False, a similarity transformation matrix is returned.
962 If also scale is False, a rigid/Euclidean transformation matrix
963 is returned.
965 By default the algorithm by Hartley and Zissermann [15] is used.
966 If usesvd is True, similarity and Euclidean transformation matrices
967 are calculated by minimizing the weighted sum of squared deviations
968 (RMSD) according to the algorithm by Kabsch [8].
969 Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]
970 is used, which is slower when using this Python implementation.
972 The returned matrix performs rotation, translation and uniform scaling
973 (if specified).
975 >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
976 >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
977 >>> mat = affine_matrix_from_points(v0, v1)
978 >>> T = translation_matrix(np.random.random(3)-0.5)
979 >>> R = random_rotation_matrix(np.random.random(3))
980 >>> S = scale_matrix(random.random())
981 >>> M = concatenate_matrices(T, R, S)
982 >>> v0 = (np.random.rand(4, 100) - 0.5) * 20
983 >>> v0[3] = 1
984 >>> v1 = np.dot(M, v0)
985 >>> v0[:3] += np.random.normal(0, 1e-8, 300).reshape(3, -1)
986 >>> M = affine_matrix_from_points(v0[:3], v1[:3])
987 >>> check = np.allclose(v1, np.dot(M, v0))
989 More examples in superimposition_matrix()
991 """
992 v0 = np.array(v0, dtype=np.float64)
993 v1 = np.array(v1, dtype=np.float64)
995 ndims = v0.shape[0]
996 if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
997 raise ValueError("input arrays are of wrong shape or type")
999 # move centroids to origin
1000 t0 = -np.mean(v0, axis=1)
1001 M0 = np.identity(ndims + 1)
1002 M0[:ndims, ndims] = t0
1003 v0 += t0.reshape(ndims, 1)
1004 t1 = -np.mean(v1, axis=1)
1005 M1 = np.identity(ndims + 1)
1006 M1[:ndims, ndims] = t1
1007 v1 += t1.reshape(ndims, 1)
1009 if shear:
1010 # Affine transformation
1011 A = np.concatenate((v0, v1), axis=0)
1012 u, s, vh = np.linalg.svd(A.T)
1013 vh = vh[:ndims].T
1014 B = vh[:ndims]
1015 C = vh[ndims : 2 * ndims]
1016 t = np.dot(C, np.linalg.pinv(B))
1017 t = np.concatenate((t, np.zeros((ndims, 1))), axis=1)
1018 M = np.vstack((t, ((0.0,) * ndims) + (1.0,)))
1019 elif usesvd or ndims != 3:
1020 # Rigid transformation via SVD of covariance matrix
1021 u, s, vh = np.linalg.svd(np.dot(v1, v0.T))
1022 # rotation matrix from SVD orthonormal bases
1023 R = np.dot(u, vh)
1024 if np.linalg.det(R) < 0.0:
1025 # R does not constitute right handed system
1026 R -= np.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)
1027 s[-1] *= -1.0
1028 # homogeneous transformation matrix
1029 M = np.identity(ndims + 1)
1030 M[:ndims, :ndims] = R
1031 else:
1032 # Rigid transformation matrix via quaternion
1033 # compute symmetric matrix N
1034 xx, yy, zz = np.sum(v0 * v1, axis=1)
1035 xy, yz, zx = np.sum(v0 * np.roll(v1, -1, axis=0), axis=1)
1036 xz, yx, zy = np.sum(v0 * np.roll(v1, -2, axis=0), axis=1)
1037 N = [
1038 [xx + yy + zz, 0.0, 0.0, 0.0],
1039 [yz - zy, xx - yy - zz, 0.0, 0.0],
1040 [zx - xz, xy + yx, yy - xx - zz, 0.0],
1041 [xy - yx, zx + xz, yz + zy, zz - xx - yy],
1042 ]
1043 # quaternion: eigenvector corresponding to most positive eigenvalue
1044 w, V = np.linalg.eigh(N)
1045 q = V[:, np.argmax(w)]
1046 q /= vector_norm(q) # unit quaternion
1047 # homogeneous transformation matrix
1048 M = quaternion_matrix(q)
1050 if scale and not shear:
1051 # Affine transformation; scale is ratio of RMS deviations from centroid
1052 v0 *= v0
1053 v1 *= v1
1054 M[:ndims, :ndims] *= np.sqrt(np.sum(v1) / np.sum(v0))
1056 # move centroids back
1057 M = np.dot(np.linalg.inv(M1), np.dot(M, M0))
1058 M /= M[ndims, ndims]
1059 return M
1062def superimposition_matrix(v0, v1, scale=False, usesvd=True):
1063 """Return matrix to transform given 3D point set into second point set.
1065 v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 points.
1067 The parameters scale and usesvd are explained in the more general
1068 affine_matrix_from_points function.
1070 The returned matrix is a similarity or Euclidean transformation matrix.
1071 This function has a fast C implementation in transformations.c.
1073 >>> v0 = np.random.rand(3, 10)
1074 >>> M = superimposition_matrix(v0, v0)
1075 >>> np.allclose(M, np.identity(4))
1076 True
1077 >>> R = random_rotation_matrix(np.random.random(3))
1078 >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]]
1079 >>> v1 = np.dot(R, v0)
1080 >>> M = superimposition_matrix(v0, v1)
1081 >>> np.allclose(v1, np.dot(M, v0))
1082 True
1083 >>> v0 = (np.random.rand(4, 100) - 0.5) * 20
1084 >>> v0[3] = 1
1085 >>> v1 = np.dot(R, v0)
1086 >>> M = superimposition_matrix(v0, v1)
1087 >>> np.allclose(v1, np.dot(M, v0))
1088 True
1089 >>> S = scale_matrix(random.random())
1090 >>> T = translation_matrix(np.random.random(3)-0.5)
1091 >>> M = concatenate_matrices(T, R, S)
1092 >>> v1 = np.dot(M, v0)
1093 >>> v0[:3] += np.random.normal(0, 1e-9, 300).reshape(3, -1)
1094 >>> M = superimposition_matrix(v0, v1, scale=True)
1095 >>> np.allclose(v1, np.dot(M, v0))
1096 True
1097 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1098 >>> np.allclose(v1, np.dot(M, v0))
1099 True
1100 >>> v = np.zeros((4, 100, 3))
1101 >>> v[:, :, 0] = v0
1102 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1103 >>> np.allclose(v1, np.dot(M, v[:, :, 0]))
1104 True
1106 """
1107 v0 = np.asarray(v0, dtype=np.float64)[:3]
1108 v1 = np.asarray(v1, dtype=np.float64)[:3]
1109 return affine_matrix_from_points(v0, v1, shear=False, scale=scale, usesvd=usesvd)
1112def euler_matrix(ai, aj, ak, axes="sxyz"):
1113 """Return homogeneous rotation matrix from Euler angles and axis sequence.
1115 ai, aj, ak : Euler's roll, pitch and yaw angles
1116 axes : One of 24 axis sequences as string or encoded tuple
1118 >>> R = euler_matrix(1, 2, 3, 'syxz')
1119 >>> np.allclose(np.sum(R[0]), -1.34786452)
1120 True
1121 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
1122 >>> np.allclose(np.sum(R[0]), -0.383436184)
1123 True
1124 >>> ai, aj, ak = (4*np.pi) * (np.random.random(3) - 0.5)
1125 >>> for axes in _AXES2TUPLE.keys():
1126 ... R = euler_matrix(ai, aj, ak, axes)
1127 >>> for axes in _TUPLE2AXES.keys():
1128 ... R = euler_matrix(ai, aj, ak, axes)
1130 """
1131 try:
1132 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
1133 except (AttributeError, KeyError):
1134 _TUPLE2AXES[axes] # validation
1135 firstaxis, parity, repetition, frame = axes
1137 i = firstaxis
1138 j = _NEXT_AXIS[i + parity]
1139 k = _NEXT_AXIS[i - parity + 1]
1141 if frame:
1142 ai, ak = ak, ai
1143 if parity:
1144 ai, aj, ak = -ai, -aj, -ak
1146 if "sympy" in str(type(ai)):
1147 # if we have been passed input values as sympy.Symbol
1148 # use symbolic cosine and identity matrix
1149 from sympy import cos, eye, sin
1151 M = eye(4)
1152 else:
1153 sin, cos = np.sin, np.cos
1154 M = np.eye(4)
1156 si, sj, sk = sin(ai), sin(aj), sin(ak)
1157 ci, cj, ck = cos(ai), cos(aj), cos(ak)
1158 cc, cs = ci * ck, ci * sk
1159 sc, ss = si * ck, si * sk
1161 if repetition:
1162 M[i, i] = cj
1163 M[i, j] = sj * si
1164 M[i, k] = sj * ci
1165 M[j, i] = sj * sk
1166 M[j, j] = -cj * ss + cc
1167 M[j, k] = -cj * cs - sc
1168 M[k, i] = -sj * ck
1169 M[k, j] = cj * sc + cs
1170 M[k, k] = cj * cc - ss
1171 else:
1172 M[i, i] = cj * ck
1173 M[i, j] = sj * sc - cs
1174 M[i, k] = sj * cc + ss
1175 M[j, i] = cj * sk
1176 M[j, j] = sj * ss + cc
1177 M[j, k] = sj * cs - sc
1178 M[k, i] = -sj
1179 M[k, j] = cj * si
1180 M[k, k] = cj * ci
1181 return M
1184def euler_from_matrix(matrix, axes="sxyz"):
1185 """Return Euler angles from rotation matrix for specified axis sequence.
1187 axes : One of 24 axis sequences as string or encoded tuple
1189 Note that many Euler angle triplets can describe one matrix.
1191 >>> R0 = euler_matrix(1, 2, 3, 'syxz')
1192 >>> al, be, ga = euler_from_matrix(R0, 'syxz')
1193 >>> R1 = euler_matrix(al, be, ga, 'syxz')
1194 >>> np.allclose(R0, R1)
1195 True
1196 >>> angles = (4*np.pi) * (np.random.random(3) - 0.5)
1197 >>> for axes in _AXES2TUPLE.keys():
1198 ... R0 = euler_matrix(axes=axes, *angles)
1199 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
1200 ... if not np.allclose(R0, R1): print(axes, "failed")
1202 """
1203 try:
1204 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1205 except (AttributeError, KeyError):
1206 _TUPLE2AXES[axes] # validation
1207 firstaxis, parity, repetition, frame = axes
1209 i = firstaxis
1210 j = _NEXT_AXIS[i + parity]
1211 k = _NEXT_AXIS[i - parity + 1]
1213 M = np.asarray(matrix, dtype=np.float64)[:3, :3]
1214 if repetition:
1215 sy = np.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
1216 if sy > _EPS:
1217 ax = np.arctan2(M[i, j], M[i, k])
1218 ay = np.arctan2(sy, M[i, i])
1219 az = np.arctan2(M[j, i], -M[k, i])
1220 else:
1221 ax = np.arctan2(-M[j, k], M[j, j])
1222 ay = np.arctan2(sy, M[i, i])
1223 az = 0.0
1224 else:
1225 cy = np.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
1226 if cy > _EPS:
1227 ax = np.arctan2(M[k, j], M[k, k])
1228 ay = np.arctan2(-M[k, i], cy)
1229 az = np.arctan2(M[j, i], M[i, i])
1230 else:
1231 ax = np.arctan2(-M[j, k], M[j, j])
1232 ay = np.arctan2(-M[k, i], cy)
1233 az = 0.0
1235 if parity:
1236 ax, ay, az = -ax, -ay, -az
1237 if frame:
1238 ax, az = az, ax
1239 return ax, ay, az
1242def euler_from_quaternion(quaternion, axes="sxyz"):
1243 """Return Euler angles from quaternion for specified axis sequence.
1245 >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
1246 >>> np.allclose(angles, [0.123, 0, 0])
1247 True
1249 """
1250 return euler_from_matrix(quaternion_matrix(quaternion), axes)
1253def quaternion_from_euler(ai, aj, ak, axes="sxyz"):
1254 """Return quaternion from Euler angles and axis sequence.
1256 ai, aj, ak : Euler's roll, pitch and yaw angles
1257 axes : One of 24 axis sequences as string or encoded tuple
1259 >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
1260 >>> np.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
1261 True
1263 """
1264 try:
1265 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1266 except (AttributeError, KeyError):
1267 _TUPLE2AXES[axes] # validation
1268 firstaxis, parity, repetition, frame = axes
1270 i = firstaxis + 1
1271 j = _NEXT_AXIS[i + parity - 1] + 1
1272 k = _NEXT_AXIS[i - parity] + 1
1274 if frame:
1275 ai, ak = ak, ai
1276 if parity:
1277 aj = -aj
1279 ai /= 2.0
1280 aj /= 2.0
1281 ak /= 2.0
1282 ci = np.cos(ai)
1283 si = np.sin(ai)
1284 cj = np.cos(aj)
1285 sj = np.sin(aj)
1286 ck = np.cos(ak)
1287 sk = np.sin(ak)
1288 cc = ci * ck
1289 cs = ci * sk
1290 sc = si * ck
1291 ss = si * sk
1293 q = np.zeros((4,))
1294 if repetition:
1295 q[0] = cj * (cc - ss)
1296 q[i] = cj * (cs + sc)
1297 q[j] = sj * (cc + ss)
1298 q[k] = sj * (cs - sc)
1299 else:
1300 q[0] = cj * cc + sj * ss
1301 q[i] = cj * sc - sj * cs
1302 q[j] = cj * ss + sj * cc
1303 q[k] = cj * cs - sj * sc
1304 if parity:
1305 q[j] *= -1.0
1307 return q
1310def quaternion_about_axis(angle, axis):
1311 """Return quaternion for rotation about axis.
1313 >>> q = quaternion_about_axis(0.123, [1, 0, 0])
1314 >>> np.allclose(q, [0.99810947, 0.06146124, 0, 0])
1315 True
1317 """
1318 q = np.array([0.0, axis[0], axis[1], axis[2]])
1319 qlen = vector_norm(q)
1320 if qlen > _EPS:
1321 q *= np.sin(angle / 2.0) / qlen
1322 q[0] = np.cos(angle / 2.0)
1323 return q
1326def quaternion_matrix(quaternion):
1327 """
1328 Return a homogeneous rotation matrix from quaternion.
1330 >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
1331 >>> np.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
1332 True
1333 >>> M = quaternion_matrix([1, 0, 0, 0])
1334 >>> np.allclose(M, np.identity(4))
1335 True
1336 >>> M = quaternion_matrix([0, 1, 0, 0])
1337 >>> np.allclose(M, np.diag([1, -1, -1, 1]))
1338 True
1339 >>> M = quaternion_matrix([[1, 0, 0, 0],[0, 1, 0, 0]])
1340 >>> np.allclose(M, np.array([np.identity(4), np.diag([1, -1, -1, 1])]))
1341 True
1344 """
1345 q = np.array(quaternion, dtype=np.float64).reshape((-1, 4))
1346 n = diagonal_dot(q, q)
1347 # how many entries do we have
1348 num_qs = len(n)
1349 identities = n < _EPS
1350 q[~identities, :] *= np.sqrt(2.0 / n[~identities, None])
1351 q = q[:, None, :] * q[:, :, None]
1353 # store the result
1354 ret = np.zeros((num_qs, 4, 4))
1356 # pack the values into the result
1357 ret[:, 0, 0] = 1.0 - q[:, 2, 2] - q[:, 3, 3]
1358 ret[:, 0, 1] = q[:, 1, 2] - q[:, 3, 0]
1359 ret[:, 0, 2] = q[:, 1, 3] + q[:, 2, 0]
1360 ret[:, 1, 0] = q[:, 1, 2] + q[:, 3, 0]
1361 ret[:, 1, 1] = 1.0 - q[:, 1, 1] - q[:, 3, 3]
1362 ret[:, 1, 2] = q[:, 2, 3] - q[:, 1, 0]
1363 ret[:, 2, 0] = q[:, 1, 3] - q[:, 2, 0]
1364 ret[:, 2, 1] = q[:, 2, 3] + q[:, 1, 0]
1365 ret[:, 2, 2] = 1.0 - q[:, 1, 1] - q[:, 2, 2]
1366 ret[:, 3, 3] = 1.0
1367 # set any identities
1368 ret[identities] = np.eye(4)[None, ...]
1370 return ret.squeeze()
1373def quaternion_from_matrix(matrix, isprecise=False):
1374 """Return quaternion from rotation matrix.
1376 If isprecise is True, the input matrix is assumed to be a precise rotation
1377 matrix and a faster algorithm is used.
1379 >>> q = quaternion_from_matrix(np.identity(4), True)
1380 >>> np.allclose(q, [1, 0, 0, 0])
1381 True
1382 >>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))
1383 >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])
1384 True
1385 >>> R = rotation_matrix(0.123, (1, 2, 3))
1386 >>> q = quaternion_from_matrix(R, True)
1387 >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
1388 True
1389 >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
1390 ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
1391 >>> q = quaternion_from_matrix(R)
1392 >>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
1393 True
1394 >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
1395 ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
1396 >>> q = quaternion_from_matrix(R)
1397 >>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
1398 True
1399 >>> R = random_rotation_matrix()
1400 >>> q = quaternion_from_matrix(R)
1401 >>> is_same_transform(R, quaternion_matrix(q))
1402 True
1403 >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1404 ... quaternion_from_matrix(R, isprecise=True))
1405 True
1406 >>> R = euler_matrix(0.0, 0.0, np.pi/2.0)
1407 >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1408 ... quaternion_from_matrix(R, isprecise=True))
1409 True
1411 """
1412 M = np.asarray(matrix, dtype=np.float64)[:4, :4]
1413 if isprecise:
1414 q = np.zeros((4,))
1415 t = np.trace(M)
1416 if t > M[3, 3]:
1417 q[0] = t
1418 q[3] = M[1, 0] - M[0, 1]
1419 q[2] = M[0, 2] - M[2, 0]
1420 q[1] = M[2, 1] - M[1, 2]
1421 else:
1422 i, j, k = 0, 1, 2
1423 if M[1, 1] > M[0, 0]:
1424 i, j, k = 1, 2, 0
1425 if M[2, 2] > M[i, i]:
1426 i, j, k = 2, 0, 1
1427 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
1428 q[i] = t
1429 q[j] = M[i, j] + M[j, i]
1430 q[k] = M[k, i] + M[i, k]
1431 q[3] = M[k, j] - M[j, k]
1432 q = q[[3, 0, 1, 2]]
1433 q *= 0.5 / np.sqrt(t * M[3, 3])
1434 else:
1435 m00 = M[0, 0]
1436 m01 = M[0, 1]
1437 m02 = M[0, 2]
1438 m10 = M[1, 0]
1439 m11 = M[1, 1]
1440 m12 = M[1, 2]
1441 m20 = M[2, 0]
1442 m21 = M[2, 1]
1443 m22 = M[2, 2]
1444 # symmetric matrix K
1445 K = np.array(
1446 [
1447 [m00 - m11 - m22, 0.0, 0.0, 0.0],
1448 [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
1449 [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
1450 [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22],
1451 ]
1452 )
1453 K /= 3.0
1454 # quaternion is eigenvector of K that corresponds to largest eigenvalue
1455 w, V = np.linalg.eigh(K)
1456 q = V[[3, 0, 1, 2], np.argmax(w)]
1457 if q[0] < 0.0:
1458 np.negative(q, q)
1459 return q
1462def quaternion_multiply(quaternion1, quaternion0):
1463 """Return multiplication of two quaternions.
1465 >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
1466 >>> np.allclose(q, [28, -44, -14, 48])
1467 True
1469 """
1470 w0, x0, y0, z0 = quaternion0
1471 w1, x1, y1, z1 = quaternion1
1472 return np.array(
1473 [
1474 -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
1475 x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
1476 -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
1477 x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
1478 ],
1479 dtype=np.float64,
1480 )
1483def quaternion_conjugate(quaternion):
1484 """Return conjugate of quaternion.
1486 >>> q0 = random_quaternion()
1487 >>> q1 = quaternion_conjugate(q0)
1488 >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:])
1489 True
1491 """
1492 q = np.array(quaternion, dtype=np.float64)
1493 np.negative(q[1:], q[1:])
1494 return q
1497def quaternion_inverse(quaternion):
1498 """Return inverse of quaternion.
1500 >>> q0 = random_quaternion()
1501 >>> q1 = quaternion_inverse(q0)
1502 >>> np.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])
1503 True
1505 """
1506 q = np.array(quaternion, dtype=np.float64)
1507 np.negative(q[1:], q[1:])
1508 return q / np.dot(q, q)
1511def quaternion_real(quaternion):
1512 """Return real part of quaternion.
1514 >>> quaternion_real([3, 0, 1, 2])
1515 3.0
1517 """
1518 return float(quaternion[0])
1521def quaternion_imag(quaternion):
1522 """Return imaginary part of quaternion.
1524 >>> quaternion_imag([3, 0, 1, 2])
1525 array([0., 1., 2.])
1527 """
1528 return np.array(quaternion[1:4], dtype=np.float64)
1531def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
1532 """Return spherical linear interpolation between two quaternions.
1534 >>> q0 = random_quaternion()
1535 >>> q1 = random_quaternion()
1536 >>> q = quaternion_slerp(q0, q1, 0)
1537 >>> np.allclose(q, q0)
1538 True
1539 >>> q = quaternion_slerp(q0, q1, 1, 1)
1540 >>> np.allclose(q, q1)
1541 True
1542 >>> q = quaternion_slerp(q0, q1, 0.5)
1543 >>> angle = np.arccos(np.dot(q0, q))
1544 >>> np.allclose(2, np.arccos(np.dot(q0, q1)) / angle) or \
1545 np.allclose(2, np.arccos(-np.dot(q0, q1)) / angle)
1546 True
1548 """
1549 q0 = unit_vector(quat0[:4])
1550 q1 = unit_vector(quat1[:4])
1551 if fraction == 0.0:
1552 return q0
1553 elif fraction == 1.0:
1554 return q1
1555 d = np.dot(q0, q1)
1556 if abs(abs(d) - 1.0) < _EPS:
1557 return q0
1558 if shortestpath and d < 0.0:
1559 # invert rotation
1560 d = -d
1561 np.negative(q1, q1)
1562 angle = np.arccos(d) + spin * np.pi
1563 if abs(angle) < _EPS:
1564 return q0
1565 isin = 1.0 / np.sin(angle)
1566 q0 *= np.sin((1.0 - fraction) * angle) * isin
1567 q1 *= np.sin(fraction * angle) * isin
1568 q0 += q1
1569 return q0
1572def random_quaternion(rand=None, num=1, seed: Seed = None):
1573 """Return uniform random unit quaternion.
1575 rand: array like or None
1576 Three independent random variables that are uniformly distributed
1577 between 0 and 1.
1578 seed : None or int
1579 Seed for deterministic results, otherwise OS entropy.
1581 >>> q = random_quaternion()
1582 >>> np.allclose(1, vector_norm(q))
1583 True
1584 >>> q = random_quaternion(num=10)
1585 >>> np.allclose(1, vector_norm(q, axis=1))
1586 True
1587 >>> q = random_quaternion(random_vector(3))
1588 >>> len(q.shape), q.shape[0]==4
1589 (1, True)
1590 >>> bool(np.allclose(random_quaternion(seed=0), random_quaternion(seed=0)))
1591 True
1593 """
1594 if rand is None:
1595 rand = random_generator(seed).random(3 * num).reshape((3, -1))
1596 else:
1597 assert rand.shape[0] == 3
1598 r1 = np.sqrt(1.0 - rand[0])
1599 r2 = np.sqrt(rand[0])
1600 pi2 = np.pi * 2.0
1601 t1 = pi2 * rand[1]
1602 t2 = pi2 * rand[2]
1603 return np.array(
1604 [np.cos(t2) * r2, np.sin(t1) * r1, np.cos(t1) * r1, np.sin(t2) * r2]
1605 ).T.squeeze()
1608def random_rotation_matrix(
1609 rand: ArrayLike | None = None,
1610 num: Integer = 1,
1611 translate: Number | None = None,
1612 seed: Seed = None,
1613):
1614 """
1615 Return uniform random rotation matrix.
1617 Parameters
1618 ------------
1619 rand : (3,)
1620 Three independent random variables that are uniformly distributed
1621 between 0 and 1 for each returned quaternion.
1622 num
1623 Number of matrices to return.
1624 translate
1625 If passed the rotation matrix will include translation
1626 that is random and between positive and negative half this value.
1627 seed
1628 Seed for deterministic results, otherwise OS entropy.
1630 >>> R = random_rotation_matrix()
1631 >>> np.allclose(np.dot(R.T, R), np.identity(4))
1632 True
1633 >>> R = random_rotation_matrix(num=10)
1634 >>> np.allclose(np.einsum('...ji,...jk->...ik', R, R), np.identity(4))
1635 True
1636 >>> bool(np.allclose(random_rotation_matrix(seed=0, translate=10),
1637 ... random_rotation_matrix(seed=0, translate=10)))
1638 True
1640 """
1641 # thread one generator through so a passed `seed` doesn't hand the
1642 # translation the same values as the quaternion below
1643 random = random_generator(seed)
1644 matrix = quaternion_matrix(random_quaternion(rand=rand, num=num, seed=random))
1645 if translate:
1646 # apply random translation with the order of magnitude requested
1647 matrix[:3, 3] = (random.random(3) - 0.5) * float(translate)
1649 return matrix
1652class Arcball:
1653 """Virtual Trackball Control.
1655 >>> ball = Arcball()
1656 >>> ball = Arcball(initial=np.identity(4))
1657 >>> ball.place([320, 320], 320)
1658 >>> ball.down([500, 250])
1659 >>> ball.drag([475, 275])
1660 >>> R = ball.matrix()
1661 >>> np.allclose(np.sum(R), 3.90583455)
1662 True
1663 >>> ball = Arcball(initial=[1, 0, 0, 0])
1664 >>> ball.place([320, 320], 320)
1665 >>> ball.setaxes([1, 1, 0], [-1, 1, 0])
1666 >>> ball.constrain = True
1667 >>> ball.down([400, 200])
1668 >>> ball.drag([200, 400])
1669 >>> R = ball.matrix()
1670 >>> np.allclose(np.sum(R), 0.2055924)
1671 True
1672 >>> ball.next()
1674 """
1676 def __init__(self, initial=None):
1677 """Initialize virtual trackball control.
1679 initial : quaternion or rotation matrix
1681 """
1682 self._axis = None
1683 self._axes = None
1684 self._radius = 1.0
1685 self._center = [0.0, 0.0]
1686 self._vdown = np.array([0.0, 0.0, 1.0])
1687 self._constrain = False
1688 if initial is None:
1689 self._qdown = np.array([1.0, 0.0, 0.0, 0.0])
1690 else:
1691 initial = np.array(initial, dtype=np.float64)
1692 if initial.shape == (4, 4):
1693 self._qdown = quaternion_from_matrix(initial)
1694 elif initial.shape == (4,):
1695 initial /= vector_norm(initial)
1696 self._qdown = initial
1697 else:
1698 raise ValueError("initial not a quaternion or matrix")
1699 self._qnow = self._qpre = self._qdown
1701 def place(self, center, radius):
1702 """Place Arcball, e.g. when window size changes.
1704 center : sequence[2]
1705 Window coordinates of trackball center.
1706 radius : float
1707 Radius of trackball in window coordinates.
1709 """
1710 self._radius = float(radius)
1711 self._center[0] = center[0]
1712 self._center[1] = center[1]
1714 def setaxes(self, *axes):
1715 """Set axes to constrain rotations."""
1716 if axes is None:
1717 self._axes = None
1718 else:
1719 self._axes = [unit_vector(axis) for axis in axes]
1721 @property
1722 def constrain(self):
1723 """Return state of constrain to axis mode."""
1724 return self._constrain
1726 @constrain.setter
1727 def constrain(self, value):
1728 """Set state of constrain to axis mode."""
1729 self._constrain = bool(value)
1731 def down(self, point):
1732 """Set initial cursor window coordinates and pick constrain-axis."""
1733 self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
1734 self._qdown = self._qpre = self._qnow
1735 if self._constrain and self._axes is not None:
1736 self._axis = arcball_nearest_axis(self._vdown, self._axes)
1737 self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
1738 else:
1739 self._axis = None
1741 def drag(self, point):
1742 """Update current cursor window coordinates."""
1743 vnow = arcball_map_to_sphere(point, self._center, self._radius)
1744 if self._axis is not None:
1745 vnow = arcball_constrain_to_axis(vnow, self._axis)
1746 self._qpre = self._qnow
1747 t = np.cross(self._vdown, vnow)
1748 if np.dot(t, t) < _EPS:
1749 self._qnow = self._qdown
1750 else:
1751 q = [np.dot(self._vdown, vnow), t[0], t[1], t[2]]
1752 self._qnow = quaternion_multiply(q, self._qdown)
1754 def next(self, acceleration=0.0):
1755 """Continue rotation in direction of last drag."""
1756 q = quaternion_slerp(self._qpre, self._qnow, 2.0 + acceleration, False)
1757 self._qpre, self._qnow = self._qnow, q
1759 def matrix(self):
1760 """Return homogeneous rotation matrix."""
1761 return quaternion_matrix(self._qnow)
1764def arcball_map_to_sphere(point, center, radius):
1765 """Return unit sphere coordinates from window coordinates."""
1766 v0 = (point[0] - center[0]) / radius
1767 v1 = (center[1] - point[1]) / radius
1768 n = v0 * v0 + v1 * v1
1769 if n > 1.0:
1770 # position outside of sphere
1771 n = np.sqrt(n)
1772 return np.array([v0 / n, v1 / n, 0.0])
1773 else:
1774 return np.array([v0, v1, np.sqrt(1.0 - n)])
1777def arcball_constrain_to_axis(point, axis):
1778 """Return sphere point perpendicular to axis."""
1779 v = np.array(point, dtype=np.float64)
1780 a = np.array(axis, dtype=np.float64)
1781 v -= a * np.dot(a, v) # on plane
1782 n = vector_norm(v)
1783 if n > _EPS:
1784 if v[2] < 0.0:
1785 np.negative(v, v)
1786 v /= n
1787 return v
1788 if a[2] == 1.0:
1789 return np.array([1.0, 0.0, 0.0])
1790 return unit_vector([-a[1], a[0], 0.0])
1793def arcball_nearest_axis(point, axes):
1794 """Return axis, which arc is nearest to point."""
1795 point = np.asarray(point, dtype=np.float64)
1796 nearest = None
1797 mx = -1.0
1798 for axis in axes:
1799 t = np.dot(arcball_constrain_to_axis(point, axis), point)
1800 if t > mx:
1801 nearest = axis
1802 mx = t
1803 return nearest
1806# epsilon for testing whether a number is close to zero
1807_EPS = np.finfo(float).eps * 4.0
1809# axis sequences for Euler angles
1810_NEXT_AXIS = [1, 2, 0, 1]
1812# map axes strings to/from tuples of inner axis, parity, repetition, frame
1813_AXES2TUPLE = {
1814 "sxyz": (0, 0, 0, 0),
1815 "sxyx": (0, 0, 1, 0),
1816 "sxzy": (0, 1, 0, 0),
1817 "sxzx": (0, 1, 1, 0),
1818 "syzx": (1, 0, 0, 0),
1819 "syzy": (1, 0, 1, 0),
1820 "syxz": (1, 1, 0, 0),
1821 "syxy": (1, 1, 1, 0),
1822 "szxy": (2, 0, 0, 0),
1823 "szxz": (2, 0, 1, 0),
1824 "szyx": (2, 1, 0, 0),
1825 "szyz": (2, 1, 1, 0),
1826 "rzyx": (0, 0, 0, 1),
1827 "rxyx": (0, 0, 1, 1),
1828 "ryzx": (0, 1, 0, 1),
1829 "rxzx": (0, 1, 1, 1),
1830 "rxzy": (1, 0, 0, 1),
1831 "ryzy": (1, 0, 1, 1),
1832 "rzxy": (1, 1, 0, 1),
1833 "ryxy": (1, 1, 1, 1),
1834 "ryxz": (2, 0, 0, 1),
1835 "rzxz": (2, 0, 1, 1),
1836 "rxyz": (2, 1, 0, 1),
1837 "rzyz": (2, 1, 1, 1),
1838}
1840_TUPLE2AXES = {v: k for k, v in _AXES2TUPLE.items()}
1843def vector_norm(data, axis=None, out=None):
1844 """Return length, i.e. Euclidean norm, of ndarray along axis.
1846 >>> v = np.random.random(3)
1847 >>> n = vector_norm(v)
1848 >>> np.allclose(n, np.linalg.norm(v))
1849 True
1850 >>> v = np.random.rand(6, 5, 3)
1851 >>> n = vector_norm(v, axis=-1)
1852 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=2)))
1853 True
1854 >>> n = vector_norm(v, axis=1)
1855 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
1856 True
1857 >>> v = np.random.rand(5, 4, 3)
1858 >>> n = np.zeros((5, 3))
1859 >>> vector_norm(v, axis=1, out=n)
1860 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
1861 True
1862 >>> float(vector_norm([]))
1863 0.0
1864 >>> float(vector_norm([1]))
1865 1.0
1867 """
1868 data = np.array(data, dtype=np.float64)
1869 if out is None:
1870 if data.ndim == 1:
1871 return np.sqrt(np.dot(data, data))
1872 data *= data
1873 out = np.atleast_1d(np.sum(data, axis=axis))
1874 np.sqrt(out, out)
1875 return out
1876 else:
1877 data *= data
1878 np.sum(data, axis=axis, out=out)
1879 np.sqrt(out, out)
1882def unit_vector(data, axis=None, out=None):
1883 """Return ndarray normalized by length, i.e. Euclidean norm, along axis.
1885 >>> v0 = np.random.random(3)
1886 >>> v1 = unit_vector(v0)
1887 >>> np.allclose(v1, v0 / np.linalg.norm(v0))
1888 True
1889 >>> v0 = np.random.rand(5, 4, 3)
1890 >>> v1 = unit_vector(v0, axis=-1)
1891 >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=2)), 2)
1892 >>> np.allclose(v1, v2)
1893 True
1894 >>> v1 = unit_vector(v0, axis=1)
1895 >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=1)), 1)
1896 >>> np.allclose(v1, v2)
1897 True
1898 >>> v1 = np.zeros((5, 4, 3))
1899 >>> unit_vector(v0, axis=1, out=v1)
1900 >>> np.allclose(v1, v2)
1901 True
1902 >>> list(unit_vector([]))
1903 []
1904 >>> [float(i) for i in unit_vector([1])]
1905 [1.0]
1907 """
1908 if out is None:
1909 data = np.array(data, dtype=np.float64)
1910 if data.ndim == 1:
1911 data /= np.sqrt(np.dot(data, data))
1912 return data
1913 else:
1914 if out is not data:
1915 out[:] = np.asarray(data)
1916 data = out
1917 length = np.atleast_1d(np.sum(data * data, axis))
1918 np.sqrt(length, length)
1919 if axis is not None:
1920 length = np.expand_dims(length, axis)
1921 data /= length
1922 if out is None:
1923 return data
1926def random_vector(size, seed: Seed = None):
1927 """Return array of random doubles in the half-open interval [0.0, 1.0).
1929 >>> v = random_vector(10000)
1930 >>> bool(np.all(v >= 0) and np.all(v < 1))
1931 True
1932 >>> v0 = random_vector(10)
1933 >>> v1 = random_vector(10)
1934 >>> bool(np.any(v0 == v1))
1935 False
1936 >>> bool(np.allclose(random_vector(10, seed=0), random_vector(10, seed=0)))
1937 True
1939 """
1940 return random_generator(seed).random(size)
1943def vector_product(v0, v1, axis=0):
1944 """Return vector perpendicular to vectors.
1946 >>> v = vector_product([2, 0, 0], [0, 3, 0])
1947 >>> np.allclose(v, [0, 0, 6])
1948 True
1949 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1950 >>> v1 = [[3], [0], [0]]
1951 >>> v = vector_product(v0, v1)
1952 >>> np.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])
1953 True
1954 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1955 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1956 >>> v = vector_product(v0, v1, axis=1)
1957 >>> np.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])
1958 True
1960 """
1961 return np.cross(v0, v1, axis=axis)
1964def angle_between_vectors(v0, v1, directed=True, axis=0):
1965 """Return angle between vectors.
1967 If directed is False, the input vectors are interpreted as undirected axes,
1968 i.e. the maximum angle is pi/2.
1970 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])
1971 >>> np.allclose(a, np.pi)
1972 True
1973 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)
1974 >>> np.allclose(a, 0)
1975 True
1976 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1977 >>> v1 = [[3], [0], [0]]
1978 >>> a = angle_between_vectors(v0, v1)
1979 >>> np.allclose(a, [0, 1.5708, 1.5708, 0.95532])
1980 True
1981 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1982 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1983 >>> a = angle_between_vectors(v0, v1, axis=1)
1984 >>> np.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])
1985 True
1987 """
1988 v0 = np.asarray(v0, dtype=np.float64)
1989 v1 = np.asarray(v1, dtype=np.float64)
1990 dot = np.sum(v0 * v1, axis=axis)
1991 dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)
1993 # clip off floating point error to avoid `nan` in the arccos`
1994 dot = np.clip(dot, -1.0, 1.0)
1995 return np.arccos(dot if directed else np.fabs(dot))
1998def inverse_matrix(matrix):
1999 """Return inverse of square transformation matrix.
2001 >>> M0 = random_rotation_matrix()
2002 >>> M1 = inverse_matrix(M0.T)
2003 >>> np.allclose(M1, np.linalg.inv(M0.T))
2004 True
2005 >>> for size in range(1, 7):
2006 ... M0 = np.random.rand(size, size)
2007 ... M1 = inverse_matrix(M0)
2008 ... if not np.allclose(M1, np.linalg.inv(M0)): print(size)
2010 """
2011 return np.linalg.inv(matrix)
2014def concatenate_matrices(*matrices):
2015 """Return concatenation of series of transformation matrices.
2017 >>> M = np.random.rand(16).reshape((4, 4)) - 0.5
2018 >>> np.allclose(M, concatenate_matrices(M))
2019 True
2020 >>> np.allclose(np.dot(M, M.T), concatenate_matrices(M, M.T))
2021 True
2023 """
2024 M = np.identity(4)
2025 for i in matrices:
2026 M = np.dot(M, i)
2027 return M
2030def is_same_transform(matrix0, matrix1):
2031 """Return True if two matrices perform same transformation.
2033 >>> is_same_transform(np.identity(4), np.identity(4))
2034 True
2035 >>> is_same_transform(np.identity(4), random_rotation_matrix())
2036 False
2038 """
2039 matrix0 = np.array(matrix0, dtype=np.float64)
2040 matrix0 /= matrix0[3, 3]
2041 matrix1 = np.array(matrix1, dtype=np.float64)
2042 matrix1 /= matrix1[3, 3]
2043 return np.allclose(matrix0, matrix1)
2046def is_same_quaternion(q0, q1):
2047 """Return True if two quaternions are equal."""
2048 q0 = np.array(q0)
2049 q1 = np.array(q1)
2050 return np.allclose(q0, q1) or np.allclose(q0, -q1)
2053def transform_around(matrix, point):
2054 """
2055 Given a transformation matrix, apply its rotation
2056 around a point in space.
2058 Parameters
2059 ----------
2060 matrix: (4,4) or (3, 3) float, transformation matrix
2061 point: (3,) or (2,) float, point in space
2063 Returns
2064 ---------
2065 result: (4,4) transformation matrix
2066 """
2067 point = np.asanyarray(point)
2068 matrix = np.asanyarray(matrix)
2069 dim = len(point)
2070 if matrix.shape != (dim + 1, dim + 1):
2071 raise ValueError("matrix must be (d+1, d+1)")
2073 translate = np.eye(dim + 1)
2074 translate[:dim, dim] = -point
2075 result = np.dot(matrix, translate)
2076 translate[:dim, dim] = point
2077 result = np.dot(translate, result)
2079 return result
2082def planar_matrix(offset=None, theta=None, point=None, scale=None):
2083 """
2084 2D homogeonous transformation matrix.
2086 Parameters
2087 ----------
2088 offset : (2,) float
2089 XY offset
2090 theta : float
2091 Rotation around Z in radians
2092 point : (2, ) float
2093 Point to rotate around
2094 scale : (2,) float or None
2095 Scale to apply
2097 Returns
2098 ----------
2099 matrix : (3, 3) flat
2100 Homogeneous 2D transformation matrix
2101 """
2102 if offset is None:
2103 offset = [0.0, 0.0]
2104 if theta is None:
2105 theta = 0.0
2106 offset = np.asanyarray(offset, dtype=np.float64)
2107 theta = float(theta)
2108 if not np.isfinite(theta):
2109 raise ValueError("theta must be finite angle!")
2110 if offset.shape != (2,):
2111 raise ValueError("offset must be length 2!")
2113 T = np.eye(3, dtype=np.float64)
2114 s = np.sin(theta)
2115 c = np.cos(theta)
2117 T[0, :2] = [c, s]
2118 T[1, :2] = [-s, c]
2119 T[:2, 2] = offset
2121 if point is not None:
2122 T = transform_around(matrix=T, point=point)
2124 if scale is not None:
2125 S = np.eye(3)
2126 S[:2, :2] *= scale
2127 T = np.dot(S, T)
2129 return T
2132def planar_matrix_to_3D(matrix_2D):
2133 """
2134 Given a 2D homogeneous rotation matrix convert it to a 3D rotation
2135 matrix that is rotating around the Z axis
2137 Parameters
2138 ----------
2139 matrix_2D: (3,3) float, homogeneous 2D rotation matrix
2141 Returns
2142 ----------
2143 matrix_3D: (4,4) float, homogeneous 3D rotation matrix
2144 """
2146 matrix_2D = np.asanyarray(matrix_2D, dtype=np.float64)
2147 if matrix_2D.shape != (3, 3):
2148 raise ValueError("Homogeneous 2D transformation matrix required!")
2150 matrix_3D = np.eye(4)
2151 # translation
2152 matrix_3D[:2, 3] = matrix_2D[:2, 2]
2153 # rotation from 2D to around Z
2154 matrix_3D[:2, :2] = matrix_2D[:2, :2]
2156 return matrix_3D
2159def spherical_matrix(theta, phi, axes="sxyz"):
2160 """
2161 Give a spherical coordinate vector, find the rotation that will
2162 transform a [0,0,1] vector to those coordinates
2164 Parameters
2165 -----------
2166 theta: float, rotation angle in radians
2167 phi: float, rotation angle in radians
2169 Returns
2170 ----------
2171 matrix: (4,4) rotation matrix where the following will
2172 be a cartesian vector in the direction of the
2173 input spherical coordinates:
2174 np.dot(matrix, [0,0,1,0])
2176 """
2177 result = euler_matrix(0.0, phi, theta, axes=axes)
2178 return result
2181def transform_points(
2182 points: ArrayLike, matrix: ArrayLike, translate: bool = True
2183) -> NDArray[np.float64]:
2184 """
2185 Returns points rotated by a homogeneous
2186 transformation matrix.
2188 If points are (n, 2) matrix must be (3, 3)
2189 If points are (n, 3) matrix must be (4, 4)
2191 Parameters
2192 ----------
2193 points : (n, dim) float
2194 Points where `dim` is 2 or 3.
2195 matrix : (3, 3) or (4, 4) float
2196 Homogeneous rotation matrix.
2197 translate : bool
2198 Apply translation from matrix or not.
2200 Returns
2201 ----------
2202 transformed : (n, dim) float
2203 Transformed points.
2204 """
2205 points = np.asanyarray(points, dtype=np.float64)
2206 if len(points) == 0 or matrix is None:
2207 return points.copy()
2209 # check the matrix against the points
2210 matrix = np.asanyarray(matrix, dtype=np.float64)
2211 # shorthand the shape
2212 count, dim = points.shape
2214 # quickly check to see if we've been passed an identity matrix
2215 if np.abs(matrix - _IDENTITY[: dim + 1, : dim + 1]).max() < 1e-8:
2216 return np.ascontiguousarray(points.copy())
2218 if translate:
2219 # apply translation and rotation
2220 stack = np.column_stack((points, np.ones(count)))
2221 return np.dot(matrix, stack.T).T[:, :dim]
2223 # only apply the rotation
2224 return np.dot(matrix[:dim, :dim], points.T).T
2227def fix_rigid(matrix, max_deviance=1e-5):
2228 """
2229 If a homogeneous transformation matrix is *almost* a rigid
2230 transform but many matrix-multiplies have accumulated some
2231 floating point error try to restore the matrix using SVD.
2233 Parameters
2234 -----------
2235 matrix : (4, 4) or (3, 3) float
2236 Homogeneous transformation matrix.
2237 max_deviance : float
2238 Do not alter the matrix if it is not rigid by more
2239 than this amount.
2241 Returns
2242 ----------
2243 repaired : (4, 4) or (3, 3) float
2244 Repaired homogeneous transformation matrix
2245 """
2246 dim = matrix.shape[0] - 1
2247 check = np.abs(
2248 np.dot(matrix[:dim, :dim], matrix[:dim, :dim].T) - _IDENTITY[:dim, :dim]
2249 ).max()
2250 # if the matrix differs by more than float-zero and less
2251 # than the threshold try to repair the matrix with SVD
2252 if check > 1e-13 and check < max_deviance:
2253 # reconstruct the rotation from the SVD
2254 U, _, V = np.linalg.svd(matrix[:dim, :dim])
2255 repaired = np.eye(dim + 1)
2256 repaired[:dim, :dim] = np.dot(U, V)
2257 # copy in the translation
2258 repaired[:dim, dim] = matrix[:dim, dim]
2259 # should be within tolerance of the original matrix
2260 assert np.allclose(repaired, matrix, atol=max_deviance)
2261 return repaired
2263 return matrix
2266def is_rigid(matrix, epsilon=1e-8):
2267 """
2268 Check to make sure a homogeonous transformation
2269 matrix is a rigid transform.
2271 Parameters
2272 -----------
2273 matrix : (4, 4) float
2274 A transformation matrix
2276 Returns
2277 -----------
2278 check : bool
2279 True if matrix is a a transform with only
2280 translation and rotation.
2281 """
2283 matrix = np.asanyarray(matrix, dtype=np.float64)
2285 if matrix.shape != (4, 4):
2286 return False
2288 # rows of a rotation dot to the identity
2289 check = np.dot(matrix[:3, :3], matrix[:3, :3].T).ravel() - _IDENTITY9
2290 # make sure last row has no perspective
2291 bottom = matrix[3] - _IDENTITY[3]
2293 # squared norms are a single dot and avoid `abs` and `ptp`
2294 return np.dot(bottom, bottom) < epsilon**2 and np.dot(check, check) < epsilon**2
2297def scale_and_translate(scale=None, translate=None):
2298 """
2299 Optimized version of `compose_matrix` for just
2300 scaling then translating.
2302 Scalar args are broadcast to arrays of shape (3,)
2304 Parameters
2305 --------------
2306 scale : float or (3,) float
2307 Scale factor
2308 translate : float or (3,) float
2309 Translation
2310 """
2311 M = np.eye(4)
2312 if np.any(scale != 1):
2313 M[:3, :3] *= scale
2314 if translate is not None:
2315 M[:3, 3] = translate
2316 return M
2319def flips_winding(matrix):
2320 """
2321 Check to see if a matrix will invert triangles.
2323 Parameters
2324 -------------
2325 matrix : (4, 4) float
2326 Homogeneous transformation matrix
2328 Returns
2329 --------------
2330 flip : bool
2331 True if matrix will flip winding of triangles.
2332 """
2333 # get input as numpy array
2334 matrix = np.asanyarray(matrix, dtype=np.float64)
2335 if matrix.shape not in ((3, 3), (4, 4)):
2336 raise ValueError(f"matrix must be (3, 3) or (4, 4), not {matrix.shape!s}")
2337 # a transform flips winding exactly when its determinant is negative
2338 return bool(np.linalg.det(matrix[:3, :3]) < 0.0)