Coverage for larch/math/transformations.py: 10%
701 statements
« prev ^ index » next coverage.py v7.6.0, created at 2024-10-16 21:04 +0000
« prev ^ index » next coverage.py v7.6.0, created at 2024-10-16 21:04 +0000
1# -*- coding: utf-8 -*-
2# transformations.py
4# Copyright (c) 2006-2015, Christoph Gohlke
5# Copyright (c) 2006-2015, The Regents of the University of California
6# Produced at the Laboratory for Fluorescence Dynamics
7# All rights reserved.
8#
9# Redistribution and use in source and binary forms, with or without
10# modification, are permitted provided that the following conditions are met:
11#
12# * Redistributions of source code must retain the above copyright
13# notice, this list of conditions and the following disclaimer.
14# * Redistributions in binary form must reproduce the above copyright
15# notice, this list of conditions and the following disclaimer in the
16# documentation and/or other materials provided with the distribution.
17# * Neither the name of the copyright holders nor the names of any
18# contributors may be used to endorse or promote products derived
19# from this software without specific prior written permission.
20#
21# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31# POSSIBILITY OF SUCH DAMAGE.
33"""Homogeneous Transformation Matrices and Quaternions.
35A library for calculating 4x4 matrices for translating, rotating, reflecting,
36scaling, shearing, projecting, orthogonalizing, and superimposing arrays of
373D homogeneous coordinates as well as for converting between rotation matrices,
38Euler angles, and quaternions. Also includes an Arcball control object and
39functions to decompose transformation matrices.
41:Author:
42 `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`_
44:Organization:
45 Laboratory for Fluorescence Dynamics, University of California, Irvine
47:Version: 2015.07.18
49Requirements
50------------
51* `CPython 2.7 or 3.4 <http://www.python.org>`_
52* `Numpy 1.9 <http://www.numpy.org>`_
53* `Transformations.c 2015.07.18 <http://www.lfd.uci.edu/~gohlke/>`_
54 (recommended for speedup of some functions)
56Notes
57-----
58The API is not stable yet and is expected to change between revisions.
60This Python code is not optimized for speed. Refer to the transformations.c
61module for a faster implementation of some functions.
63Documentation in HTML format can be generated with epydoc.
65Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using
66numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using
67numpy.dot(M, v) for shape (4, *) column vectors, respectively
68numpy.dot(v, M.T) for shape (*, 4) row vectors ("array of points").
70This module follows the "column vectors on the right" and "row major storage"
71(C contiguous) conventions. The translation components are in the right column
72of the transformation matrix, i.e. M[:3, 3].
73The transpose of the transformation matrices may have to be used to interface
74with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16].
76Calculations are carried out with numpy.float64 precision.
78Vector, point, quaternion, and matrix function arguments are expected to be
79"array like", i.e. tuple, list, or numpy arrays.
81Return types are numpy arrays unless specified otherwise.
83Angles are in radians unless specified otherwise.
85Quaternions w+ix+jy+kz are represented as [w, x, y, z].
87A triple of Euler angles can be applied/interpreted in 24 ways, which can
88be specified using a 4 character string or encoded 4-tuple:
90 *Axes 4-string*: e.g. 'sxyz' or 'ryxy'
92 - first character : rotations are applied to 's'tatic or 'r'otating frame
93 - remaining characters : successive rotation axis 'x', 'y', or 'z'
95 *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
97 - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
98 - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed
99 by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
100 - repetition : first and last axis are same (1) or different (0).
101 - frame : rotations are applied to static (0) or rotating (1) frame.
103Other Python packages and modules for 3D transformations and quaternions:
105* `Transforms3d <https://pypi.python.org/pypi/transforms3d>`_
106 includes most code of this module.
107* `Blender.mathutils <http://www.blender.org/api/blender_python_api>`_
108* `numpy-dtypes <https://github.com/numpy/numpy-dtypes>`_
110References
111----------
112(1) Matrices and transformations. Ronald Goldman.
113 In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
114(2) More matrices and transformations: shear and pseudo-perspective.
115 Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
116(3) Decomposing a matrix into simple transformations. Spencer Thomas.
117 In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
118(4) Recovering the data from the transformation matrix. Ronald Goldman.
119 In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991.
120(5) Euler angle conversion. Ken Shoemake.
121 In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
122(6) Arcball rotation control. Ken Shoemake.
123 In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
124(7) Representing attitude: Euler angles, unit quaternions, and rotation
125 vectors. James Diebel. 2006.
126(8) A discussion of the solution for the best rotation to relate two sets
127 of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.
128(9) Closed-form solution of absolute orientation using unit quaternions.
129 BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642.
130(10) Quaternions. Ken Shoemake.
131 http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf
132(11) From quaternion to matrix and back. JMP van Waveren. 2005.
133 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
134(12) Uniform random rotations. Ken Shoemake.
135 In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992.
136(13) Quaternion in molecular modeling. CFF Karney.
137 J Mol Graph Mod, 25(5):595-604
138(14) New method for extracting the quaternion from a rotation matrix.
139 Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087.
140(15) Multiple View Geometry in Computer Vision. Hartley and Zissermann.
141 Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130.
142(16) Column Vectors vs. Row Vectors.
143 http://steve.hollasch.net/cgindex/math/matrix/column-vec.html
145Examples
146--------
147>>> alpha, beta, gamma = 0.123, -1.234, 2.345
148>>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
149>>> I = identity_matrix()
150>>> Rx = rotation_matrix(alpha, xaxis)
151>>> Ry = rotation_matrix(beta, yaxis)
152>>> Rz = rotation_matrix(gamma, zaxis)
153>>> R = concatenate_matrices(Rx, Ry, Rz)
154>>> euler = euler_from_matrix(R, 'rxyz')
155>>> numpy.allclose([alpha, beta, gamma], euler)
156True
157>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
158>>> is_same_transform(R, Re)
159True
160>>> al, be, ga = euler_from_matrix(Re, 'rxyz')
161>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
162True
163>>> qx = quaternion_about_axis(alpha, xaxis)
164>>> qy = quaternion_about_axis(beta, yaxis)
165>>> qz = quaternion_about_axis(gamma, zaxis)
166>>> q = quaternion_multiply(qx, qy)
167>>> q = quaternion_multiply(q, qz)
168>>> Rq = quaternion_matrix(q)
169>>> is_same_transform(R, Rq)
170True
171>>> S = scale_matrix(1.23, origin)
172>>> T = translation_matrix([1, 2, 3])
173>>> Z = shear_matrix(beta, xaxis, origin, zaxis)
174>>> R = random_rotation_matrix(numpy.random.rand(3))
175>>> M = concatenate_matrices(T, R, Z, S)
176>>> scale, shear, angles, trans, persp = decompose_matrix(M)
177>>> numpy.allclose(scale, 1.23)
178True
179>>> numpy.allclose(trans, [1, 2, 3])
180True
181>>> numpy.allclose(shear, [0, math.tan(beta), 0])
182True
183>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
184True
185>>> M1 = compose_matrix(scale, shear, angles, trans, persp)
186>>> is_same_transform(M, M1)
187True
188>>> v0, v1 = random_vector(3), random_vector(3)
189>>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1))
190>>> v2 = numpy.dot(v0, M[:3,:3].T)
191>>> numpy.allclose(unit_vector(v1), unit_vector(v2))
192True
194"""
196from __future__ import division, print_function
198import math
200import numpy
202__version__ = '2015.07.18'
203__docformat__ = 'restructuredtext en'
204__all__ = ()
207def identity_matrix():
208 """Return 4x4 identity/unit matrix.
210 >>> I = identity_matrix()
211 >>> numpy.allclose(I, numpy.dot(I, I))
212 True
213 >>> numpy.sum(I), numpy.trace(I)
214 (4.0, 4.0)
215 >>> numpy.allclose(I, numpy.identity(4))
216 True
218 """
219 return numpy.identity(4)
222def translation_matrix(direction):
223 """Return matrix to translate by direction vector.
225 >>> v = numpy.random.random(3) - 0.5
226 >>> numpy.allclose(v, translation_matrix(v)[:3, 3])
227 True
229 """
230 M = numpy.identity(4)
231 M[:3, 3] = direction[:3]
232 return M
235def translation_from_matrix(matrix):
236 """Return translation vector from translation matrix.
238 >>> v0 = numpy.random.random(3) - 0.5
239 >>> v1 = translation_from_matrix(translation_matrix(v0))
240 >>> numpy.allclose(v0, v1)
241 True
243 """
244 return numpy.array(matrix, copy=False)[:3, 3].copy()
247def reflection_matrix(point, normal):
248 """Return matrix to mirror at plane defined by point and normal vector.
250 >>> v0 = numpy.random.random(4) - 0.5
251 >>> v0[3] = 1.
252 >>> v1 = numpy.random.random(3) - 0.5
253 >>> R = reflection_matrix(v0, v1)
254 >>> numpy.allclose(2, numpy.trace(R))
255 True
256 >>> numpy.allclose(v0, numpy.dot(R, v0))
257 True
258 >>> v2 = v0.copy()
259 >>> v2[:3] += v1
260 >>> v3 = v0.copy()
261 >>> v2[:3] -= v1
262 >>> numpy.allclose(v2, numpy.dot(R, v3))
263 True
265 """
266 normal = unit_vector(normal[:3])
267 M = numpy.identity(4)
268 M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
269 M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
270 return M
273def reflection_from_matrix(matrix):
274 """Return mirror plane point and normal vector from reflection matrix.
276 >>> v0 = numpy.random.random(3) - 0.5
277 >>> v1 = numpy.random.random(3) - 0.5
278 >>> M0 = reflection_matrix(v0, v1)
279 >>> point, normal = reflection_from_matrix(M0)
280 >>> M1 = reflection_matrix(point, normal)
281 >>> is_same_transform(M0, M1)
282 True
284 """
285 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
286 # normal: unit eigenvector corresponding to eigenvalue -1
287 w, V = numpy.linalg.eig(M[:3, :3])
288 i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0]
289 if not len(i):
290 raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
291 normal = numpy.real(V[:, i[0]]).squeeze()
292 # point: any unit eigenvector corresponding to eigenvalue 1
293 w, V = numpy.linalg.eig(M)
294 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
295 if not len(i):
296 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
297 point = numpy.real(V[:, i[-1]]).squeeze()
298 point /= point[3]
299 return point, normal
302def rotation_matrix(angle, direction, point=None):
303 """Return matrix to rotate about axis defined by point and direction.
305 >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0])
306 >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])
307 True
308 >>> angle = (random.random() - 0.5) * (2*math.pi)
309 >>> direc = numpy.random.random(3) - 0.5
310 >>> point = numpy.random.random(3) - 0.5
311 >>> R0 = rotation_matrix(angle, direc, point)
312 >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
313 >>> is_same_transform(R0, R1)
314 True
315 >>> R0 = rotation_matrix(angle, direc, point)
316 >>> R1 = rotation_matrix(-angle, -direc, point)
317 >>> is_same_transform(R0, R1)
318 True
319 >>> I = numpy.identity(4, numpy.float64)
320 >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
321 True
322 >>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2,
323 ... direc, point)))
324 True
326 """
327 sina = math.sin(angle)
328 cosa = math.cos(angle)
329 direction = unit_vector(direction[:3])
330 # rotation matrix around unit vector
331 R = numpy.diag([cosa, cosa, cosa])
332 R += numpy.outer(direction, direction) * (1.0 - cosa)
333 direction *= sina
334 R += numpy.array([[ 0.0, -direction[2], direction[1]],
335 [ direction[2], 0.0, -direction[0]],
336 [-direction[1], direction[0], 0.0]])
337 M = numpy.identity(4)
338 M[:3, :3] = R
339 if point is not None:
340 # rotation not around origin
341 point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
342 M[:3, 3] = point - numpy.dot(R, point)
343 return M
346def rotation_from_matrix(matrix):
347 """Return rotation angle and axis from rotation matrix.
349 >>> angle = (random.random() - 0.5) * (2*math.pi)
350 >>> direc = numpy.random.random(3) - 0.5
351 >>> point = numpy.random.random(3) - 0.5
352 >>> R0 = rotation_matrix(angle, direc, point)
353 >>> angle, direc, point = rotation_from_matrix(R0)
354 >>> R1 = rotation_matrix(angle, direc, point)
355 >>> is_same_transform(R0, R1)
356 True
358 """
359 R = numpy.array(matrix, dtype=numpy.float64, copy=False)
360 R33 = R[:3, :3]
361 # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
362 w, W = numpy.linalg.eig(R33.T)
363 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
364 if not len(i):
365 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
366 direction = numpy.real(W[:, i[-1]]).squeeze()
367 # point: unit eigenvector of R33 corresponding to eigenvalue of 1
368 w, Q = numpy.linalg.eig(R)
369 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
370 if not len(i):
371 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
372 point = numpy.real(Q[:, i[-1]]).squeeze()
373 point /= point[3]
374 # rotation angle depending on direction
375 cosa = (numpy.trace(R33) - 1.0) / 2.0
376 if abs(direction[2]) > 1e-8:
377 sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
378 elif abs(direction[1]) > 1e-8:
379 sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
380 else:
381 sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
382 angle = math.atan2(sina, cosa)
383 return angle, direction, point
386def scale_matrix(factor, origin=None, direction=None):
387 """Return matrix to scale by factor around origin in direction.
389 Use factor -1 for point symmetry.
391 >>> v = (numpy.random.rand(4, 5) - 0.5) * 20
392 >>> v[3] = 1
393 >>> S = scale_matrix(-1.234)
394 >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3])
395 True
396 >>> factor = random.random() * 10 - 5
397 >>> origin = numpy.random.random(3) - 0.5
398 >>> direct = numpy.random.random(3) - 0.5
399 >>> S = scale_matrix(factor, origin)
400 >>> S = scale_matrix(factor, origin, direct)
402 """
403 if direction is None:
404 # uniform scaling
405 M = numpy.diag([factor, factor, factor, 1.0])
406 if origin is not None:
407 M[:3, 3] = origin[:3]
408 M[:3, 3] *= 1.0 - factor
409 else:
410 # nonuniform scaling
411 direction = unit_vector(direction[:3])
412 factor = 1.0 - factor
413 M = numpy.identity(4)
414 M[:3, :3] -= factor * numpy.outer(direction, direction)
415 if origin is not None:
416 M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction
417 return M
420def scale_from_matrix(matrix):
421 """Return scaling factor, origin and direction from scaling matrix.
423 >>> factor = random.random() * 10 - 5
424 >>> origin = numpy.random.random(3) - 0.5
425 >>> direct = numpy.random.random(3) - 0.5
426 >>> S0 = scale_matrix(factor, origin)
427 >>> factor, origin, direction = scale_from_matrix(S0)
428 >>> S1 = scale_matrix(factor, origin, direction)
429 >>> is_same_transform(S0, S1)
430 True
431 >>> S0 = scale_matrix(factor, origin, direct)
432 >>> factor, origin, direction = scale_from_matrix(S0)
433 >>> S1 = scale_matrix(factor, origin, direction)
434 >>> is_same_transform(S0, S1)
435 True
437 """
438 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
439 M33 = M[:3, :3]
440 factor = numpy.trace(M33) - 2.0
441 try:
442 # direction: unit eigenvector corresponding to eigenvalue factor
443 w, V = numpy.linalg.eig(M33)
444 i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0]
445 direction = numpy.real(V[:, i]).squeeze()
446 direction /= vector_norm(direction)
447 except IndexError:
448 # uniform scaling
449 factor = (factor + 2.0) / 3.0
450 direction = None
451 # origin: any eigenvector corresponding to eigenvalue 1
452 w, V = numpy.linalg.eig(M)
453 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
454 if not len(i):
455 raise ValueError("no eigenvector corresponding to eigenvalue 1")
456 origin = numpy.real(V[:, i[-1]]).squeeze()
457 origin /= origin[3]
458 return factor, origin, direction
461def projection_matrix(point, normal, direction=None,
462 perspective=None, pseudo=False):
463 """Return matrix to project onto plane defined by point and normal.
465 Using either perspective point, projection direction, or none of both.
467 If pseudo is True, perspective projections will preserve relative depth
468 such that Perspective = dot(Orthogonal, PseudoPerspective).
470 >>> P = projection_matrix([0, 0, 0], [1, 0, 0])
471 >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:])
472 True
473 >>> point = numpy.random.random(3) - 0.5
474 >>> normal = numpy.random.random(3) - 0.5
475 >>> direct = numpy.random.random(3) - 0.5
476 >>> persp = numpy.random.random(3) - 0.5
477 >>> P0 = projection_matrix(point, normal)
478 >>> P1 = projection_matrix(point, normal, direction=direct)
479 >>> P2 = projection_matrix(point, normal, perspective=persp)
480 >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
481 >>> is_same_transform(P2, numpy.dot(P0, P3))
482 True
483 >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0])
484 >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20
485 >>> v0[3] = 1
486 >>> v1 = numpy.dot(P, v0)
487 >>> numpy.allclose(v1[1], v0[1])
488 True
489 >>> numpy.allclose(v1[0], 3-v1[1])
490 True
492 """
493 M = numpy.identity(4)
494 point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
495 normal = unit_vector(normal[:3])
496 if perspective is not None:
497 # perspective projection
498 perspective = numpy.array(perspective[:3], dtype=numpy.float64,
499 copy=False)
500 M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal)
501 M[:3, :3] -= numpy.outer(perspective, normal)
502 if pseudo:
503 # preserve relative depth
504 M[:3, :3] -= numpy.outer(normal, normal)
505 M[:3, 3] = numpy.dot(point, normal) * (perspective+normal)
506 else:
507 M[:3, 3] = numpy.dot(point, normal) * perspective
508 M[3, :3] = -normal
509 M[3, 3] = numpy.dot(perspective, normal)
510 elif direction is not None:
511 # parallel projection
512 direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False)
513 scale = numpy.dot(direction, normal)
514 M[:3, :3] -= numpy.outer(direction, normal) / scale
515 M[:3, 3] = direction * (numpy.dot(point, normal) / scale)
516 else:
517 # orthogonal projection
518 M[:3, :3] -= numpy.outer(normal, normal)
519 M[:3, 3] = numpy.dot(point, normal) * normal
520 return M
523def projection_from_matrix(matrix, pseudo=False):
524 """Return projection plane and perspective point from projection matrix.
526 Return values are same as arguments for projection_matrix function:
527 point, normal, direction, perspective, and pseudo.
529 >>> point = numpy.random.random(3) - 0.5
530 >>> normal = numpy.random.random(3) - 0.5
531 >>> direct = numpy.random.random(3) - 0.5
532 >>> persp = numpy.random.random(3) - 0.5
533 >>> P0 = projection_matrix(point, normal)
534 >>> result = projection_from_matrix(P0)
535 >>> P1 = projection_matrix(*result)
536 >>> is_same_transform(P0, P1)
537 True
538 >>> P0 = projection_matrix(point, normal, direct)
539 >>> result = projection_from_matrix(P0)
540 >>> P1 = projection_matrix(*result)
541 >>> is_same_transform(P0, P1)
542 True
543 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
544 >>> result = projection_from_matrix(P0, pseudo=False)
545 >>> P1 = projection_matrix(*result)
546 >>> is_same_transform(P0, P1)
547 True
548 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
549 >>> result = projection_from_matrix(P0, pseudo=True)
550 >>> P1 = projection_matrix(*result)
551 >>> is_same_transform(P0, P1)
552 True
554 """
555 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
556 M33 = M[:3, :3]
557 w, V = numpy.linalg.eig(M)
558 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
559 if not pseudo and len(i):
560 # point: any eigenvector corresponding to eigenvalue 1
561 point = numpy.real(V[:, i[-1]]).squeeze()
562 point /= point[3]
563 # direction: unit eigenvector corresponding to eigenvalue 0
564 w, V = numpy.linalg.eig(M33)
565 i = numpy.where(abs(numpy.real(w)) < 1e-8)[0]
566 if not len(i):
567 raise ValueError("no eigenvector corresponding to eigenvalue 0")
568 direction = numpy.real(V[:, i[0]]).squeeze()
569 direction /= vector_norm(direction)
570 # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
571 w, V = numpy.linalg.eig(M33.T)
572 i = numpy.where(abs(numpy.real(w)) < 1e-8)[0]
573 if len(i):
574 # parallel projection
575 normal = numpy.real(V[:, i[0]]).squeeze()
576 normal /= vector_norm(normal)
577 return point, normal, direction, None, False
578 else:
579 # orthogonal projection, where normal equals direction vector
580 return point, direction, None, None, False
581 else:
582 # perspective projection
583 i = numpy.where(abs(numpy.real(w)) > 1e-8)[0]
584 if not len(i):
585 raise ValueError(
586 "no eigenvector not corresponding to eigenvalue 0")
587 point = numpy.real(V[:, i[-1]]).squeeze()
588 point /= point[3]
589 normal = - M[3, :3]
590 perspective = M[:3, 3] / numpy.dot(point[:3], normal)
591 if pseudo:
592 perspective -= normal
593 return point, normal, None, perspective, pseudo
596def clip_matrix(left, right, bottom, top, near, far, perspective=False):
597 """Return matrix to obtain normalized device coordinates from frustum.
599 The frustum bounds are axis-aligned along x (left, right),
600 y (bottom, top) and z (near, far).
602 Normalized device coordinates are in range [-1, 1] if coordinates are
603 inside the frustum.
605 If perspective is True the frustum is a truncated pyramid with the
606 perspective point at origin and direction along z axis, otherwise an
607 orthographic canonical view volume (a box).
609 Homogeneous coordinates transformed by the perspective clip matrix
610 need to be dehomogenized (divided by w coordinate).
612 >>> frustum = numpy.random.rand(6)
613 >>> frustum[1] += frustum[0]
614 >>> frustum[3] += frustum[2]
615 >>> frustum[5] += frustum[4]
616 >>> M = clip_matrix(perspective=False, *frustum)
617 >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1])
618 array([-1., -1., -1., 1.])
619 >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1])
620 array([ 1., 1., 1., 1.])
621 >>> M = clip_matrix(perspective=True, *frustum)
622 >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1])
623 >>> v / v[3]
624 array([-1., -1., -1., 1.])
625 >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1])
626 >>> v / v[3]
627 array([ 1., 1., -1., 1.])
629 """
630 if left >= right or bottom >= top or near >= far:
631 raise ValueError("invalid frustum")
632 if perspective:
633 if near <= _EPS:
634 raise ValueError("invalid frustum: near <= 0")
635 t = 2.0 * near
636 M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0],
637 [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0],
638 [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)],
639 [0.0, 0.0, -1.0, 0.0]]
640 else:
641 M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)],
642 [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)],
643 [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)],
644 [0.0, 0.0, 0.0, 1.0]]
645 return numpy.array(M)
648def shear_matrix(angle, direction, point, normal):
649 """Return matrix to shear by angle along direction vector on shear plane.
651 The shear plane is defined by a point and normal vector. The direction
652 vector must be orthogonal to the plane's normal vector.
654 A point P is transformed by the shear matrix into P'' such that
655 the vector P-P'' is parallel to the direction vector and its extent is
656 given by the angle of P-P'-P'', where P' is the orthogonal projection
657 of P onto the shear plane.
659 >>> angle = (random.random() - 0.5) * 4*math.pi
660 >>> direct = numpy.random.random(3) - 0.5
661 >>> point = numpy.random.random(3) - 0.5
662 >>> normal = numpy.cross(direct, numpy.random.random(3))
663 >>> S = shear_matrix(angle, direct, point, normal)
664 >>> numpy.allclose(1, numpy.linalg.det(S))
665 True
667 """
668 normal = unit_vector(normal[:3])
669 direction = unit_vector(direction[:3])
670 if abs(numpy.dot(normal, direction)) > 1e-6:
671 raise ValueError("direction and normal vectors are not orthogonal")
672 angle = math.tan(angle)
673 M = numpy.identity(4)
674 M[:3, :3] += angle * numpy.outer(direction, normal)
675 M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
676 return M
679def shear_from_matrix(matrix):
680 """Return shear angle, direction and plane from shear matrix.
682 >>> angle = (random.random() - 0.5) * 4*math.pi
683 >>> direct = numpy.random.random(3) - 0.5
684 >>> point = numpy.random.random(3) - 0.5
685 >>> normal = numpy.cross(direct, numpy.random.random(3))
686 >>> S0 = shear_matrix(angle, direct, point, normal)
687 >>> angle, direct, point, normal = shear_from_matrix(S0)
688 >>> S1 = shear_matrix(angle, direct, point, normal)
689 >>> is_same_transform(S0, S1)
690 True
692 """
693 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
694 M33 = M[:3, :3]
695 # normal: cross independent eigenvectors corresponding to the eigenvalue 1
696 w, V = numpy.linalg.eig(M33)
697 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0]
698 if len(i) < 2:
699 raise ValueError("no two linear independent eigenvectors found %s" % w)
700 V = numpy.real(V[:, i]).squeeze().T
701 lenorm = -1.0
702 for i0, i1 in ((0, 1), (0, 2), (1, 2)):
703 n = numpy.cross(V[i0], V[i1])
704 w = vector_norm(n)
705 if w > lenorm:
706 lenorm = w
707 normal = n
708 normal /= lenorm
709 # direction and angle
710 direction = numpy.dot(M33 - numpy.identity(3), normal)
711 angle = vector_norm(direction)
712 direction /= angle
713 angle = math.atan(angle)
714 # point: eigenvector corresponding to eigenvalue 1
715 w, V = numpy.linalg.eig(M)
716 i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0]
717 if not len(i):
718 raise ValueError("no eigenvector corresponding to eigenvalue 1")
719 point = numpy.real(V[:, i[-1]]).squeeze()
720 point /= point[3]
721 return angle, direction, point, normal
724def decompose_matrix(matrix):
725 """Return sequence of transformations from transformation matrix.
727 matrix : array_like
728 Non-degenerative homogeneous transformation matrix
730 Return tuple of:
731 scale : vector of 3 scaling factors
732 shear : list of shear factors for x-y, x-z, y-z axes
733 angles : list of Euler angles about static x, y, z axes
734 translate : translation vector along x, y, z axes
735 perspective : perspective partition of matrix
737 Raise ValueError if matrix is of wrong type or degenerative.
739 >>> T0 = translation_matrix([1, 2, 3])
740 >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
741 >>> T1 = translation_matrix(trans)
742 >>> numpy.allclose(T0, T1)
743 True
744 >>> S = scale_matrix(0.123)
745 >>> scale, shear, angles, trans, persp = decompose_matrix(S)
746 >>> scale[0]
747 0.123
748 >>> R0 = euler_matrix(1, 2, 3)
749 >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
750 >>> R1 = euler_matrix(*angles)
751 >>> numpy.allclose(R0, R1)
752 True
754 """
755 M = numpy.array(matrix, dtype=numpy.float64, copy=True).T
756 if abs(M[3, 3]) < _EPS:
757 raise ValueError("M[3, 3] is zero")
758 M /= M[3, 3]
759 P = M.copy()
760 P[:, 3] = 0.0, 0.0, 0.0, 1.0
761 if not numpy.linalg.det(P):
762 raise ValueError("matrix is singular")
764 scale = numpy.zeros((3, ))
765 shear = [0.0, 0.0, 0.0]
766 angles = [0.0, 0.0, 0.0]
768 if any(abs(M[:3, 3]) > _EPS):
769 perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T))
770 M[:, 3] = 0.0, 0.0, 0.0, 1.0
771 else:
772 perspective = numpy.array([0.0, 0.0, 0.0, 1.0])
774 translate = M[3, :3].copy()
775 M[3, :3] = 0.0
777 row = M[:3, :3].copy()
778 scale[0] = vector_norm(row[0])
779 row[0] /= scale[0]
780 shear[0] = numpy.dot(row[0], row[1])
781 row[1] -= row[0] * shear[0]
782 scale[1] = vector_norm(row[1])
783 row[1] /= scale[1]
784 shear[0] /= scale[1]
785 shear[1] = numpy.dot(row[0], row[2])
786 row[2] -= row[0] * shear[1]
787 shear[2] = numpy.dot(row[1], row[2])
788 row[2] -= row[1] * shear[2]
789 scale[2] = vector_norm(row[2])
790 row[2] /= scale[2]
791 shear[1:] /= scale[2]
793 if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0:
794 numpy.negative(scale, scale)
795 numpy.negative(row, row)
797 angles[1] = math.asin(-row[0, 2])
798 if math.cos(angles[1]):
799 angles[0] = math.atan2(row[1, 2], row[2, 2])
800 angles[2] = math.atan2(row[0, 1], row[0, 0])
801 else:
802 #angles[0] = math.atan2(row[1, 0], row[1, 1])
803 angles[0] = math.atan2(-row[2, 1], row[1, 1])
804 angles[2] = 0.0
806 return scale, shear, angles, translate, perspective
809def compose_matrix(scale=None, shear=None, angles=None, translate=None,
810 perspective=None):
811 """Return transformation matrix from sequence of transformations.
813 This is the inverse of the decompose_matrix function.
815 Sequence of transformations:
816 scale : vector of 3 scaling factors
817 shear : list of shear factors for x-y, x-z, y-z axes
818 angles : list of Euler angles about static x, y, z axes
819 translate : translation vector along x, y, z axes
820 perspective : perspective partition of matrix
822 >>> scale = numpy.random.random(3) - 0.5
823 >>> shear = numpy.random.random(3) - 0.5
824 >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi)
825 >>> trans = numpy.random.random(3) - 0.5
826 >>> persp = numpy.random.random(4) - 0.5
827 >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
828 >>> result = decompose_matrix(M0)
829 >>> M1 = compose_matrix(*result)
830 >>> is_same_transform(M0, M1)
831 True
833 """
834 M = numpy.identity(4)
835 if perspective is not None:
836 P = numpy.identity(4)
837 P[3, :] = perspective[:4]
838 M = numpy.dot(M, P)
839 if translate is not None:
840 T = numpy.identity(4)
841 T[:3, 3] = translate[:3]
842 M = numpy.dot(M, T)
843 if angles is not None:
844 R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz')
845 M = numpy.dot(M, R)
846 if shear is not None:
847 Z = numpy.identity(4)
848 Z[1, 2] = shear[2]
849 Z[0, 2] = shear[1]
850 Z[0, 1] = shear[0]
851 M = numpy.dot(M, Z)
852 if scale is not None:
853 S = numpy.identity(4)
854 S[0, 0] = scale[0]
855 S[1, 1] = scale[1]
856 S[2, 2] = scale[2]
857 M = numpy.dot(M, S)
858 M /= M[3, 3]
859 return M
862def orthogonalization_matrix(lengths, angles):
863 """Return orthogonalization matrix for crystallographic cell coordinates.
865 Angles are expected in degrees.
867 The de-orthogonalization matrix is the inverse.
869 >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90])
870 >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
871 True
872 >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
873 >>> numpy.allclose(numpy.sum(O), 43.063229)
874 True
876 """
877 a, b, c = lengths
878 angles = numpy.radians(angles)
879 sina, sinb, _ = numpy.sin(angles)
880 cosa, cosb, cosg = numpy.cos(angles)
881 co = (cosa * cosb - cosg) / (sina * sinb)
882 return numpy.array([
883 [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0],
884 [-a*sinb*co, b*sina, 0.0, 0.0],
885 [ a*cosb, b*cosa, c, 0.0],
886 [ 0.0, 0.0, 0.0, 1.0]])
889def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
890 """Return affine transform matrix to register two point sets.
892 v0 and v1 are shape (ndims, *) arrays of at least ndims non-homogeneous
893 coordinates, where ndims is the dimensionality of the coordinate space.
895 If shear is False, a similarity transformation matrix is returned.
896 If also scale is False, a rigid/Euclidean transformation matrix
897 is returned.
899 By default the algorithm by Hartley and Zissermann [15] is used.
900 If usesvd is True, similarity and Euclidean transformation matrices
901 are calculated by minimizing the weighted sum of squared deviations
902 (RMSD) according to the algorithm by Kabsch [8].
903 Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]
904 is used, which is slower when using this Python implementation.
906 The returned matrix performs rotation, translation and uniform scaling
907 (if specified).
909 >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
910 >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
911 >>> affine_matrix_from_points(v0, v1)
912 array([[ 0.14549, 0.00062, 675.50008],
913 [ 0.00048, 0.14094, 53.24971],
914 [ 0. , 0. , 1. ]])
915 >>> T = translation_matrix(numpy.random.random(3)-0.5)
916 >>> R = random_rotation_matrix(numpy.random.random(3))
917 >>> S = scale_matrix(random.random())
918 >>> M = concatenate_matrices(T, R, S)
919 >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
920 >>> v0[3] = 1
921 >>> v1 = numpy.dot(M, v0)
922 >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1)
923 >>> M = affine_matrix_from_points(v0[:3], v1[:3])
924 >>> numpy.allclose(v1, numpy.dot(M, v0))
925 True
927 More examples in superimposition_matrix()
929 """
930 v0 = numpy.array(v0, dtype=numpy.float64, copy=True)
931 v1 = numpy.array(v1, dtype=numpy.float64, copy=True)
933 ndims = v0.shape[0]
934 if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
935 raise ValueError("input arrays are of wrong shape or type")
937 # move centroids to origin
938 t0 = -numpy.mean(v0, axis=1)
939 M0 = numpy.identity(ndims+1)
940 M0[:ndims, ndims] = t0
941 v0 += t0.reshape(ndims, 1)
942 t1 = -numpy.mean(v1, axis=1)
943 M1 = numpy.identity(ndims+1)
944 M1[:ndims, ndims] = t1
945 v1 += t1.reshape(ndims, 1)
947 if shear:
948 # Affine transformation
949 A = numpy.concatenate((v0, v1), axis=0)
950 u, s, vh = numpy.linalg.svd(A.T)
951 vh = vh[:ndims].T
952 B = vh[:ndims]
953 C = vh[ndims:2*ndims]
954 t = numpy.dot(C, numpy.linalg.pinv(B))
955 t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1)
956 M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,)))
957 elif usesvd or ndims != 3:
958 # Rigid transformation via SVD of covariance matrix
959 u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T))
960 # rotation matrix from SVD orthonormal bases
961 R = numpy.dot(u, vh)
962 if numpy.linalg.det(R) < 0.0:
963 # R does not constitute right handed system
964 R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0)
965 s[-1] *= -1.0
966 # homogeneous transformation matrix
967 M = numpy.identity(ndims+1)
968 M[:ndims, :ndims] = R
969 else:
970 # Rigid transformation matrix via quaternion
971 # compute symmetric matrix N
972 xx, yy, zz = numpy.sum(v0 * v1, axis=1)
973 xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1)
974 xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1)
975 N = [[xx+yy+zz, 0.0, 0.0, 0.0],
976 [yz-zy, xx-yy-zz, 0.0, 0.0],
977 [zx-xz, xy+yx, yy-xx-zz, 0.0],
978 [xy-yx, zx+xz, yz+zy, zz-xx-yy]]
979 # quaternion: eigenvector corresponding to most positive eigenvalue
980 w, V = numpy.linalg.eigh(N)
981 q = V[:, numpy.argmax(w)]
982 q /= vector_norm(q) # unit quaternion
983 # homogeneous transformation matrix
984 M = quaternion_matrix(q)
986 if scale and not shear:
987 # Affine transformation; scale is ratio of RMS deviations from centroid
988 v0 *= v0
989 v1 *= v1
990 M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0))
992 # move centroids back
993 M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0))
994 M /= M[ndims, ndims]
995 return M
998def superimposition_matrix(v0, v1, scale=False, usesvd=True):
999 """Return matrix to transform given 3D point set into second point set.
1001 v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 points.
1003 The parameters scale and usesvd are explained in the more general
1004 affine_matrix_from_points function.
1006 The returned matrix is a similarity or Euclidean transformation matrix.
1007 This function has a fast C implementation in transformations.c.
1009 >>> v0 = numpy.random.rand(3, 10)
1010 >>> M = superimposition_matrix(v0, v0)
1011 >>> numpy.allclose(M, numpy.identity(4))
1012 True
1013 >>> R = random_rotation_matrix(numpy.random.random(3))
1014 >>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]]
1015 >>> v1 = numpy.dot(R, v0)
1016 >>> M = superimposition_matrix(v0, v1)
1017 >>> numpy.allclose(v1, numpy.dot(M, v0))
1018 True
1019 >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
1020 >>> v0[3] = 1
1021 >>> v1 = numpy.dot(R, v0)
1022 >>> M = superimposition_matrix(v0, v1)
1023 >>> numpy.allclose(v1, numpy.dot(M, v0))
1024 True
1025 >>> S = scale_matrix(random.random())
1026 >>> T = translation_matrix(numpy.random.random(3)-0.5)
1027 >>> M = concatenate_matrices(T, R, S)
1028 >>> v1 = numpy.dot(M, v0)
1029 >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1)
1030 >>> M = superimposition_matrix(v0, v1, scale=True)
1031 >>> numpy.allclose(v1, numpy.dot(M, v0))
1032 True
1033 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1034 >>> numpy.allclose(v1, numpy.dot(M, v0))
1035 True
1036 >>> v = numpy.empty((4, 100, 3))
1037 >>> v[:, :, 0] = v0
1038 >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False)
1039 >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0]))
1040 True
1042 """
1043 v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3]
1044 v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3]
1045 return affine_matrix_from_points(v0, v1, shear=False,
1046 scale=scale, usesvd=usesvd)
1049def euler_matrix(ai, aj, ak, axes='sxyz'):
1050 """Return homogeneous rotation matrix from Euler angles and axis sequence.
1052 ai, aj, ak : Euler's roll, pitch and yaw angles
1053 axes : One of 24 axis sequences as string or encoded tuple
1055 >>> R = euler_matrix(1, 2, 3, 'syxz')
1056 >>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
1057 True
1058 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
1059 >>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
1060 True
1061 >>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5)
1062 >>> for axes in _AXES2TUPLE.keys():
1063 ... R = euler_matrix(ai, aj, ak, axes)
1064 >>> for axes in _TUPLE2AXES.keys():
1065 ... R = euler_matrix(ai, aj, ak, axes)
1067 """
1068 try:
1069 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
1070 except (AttributeError, KeyError):
1071 _TUPLE2AXES[axes] # validation
1072 firstaxis, parity, repetition, frame = axes
1074 i = firstaxis
1075 j = _NEXT_AXIS[i+parity]
1076 k = _NEXT_AXIS[i-parity+1]
1078 if frame:
1079 ai, ak = ak, ai
1080 if parity:
1081 ai, aj, ak = -ai, -aj, -ak
1083 si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
1084 ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
1085 cc, cs = ci*ck, ci*sk
1086 sc, ss = si*ck, si*sk
1088 M = numpy.identity(4)
1089 if repetition:
1090 M[i, i] = cj
1091 M[i, j] = sj*si
1092 M[i, k] = sj*ci
1093 M[j, i] = sj*sk
1094 M[j, j] = -cj*ss+cc
1095 M[j, k] = -cj*cs-sc
1096 M[k, i] = -sj*ck
1097 M[k, j] = cj*sc+cs
1098 M[k, k] = cj*cc-ss
1099 else:
1100 M[i, i] = cj*ck
1101 M[i, j] = sj*sc-cs
1102 M[i, k] = sj*cc+ss
1103 M[j, i] = cj*sk
1104 M[j, j] = sj*ss+cc
1105 M[j, k] = sj*cs-sc
1106 M[k, i] = -sj
1107 M[k, j] = cj*si
1108 M[k, k] = cj*ci
1109 return M
1112def euler_from_matrix(matrix, axes='sxyz'):
1113 """Return Euler angles from rotation matrix for specified axis sequence.
1115 axes : One of 24 axis sequences as string or encoded tuple
1117 Note that many Euler angle triplets can describe one matrix.
1119 >>> R0 = euler_matrix(1, 2, 3, 'syxz')
1120 >>> al, be, ga = euler_from_matrix(R0, 'syxz')
1121 >>> R1 = euler_matrix(al, be, ga, 'syxz')
1122 >>> numpy.allclose(R0, R1)
1123 True
1124 >>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5)
1125 >>> for axes in _AXES2TUPLE.keys():
1126 ... R0 = euler_matrix(axes=axes, *angles)
1127 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
1128 ... if not numpy.allclose(R0, R1): print(axes, "failed")
1130 """
1131 try:
1132 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
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 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]
1142 if repetition:
1143 sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
1144 if sy > _EPS:
1145 ax = math.atan2( M[i, j], M[i, k])
1146 ay = math.atan2( sy, M[i, i])
1147 az = math.atan2( M[j, i], -M[k, i])
1148 else:
1149 ax = math.atan2(-M[j, k], M[j, j])
1150 ay = math.atan2( sy, M[i, i])
1151 az = 0.0
1152 else:
1153 cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
1154 if cy > _EPS:
1155 ax = math.atan2( M[k, j], M[k, k])
1156 ay = math.atan2(-M[k, i], cy)
1157 az = math.atan2( M[j, i], M[i, i])
1158 else:
1159 ax = math.atan2(-M[j, k], M[j, j])
1160 ay = math.atan2(-M[k, i], cy)
1161 az = 0.0
1163 if parity:
1164 ax, ay, az = -ax, -ay, -az
1165 if frame:
1166 ax, az = az, ax
1167 return ax, ay, az
1170def euler_from_quaternion(quaternion, axes='sxyz'):
1171 """Return Euler angles from quaternion for specified axis sequence.
1173 >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
1174 >>> numpy.allclose(angles, [0.123, 0, 0])
1175 True
1177 """
1178 return euler_from_matrix(quaternion_matrix(quaternion), axes)
1181def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
1182 """Return quaternion from Euler angles and axis sequence.
1184 ai, aj, ak : Euler's roll, pitch and yaw angles
1185 axes : One of 24 axis sequences as string or encoded tuple
1187 >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
1188 >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
1189 True
1191 """
1192 try:
1193 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1194 except (AttributeError, KeyError):
1195 _TUPLE2AXES[axes] # validation
1196 firstaxis, parity, repetition, frame = axes
1198 i = firstaxis + 1
1199 j = _NEXT_AXIS[i+parity-1] + 1
1200 k = _NEXT_AXIS[i-parity] + 1
1202 if frame:
1203 ai, ak = ak, ai
1204 if parity:
1205 aj = -aj
1207 ai /= 2.0
1208 aj /= 2.0
1209 ak /= 2.0
1210 ci = math.cos(ai)
1211 si = math.sin(ai)
1212 cj = math.cos(aj)
1213 sj = math.sin(aj)
1214 ck = math.cos(ak)
1215 sk = math.sin(ak)
1216 cc = ci*ck
1217 cs = ci*sk
1218 sc = si*ck
1219 ss = si*sk
1221 q = numpy.empty((4, ))
1222 if repetition:
1223 q[0] = cj*(cc - ss)
1224 q[i] = cj*(cs + sc)
1225 q[j] = sj*(cc + ss)
1226 q[k] = sj*(cs - sc)
1227 else:
1228 q[0] = cj*cc + sj*ss
1229 q[i] = cj*sc - sj*cs
1230 q[j] = cj*ss + sj*cc
1231 q[k] = cj*cs - sj*sc
1232 if parity:
1233 q[j] *= -1.0
1235 return q
1238def quaternion_about_axis(angle, axis):
1239 """Return quaternion for rotation about axis.
1241 >>> q = quaternion_about_axis(0.123, [1, 0, 0])
1242 >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0])
1243 True
1245 """
1246 q = numpy.array([0.0, axis[0], axis[1], axis[2]])
1247 qlen = vector_norm(q)
1248 if qlen > _EPS:
1249 q *= math.sin(angle/2.0) / qlen
1250 q[0] = math.cos(angle/2.0)
1251 return q
1254def quaternion_matrix(quaternion):
1255 """Return homogeneous rotation matrix from quaternion.
1257 >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
1258 >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
1259 True
1260 >>> M = quaternion_matrix([1, 0, 0, 0])
1261 >>> numpy.allclose(M, numpy.identity(4))
1262 True
1263 >>> M = quaternion_matrix([0, 1, 0, 0])
1264 >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
1265 True
1267 """
1268 q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1269 n = numpy.dot(q, q)
1270 if n < _EPS:
1271 return numpy.identity(4)
1272 q *= math.sqrt(2.0 / n)
1273 q = numpy.outer(q, q)
1274 return numpy.array([
1275 [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
1276 [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
1277 [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
1278 [ 0.0, 0.0, 0.0, 1.0]])
1281def quaternion_from_matrix(matrix, isprecise=False):
1282 """Return quaternion from rotation matrix.
1284 If isprecise is True, the input matrix is assumed to be a precise rotation
1285 matrix and a faster algorithm is used.
1287 >>> q = quaternion_from_matrix(numpy.identity(4), True)
1288 >>> numpy.allclose(q, [1, 0, 0, 0])
1289 True
1290 >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1]))
1291 >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0])
1292 True
1293 >>> R = rotation_matrix(0.123, (1, 2, 3))
1294 >>> q = quaternion_from_matrix(R, True)
1295 >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
1296 True
1297 >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
1298 ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
1299 >>> q = quaternion_from_matrix(R)
1300 >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
1301 True
1302 >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
1303 ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
1304 >>> q = quaternion_from_matrix(R)
1305 >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
1306 True
1307 >>> R = random_rotation_matrix()
1308 >>> q = quaternion_from_matrix(R)
1309 >>> is_same_transform(R, quaternion_matrix(q))
1310 True
1311 >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0)
1312 >>> numpy.allclose(quaternion_from_matrix(R, isprecise=False),
1313 ... quaternion_from_matrix(R, isprecise=True))
1314 True
1316 """
1317 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
1318 if isprecise:
1319 q = numpy.empty((4, ))
1320 t = numpy.trace(M)
1321 if t > M[3, 3]:
1322 q[0] = t
1323 q[3] = M[1, 0] - M[0, 1]
1324 q[2] = M[0, 2] - M[2, 0]
1325 q[1] = M[2, 1] - M[1, 2]
1326 else:
1327 i, j, k = 1, 2, 3
1328 if M[1, 1] > M[0, 0]:
1329 i, j, k = 2, 3, 1
1330 if M[2, 2] > M[i, i]:
1331 i, j, k = 3, 1, 2
1332 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
1333 q[i] = t
1334 q[j] = M[i, j] + M[j, i]
1335 q[k] = M[k, i] + M[i, k]
1336 q[3] = M[k, j] - M[j, k]
1337 q *= 0.5 / math.sqrt(t * M[3, 3])
1338 else:
1339 m00 = M[0, 0]
1340 m01 = M[0, 1]
1341 m02 = M[0, 2]
1342 m10 = M[1, 0]
1343 m11 = M[1, 1]
1344 m12 = M[1, 2]
1345 m20 = M[2, 0]
1346 m21 = M[2, 1]
1347 m22 = M[2, 2]
1348 # symmetric matrix K
1349 K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0],
1350 [m01+m10, m11-m00-m22, 0.0, 0.0],
1351 [m02+m20, m12+m21, m22-m00-m11, 0.0],
1352 [m21-m12, m02-m20, m10-m01, m00+m11+m22]])
1353 K /= 3.0
1354 # quaternion is eigenvector of K that corresponds to largest eigenvalue
1355 w, V = numpy.linalg.eigh(K)
1356 q = V[[3, 0, 1, 2], numpy.argmax(w)]
1357 if q[0] < 0.0:
1358 numpy.negative(q, q)
1359 return q
1362def quaternion_multiply(quaternion1, quaternion0):
1363 """Return multiplication of two quaternions.
1365 >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
1366 >>> numpy.allclose(q, [28, -44, -14, 48])
1367 True
1369 """
1370 w0, x0, y0, z0 = quaternion0
1371 w1, x1, y1, z1 = quaternion1
1372 return numpy.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0,
1373 x1*w0 + y1*z0 - z1*y0 + w1*x0,
1374 -x1*z0 + y1*w0 + z1*x0 + w1*y0,
1375 x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64)
1378def quaternion_conjugate(quaternion):
1379 """Return conjugate of quaternion.
1381 >>> q0 = random_quaternion()
1382 >>> q1 = quaternion_conjugate(q0)
1383 >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:])
1384 True
1386 """
1387 q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1388 numpy.negative(q[1:], q[1:])
1389 return q
1392def quaternion_inverse(quaternion):
1393 """Return inverse of quaternion.
1395 >>> q0 = random_quaternion()
1396 >>> q1 = quaternion_inverse(q0)
1397 >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])
1398 True
1400 """
1401 q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1402 numpy.negative(q[1:], q[1:])
1403 return q / numpy.dot(q, q)
1406def quaternion_real(quaternion):
1407 """Return real part of quaternion.
1409 >>> quaternion_real([3, 0, 1, 2])
1410 3.0
1412 """
1413 return float(quaternion[0])
1416def quaternion_imag(quaternion):
1417 """Return imaginary part of quaternion.
1419 >>> quaternion_imag([3, 0, 1, 2])
1420 array([ 0., 1., 2.])
1422 """
1423 return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True)
1426def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
1427 """Return spherical linear interpolation between two quaternions.
1429 >>> q0 = random_quaternion()
1430 >>> q1 = random_quaternion()
1431 >>> q = quaternion_slerp(q0, q1, 0)
1432 >>> numpy.allclose(q, q0)
1433 True
1434 >>> q = quaternion_slerp(q0, q1, 1, 1)
1435 >>> numpy.allclose(q, q1)
1436 True
1437 >>> q = quaternion_slerp(q0, q1, 0.5)
1438 >>> angle = math.acos(numpy.dot(q0, q))
1439 >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \
1440 numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle)
1441 True
1443 """
1444 q0 = unit_vector(quat0[:4])
1445 q1 = unit_vector(quat1[:4])
1446 if fraction == 0.0:
1447 return q0
1448 elif fraction == 1.0:
1449 return q1
1450 d = numpy.dot(q0, q1)
1451 if abs(abs(d) - 1.0) < _EPS:
1452 return q0
1453 if shortestpath and d < 0.0:
1454 # invert rotation
1455 d = -d
1456 numpy.negative(q1, q1)
1457 angle = math.acos(d) + spin * math.pi
1458 if abs(angle) < _EPS:
1459 return q0
1460 isin = 1.0 / math.sin(angle)
1461 q0 *= math.sin((1.0 - fraction) * angle) * isin
1462 q1 *= math.sin(fraction * angle) * isin
1463 q0 += q1
1464 return q0
1467def random_quaternion(rand=None):
1468 """Return uniform random unit quaternion.
1470 rand: array like or None
1471 Three independent random variables that are uniformly distributed
1472 between 0 and 1.
1474 >>> q = random_quaternion()
1475 >>> numpy.allclose(1, vector_norm(q))
1476 True
1477 >>> q = random_quaternion(numpy.random.random(3))
1478 >>> len(q.shape), q.shape[0]==4
1479 (1, True)
1481 """
1482 if rand is None:
1483 rand = numpy.random.rand(3)
1484 else:
1485 assert len(rand) == 3
1486 r1 = numpy.sqrt(1.0 - rand[0])
1487 r2 = numpy.sqrt(rand[0])
1488 pi2 = math.pi * 2.0
1489 t1 = pi2 * rand[1]
1490 t2 = pi2 * rand[2]
1491 return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1,
1492 numpy.cos(t1)*r1, numpy.sin(t2)*r2])
1495def random_rotation_matrix(rand=None):
1496 """Return uniform random rotation matrix.
1498 rand: array like
1499 Three independent random variables that are uniformly distributed
1500 between 0 and 1 for each returned quaternion.
1502 >>> R = random_rotation_matrix()
1503 >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
1504 True
1506 """
1507 return quaternion_matrix(random_quaternion(rand))
1510class Arcball(object):
1511 """Virtual Trackball Control.
1513 >>> ball = Arcball()
1514 >>> ball = Arcball(initial=numpy.identity(4))
1515 >>> ball.place([320, 320], 320)
1516 >>> ball.down([500, 250])
1517 >>> ball.drag([475, 275])
1518 >>> R = ball.matrix()
1519 >>> numpy.allclose(numpy.sum(R), 3.90583455)
1520 True
1521 >>> ball = Arcball(initial=[1, 0, 0, 0])
1522 >>> ball.place([320, 320], 320)
1523 >>> ball.setaxes([1, 1, 0], [-1, 1, 0])
1524 >>> ball.constrain = True
1525 >>> ball.down([400, 200])
1526 >>> ball.drag([200, 400])
1527 >>> R = ball.matrix()
1528 >>> numpy.allclose(numpy.sum(R), 0.2055924)
1529 True
1530 >>> ball.next()
1532 """
1533 def __init__(self, initial=None):
1534 """Initialize virtual trackball control.
1536 initial : quaternion or rotation matrix
1538 """
1539 self._axis = None
1540 self._axes = None
1541 self._radius = 1.0
1542 self._center = [0.0, 0.0]
1543 self._vdown = numpy.array([0.0, 0.0, 1.0])
1544 self._constrain = False
1545 if initial is None:
1546 self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0])
1547 else:
1548 initial = numpy.array(initial, dtype=numpy.float64)
1549 if initial.shape == (4, 4):
1550 self._qdown = quaternion_from_matrix(initial)
1551 elif initial.shape == (4, ):
1552 initial /= vector_norm(initial)
1553 self._qdown = initial
1554 else:
1555 raise ValueError("initial not a quaternion or matrix")
1556 self._qnow = self._qpre = self._qdown
1558 def place(self, center, radius):
1559 """Place Arcball, e.g. when window size changes.
1561 center : sequence[2]
1562 Window coordinates of trackball center.
1563 radius : float
1564 Radius of trackball in window coordinates.
1566 """
1567 self._radius = float(radius)
1568 self._center[0] = center[0]
1569 self._center[1] = center[1]
1571 def setaxes(self, *axes):
1572 """Set axes to constrain rotations."""
1573 if axes is None:
1574 self._axes = None
1575 else:
1576 self._axes = [unit_vector(axis) for axis in axes]
1578 @property
1579 def constrain(self):
1580 """Return state of constrain to axis mode."""
1581 return self._constrain
1583 @constrain.setter
1584 def constrain(self, value):
1585 """Set state of constrain to axis mode."""
1586 self._constrain = bool(value)
1588 def down(self, point):
1589 """Set initial cursor window coordinates and pick constrain-axis."""
1590 self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
1591 self._qdown = self._qpre = self._qnow
1592 if self._constrain and self._axes is not None:
1593 self._axis = arcball_nearest_axis(self._vdown, self._axes)
1594 self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
1595 else:
1596 self._axis = None
1598 def drag(self, point):
1599 """Update current cursor window coordinates."""
1600 vnow = arcball_map_to_sphere(point, self._center, self._radius)
1601 if self._axis is not None:
1602 vnow = arcball_constrain_to_axis(vnow, self._axis)
1603 self._qpre = self._qnow
1604 t = numpy.cross(self._vdown, vnow)
1605 if numpy.dot(t, t) < _EPS:
1606 self._qnow = self._qdown
1607 else:
1608 q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]]
1609 self._qnow = quaternion_multiply(q, self._qdown)
1611 def next(self, acceleration=0.0):
1612 """Continue rotation in direction of last drag."""
1613 q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False)
1614 self._qpre, self._qnow = self._qnow, q
1616 def matrix(self):
1617 """Return homogeneous rotation matrix."""
1618 return quaternion_matrix(self._qnow)
1621def arcball_map_to_sphere(point, center, radius):
1622 """Return unit sphere coordinates from window coordinates."""
1623 v0 = (point[0] - center[0]) / radius
1624 v1 = (center[1] - point[1]) / radius
1625 n = v0*v0 + v1*v1
1626 if n > 1.0:
1627 # position outside of sphere
1628 n = math.sqrt(n)
1629 return numpy.array([v0/n, v1/n, 0.0])
1630 else:
1631 return numpy.array([v0, v1, math.sqrt(1.0 - n)])
1634def arcball_constrain_to_axis(point, axis):
1635 """Return sphere point perpendicular to axis."""
1636 v = numpy.array(point, dtype=numpy.float64, copy=True)
1637 a = numpy.array(axis, dtype=numpy.float64, copy=True)
1638 v -= a * numpy.dot(a, v) # on plane
1639 n = vector_norm(v)
1640 if n > _EPS:
1641 if v[2] < 0.0:
1642 numpy.negative(v, v)
1643 v /= n
1644 return v
1645 if a[2] == 1.0:
1646 return numpy.array([1.0, 0.0, 0.0])
1647 return unit_vector([-a[1], a[0], 0.0])
1650def arcball_nearest_axis(point, axes):
1651 """Return axis, which arc is nearest to point."""
1652 point = numpy.array(point, dtype=numpy.float64, copy=False)
1653 nearest = None
1654 mx = -1.0
1655 for axis in axes:
1656 t = numpy.dot(arcball_constrain_to_axis(point, axis), point)
1657 if t > mx:
1658 nearest = axis
1659 mx = t
1660 return nearest
1663# epsilon for testing whether a number is close to zero
1664_EPS = numpy.finfo(float).eps * 4.0
1666# axis sequences for Euler angles
1667_NEXT_AXIS = [1, 2, 0, 1]
1669# map axes strings to/from tuples of inner axis, parity, repetition, frame
1670_AXES2TUPLE = {
1671 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
1672 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
1673 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
1674 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
1675 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
1676 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
1677 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
1678 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
1680_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
1683def vector_norm(data, axis=None, out=None):
1684 """Return length, i.e. Euclidean norm, of ndarray along axis.
1686 >>> v = numpy.random.random(3)
1687 >>> n = vector_norm(v)
1688 >>> numpy.allclose(n, numpy.linalg.norm(v))
1689 True
1690 >>> v = numpy.random.rand(6, 5, 3)
1691 >>> n = vector_norm(v, axis=-1)
1692 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
1693 True
1694 >>> n = vector_norm(v, axis=1)
1695 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1696 True
1697 >>> v = numpy.random.rand(5, 4, 3)
1698 >>> n = numpy.empty((5, 3))
1699 >>> vector_norm(v, axis=1, out=n)
1700 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1701 True
1702 >>> vector_norm([])
1703 0.0
1704 >>> vector_norm([1])
1705 1.0
1707 """
1708 data = numpy.array(data, dtype=numpy.float64, copy=True)
1709 if out is None:
1710 if data.ndim == 1:
1711 return math.sqrt(numpy.dot(data, data))
1712 data *= data
1713 out = numpy.atleast_1d(numpy.sum(data, axis=axis))
1714 numpy.sqrt(out, out)
1715 return out
1716 else:
1717 data *= data
1718 numpy.sum(data, axis=axis, out=out)
1719 numpy.sqrt(out, out)
1722def unit_vector(data, axis=None, out=None):
1723 """Return ndarray normalized by length, i.e. Euclidean norm, along axis.
1725 >>> v0 = numpy.random.random(3)
1726 >>> v1 = unit_vector(v0)
1727 >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
1728 True
1729 >>> v0 = numpy.random.rand(5, 4, 3)
1730 >>> v1 = unit_vector(v0, axis=-1)
1731 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
1732 >>> numpy.allclose(v1, v2)
1733 True
1734 >>> v1 = unit_vector(v0, axis=1)
1735 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
1736 >>> numpy.allclose(v1, v2)
1737 True
1738 >>> v1 = numpy.empty((5, 4, 3))
1739 >>> unit_vector(v0, axis=1, out=v1)
1740 >>> numpy.allclose(v1, v2)
1741 True
1742 >>> list(unit_vector([]))
1743 []
1744 >>> list(unit_vector([1]))
1745 [1.0]
1747 """
1748 if out is None:
1749 data = numpy.array(data, dtype=numpy.float64, copy=True)
1750 if data.ndim == 1:
1751 data /= math.sqrt(numpy.dot(data, data))
1752 return data
1753 else:
1754 if out is not data:
1755 out = numpy.array(data, copy=False)
1756 data = out
1757 length = numpy.atleast_1d(numpy.sum(data*data, axis))
1758 numpy.sqrt(length, length)
1759 if axis is not None:
1760 length = numpy.expand_dims(length, axis)
1761 data /= length
1762 if out is None:
1763 return data
1766def random_vector(size):
1767 """Return array of random doubles in the half-open interval [0.0, 1.0).
1769 >>> v = random_vector(10000)
1770 >>> numpy.all(v >= 0) and numpy.all(v < 1)
1771 True
1772 >>> v0 = random_vector(10)
1773 >>> v1 = random_vector(10)
1774 >>> numpy.any(v0 == v1)
1775 False
1777 """
1778 return numpy.random.random(size)
1781def vector_product(v0, v1, axis=0):
1782 """Return vector perpendicular to vectors.
1784 >>> v = vector_product([2, 0, 0], [0, 3, 0])
1785 >>> numpy.allclose(v, [0, 0, 6])
1786 True
1787 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1788 >>> v1 = [[3], [0], [0]]
1789 >>> v = vector_product(v0, v1)
1790 >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])
1791 True
1792 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1793 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1794 >>> v = vector_product(v0, v1, axis=1)
1795 >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])
1796 True
1798 """
1799 return numpy.cross(v0, v1, axis=axis)
1802def angle_between_vectors(v0, v1, directed=True, axis=0):
1803 """Return angle between vectors.
1805 If directed is False, the input vectors are interpreted as undirected axes,
1806 i.e. the maximum angle is pi/2.
1808 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])
1809 >>> numpy.allclose(a, math.pi)
1810 True
1811 >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)
1812 >>> numpy.allclose(a, 0)
1813 True
1814 >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1815 >>> v1 = [[3], [0], [0]]
1816 >>> a = angle_between_vectors(v0, v1)
1817 >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532])
1818 True
1819 >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1820 >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1821 >>> a = angle_between_vectors(v0, v1, axis=1)
1822 >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])
1823 True
1825 """
1826 v0 = numpy.array(v0, dtype=numpy.float64, copy=False)
1827 v1 = numpy.array(v1, dtype=numpy.float64, copy=False)
1828 dot = numpy.sum(v0 * v1, axis=axis)
1829 dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)
1830 return numpy.arccos(dot if directed else numpy.fabs(dot))
1833def inverse_matrix(matrix):
1834 """Return inverse of square transformation matrix.
1836 >>> M0 = random_rotation_matrix()
1837 >>> M1 = inverse_matrix(M0.T)
1838 >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))
1839 True
1840 >>> for size in range(1, 7):
1841 ... M0 = numpy.random.rand(size, size)
1842 ... M1 = inverse_matrix(M0)
1843 ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size)
1845 """
1846 return numpy.linalg.inv(matrix)
1849def concatenate_matrices(*matrices):
1850 """Return concatenation of series of transformation matrices.
1852 >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5
1853 >>> numpy.allclose(M, concatenate_matrices(M))
1854 True
1855 >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))
1856 True
1858 """
1859 M = numpy.identity(4)
1860 for i in matrices:
1861 M = numpy.dot(M, i)
1862 return M
1865def is_same_transform(matrix0, matrix1):
1866 """Return True if two matrices perform same transformation.
1868 >>> is_same_transform(numpy.identity(4), numpy.identity(4))
1869 True
1870 >>> is_same_transform(numpy.identity(4), random_rotation_matrix())
1871 False
1873 """
1874 matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True)
1875 matrix0 /= matrix0[3, 3]
1876 matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True)
1877 matrix1 /= matrix1[3, 3]
1878 return numpy.allclose(matrix0, matrix1)
1881def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'):
1882 """Try import all public attributes from module into global namespace.
1884 Existing attributes with name clashes are renamed with prefix.
1885 Attributes starting with underscore are ignored by default.
1887 Return True on successful import.
1889 """
1890 import warnings
1891 from importlib import import_module
1892 try:
1893 if not package:
1894 module = import_module(name)
1895 else:
1896 module = import_module('.' + name, package=package)
1897 except ImportError:
1898 if warn:
1899 warnings.warn("failed to import module %s" % name)
1900 else:
1901 for attr in dir(module):
1902 if ignore and attr.startswith(ignore):
1903 continue
1904 if prefix:
1905 if attr in globals():
1906 globals()[prefix + attr] = globals()[attr]
1907 elif warn:
1908 warnings.warn("no Python implementation of " + attr)
1909 globals()[attr] = getattr(module, attr)
1910 return True
1912# _import_module('_transformations')
1914# if __name__ == "__main__":
1915# import doctest
1916# import random # used in doctests
1917# numpy.set_printoptions(suppress=True, precision=5)
1918# doctest.testmod()
1919#