Coverage for trimesh/transformations.py: 93%
815 statements
« prev ^ index » next coverage.py v7.14.1, created at 2026-06-24 04:40 +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
203from .util import diagonal_dot
205# a cached immutable identity matrix provides a speedup sometimes
206_IDENTITY = np.eye(4)
207_IDENTITY.flags["WRITEABLE"] = False
210def identity_matrix():
211 """Return 4x4 identity/unit matrix.
213 >>> I = identity_matrix()
214 >>> np.allclose(I, np.dot(I, I))
215 True
216 >>> float(np.sum(I)), float(np.trace(I))
217 (4.0, 4.0)
218 >>> np.allclose(I, np.identity(4))
219 True
221 """
222 return np.identity(4)
225def translation_matrix(direction):
226 """
227 Return matrix to translate by direction vector.
229 >>> v = np.random.random(3) - 0.5
230 >>> np.allclose(v, translation_matrix(v)[:3, 3])
231 True
233 """
234 # are we 2D or 3D
235 dim = len(direction)
237 # start with identity matrix
239 if any("sympy" in str(type(v)) for v in direction):
240 # if we have been passed input values as sympy.Symbol
241 from sympy import eye
243 M = eye(dim + 1)
244 else:
245 M = np.eye(dim + 1)
247 # apply the offset
248 M[:dim, dim] = direction[:dim]
250 return M
253def translation_from_matrix(matrix):
254 """Return translation vector from translation matrix.
256 >>> v0 = np.random.random(3) - 0.5
257 >>> v1 = translation_from_matrix(translation_matrix(v0))
258 >>> np.allclose(v0, v1)
259 True
261 """
262 return np.asarray(matrix)[:3, 3].copy()
265def reflection_matrix(point, normal):
266 """Return matrix to mirror at plane defined by point and normal vector.
268 >>> v0 = np.random.random(4) - 0.5
269 >>> v0[3] = 1.
270 >>> v1 = np.random.random(3) - 0.5
271 >>> R = reflection_matrix(v0, v1)
272 >>> np.allclose(2, np.trace(R))
273 True
274 >>> np.allclose(v0, np.dot(R, v0))
275 True
276 >>> v2 = v0.copy()
277 >>> v2[:3] += v1
278 >>> v3 = v0.copy()
279 >>> v2[:3] -= v1
280 >>> np.allclose(v2, np.dot(R, v3))
281 True
283 """
284 normal = unit_vector(normal[:3])
285 M = np.identity(4)
286 M[:3, :3] -= 2.0 * np.outer(normal, normal)
287 M[:3, 3] = (2.0 * np.dot(point[:3], normal)) * normal
288 return M
291def reflection_from_matrix(matrix):
292 """Return mirror plane point and normal vector from reflection matrix.
294 >>> v0 = np.random.random(3) - 0.5
295 >>> v1 = np.random.random(3) - 0.5
296 >>> M0 = reflection_matrix(v0, v1)
297 >>> point, normal = reflection_from_matrix(M0)
298 >>> M1 = reflection_matrix(point, normal)
299 >>> is_same_transform(M0, M1)
300 True
302 """
303 M = np.asarray(matrix, dtype=np.float64)
304 # normal: unit eigenvector corresponding to eigenvalue -1
305 w, V = np.linalg.eig(M[:3, :3])
306 i = np.where(abs(np.real(w) + 1.0) < 1e-8)[0]
307 if not len(i):
308 raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
309 normal = np.real(V[:, i[0]]).squeeze()
310 # point: any unit eigenvector corresponding to eigenvalue 1
311 w, V = np.linalg.eig(M)
312 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
313 if not len(i):
314 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
315 point = np.real(V[:, i[-1]]).squeeze()
316 point /= point[3]
317 return point, normal
320def rotation_matrix(angle, direction, point=None):
321 """
322 Return matrix to rotate about axis defined by point and
323 direction.
325 Parameters
326 -------------
327 angle : float, or sympy.Symbol
328 Angle, in radians or symbolic angle
329 direction : (3,) float
330 Any vector along rotation axis
331 point : (3, ) float, or None
332 Origin point of rotation axis
334 Returns
335 -------------
336 matrix : (4, 4) float, or (4, 4) sympy.Matrix
337 Homogeneous transformation matrix
339 Examples
340 -------------
341 >>> R = rotation_matrix(np.pi/2, [0, 0, 1], [1, 0, 0])
342 >>> np.allclose(np.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])
343 True
344 >>> angle = (random.random() - 0.5) * (2*np.pi)
345 >>> direc = np.random.random(3) - 0.5
346 >>> point = np.random.random(3) - 0.5
347 >>> R0 = rotation_matrix(angle, direc, point)
348 >>> R1 = rotation_matrix(angle-2*np.pi, direc, point)
349 >>> is_same_transform(R0, R1)
350 True
351 >>> R0 = rotation_matrix(angle, direc, point)
352 >>> R1 = rotation_matrix(-angle, -direc, point)
353 >>> is_same_transform(R0, R1)
354 True
355 >>> I = np.identity(4, np.float64)
356 >>> np.allclose(I, rotation_matrix(np.pi*2, direc))
357 True
358 >>> np.allclose(2, np.trace(rotation_matrix(np.pi/2,direc,point)))
359 True
361 """
362 if "sympy" in str(type(angle)):
363 # special case sympy symbolic angles
364 import sympy as sp
366 symbolic = True
367 sina = sp.sin(angle)
368 cosa = sp.cos(angle)
369 else:
370 symbolic = False
371 sina = np.sin(angle)
372 cosa = np.cos(angle)
374 direction = unit_vector(direction[:3])
376 # rotation matrix around unit vector
377 M = np.diag([cosa, cosa, cosa, 1.0])
378 M[:3, :3] += np.outer(direction, direction) * (1.0 - cosa)
380 direction = direction * sina
381 M[:3, :3] += np.array(
382 [
383 [0.0, -direction[2], direction[1]],
384 [direction[2], 0.0, -direction[0]],
385 [-direction[1], direction[0], 0.0],
386 ]
387 )
389 # if point is specified, rotation is not around origin
390 if point is not None:
391 point = np.asarray(point[:3], dtype=np.float64)
392 M[:3, 3] = point - np.dot(M[:3, :3], point)
394 # return symbolic angles as sympy Matrix objects
395 if symbolic:
396 return sp.Matrix(M)
398 return M
401def rotation_from_matrix(matrix):
402 """Return rotation angle and axis from rotation matrix.
404 >>> angle = (random.random() - 0.5) * (2*np.pi)
405 >>> direc = np.random.random(3) - 0.5
406 >>> point = np.random.random(3) - 0.5
407 >>> R0 = rotation_matrix(angle, direc, point)
408 >>> angle, direc, point = rotation_from_matrix(R0)
409 >>> R1 = rotation_matrix(angle, direc, point)
410 >>> is_same_transform(R0, R1)
411 True
413 """
414 R = np.asarray(matrix, dtype=np.float64)
415 R33 = R[:3, :3]
416 # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
417 w, W = np.linalg.eig(R33.T)
418 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
419 if not len(i):
420 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
421 direction = np.real(W[:, i[-1]]).squeeze()
422 # point: unit eigenvector of R33 corresponding to eigenvalue of 1
423 w, Q = np.linalg.eig(R)
424 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
425 if not len(i):
426 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
427 point = np.real(Q[:, i[-1]]).squeeze()
428 point /= point[3]
429 # rotation angle depending on direction
430 cosa = (np.trace(R33) - 1.0) / 2.0
431 if abs(direction[2]) > 1e-8:
432 sina = (R[1, 0] + (cosa - 1.0) * direction[0] * direction[1]) / direction[2]
433 elif abs(direction[1]) > 1e-8:
434 sina = (R[0, 2] + (cosa - 1.0) * direction[0] * direction[2]) / direction[1]
435 else:
436 sina = (R[2, 1] + (cosa - 1.0) * direction[1] * direction[2]) / direction[0]
437 angle = np.arctan2(sina, cosa)
438 return angle, direction, point
441def scale_matrix(factor, origin=None, direction=None):
442 """Return matrix to scale by factor around origin in direction.
444 Use factor -1 for point symmetry.
446 >>> v = (np.random.rand(4, 5) - 0.5) * 20
447 >>> v[3] = 1
448 >>> S = scale_matrix(-1.234)
449 >>> np.allclose(np.dot(S, v)[:3], -1.234*v[:3])
450 True
451 >>> factor = random.random() * 10 - 5
452 >>> origin = np.random.random(3) - 0.5
453 >>> direct = np.random.random(3) - 0.5
454 >>> S = scale_matrix(factor, origin)
455 >>> S = scale_matrix(factor, origin, direct)
457 """
458 if direction is None:
459 # uniform scaling
460 M = np.diag([factor, factor, factor, 1.0])
461 if origin is not None:
462 M[:3, 3] = origin[:3]
463 M[:3, 3] *= 1.0 - factor
464 else:
465 # nonuniform scaling
466 direction = unit_vector(direction[:3])
467 factor = 1.0 - factor
468 M = np.identity(4)
469 M[:3, :3] -= factor * np.outer(direction, direction)
470 if origin is not None:
471 M[:3, 3] = (factor * np.dot(origin[:3], direction)) * direction
472 return M
475def scale_from_matrix(matrix):
476 """Return scaling factor, origin and direction from scaling matrix.
478 >>> factor = random.random() * 10 - 5
479 >>> origin = np.random.random(3) - 0.5
480 >>> direct = np.random.random(3) - 0.5
481 >>> S0 = scale_matrix(factor, origin)
482 >>> factor, origin, direction = scale_from_matrix(S0)
483 >>> S1 = scale_matrix(factor, origin, direction)
484 >>> is_same_transform(S0, S1)
485 True
486 >>> S0 = scale_matrix(factor, origin, direct)
487 >>> factor, origin, direction = scale_from_matrix(S0)
488 >>> S1 = scale_matrix(factor, origin, direction)
489 >>> is_same_transform(S0, S1)
490 True
492 """
493 M = np.asarray(matrix, dtype=np.float64)
494 M33 = M[:3, :3]
495 factor = np.trace(M33) - 2.0
496 try:
497 # direction: unit eigenvector corresponding to eigenvalue factor
498 w, V = np.linalg.eig(M33)
499 i = np.where(abs(np.real(w) - factor) < 1e-8)[0][0]
500 direction = np.real(V[:, i]).squeeze()
501 direction /= vector_norm(direction)
502 except IndexError:
503 # uniform scaling
504 factor = (factor + 2.0) / 3.0
505 direction = None
506 # origin: any eigenvector corresponding to eigenvalue 1
507 w, V = np.linalg.eig(M)
508 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
509 if not len(i):
510 raise ValueError("no eigenvector corresponding to eigenvalue 1")
511 origin = np.real(V[:, i[-1]]).squeeze()
512 origin /= origin[3]
513 return factor, origin, direction
516def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False):
517 """Return matrix to project onto plane defined by point and normal.
519 Using either perspective point, projection direction, or none of both.
521 If pseudo is True, perspective projections will preserve relative depth
522 such that Perspective = dot(Orthogonal, PseudoPerspective).
524 >>> P = projection_matrix([0, 0, 0], [1, 0, 0])
525 >>> np.allclose(P[1:, 1:], np.identity(4)[1:, 1:])
526 True
527 >>> point = np.random.random(3) - 0.5
528 >>> normal = np.random.random(3) - 0.5
529 >>> direct = np.random.random(3) - 0.5
530 >>> persp = np.random.random(3) - 0.5
531 >>> P0 = projection_matrix(point, normal)
532 >>> P1 = projection_matrix(point, normal, direction=direct)
533 >>> P2 = projection_matrix(point, normal, perspective=persp)
534 >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
535 >>> is_same_transform(P2, np.dot(P0, P3))
536 True
537 >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0])
538 >>> v0 = (np.random.rand(4, 5) - 0.5) * 20
539 >>> v0[3] = 1
540 >>> v1 = np.dot(P, v0)
541 >>> np.allclose(v1[1], v0[1])
542 True
543 >>> np.allclose(v1[0], 3-v1[1])
544 True
546 """
547 M = np.identity(4)
548 point = np.asarray(point[:3], dtype=np.float64)
549 normal = unit_vector(normal[:3])
550 if perspective is not None:
551 # perspective projection
552 perspective = np.asarray(perspective[:3], dtype=np.float64)
553 M[0, 0] = M[1, 1] = M[2, 2] = np.dot(perspective - point, normal)
554 M[:3, :3] -= np.outer(perspective, normal)
555 if pseudo:
556 # preserve relative depth
557 M[:3, :3] -= np.outer(normal, normal)
558 M[:3, 3] = np.dot(point, normal) * (perspective + normal)
559 else:
560 M[:3, 3] = np.dot(point, normal) * perspective
561 M[3, :3] = -normal
562 M[3, 3] = np.dot(perspective, normal)
563 elif direction is not None:
564 # parallel projection
565 direction = np.asarray(direction[:3], dtype=np.float64)
566 scale = np.dot(direction, normal)
567 M[:3, :3] -= np.outer(direction, normal) / scale
568 M[:3, 3] = direction * (np.dot(point, normal) / scale)
569 else:
570 # orthogonal projection
571 M[:3, :3] -= np.outer(normal, normal)
572 M[:3, 3] = np.dot(point, normal) * normal
573 return M
576def projection_from_matrix(matrix, pseudo=False):
577 """Return projection plane and perspective point from projection matrix.
579 Return values are same as arguments for projection_matrix function:
580 point, normal, direction, perspective, and pseudo.
582 >>> point = np.random.random(3) - 0.5
583 >>> normal = np.random.random(3) - 0.5
584 >>> direct = np.random.random(3) - 0.5
585 >>> persp = np.random.random(3) - 0.5
586 >>> P0 = projection_matrix(point, normal)
587 >>> result = projection_from_matrix(P0)
588 >>> P1 = projection_matrix(*result)
589 >>> is_same_transform(P0, P1)
590 True
591 >>> P0 = projection_matrix(point, normal, direct)
592 >>> result = projection_from_matrix(P0)
593 >>> P1 = projection_matrix(*result)
594 >>> is_same_transform(P0, P1)
595 True
596 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
597 >>> result = projection_from_matrix(P0, pseudo=False)
598 >>> P1 = projection_matrix(*result)
599 >>> is_same_transform(P0, P1)
600 True
601 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
602 >>> result = projection_from_matrix(P0, pseudo=True)
603 >>> P1 = projection_matrix(*result)
604 >>> is_same_transform(P0, P1)
605 True
607 """
608 M = np.asarray(matrix, dtype=np.float64)
609 M33 = M[:3, :3]
610 w, V = np.linalg.eig(M)
611 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
612 if not pseudo and len(i):
613 # point: any eigenvector corresponding to eigenvalue 1
614 point = np.real(V[:, i[-1]]).squeeze()
615 point /= point[3]
616 # direction: unit eigenvector corresponding to eigenvalue 0
617 w, V = np.linalg.eig(M33)
618 i = np.where(abs(np.real(w)) < 1e-8)[0]
619 if not len(i):
620 raise ValueError("no eigenvector corresponding to eigenvalue 0")
621 direction = np.real(V[:, i[0]]).squeeze()
622 direction /= vector_norm(direction)
623 # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
624 w, V = np.linalg.eig(M33.T)
625 i = np.where(abs(np.real(w)) < 1e-8)[0]
626 if len(i):
627 # parallel projection
628 normal = np.real(V[:, i[0]]).squeeze()
629 normal /= vector_norm(normal)
630 return point, normal, direction, None, False
631 else:
632 # orthogonal projection, where normal equals direction vector
633 return point, direction, None, None, False
634 else:
635 # perspective projection
636 i = np.where(abs(np.real(w)) > 1e-8)[0]
637 if not len(i):
638 raise ValueError("no eigenvector not corresponding to eigenvalue 0")
639 point = np.real(V[:, i[-1]]).squeeze()
640 point /= point[3]
641 normal = -M[3, :3]
642 perspective = M[:3, 3] / np.dot(point[:3], normal)
643 if pseudo:
644 perspective -= normal
645 return point, normal, None, perspective, pseudo
648def clip_matrix(left, right, bottom, top, near, far, perspective=False):
649 """Return matrix to obtain normalized device coordinates from frustum.
651 The frustum bounds are axis-aligned along x (left, right),
652 y (bottom, top) and z (near, far).
654 Normalized device coordinates are in range [-1, 1] if coordinates are
655 inside the frustum.
657 If perspective is True the frustum is a truncated pyramid with the
658 perspective point at origin and direction along z axis, otherwise an
659 orthographic canonical view volume (a box).
661 Homogeneous coordinates transformed by the perspective clip matrix
662 need to be dehomogenized (divided by w coordinate).
664 >>> frustum = np.random.rand(6)
665 >>> frustum[1] += frustum[0]
666 >>> frustum[3] += frustum[2]
667 >>> frustum[5] += frustum[4]
668 >>> M = clip_matrix(perspective=False, *frustum)
669 >>> a = np.dot(M, [frustum[0], frustum[2], frustum[4], 1])
670 >>> np.allclose(a, [-1., -1., -1., 1.])
671 True
672 >>> b = np.dot(M, [frustum[1], frustum[3], frustum[5], 1])
673 >>> np.allclose(b, [ 1., 1., 1., 1.])
674 True
675 >>> M = clip_matrix(perspective=True, *frustum)
676 >>> v = np.dot(M, [frustum[0], frustum[2], frustum[4], 1])
677 >>> c = v / v[3]
678 >>> np.allclose(c, [-1., -1., -1., 1.])
679 True
680 >>> v = np.dot(M, [frustum[1], frustum[3], frustum[4], 1])
681 >>> d = v / v[3]
682 >>> np.allclose(d, [ 1., 1., -1., 1.])
683 True
685 """
686 if left >= right or bottom >= top or near >= far:
687 raise ValueError("invalid frustum")
688 if perspective:
689 if near <= _EPS:
690 raise ValueError("invalid frustum: near <= 0")
691 t = 2.0 * near
692 M = [
693 [t / (left - right), 0.0, (right + left) / (right - left), 0.0],
694 [0.0, t / (bottom - top), (top + bottom) / (top - bottom), 0.0],
695 [0.0, 0.0, (far + near) / (near - far), t * far / (far - near)],
696 [0.0, 0.0, -1.0, 0.0],
697 ]
698 else:
699 M = [
700 [2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)],
701 [0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)],
702 [0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)],
703 [0.0, 0.0, 0.0, 1.0],
704 ]
705 return np.array(M)
708def shear_matrix(angle, direction, point, normal):
709 """Return matrix to shear by angle along direction vector on shear plane.
711 The shear plane is defined by a point and normal vector. The direction
712 vector must be orthogonal to the plane's normal vector.
714 A point P is transformed by the shear matrix into P" such that
715 the vector P-P" is parallel to the direction vector and its extent is
716 given by the angle of P-P'-P", where P' is the orthogonal projection
717 of P onto the shear plane.
719 >>> angle = (random.random() - 0.5) * 4*np.pi
720 >>> direct = np.random.random(3) - 0.5
721 >>> point = np.random.random(3) - 0.5
722 >>> normal = np.cross(direct, np.random.random(3))
723 >>> S = shear_matrix(angle, direct, point, normal)
724 >>> np.allclose(1, np.linalg.det(S))
725 True
727 """
728 normal = unit_vector(normal[:3])
729 direction = unit_vector(direction[:3])
730 if abs(np.dot(normal, direction)) > 1e-6:
731 raise ValueError("direction and normal vectors are not orthogonal")
732 angle = np.tan(angle)
733 M = np.identity(4)
734 M[:3, :3] += angle * np.outer(direction, normal)
735 M[:3, 3] = -angle * np.dot(point[:3], normal) * direction
736 return M
739def shear_from_matrix(matrix):
740 """Return shear angle, direction and plane from shear matrix.
742 >>> angle = np.pi / 2.0
743 >>> direct = [0.0, 1.0, 0.0]
744 >>> point = [0.0, 0.0, 0.0]
745 >>> normal = np.cross(direct, np.roll(direct,1))
746 >>> S0 = shear_matrix(angle, direct, point, normal)
747 >>> angle, direct, point, normal = shear_from_matrix(S0)
748 >>> S1 = shear_matrix(angle, direct, point, normal)
749 >>> is_same_transform(S0, S1)
750 True
752 """
753 M = np.asarray(matrix, dtype=np.float64)
754 M33 = M[:3, :3]
755 # normal: cross independent eigenvectors corresponding to the eigenvalue 1
756 w, V = np.linalg.eig(M33)
758 i = np.where(abs(np.real(w) - 1.0) < 1e-4)[0]
759 if len(i) < 2:
760 raise ValueError(f"no two linear independent eigenvectors found {w}")
761 V = np.real(V[:, i]).squeeze().T
762 lenorm = -1.0
763 for i0, i1 in ((0, 1), (0, 2), (1, 2)):
764 n = np.cross(V[i0], V[i1])
765 w = vector_norm(n)
766 if w > lenorm:
767 lenorm = w
768 normal = n
769 normal /= lenorm
770 # direction and angle
771 direction = np.dot(M33 - np.identity(3), normal)
772 angle = vector_norm(direction)
773 direction /= angle
774 angle = np.arctan(angle)
775 # point: eigenvector corresponding to eigenvalue 1
776 w, V = np.linalg.eig(M)
778 i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
779 if not len(i):
780 raise ValueError("no eigenvector corresponding to eigenvalue 1")
781 point = np.real(V[:, i[-1]]).squeeze()
782 point /= point[3]
783 return angle, direction, point, normal
786def decompose_matrix(matrix):
787 """Return sequence of transformations from transformation matrix.
789 matrix : array_like
790 Non-degenerative homogeneous transformation matrix
792 Return tuple of:
793 scale : vector of 3 scaling factors
794 shear : list of shear factors for x-y, x-z, y-z axes
795 angles : list of Euler angles about static x, y, z axes
796 translate : translation vector along x, y, z axes
797 perspective : perspective partition of matrix
799 Raise ValueError if matrix is of wrong type or degenerative.
801 >>> T0 = translation_matrix([1, 2, 3])
802 >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
803 >>> T1 = translation_matrix(trans)
804 >>> np.allclose(T0, T1)
805 True
806 >>> S = scale_matrix(0.123)
807 >>> scale, shear, angles, trans, persp = decompose_matrix(S)
808 >>> bool(np.isclose(scale[0], 0.123))
809 True
810 >>> R0 = euler_matrix(1, 2, 3)
811 >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
812 >>> R1 = euler_matrix(*angles)
813 >>> np.allclose(R0, R1)
814 True
816 """
817 M = np.array(matrix, dtype=np.float64).T
818 if abs(M[3, 3]) < _EPS:
819 raise ValueError("M[3, 3] is zero")
820 M /= M[3, 3]
821 P = M.copy()
822 P[:, 3] = 0.0, 0.0, 0.0, 1.0
823 if not np.linalg.det(P):
824 raise ValueError("matrix is singular")
826 scale = np.zeros((3,))
827 shear = [0.0, 0.0, 0.0]
828 angles = [0.0, 0.0, 0.0]
830 if any(abs(M[:3, 3]) > _EPS):
831 perspective = np.dot(M[:, 3], np.linalg.inv(P.T))
832 M[:, 3] = 0.0, 0.0, 0.0, 1.0
833 else:
834 perspective = np.array([0.0, 0.0, 0.0, 1.0])
836 translate = M[3, :3].copy()
837 M[3, :3] = 0.0
839 row = M[:3, :3].copy()
840 scale[0] = vector_norm(row[0])
841 row[0] /= scale[0]
842 shear[0] = np.dot(row[0], row[1])
843 row[1] -= row[0] * shear[0]
844 scale[1] = vector_norm(row[1])
845 row[1] /= scale[1]
846 shear[0] /= scale[1]
847 shear[1] = np.dot(row[0], row[2])
848 row[2] -= row[0] * shear[1]
849 shear[2] = np.dot(row[1], row[2])
850 row[2] -= row[1] * shear[2]
851 scale[2] = vector_norm(row[2])
852 row[2] /= scale[2]
853 shear[1:] /= scale[2]
855 if np.dot(row[0], np.cross(row[1], row[2])) < 0:
856 np.negative(scale, scale)
857 np.negative(row, row)
859 angles[1] = np.arcsin(-row[0, 2])
860 if np.cos(angles[1]):
861 angles[0] = np.arctan2(row[1, 2], row[2, 2])
862 angles[2] = np.arctan2(row[0, 1], row[0, 0])
863 else:
864 angles[0] = np.arctan2(-row[2, 1], row[1, 1])
865 angles[2] = 0.0
867 return scale, shear, angles, translate, perspective
870def compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None):
871 """Return transformation matrix from sequence of transformations.
873 This is the inverse of the decompose_matrix function.
875 Sequence of transformations:
876 scale : vector of 3 scaling factors
877 shear : list of shear factors for x-y, x-z, y-z axes
878 angles : list of Euler angles about static x, y, z axes
879 translate : translation vector along x, y, z axes
880 perspective : perspective partition of matrix
882 >>> scale = np.random.random(3) - 0.5
883 >>> shear = np.random.random(3) - 0.5
884 >>> angles = (np.random.random(3) - 0.5) * (2*np.pi)
885 >>> trans = np.random.random(3) - 0.5
886 >>> persp = np.random.random(4) - 0.5
887 >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
888 >>> result = decompose_matrix(M0)
889 >>> M1 = compose_matrix(*result)
890 >>> is_same_transform(M0, M1)
891 True
893 """
894 M = np.identity(4)
895 if perspective is not None:
896 P = np.identity(4)
897 P[3, :] = perspective[:4]
898 M = np.dot(M, P)
899 if translate is not None:
900 T = np.identity(4)
901 T[:3, 3] = translate[:3]
902 M = np.dot(M, T)
903 if angles is not None:
904 R = euler_matrix(angles[0], angles[1], angles[2], "sxyz")
905 M = np.dot(M, R)
906 if shear is not None:
907 Z = np.identity(4)
908 Z[1, 2] = shear[2]
909 Z[0, 2] = shear[1]
910 Z[0, 1] = shear[0]
911 M = np.dot(M, Z)
912 if scale is not None:
913 S = np.identity(4)
914 S[0, 0] = scale[0]
915 S[1, 1] = scale[1]
916 S[2, 2] = scale[2]
917 M = np.dot(M, S)
918 M /= M[3, 3]
919 return M
922def orthogonalization_matrix(lengths, angles):
923 """Return orthogonalization matrix for crystallographic cell coordinates.
925 Angles are expected in degrees.
927 The de-orthogonalization matrix is the inverse.
929 >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
930 >>> np.allclose(O[:3, :3], np.identity(3, float) * 10)
931 True
932 >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
933 >>> np.allclose(np.sum(O), 43.063229)
934 True
936 """
937 a, b, c = lengths
938 angles = np.radians(angles)
939 sina, sinb, _ = np.sin(angles)
940 cosa, cosb, cosg = np.cos(angles)
941 co = (cosa * cosb - cosg) / (sina * sinb)
942 return np.array(
943 [
944 [a * sinb * np.sqrt(1.0 - co * co), 0.0, 0.0, 0.0],
945 [-a * sinb * co, b * sina, 0.0, 0.0],
946 [a * cosb, b * cosa, c, 0.0],
947 [0.0, 0.0, 0.0, 1.0],
948 ]
949 )
952def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
953 """Return affine transform matrix to register two point sets.
955 v0 and v1 are shape (ndims, *) arrays of at least ndims non-homogeneous
956 coordinates, where ndims is the dimensionality of the coordinate space.
958 If shear is False, a similarity transformation matrix is returned.
959 If also scale is False, a rigid/Euclidean transformation matrix
960 is returned.
962 By default the algorithm by Hartley and Zissermann [15] is used.
963 If usesvd is True, similarity and Euclidean transformation matrices
964 are calculated by minimizing the weighted sum of squared deviations
965 (RMSD) according to the algorithm by Kabsch [8].
966 Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]
967 is used, which is slower when using this Python implementation.
969 The returned matrix performs rotation, translation and uniform scaling
970 (if specified).
972 >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
973 >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
974 >>> mat = affine_matrix_from_points(v0, v1)
975 >>> T = translation_matrix(np.random.random(3)-0.5)
976 >>> R = random_rotation_matrix(np.random.random(3))
977 >>> S = scale_matrix(random.random())
978 >>> M = concatenate_matrices(T, R, S)
979 >>> v0 = (np.random.rand(4, 100) - 0.5) * 20
980 >>> v0[3] = 1
981 >>> v1 = np.dot(M, v0)
982 >>> v0[:3] += np.random.normal(0, 1e-8, 300).reshape(3, -1)
983 >>> M = affine_matrix_from_points(v0[:3], v1[:3])
984 >>> check = np.allclose(v1, np.dot(M, v0))
986 More examples in superimposition_matrix()
988 """
989 v0 = np.array(v0, dtype=np.float64)
990 v1 = np.array(v1, dtype=np.float64)
992 ndims = v0.shape[0]
993 if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
994 raise ValueError("input arrays are of wrong shape or type")
996 # move centroids to origin
997 t0 = -np.mean(v0, axis=1)
998 M0 = np.identity(ndims + 1)
999 M0[:ndims, ndims] = t0
1000 v0 += t0.reshape(ndims, 1)
1001 t1 = -np.mean(v1, axis=1)
1002 M1 = np.identity(ndims + 1)
1003 M1[:ndims, ndims] = t1
1004 v1 += t1.reshape(ndims, 1)
1006 if shear:
1007 # Affine transformation
1008 A = np.concatenate((v0, v1), axis=0)
1009 u, s, vh = np.linalg.svd(A.T)
1010 vh = vh[:ndims].T
1011 B = vh[:ndims]
1012 C = vh[ndims : 2 * ndims]
1013 t = np.dot(C, np.linalg.pinv(B))
1014 t = np.concatenate((t, np.zeros((ndims, 1))), axis=1)
1015 M = np.vstack((t, ((0.0,) * ndims) + (1.0,)))
1016 elif usesvd or ndims != 3:
1017 # Rigid transformation via SVD of covariance matrix
1018 u, s, vh = np.linalg.svd(np.dot(v1, v0.T))
1019 # rotation matrix from SVD orthonormal bases
1020 R = np.dot(u, vh)
1021 if np.linalg.det(R) < 0.0:
1022 # R does not constitute right handed system
1023 R -= np.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)
1024 s[-1] *= -1.0
1025 # homogeneous transformation matrix
1026 M = np.identity(ndims + 1)
1027 M[:ndims, :ndims] = R
1028 else:
1029 # Rigid transformation matrix via quaternion
1030 # compute symmetric matrix N
1031 xx, yy, zz = np.sum(v0 * v1, axis=1)
1032 xy, yz, zx = np.sum(v0 * np.roll(v1, -1, axis=0), axis=1)
1033 xz, yx, zy = np.sum(v0 * np.roll(v1, -2, axis=0), axis=1)
1034 N = [
1035 [xx + yy + zz, 0.0, 0.0, 0.0],
1036 [yz - zy, xx - yy - zz, 0.0, 0.0],
1037 [zx - xz, xy + yx, yy - xx - zz, 0.0],
1038 [xy - yx, zx + xz, yz + zy, zz - xx - yy],
1039 ]
1040 # quaternion: eigenvector corresponding to most positive eigenvalue
1041 w, V = np.linalg.eigh(N)
1042 q = V[:, np.argmax(w)]
1043 q /= vector_norm(q) # unit quaternion
1044 # homogeneous transformation matrix
1045 M = quaternion_matrix(q)
1047 if scale and not shear:
1048 # Affine transformation; scale is ratio of RMS deviations from centroid
1049 v0 *= v0
1050 v1 *= v1
1051 M[:ndims, :ndims] *= np.sqrt(np.sum(v1) / np.sum(v0))
1053 # move centroids back
1054 M = np.dot(np.linalg.inv(M1), np.dot(M, M0))
1055 M /= M[ndims, ndims]
1056 return M
1059def superimposition_matrix(v0, v1, scale=False, usesvd=True):
1060 """Return matrix to transform given 3D point set into second point set.
1062 v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 points.
1064 The parameters scale and usesvd are explained in the more general
1065 affine_matrix_from_points function.
1067 The returned matrix is a similarity or Euclidean transformation matrix.
1068 This function has a fast C implementation in transformations.c.
1070 >>> v0 = np.random.rand(3, 10)
1071 >>> M = superimposition_matrix(v0, v0)
1072 >>> np.allclose(M, np.identity(4))
1073 True
1074 >>> R = random_rotation_matrix(np.random.random(3))
1075 >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]]
1076 >>> v1 = np.dot(R, v0)
1077 >>> M = superimposition_matrix(v0, v1)
1078 >>> np.allclose(v1, np.dot(M, v0))
1079 True
1080 >>> v0 = (np.random.rand(4, 100) - 0.5) * 20
1081 >>> v0[3] = 1
1082 >>> v1 = np.dot(R, v0)
1083 >>> M = superimposition_matrix(v0, v1)
1084 >>> np.allclose(v1, np.dot(M, v0))
1085 True
1086 >>> S = scale_matrix(random.random())
1087 >>> T = translation_matrix(np.random.random(3)-0.5)
1088 >>> M = concatenate_matrices(T, R, S)
1089 >>> v1 = np.dot(M, v0)
1090 >>> v0[:3] += np.random.normal(0, 1e-9, 300).reshape(3, -1)
1091 >>> M = superimposition_matrix(v0, v1, scale=True)
1092 >>> np.allclose(v1, np.dot(M, v0))
1093 True
1094 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1095 >>> np.allclose(v1, np.dot(M, v0))
1096 True
1097 >>> v = np.zeros((4, 100, 3))
1098 >>> v[:, :, 0] = v0
1099 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1100 >>> np.allclose(v1, np.dot(M, v[:, :, 0]))
1101 True
1103 """
1104 v0 = np.asarray(v0, dtype=np.float64)[:3]
1105 v1 = np.asarray(v1, dtype=np.float64)[:3]
1106 return affine_matrix_from_points(v0, v1, shear=False, scale=scale, usesvd=usesvd)
1109def euler_matrix(ai, aj, ak, axes="sxyz"):
1110 """Return homogeneous rotation matrix from Euler angles and axis sequence.
1112 ai, aj, ak : Euler's roll, pitch and yaw angles
1113 axes : One of 24 axis sequences as string or encoded tuple
1115 >>> R = euler_matrix(1, 2, 3, 'syxz')
1116 >>> np.allclose(np.sum(R[0]), -1.34786452)
1117 True
1118 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
1119 >>> np.allclose(np.sum(R[0]), -0.383436184)
1120 True
1121 >>> ai, aj, ak = (4*np.pi) * (np.random.random(3) - 0.5)
1122 >>> for axes in _AXES2TUPLE.keys():
1123 ... R = euler_matrix(ai, aj, ak, axes)
1124 >>> for axes in _TUPLE2AXES.keys():
1125 ... R = euler_matrix(ai, aj, ak, axes)
1127 """
1128 try:
1129 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
1130 except (AttributeError, KeyError):
1131 _TUPLE2AXES[axes] # validation
1132 firstaxis, parity, repetition, frame = axes
1134 i = firstaxis
1135 j = _NEXT_AXIS[i + parity]
1136 k = _NEXT_AXIS[i - parity + 1]
1138 if frame:
1139 ai, ak = ak, ai
1140 if parity:
1141 ai, aj, ak = -ai, -aj, -ak
1143 if "sympy" in str(type(ai)):
1144 # if we have been passed input values as sympy.Symbol
1145 # use symbolic cosine and identity matrix
1146 from sympy import cos, eye, sin
1148 M = eye(4)
1149 else:
1150 sin, cos = np.sin, np.cos
1151 M = np.eye(4)
1153 si, sj, sk = sin(ai), sin(aj), sin(ak)
1154 ci, cj, ck = cos(ai), cos(aj), cos(ak)
1155 cc, cs = ci * ck, ci * sk
1156 sc, ss = si * ck, si * sk
1158 if repetition:
1159 M[i, i] = cj
1160 M[i, j] = sj * si
1161 M[i, k] = sj * ci
1162 M[j, i] = sj * sk
1163 M[j, j] = -cj * ss + cc
1164 M[j, k] = -cj * cs - sc
1165 M[k, i] = -sj * ck
1166 M[k, j] = cj * sc + cs
1167 M[k, k] = cj * cc - ss
1168 else:
1169 M[i, i] = cj * ck
1170 M[i, j] = sj * sc - cs
1171 M[i, k] = sj * cc + ss
1172 M[j, i] = cj * sk
1173 M[j, j] = sj * ss + cc
1174 M[j, k] = sj * cs - sc
1175 M[k, i] = -sj
1176 M[k, j] = cj * si
1177 M[k, k] = cj * ci
1178 return M
1181def euler_from_matrix(matrix, axes="sxyz"):
1182 """Return Euler angles from rotation matrix for specified axis sequence.
1184 axes : One of 24 axis sequences as string or encoded tuple
1186 Note that many Euler angle triplets can describe one matrix.
1188 >>> R0 = euler_matrix(1, 2, 3, 'syxz')
1189 >>> al, be, ga = euler_from_matrix(R0, 'syxz')
1190 >>> R1 = euler_matrix(al, be, ga, 'syxz')
1191 >>> np.allclose(R0, R1)
1192 True
1193 >>> angles = (4*np.pi) * (np.random.random(3) - 0.5)
1194 >>> for axes in _AXES2TUPLE.keys():
1195 ... R0 = euler_matrix(axes=axes, *angles)
1196 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
1197 ... if not np.allclose(R0, R1): print(axes, "failed")
1199 """
1200 try:
1201 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1202 except (AttributeError, KeyError):
1203 _TUPLE2AXES[axes] # validation
1204 firstaxis, parity, repetition, frame = axes
1206 i = firstaxis
1207 j = _NEXT_AXIS[i + parity]
1208 k = _NEXT_AXIS[i - parity + 1]
1210 M = np.asarray(matrix, dtype=np.float64)[:3, :3]
1211 if repetition:
1212 sy = np.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
1213 if sy > _EPS:
1214 ax = np.arctan2(M[i, j], M[i, k])
1215 ay = np.arctan2(sy, M[i, i])
1216 az = np.arctan2(M[j, i], -M[k, i])
1217 else:
1218 ax = np.arctan2(-M[j, k], M[j, j])
1219 ay = np.arctan2(sy, M[i, i])
1220 az = 0.0
1221 else:
1222 cy = np.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
1223 if cy > _EPS:
1224 ax = np.arctan2(M[k, j], M[k, k])
1225 ay = np.arctan2(-M[k, i], cy)
1226 az = np.arctan2(M[j, i], M[i, i])
1227 else:
1228 ax = np.arctan2(-M[j, k], M[j, j])
1229 ay = np.arctan2(-M[k, i], cy)
1230 az = 0.0
1232 if parity:
1233 ax, ay, az = -ax, -ay, -az
1234 if frame:
1235 ax, az = az, ax
1236 return ax, ay, az
1239def euler_from_quaternion(quaternion, axes="sxyz"):
1240 """Return Euler angles from quaternion for specified axis sequence.
1242 >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
1243 >>> np.allclose(angles, [0.123, 0, 0])
1244 True
1246 """
1247 return euler_from_matrix(quaternion_matrix(quaternion), axes)
1250def quaternion_from_euler(ai, aj, ak, axes="sxyz"):
1251 """Return quaternion from Euler angles and axis sequence.
1253 ai, aj, ak : Euler's roll, pitch and yaw angles
1254 axes : One of 24 axis sequences as string or encoded tuple
1256 >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
1257 >>> np.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
1258 True
1260 """
1261 try:
1262 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1263 except (AttributeError, KeyError):
1264 _TUPLE2AXES[axes] # validation
1265 firstaxis, parity, repetition, frame = axes
1267 i = firstaxis + 1
1268 j = _NEXT_AXIS[i + parity - 1] + 1
1269 k = _NEXT_AXIS[i - parity] + 1
1271 if frame:
1272 ai, ak = ak, ai
1273 if parity:
1274 aj = -aj
1276 ai /= 2.0
1277 aj /= 2.0
1278 ak /= 2.0
1279 ci = np.cos(ai)
1280 si = np.sin(ai)
1281 cj = np.cos(aj)
1282 sj = np.sin(aj)
1283 ck = np.cos(ak)
1284 sk = np.sin(ak)
1285 cc = ci * ck
1286 cs = ci * sk
1287 sc = si * ck
1288 ss = si * sk
1290 q = np.zeros((4,))
1291 if repetition:
1292 q[0] = cj * (cc - ss)
1293 q[i] = cj * (cs + sc)
1294 q[j] = sj * (cc + ss)
1295 q[k] = sj * (cs - sc)
1296 else:
1297 q[0] = cj * cc + sj * ss
1298 q[i] = cj * sc - sj * cs
1299 q[j] = cj * ss + sj * cc
1300 q[k] = cj * cs - sj * sc
1301 if parity:
1302 q[j] *= -1.0
1304 return q
1307def quaternion_about_axis(angle, axis):
1308 """Return quaternion for rotation about axis.
1310 >>> q = quaternion_about_axis(0.123, [1, 0, 0])
1311 >>> np.allclose(q, [0.99810947, 0.06146124, 0, 0])
1312 True
1314 """
1315 q = np.array([0.0, axis[0], axis[1], axis[2]])
1316 qlen = vector_norm(q)
1317 if qlen > _EPS:
1318 q *= np.sin(angle / 2.0) / qlen
1319 q[0] = np.cos(angle / 2.0)
1320 return q
1323def quaternion_matrix(quaternion):
1324 """
1325 Return a homogeneous rotation matrix from quaternion.
1327 >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
1328 >>> np.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
1329 True
1330 >>> M = quaternion_matrix([1, 0, 0, 0])
1331 >>> np.allclose(M, np.identity(4))
1332 True
1333 >>> M = quaternion_matrix([0, 1, 0, 0])
1334 >>> np.allclose(M, np.diag([1, -1, -1, 1]))
1335 True
1336 >>> M = quaternion_matrix([[1, 0, 0, 0],[0, 1, 0, 0]])
1337 >>> np.allclose(M, np.array([np.identity(4), np.diag([1, -1, -1, 1])]))
1338 True
1341 """
1342 q = np.array(quaternion, dtype=np.float64).reshape((-1, 4))
1343 n = diagonal_dot(q, q)
1344 # how many entries do we have
1345 num_qs = len(n)
1346 identities = n < _EPS
1347 q[~identities, :] *= np.sqrt(2.0 / n[~identities, None])
1348 q = q[:, None, :] * q[:, :, None]
1350 # store the result
1351 ret = np.zeros((num_qs, 4, 4))
1353 # pack the values into the result
1354 ret[:, 0, 0] = 1.0 - q[:, 2, 2] - q[:, 3, 3]
1355 ret[:, 0, 1] = q[:, 1, 2] - q[:, 3, 0]
1356 ret[:, 0, 2] = q[:, 1, 3] + q[:, 2, 0]
1357 ret[:, 1, 0] = q[:, 1, 2] + q[:, 3, 0]
1358 ret[:, 1, 1] = 1.0 - q[:, 1, 1] - q[:, 3, 3]
1359 ret[:, 1, 2] = q[:, 2, 3] - q[:, 1, 0]
1360 ret[:, 2, 0] = q[:, 1, 3] - q[:, 2, 0]
1361 ret[:, 2, 1] = q[:, 2, 3] + q[:, 1, 0]
1362 ret[:, 2, 2] = 1.0 - q[:, 1, 1] - q[:, 2, 2]
1363 ret[:, 3, 3] = 1.0
1364 # set any identities
1365 ret[identities] = np.eye(4)[None, ...]
1367 return ret.squeeze()
1370def quaternion_from_matrix(matrix, isprecise=False):
1371 """Return quaternion from rotation matrix.
1373 If isprecise is True, the input matrix is assumed to be a precise rotation
1374 matrix and a faster algorithm is used.
1376 >>> q = quaternion_from_matrix(np.identity(4), True)
1377 >>> np.allclose(q, [1, 0, 0, 0])
1378 True
1379 >>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))
1380 >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])
1381 True
1382 >>> R = rotation_matrix(0.123, (1, 2, 3))
1383 >>> q = quaternion_from_matrix(R, True)
1384 >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
1385 True
1386 >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
1387 ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
1388 >>> q = quaternion_from_matrix(R)
1389 >>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
1390 True
1391 >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
1392 ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
1393 >>> q = quaternion_from_matrix(R)
1394 >>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
1395 True
1396 >>> R = random_rotation_matrix()
1397 >>> q = quaternion_from_matrix(R)
1398 >>> is_same_transform(R, quaternion_matrix(q))
1399 True
1400 >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1401 ... quaternion_from_matrix(R, isprecise=True))
1402 True
1403 >>> R = euler_matrix(0.0, 0.0, np.pi/2.0)
1404 >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1405 ... quaternion_from_matrix(R, isprecise=True))
1406 True
1408 """
1409 M = np.asarray(matrix, dtype=np.float64)[:4, :4]
1410 if isprecise:
1411 q = np.zeros((4,))
1412 t = np.trace(M)
1413 if t > M[3, 3]:
1414 q[0] = t
1415 q[3] = M[1, 0] - M[0, 1]
1416 q[2] = M[0, 2] - M[2, 0]
1417 q[1] = M[2, 1] - M[1, 2]
1418 else:
1419 i, j, k = 0, 1, 2
1420 if M[1, 1] > M[0, 0]:
1421 i, j, k = 1, 2, 0
1422 if M[2, 2] > M[i, i]:
1423 i, j, k = 2, 0, 1
1424 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
1425 q[i] = t
1426 q[j] = M[i, j] + M[j, i]
1427 q[k] = M[k, i] + M[i, k]
1428 q[3] = M[k, j] - M[j, k]
1429 q = q[[3, 0, 1, 2]]
1430 q *= 0.5 / np.sqrt(t * M[3, 3])
1431 else:
1432 m00 = M[0, 0]
1433 m01 = M[0, 1]
1434 m02 = M[0, 2]
1435 m10 = M[1, 0]
1436 m11 = M[1, 1]
1437 m12 = M[1, 2]
1438 m20 = M[2, 0]
1439 m21 = M[2, 1]
1440 m22 = M[2, 2]
1441 # symmetric matrix K
1442 K = np.array(
1443 [
1444 [m00 - m11 - m22, 0.0, 0.0, 0.0],
1445 [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
1446 [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
1447 [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22],
1448 ]
1449 )
1450 K /= 3.0
1451 # quaternion is eigenvector of K that corresponds to largest eigenvalue
1452 w, V = np.linalg.eigh(K)
1453 q = V[[3, 0, 1, 2], np.argmax(w)]
1454 if q[0] < 0.0:
1455 np.negative(q, q)
1456 return q
1459def quaternion_multiply(quaternion1, quaternion0):
1460 """Return multiplication of two quaternions.
1462 >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
1463 >>> np.allclose(q, [28, -44, -14, 48])
1464 True
1466 """
1467 w0, x0, y0, z0 = quaternion0
1468 w1, x1, y1, z1 = quaternion1
1469 return np.array(
1470 [
1471 -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
1472 x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
1473 -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
1474 x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
1475 ],
1476 dtype=np.float64,
1477 )
1480def quaternion_conjugate(quaternion):
1481 """Return conjugate of quaternion.
1483 >>> q0 = random_quaternion()
1484 >>> q1 = quaternion_conjugate(q0)
1485 >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:])
1486 True
1488 """
1489 q = np.array(quaternion, dtype=np.float64)
1490 np.negative(q[1:], q[1:])
1491 return q
1494def quaternion_inverse(quaternion):
1495 """Return inverse of quaternion.
1497 >>> q0 = random_quaternion()
1498 >>> q1 = quaternion_inverse(q0)
1499 >>> np.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])
1500 True
1502 """
1503 q = np.array(quaternion, dtype=np.float64)
1504 np.negative(q[1:], q[1:])
1505 return q / np.dot(q, q)
1508def quaternion_real(quaternion):
1509 """Return real part of quaternion.
1511 >>> quaternion_real([3, 0, 1, 2])
1512 3.0
1514 """
1515 return float(quaternion[0])
1518def quaternion_imag(quaternion):
1519 """Return imaginary part of quaternion.
1521 >>> quaternion_imag([3, 0, 1, 2])
1522 array([0., 1., 2.])
1524 """
1525 return np.array(quaternion[1:4], dtype=np.float64)
1528def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
1529 """Return spherical linear interpolation between two quaternions.
1531 >>> q0 = random_quaternion()
1532 >>> q1 = random_quaternion()
1533 >>> q = quaternion_slerp(q0, q1, 0)
1534 >>> np.allclose(q, q0)
1535 True
1536 >>> q = quaternion_slerp(q0, q1, 1, 1)
1537 >>> np.allclose(q, q1)
1538 True
1539 >>> q = quaternion_slerp(q0, q1, 0.5)
1540 >>> angle = np.arccos(np.dot(q0, q))
1541 >>> np.allclose(2, np.arccos(np.dot(q0, q1)) / angle) or \
1542 np.allclose(2, np.arccos(-np.dot(q0, q1)) / angle)
1543 True
1545 """
1546 q0 = unit_vector(quat0[:4])
1547 q1 = unit_vector(quat1[:4])
1548 if fraction == 0.0:
1549 return q0
1550 elif fraction == 1.0:
1551 return q1
1552 d = np.dot(q0, q1)
1553 if abs(abs(d) - 1.0) < _EPS:
1554 return q0
1555 if shortestpath and d < 0.0:
1556 # invert rotation
1557 d = -d
1558 np.negative(q1, q1)
1559 angle = np.arccos(d) + spin * np.pi
1560 if abs(angle) < _EPS:
1561 return q0
1562 isin = 1.0 / np.sin(angle)
1563 q0 *= np.sin((1.0 - fraction) * angle) * isin
1564 q1 *= np.sin(fraction * angle) * isin
1565 q0 += q1
1566 return q0
1569def random_quaternion(rand=None, num=1):
1570 """Return uniform random unit quaternion.
1572 rand: array like or None
1573 Three independent random variables that are uniformly distributed
1574 between 0 and 1.
1576 >>> q = random_quaternion()
1577 >>> np.allclose(1, vector_norm(q))
1578 True
1579 >>> q = random_quaternion(num=10)
1580 >>> np.allclose(1, vector_norm(q, axis=1))
1581 True
1582 >>> q = random_quaternion(np.random.random(3))
1583 >>> len(q.shape), q.shape[0]==4
1584 (1, True)
1586 """
1587 if rand is None:
1588 rand = np.random.rand(3 * num).reshape((3, -1))
1589 else:
1590 assert rand.shape[0] == 3
1591 r1 = np.sqrt(1.0 - rand[0])
1592 r2 = np.sqrt(rand[0])
1593 pi2 = np.pi * 2.0
1594 t1 = pi2 * rand[1]
1595 t2 = pi2 * rand[2]
1596 return np.array(
1597 [np.cos(t2) * r2, np.sin(t1) * r1, np.cos(t1) * r1, np.sin(t2) * r2]
1598 ).T.squeeze()
1601def random_rotation_matrix(
1602 rand: ArrayLike | None = None, num: Integer = 1, translate: Number | None = None
1603):
1604 """
1605 Return uniform random rotation matrix.
1607 Parameters
1608 ------------
1609 rand : (3,)
1610 Three independent random variables that are uniformly distributed
1611 between 0 and 1 for each returned quaternion.
1612 num
1613 Number of matrices to return.
1614 translate
1615 If passed the rotation matrix will include translation
1616 that is random and between positive and negative half this value.
1618 >>> R = random_rotation_matrix()
1619 >>> np.allclose(np.dot(R.T, R), np.identity(4))
1620 True
1621 >>> R = random_rotation_matrix(num=10)
1622 >>> np.allclose(np.einsum('...ji,...jk->...ik', R, R), np.identity(4))
1623 True
1625 """
1626 matrix = quaternion_matrix(random_quaternion(rand=rand, num=num))
1627 if translate:
1628 # apply random translation with the order of magnitude requested
1629 matrix[:3, 3] = (np.random.random(3) - 0.5) * float(translate)
1631 return matrix
1634class Arcball:
1635 """Virtual Trackball Control.
1637 >>> ball = Arcball()
1638 >>> ball = Arcball(initial=np.identity(4))
1639 >>> ball.place([320, 320], 320)
1640 >>> ball.down([500, 250])
1641 >>> ball.drag([475, 275])
1642 >>> R = ball.matrix()
1643 >>> np.allclose(np.sum(R), 3.90583455)
1644 True
1645 >>> ball = Arcball(initial=[1, 0, 0, 0])
1646 >>> ball.place([320, 320], 320)
1647 >>> ball.setaxes([1, 1, 0], [-1, 1, 0])
1648 >>> ball.constrain = True
1649 >>> ball.down([400, 200])
1650 >>> ball.drag([200, 400])
1651 >>> R = ball.matrix()
1652 >>> np.allclose(np.sum(R), 0.2055924)
1653 True
1654 >>> ball.next()
1656 """
1658 def __init__(self, initial=None):
1659 """Initialize virtual trackball control.
1661 initial : quaternion or rotation matrix
1663 """
1664 self._axis = None
1665 self._axes = None
1666 self._radius = 1.0
1667 self._center = [0.0, 0.0]
1668 self._vdown = np.array([0.0, 0.0, 1.0])
1669 self._constrain = False
1670 if initial is None:
1671 self._qdown = np.array([1.0, 0.0, 0.0, 0.0])
1672 else:
1673 initial = np.array(initial, dtype=np.float64)
1674 if initial.shape == (4, 4):
1675 self._qdown = quaternion_from_matrix(initial)
1676 elif initial.shape == (4,):
1677 initial /= vector_norm(initial)
1678 self._qdown = initial
1679 else:
1680 raise ValueError("initial not a quaternion or matrix")
1681 self._qnow = self._qpre = self._qdown
1683 def place(self, center, radius):
1684 """Place Arcball, e.g. when window size changes.
1686 center : sequence[2]
1687 Window coordinates of trackball center.
1688 radius : float
1689 Radius of trackball in window coordinates.
1691 """
1692 self._radius = float(radius)
1693 self._center[0] = center[0]
1694 self._center[1] = center[1]
1696 def setaxes(self, *axes):
1697 """Set axes to constrain rotations."""
1698 if axes is None:
1699 self._axes = None
1700 else:
1701 self._axes = [unit_vector(axis) for axis in axes]
1703 @property
1704 def constrain(self):
1705 """Return state of constrain to axis mode."""
1706 return self._constrain
1708 @constrain.setter
1709 def constrain(self, value):
1710 """Set state of constrain to axis mode."""
1711 self._constrain = bool(value)
1713 def down(self, point):
1714 """Set initial cursor window coordinates and pick constrain-axis."""
1715 self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
1716 self._qdown = self._qpre = self._qnow
1717 if self._constrain and self._axes is not None:
1718 self._axis = arcball_nearest_axis(self._vdown, self._axes)
1719 self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
1720 else:
1721 self._axis = None
1723 def drag(self, point):
1724 """Update current cursor window coordinates."""
1725 vnow = arcball_map_to_sphere(point, self._center, self._radius)
1726 if self._axis is not None:
1727 vnow = arcball_constrain_to_axis(vnow, self._axis)
1728 self._qpre = self._qnow
1729 t = np.cross(self._vdown, vnow)
1730 if np.dot(t, t) < _EPS:
1731 self._qnow = self._qdown
1732 else:
1733 q = [np.dot(self._vdown, vnow), t[0], t[1], t[2]]
1734 self._qnow = quaternion_multiply(q, self._qdown)
1736 def next(self, acceleration=0.0):
1737 """Continue rotation in direction of last drag."""
1738 q = quaternion_slerp(self._qpre, self._qnow, 2.0 + acceleration, False)
1739 self._qpre, self._qnow = self._qnow, q
1741 def matrix(self):
1742 """Return homogeneous rotation matrix."""
1743 return quaternion_matrix(self._qnow)
1746def arcball_map_to_sphere(point, center, radius):
1747 """Return unit sphere coordinates from window coordinates."""
1748 v0 = (point[0] - center[0]) / radius
1749 v1 = (center[1] - point[1]) / radius
1750 n = v0 * v0 + v1 * v1
1751 if n > 1.0:
1752 # position outside of sphere
1753 n = np.sqrt(n)
1754 return np.array([v0 / n, v1 / n, 0.0])
1755 else:
1756 return np.array([v0, v1, np.sqrt(1.0 - n)])
1759def arcball_constrain_to_axis(point, axis):
1760 """Return sphere point perpendicular to axis."""
1761 v = np.array(point, dtype=np.float64)
1762 a = np.array(axis, dtype=np.float64)
1763 v -= a * np.dot(a, v) # on plane
1764 n = vector_norm(v)
1765 if n > _EPS:
1766 if v[2] < 0.0:
1767 np.negative(v, v)
1768 v /= n
1769 return v
1770 if a[2] == 1.0:
1771 return np.array([1.0, 0.0, 0.0])
1772 return unit_vector([-a[1], a[0], 0.0])
1775def arcball_nearest_axis(point, axes):
1776 """Return axis, which arc is nearest to point."""
1777 point = np.asarray(point, dtype=np.float64)
1778 nearest = None
1779 mx = -1.0
1780 for axis in axes:
1781 t = np.dot(arcball_constrain_to_axis(point, axis), point)
1782 if t > mx:
1783 nearest = axis
1784 mx = t
1785 return nearest
1788# epsilon for testing whether a number is close to zero
1789_EPS = np.finfo(float).eps * 4.0
1791# axis sequences for Euler angles
1792_NEXT_AXIS = [1, 2, 0, 1]
1794# map axes strings to/from tuples of inner axis, parity, repetition, frame
1795_AXES2TUPLE = {
1796 "sxyz": (0, 0, 0, 0),
1797 "sxyx": (0, 0, 1, 0),
1798 "sxzy": (0, 1, 0, 0),
1799 "sxzx": (0, 1, 1, 0),
1800 "syzx": (1, 0, 0, 0),
1801 "syzy": (1, 0, 1, 0),
1802 "syxz": (1, 1, 0, 0),
1803 "syxy": (1, 1, 1, 0),
1804 "szxy": (2, 0, 0, 0),
1805 "szxz": (2, 0, 1, 0),
1806 "szyx": (2, 1, 0, 0),
1807 "szyz": (2, 1, 1, 0),
1808 "rzyx": (0, 0, 0, 1),
1809 "rxyx": (0, 0, 1, 1),
1810 "ryzx": (0, 1, 0, 1),
1811 "rxzx": (0, 1, 1, 1),
1812 "rxzy": (1, 0, 0, 1),
1813 "ryzy": (1, 0, 1, 1),
1814 "rzxy": (1, 1, 0, 1),
1815 "ryxy": (1, 1, 1, 1),
1816 "ryxz": (2, 0, 0, 1),
1817 "rzxz": (2, 0, 1, 1),
1818 "rxyz": (2, 1, 0, 1),
1819 "rzyz": (2, 1, 1, 1),
1820}
1822_TUPLE2AXES = {v: k for k, v in _AXES2TUPLE.items()}
1825def vector_norm(data, axis=None, out=None):
1826 """Return length, i.e. Euclidean norm, of ndarray along axis.
1828 >>> v = np.random.random(3)
1829 >>> n = vector_norm(v)
1830 >>> np.allclose(n, np.linalg.norm(v))
1831 True
1832 >>> v = np.random.rand(6, 5, 3)
1833 >>> n = vector_norm(v, axis=-1)
1834 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=2)))
1835 True
1836 >>> n = vector_norm(v, axis=1)
1837 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
1838 True
1839 >>> v = np.random.rand(5, 4, 3)
1840 >>> n = np.zeros((5, 3))
1841 >>> vector_norm(v, axis=1, out=n)
1842 >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
1843 True
1844 >>> float(vector_norm([]))
1845 0.0
1846 >>> float(vector_norm([1]))
1847 1.0
1849 """
1850 data = np.array(data, dtype=np.float64)
1851 if out is None:
1852 if data.ndim == 1:
1853 return np.sqrt(np.dot(data, data))
1854 data *= data
1855 out = np.atleast_1d(np.sum(data, axis=axis))
1856 np.sqrt(out, out)
1857 return out
1858 else:
1859 data *= data
1860 np.sum(data, axis=axis, out=out)
1861 np.sqrt(out, out)
1864def unit_vector(data, axis=None, out=None):
1865 """Return ndarray normalized by length, i.e. Euclidean norm, along axis.
1867 >>> v0 = np.random.random(3)
1868 >>> v1 = unit_vector(v0)
1869 >>> np.allclose(v1, v0 / np.linalg.norm(v0))
1870 True
1871 >>> v0 = np.random.rand(5, 4, 3)
1872 >>> v1 = unit_vector(v0, axis=-1)
1873 >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=2)), 2)
1874 >>> np.allclose(v1, v2)
1875 True
1876 >>> v1 = unit_vector(v0, axis=1)
1877 >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=1)), 1)
1878 >>> np.allclose(v1, v2)
1879 True
1880 >>> v1 = np.zeros((5, 4, 3))
1881 >>> unit_vector(v0, axis=1, out=v1)
1882 >>> np.allclose(v1, v2)
1883 True
1884 >>> list(unit_vector([]))
1885 []
1886 >>> [float(i) for i in unit_vector([1])]
1887 [1.0]
1889 """
1890 if out is None:
1891 data = np.array(data, dtype=np.float64)
1892 if data.ndim == 1:
1893 data /= np.sqrt(np.dot(data, data))
1894 return data
1895 else:
1896 if out is not data:
1897 out[:] = np.asarray(data)
1898 data = out
1899 length = np.atleast_1d(np.sum(data * data, axis))
1900 np.sqrt(length, length)
1901 if axis is not None:
1902 length = np.expand_dims(length, axis)
1903 data /= length
1904 if out is None:
1905 return data
1908def random_vector(size):
1909 """Return array of random doubles in the half-open interval [0.0, 1.0).
1911 >>> v = random_vector(10000)
1912 >>> bool(np.all(v >= 0) and np.all(v < 1))
1913 True
1914 >>> v0 = random_vector(10)
1915 >>> v1 = random_vector(10)
1916 >>> bool(np.any(v0 == v1))
1917 False
1919 """
1920 return np.random.random(size)
1923def vector_product(v0, v1, axis=0):
1924 """Return vector perpendicular to vectors.
1926 >>> v = vector_product([2, 0, 0], [0, 3, 0])
1927 >>> np.allclose(v, [0, 0, 6])
1928 True
1929 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1930 >>> v1 = [[3], [0], [0]]
1931 >>> v = vector_product(v0, v1)
1932 >>> np.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])
1933 True
1934 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1935 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1936 >>> v = vector_product(v0, v1, axis=1)
1937 >>> np.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])
1938 True
1940 """
1941 return np.cross(v0, v1, axis=axis)
1944def angle_between_vectors(v0, v1, directed=True, axis=0):
1945 """Return angle between vectors.
1947 If directed is False, the input vectors are interpreted as undirected axes,
1948 i.e. the maximum angle is pi/2.
1950 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])
1951 >>> np.allclose(a, np.pi)
1952 True
1953 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)
1954 >>> np.allclose(a, 0)
1955 True
1956 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1957 >>> v1 = [[3], [0], [0]]
1958 >>> a = angle_between_vectors(v0, v1)
1959 >>> np.allclose(a, [0, 1.5708, 1.5708, 0.95532])
1960 True
1961 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1962 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1963 >>> a = angle_between_vectors(v0, v1, axis=1)
1964 >>> np.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])
1965 True
1967 """
1968 v0 = np.asarray(v0, dtype=np.float64)
1969 v1 = np.asarray(v1, dtype=np.float64)
1970 dot = np.sum(v0 * v1, axis=axis)
1971 dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)
1973 # clip off floating point error to avoid `nan` in the arccos`
1974 dot = np.clip(dot, -1.0, 1.0)
1975 return np.arccos(dot if directed else np.fabs(dot))
1978def inverse_matrix(matrix):
1979 """Return inverse of square transformation matrix.
1981 >>> M0 = random_rotation_matrix()
1982 >>> M1 = inverse_matrix(M0.T)
1983 >>> np.allclose(M1, np.linalg.inv(M0.T))
1984 True
1985 >>> for size in range(1, 7):
1986 ... M0 = np.random.rand(size, size)
1987 ... M1 = inverse_matrix(M0)
1988 ... if not np.allclose(M1, np.linalg.inv(M0)): print(size)
1990 """
1991 return np.linalg.inv(matrix)
1994def concatenate_matrices(*matrices):
1995 """Return concatenation of series of transformation matrices.
1997 >>> M = np.random.rand(16).reshape((4, 4)) - 0.5
1998 >>> np.allclose(M, concatenate_matrices(M))
1999 True
2000 >>> np.allclose(np.dot(M, M.T), concatenate_matrices(M, M.T))
2001 True
2003 """
2004 M = np.identity(4)
2005 for i in matrices:
2006 M = np.dot(M, i)
2007 return M
2010def is_same_transform(matrix0, matrix1):
2011 """Return True if two matrices perform same transformation.
2013 >>> is_same_transform(np.identity(4), np.identity(4))
2014 True
2015 >>> is_same_transform(np.identity(4), random_rotation_matrix())
2016 False
2018 """
2019 matrix0 = np.array(matrix0, dtype=np.float64)
2020 matrix0 /= matrix0[3, 3]
2021 matrix1 = np.array(matrix1, dtype=np.float64)
2022 matrix1 /= matrix1[3, 3]
2023 return np.allclose(matrix0, matrix1)
2026def is_same_quaternion(q0, q1):
2027 """Return True if two quaternions are equal."""
2028 q0 = np.array(q0)
2029 q1 = np.array(q1)
2030 return np.allclose(q0, q1) or np.allclose(q0, -q1)
2033def transform_around(matrix, point):
2034 """
2035 Given a transformation matrix, apply its rotation
2036 around a point in space.
2038 Parameters
2039 ----------
2040 matrix: (4,4) or (3, 3) float, transformation matrix
2041 point: (3,) or (2,) float, point in space
2043 Returns
2044 ---------
2045 result: (4,4) transformation matrix
2046 """
2047 point = np.asanyarray(point)
2048 matrix = np.asanyarray(matrix)
2049 dim = len(point)
2050 if matrix.shape != (dim + 1, dim + 1):
2051 raise ValueError("matrix must be (d+1, d+1)")
2053 translate = np.eye(dim + 1)
2054 translate[:dim, dim] = -point
2055 result = np.dot(matrix, translate)
2056 translate[:dim, dim] = point
2057 result = np.dot(translate, result)
2059 return result
2062def planar_matrix(offset=None, theta=None, point=None, scale=None):
2063 """
2064 2D homogeonous transformation matrix.
2066 Parameters
2067 ----------
2068 offset : (2,) float
2069 XY offset
2070 theta : float
2071 Rotation around Z in radians
2072 point : (2, ) float
2073 Point to rotate around
2074 scale : (2,) float or None
2075 Scale to apply
2077 Returns
2078 ----------
2079 matrix : (3, 3) flat
2080 Homogeneous 2D transformation matrix
2081 """
2082 if offset is None:
2083 offset = [0.0, 0.0]
2084 if theta is None:
2085 theta = 0.0
2086 offset = np.asanyarray(offset, dtype=np.float64)
2087 theta = float(theta)
2088 if not np.isfinite(theta):
2089 raise ValueError("theta must be finite angle!")
2090 if offset.shape != (2,):
2091 raise ValueError("offset must be length 2!")
2093 T = np.eye(3, dtype=np.float64)
2094 s = np.sin(theta)
2095 c = np.cos(theta)
2097 T[0, :2] = [c, s]
2098 T[1, :2] = [-s, c]
2099 T[:2, 2] = offset
2101 if point is not None:
2102 T = transform_around(matrix=T, point=point)
2104 if scale is not None:
2105 S = np.eye(3)
2106 S[:2, :2] *= scale
2107 T = np.dot(S, T)
2109 return T
2112def planar_matrix_to_3D(matrix_2D):
2113 """
2114 Given a 2D homogeneous rotation matrix convert it to a 3D rotation
2115 matrix that is rotating around the Z axis
2117 Parameters
2118 ----------
2119 matrix_2D: (3,3) float, homogeneous 2D rotation matrix
2121 Returns
2122 ----------
2123 matrix_3D: (4,4) float, homogeneous 3D rotation matrix
2124 """
2126 matrix_2D = np.asanyarray(matrix_2D, dtype=np.float64)
2127 if matrix_2D.shape != (3, 3):
2128 raise ValueError("Homogeneous 2D transformation matrix required!")
2130 matrix_3D = np.eye(4)
2131 # translation
2132 matrix_3D[:2, 3] = matrix_2D[:2, 2]
2133 # rotation from 2D to around Z
2134 matrix_3D[:2, :2] = matrix_2D[:2, :2]
2136 return matrix_3D
2139def spherical_matrix(theta, phi, axes="sxyz"):
2140 """
2141 Give a spherical coordinate vector, find the rotation that will
2142 transform a [0,0,1] vector to those coordinates
2144 Parameters
2145 -----------
2146 theta: float, rotation angle in radians
2147 phi: float, rotation angle in radians
2149 Returns
2150 ----------
2151 matrix: (4,4) rotation matrix where the following will
2152 be a cartesian vector in the direction of the
2153 input spherical coordinates:
2154 np.dot(matrix, [0,0,1,0])
2156 """
2157 result = euler_matrix(0.0, phi, theta, axes=axes)
2158 return result
2161def transform_points(
2162 points: ArrayLike, matrix: ArrayLike, translate: bool = True
2163) -> NDArray[np.float64]:
2164 """
2165 Returns points rotated by a homogeneous
2166 transformation matrix.
2168 If points are (n, 2) matrix must be (3, 3)
2169 If points are (n, 3) matrix must be (4, 4)
2171 Parameters
2172 ----------
2173 points : (n, dim) float
2174 Points where `dim` is 2 or 3.
2175 matrix : (3, 3) or (4, 4) float
2176 Homogeneous rotation matrix.
2177 translate : bool
2178 Apply translation from matrix or not.
2180 Returns
2181 ----------
2182 transformed : (n, dim) float
2183 Transformed points.
2184 """
2185 points = np.asanyarray(points, dtype=np.float64)
2186 if len(points) == 0 or matrix is None:
2187 return points.copy()
2189 # check the matrix against the points
2190 matrix = np.asanyarray(matrix, dtype=np.float64)
2191 # shorthand the shape
2192 count, dim = points.shape
2194 # quickly check to see if we've been passed an identity matrix
2195 if np.abs(matrix - _IDENTITY[: dim + 1, : dim + 1]).max() < 1e-8:
2196 return np.ascontiguousarray(points.copy())
2198 if translate:
2199 # apply translation and rotation
2200 stack = np.column_stack((points, np.ones(count)))
2201 return np.dot(matrix, stack.T).T[:, :dim]
2203 # only apply the rotation
2204 return np.dot(matrix[:dim, :dim], points.T).T
2207def fix_rigid(matrix, max_deviance=1e-5):
2208 """
2209 If a homogeneous transformation matrix is *almost* a rigid
2210 transform but many matrix-multiplies have accumulated some
2211 floating point error try to restore the matrix using SVD.
2213 Parameters
2214 -----------
2215 matrix : (4, 4) or (3, 3) float
2216 Homogeneous transformation matrix.
2217 max_deviance : float
2218 Do not alter the matrix if it is not rigid by more
2219 than this amount.
2221 Returns
2222 ----------
2223 repaired : (4, 4) or (3, 3) float
2224 Repaired homogeneous transformation matrix
2225 """
2226 dim = matrix.shape[0] - 1
2227 check = np.abs(
2228 np.dot(matrix[:dim, :dim], matrix[:dim, :dim].T) - _IDENTITY[:dim, :dim]
2229 ).max()
2230 # if the matrix differs by more than float-zero and less
2231 # than the threshold try to repair the matrix with SVD
2232 if check > 1e-13 and check < max_deviance:
2233 # reconstruct the rotation from the SVD
2234 U, _, V = np.linalg.svd(matrix[:dim, :dim])
2235 repaired = np.eye(dim + 1)
2236 repaired[:dim, :dim] = np.dot(U, V)
2237 # copy in the translation
2238 repaired[:dim, dim] = matrix[:dim, dim]
2239 # should be within tolerance of the original matrix
2240 assert np.allclose(repaired, matrix, atol=max_deviance)
2241 return repaired
2243 return matrix
2246def is_rigid(matrix, epsilon=1e-8):
2247 """
2248 Check to make sure a homogeonous transformation
2249 matrix is a rigid transform.
2251 Parameters
2252 -----------
2253 matrix : (4, 4) float
2254 A transformation matrix
2256 Returns
2257 -----------
2258 check : bool
2259 True if matrix is a a transform with
2260 only translation, scale, and rotation
2261 """
2263 matrix = np.asanyarray(matrix, dtype=np.float64)
2265 if matrix.shape != (4, 4):
2266 return False
2268 # make sure last row has no scaling
2269 if np.ptp(matrix[-1] - [0, 0, 0, 1]) > epsilon:
2270 return False
2272 # check dot product of rotation against transpose
2273 check = np.dot(matrix[:3, :3], matrix[:3, :3].T) - _IDENTITY[:3, :3]
2275 return np.ptp(check) < epsilon
2278def scale_and_translate(scale=None, translate=None):
2279 """
2280 Optimized version of `compose_matrix` for just
2281 scaling then translating.
2283 Scalar args are broadcast to arrays of shape (3,)
2285 Parameters
2286 --------------
2287 scale : float or (3,) float
2288 Scale factor
2289 translate : float or (3,) float
2290 Translation
2291 """
2292 M = np.eye(4)
2293 if np.any(scale != 1):
2294 M[:3, :3] *= scale
2295 if translate is not None:
2296 M[:3, 3] = translate
2297 return M
2300def flips_winding(matrix):
2301 """
2302 Check to see if a matrix will invert triangles.
2304 Parameters
2305 -------------
2306 matrix : (4, 4) float
2307 Homogeneous transformation matrix
2309 Returns
2310 --------------
2311 flip : bool
2312 True if matrix will flip winding of triangles.
2313 """
2314 # get input as numpy array
2315 matrix = np.asanyarray(matrix, dtype=np.float64)
2316 # how many random triangles do we really want
2317 count = 3
2318 # test rotation against some random triangles
2319 tri = np.random.random((count * 3, 3))
2320 rot = np.dot(matrix[:3, :3], tri.T).T
2322 # stack them into one triangle soup
2323 triangles = np.vstack((tri, rot)).reshape((-1, 3, 3))
2324 # find the normals of every triangle
2325 vectors = np.diff(triangles, axis=1)
2326 cross = np.cross(vectors[:, 0], vectors[:, 1])
2327 # rotate the original normals to match
2328 cross[:count] = np.dot(matrix[:3, :3], cross[:count].T).T
2329 # unitize normals
2330 norm = np.sqrt(np.dot(cross * cross, [1, 1, 1])).reshape((-1, 1))
2331 cross = cross / norm
2332 # find the projection of the two normals
2333 projection = np.dot(cross[:count] * cross[count:], [1.0] * 3)
2334 # if the winding was flipped but not the normal
2335 # the projection will be negative, and since we're
2336 # checking a few triangles check against the mean
2337 flip = projection.mean() < 0.0
2339 return flip