transformations.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 # transformations.py
3 
4 # Copyright (c) 2006-2017, Christoph Gohlke
5 # Copyright (c) 2006-2017, 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.
32 
33 """Homogeneous Transformation Matrices and Quaternions.
34 
35 A library for calculating 4x4 matrices for translating, rotating, reflecting,
36 scaling, shearing, projecting, orthogonalizing, and superimposing arrays of
37 3D homogeneous coordinates as well as for converting between rotation matrices,
38 Euler angles, and quaternions. Also includes an Arcball control object and
39 functions to decompose transformation matrices.
40 
41 :Author:
42  `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`_
43 
44 :Organization:
45  Laboratory for Fluorescence Dynamics, University of California, Irvine
46 
47 :Version: 2017.02.17
48 
49 Requirements
50 ------------
51 * `CPython 2.7 or 3.5 <http://www.python.org>`_
52 * `Numpy 1.11 <http://www.numpy.org>`_
53 * `Transformations.c 2017.02.17 <http://www.lfd.uci.edu/~gohlke/>`_
54  (recommended for speedup of some functions)
55 
56 Notes
57 -----
58 The API is not stable yet and is expected to change between revisions.
59 
60 This Python code is not optimized for speed. Refer to the transformations.c
61 module for a faster implementation of some functions.
62 
63 Documentation in HTML format can be generated with epydoc.
64 
65 Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using
66 numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using
67 numpy.dot(M, v) for shape (4, \*) column vectors, respectively
68 numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points").
69 
70 This module follows the "column vectors on the right" and "row major storage"
71 (C contiguous) conventions. The translation components are in the right column
72 of the transformation matrix, i.e. M[:3, 3].
73 The transpose of the transformation matrices may have to be used to interface
74 with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16].
75 
76 Calculations are carried out with numpy.float64 precision.
77 
78 Vector, point, quaternion, and matrix function arguments are expected to be
79 "array like", i.e. tuple, list, or numpy arrays.
80 
81 Return types are numpy arrays unless specified otherwise.
82 
83 Angles are in radians unless specified otherwise.
84 
85 Quaternions w+ix+jy+kz are represented as [w, x, y, z].
86 
87 A triple of Euler angles can be applied/interpreted in 24 ways, which can
88 be specified using a 4 character string or encoded 4-tuple:
89 
90  *Axes 4-string*: e.g. 'sxyz' or 'ryxy'
91 
92  - first character : rotations are applied to 's'tatic or 'r'otating frame
93  - remaining characters : successive rotation axis 'x', 'y', or 'z'
94 
95  *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
96 
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.
102 
103 Other Python packages and modules for 3D transformations and quaternions:
104 
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>`_
109 
110 References
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
144 
145 Examples
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)
156 True
157 >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
158 >>> is_same_transform(R, Re)
159 True
160 >>> al, be, ga = euler_from_matrix(Re, 'rxyz')
161 >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
162 True
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)
170 True
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)
178 True
179 >>> numpy.allclose(trans, [1, 2, 3])
180 True
181 >>> numpy.allclose(shear, [0, math.tan(beta), 0])
182 True
183 >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
184 True
185 >>> M1 = compose_matrix(scale, shear, angles, trans, persp)
186 >>> is_same_transform(M, M1)
187 True
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))
192 True
193 
194 """
195 
196 from __future__ import division, print_function
197 
198 import math
199 
200 import numpy
201 
202 __version__ = '2017.02.17'
203 __docformat__ = 'restructuredtext en'
204 __all__ = ()
205 
206 
207 def identity_matrix():
208  """Return 4x4 identity/unit matrix.
209 
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
217 
218  """
219  return numpy.identity(4)
220 
221 
222 def translation_matrix(direction):
223  """Return matrix to translate by direction vector.
224 
225  >>> v = numpy.random.random(3) - 0.5
226  >>> numpy.allclose(v, translation_matrix(v)[:3, 3])
227  True
228 
229  """
230  M = numpy.identity(4)
231  M[:3, 3] = direction[:3]
232  return M
233 
234 
235 def translation_from_matrix(matrix):
236  """Return translation vector from translation matrix.
237 
238  >>> v0 = numpy.random.random(3) - 0.5
240  >>> numpy.allclose(v0, v1)
241  True
242 
243  """
244  return numpy.array(matrix, copy=False)[:3, 3].copy()
245 
246 
247 def reflection_matrix(point, normal):
248  """Return matrix to mirror at plane defined by point and normal vector.
249 
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
264 
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
271 
272 
273 def reflection_from_matrix(matrix):
274  """Return mirror plane point and normal vector from reflection matrix.
275 
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
283 
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
300 
301 
302 def rotation_matrix(angle, direction, point=None):
303  """Return matrix to rotate about axis defined by point and direction.
304 
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
325 
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
344 
345 
346 def rotation_from_matrix(matrix):
347  """Return rotation angle and axis from rotation matrix.
348 
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
357 
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
384 
385 
386 def scale_matrix(factor, origin=None, direction=None):
387  """Return matrix to scale by factor around origin in direction.
388 
389  Use factor -1 for point symmetry.
390 
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)
401 
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
418 
419 
420 def scale_from_matrix(matrix):
421  """Return scaling factor, origin and direction from scaling matrix.
422 
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
436 
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
459 
460 
461 def projection_matrix(point, normal, direction=None,
462  perspective=None, pseudo=False):
463  """Return matrix to project onto plane defined by point and normal.
464 
465  Using either perspective point, projection direction, or none of both.
466 
467  If pseudo is True, perspective projections will preserve relative depth
468  such that Perspective = dot(Orthogonal, PseudoPerspective).
469 
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
491 
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
521 
522 
523 def projection_from_matrix(matrix, pseudo=False):
524  """Return projection plane and perspective point from projection matrix.
525 
526  Return values are same as arguments for projection_matrix function:
527  point, normal, direction, perspective, and pseudo.
528 
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
553 
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
594 
595 
596 def clip_matrix(left, right, bottom, top, near, far, perspective=False):
597  """Return matrix to obtain normalized device coordinates from frustum.
598 
599  The frustum bounds are axis-aligned along x (left, right),
600  y (bottom, top) and z (near, far).
601 
602  Normalized device coordinates are in range [-1, 1] if coordinates are
603  inside the frustum.
604 
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).
608 
609  Homogeneous coordinates transformed by the perspective clip matrix
610  need to be dehomogenized (divided by w coordinate).
611 
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.])
628 
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)
646 
647 
648 def shear_matrix(angle, direction, point, normal):
649  """Return matrix to shear by angle along direction vector on shear plane.
650 
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.
653 
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.
658 
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
666 
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
677 
678 
679 def shear_from_matrix(matrix):
680  """Return shear angle, direction and plane from shear matrix.
681 
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
691 
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
722 
723 
724 def decompose_matrix(matrix):
725  """Return sequence of transformations from transformation matrix.
726 
727  matrix : array_like
728  Non-degenerative homogeneous transformation matrix
729 
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
736 
737  Raise ValueError if matrix is of wrong type or degenerative.
738 
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
753 
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")
763 
764  scale = numpy.zeros((3, ))
765  shear = [0.0, 0.0, 0.0]
766  angles = [0.0, 0.0, 0.0]
767 
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])
773 
774  translate = M[3, :3].copy()
775  M[3, :3] = 0.0
776 
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]
792 
793  if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0:
794  numpy.negative(scale, scale)
795  numpy.negative(row, row)
796 
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
805 
806  return scale, shear, angles, translate, perspective
807 
808 
809 def compose_matrix(scale=None, shear=None, angles=None, translate=None,
810  perspective=None):
811  """Return transformation matrix from sequence of transformations.
812 
813  This is the inverse of the decompose_matrix function.
814 
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
821 
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
832 
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
860 
861 
862 def orthogonalization_matrix(lengths, angles):
863  """Return orthogonalization matrix for crystallographic cell coordinates.
864 
865  Angles are expected in degrees.
866 
867  The de-orthogonalization matrix is the inverse.
868 
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
875 
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]])
887 
888 
889 def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
890  """Return affine transform matrix to register two point sets.
891 
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.
894 
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.
898 
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.
905 
906  The returned matrix performs rotation, translation and uniform scaling
907  (if specified).
908 
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
926 
927  More examples in superimposition_matrix()
928 
929  """
930  v0 = numpy.array(v0, dtype=numpy.float64, copy=True)
931  v1 = numpy.array(v1, dtype=numpy.float64, copy=True)
932 
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")
936 
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)
946 
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)
985 
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))
991 
992  # move centroids back
993  M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0))
994  M /= M[ndims, ndims]
995  return M
996 
997 
998 def superimposition_matrix(v0, v1, scale=False, usesvd=True):
999  """Return matrix to transform given 3D point set into second point set.
1000 
1001  v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points.
1002 
1003  The parameters scale and usesvd are explained in the more general
1004  affine_matrix_from_points function.
1005 
1006  The returned matrix is a similarity or Euclidean transformation matrix.
1007  This function has a fast C implementation in transformations.c.
1008 
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
1041 
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)
1047 
1048 
1049 def euler_matrix(ai, aj, ak, axes='sxyz'):
1050  """Return homogeneous rotation matrix from Euler angles and axis sequence.
1051 
1052  ai, aj, ak : Euler's roll, pitch and yaw angles
1053  axes : One of 24 axis sequences as string or encoded tuple
1054 
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)
1066 
1067  """
1068  try:
1069  firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
1070  except (AttributeError, KeyError):
1071  _TUPLE2AXES[axes] # validation
1072  firstaxis, parity, repetition, frame = axes
1073 
1074  i = firstaxis
1075  j = _NEXT_AXIS[i+parity]
1076  k = _NEXT_AXIS[i-parity+1]
1077 
1078  if frame:
1079  ai, ak = ak, ai
1080  if parity:
1081  ai, aj, ak = -ai, -aj, -ak
1082 
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
1087 
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
1110 
1111 
1112 def euler_from_matrix(matrix, axes='sxyz'):
1113  """Return Euler angles from rotation matrix for specified axis sequence.
1114 
1115  axes : One of 24 axis sequences as string or encoded tuple
1116 
1117  Note that many Euler angle triplets can describe one matrix.
1118 
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")
1129 
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
1136 
1137  i = firstaxis
1138  j = _NEXT_AXIS[i+parity]
1139  k = _NEXT_AXIS[i-parity+1]
1140 
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
1162 
1163  if parity:
1164  ax, ay, az = -ax, -ay, -az
1165  if frame:
1166  ax, az = az, ax
1167  return ax, ay, az
1168 
1169 
1170 def euler_from_quaternion(quaternion, axes='sxyz'):
1171  """Return Euler angles from quaternion for specified axis sequence.
1172 
1173  >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
1174  >>> numpy.allclose(angles, [0.123, 0, 0])
1175  True
1176 
1177  """
1178  return euler_from_matrix(quaternion_matrix(quaternion), axes)
1179 
1180 
1181 def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
1182  """Return quaternion from Euler angles and axis sequence.
1183 
1184  ai, aj, ak : Euler's roll, pitch and yaw angles
1185  axes : One of 24 axis sequences as string or encoded tuple
1186 
1187  >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
1188  >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
1189  True
1190 
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
1197 
1198  i = firstaxis + 1
1199  j = _NEXT_AXIS[i+parity-1] + 1
1200  k = _NEXT_AXIS[i-parity] + 1
1201 
1202  if frame:
1203  ai, ak = ak, ai
1204  if parity:
1205  aj = -aj
1206 
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
1220 
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
1234 
1235  return q
1236 
1237 
1238 def quaternion_about_axis(angle, axis):
1239  """Return quaternion for rotation about axis.
1240 
1241  >>> q = quaternion_about_axis(0.123, [1, 0, 0])
1242  >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0])
1243  True
1244 
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
1252 
1253 
1254 def quaternion_matrix(quaternion):
1255  """Return homogeneous rotation matrix from quaternion.
1256 
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
1266 
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]])
1279 
1280 
1281 def quaternion_from_matrix(matrix, isprecise=False):
1282  """Return quaternion from rotation matrix.
1283 
1284  If isprecise is True, the input matrix is assumed to be a precise rotation
1285  matrix and a faster algorithm is used.
1286 
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)
1310  True
1311  >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1312  ... quaternion_from_matrix(R, isprecise=True))
1313  True
1314  >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0)
1315  >>> is_same_quaternion(quaternion_from_matrix(R, isprecise=False),
1316  ... quaternion_from_matrix(R, isprecise=True))
1317  True
1318 
1319  """
1320  M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
1321  if isprecise:
1322  q = numpy.empty((4, ))
1323  t = numpy.trace(M)
1324  if t > M[3, 3]:
1325  q[0] = t
1326  q[3] = M[1, 0] - M[0, 1]
1327  q[2] = M[0, 2] - M[2, 0]
1328  q[1] = M[2, 1] - M[1, 2]
1329  else:
1330  i, j, k = 0, 1, 2
1331  if M[1, 1] > M[0, 0]:
1332  i, j, k = 1, 2, 0
1333  if M[2, 2] > M[i, i]:
1334  i, j, k = 2, 0, 1
1335  t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
1336  q[i] = t
1337  q[j] = M[i, j] + M[j, i]
1338  q[k] = M[k, i] + M[i, k]
1339  q[3] = M[k, j] - M[j, k]
1340  q = q[[3, 0, 1, 2]]
1341  q *= 0.5 / math.sqrt(t * M[3, 3])
1342  else:
1343  m00 = M[0, 0]
1344  m01 = M[0, 1]
1345  m02 = M[0, 2]
1346  m10 = M[1, 0]
1347  m11 = M[1, 1]
1348  m12 = M[1, 2]
1349  m20 = M[2, 0]
1350  m21 = M[2, 1]
1351  m22 = M[2, 2]
1352  # symmetric matrix K
1353  K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0],
1354  [m01+m10, m11-m00-m22, 0.0, 0.0],
1355  [m02+m20, m12+m21, m22-m00-m11, 0.0],
1356  [m21-m12, m02-m20, m10-m01, m00+m11+m22]])
1357  K /= 3.0
1358  # quaternion is eigenvector of K that corresponds to largest eigenvalue
1359  w, V = numpy.linalg.eigh(K)
1360  q = V[[3, 0, 1, 2], numpy.argmax(w)]
1361  if q[0] < 0.0:
1362  numpy.negative(q, q)
1363  return q
1364 
1365 
1366 def quaternion_multiply(quaternion1, quaternion0):
1367  """Return multiplication of two quaternions.
1368 
1369  >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7])
1370  >>> numpy.allclose(q, [28, -44, -14, 48])
1371  True
1372 
1373  """
1374  w0, x0, y0, z0 = quaternion0
1375  w1, x1, y1, z1 = quaternion1
1376  return numpy.array([
1377  -x1*x0 - y1*y0 - z1*z0 + w1*w0,
1378  x1*w0 + y1*z0 - z1*y0 + w1*x0,
1379  -x1*z0 + y1*w0 + z1*x0 + w1*y0,
1380  x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64)
1381 
1382 
1383 def quaternion_conjugate(quaternion):
1384  """Return conjugate of quaternion.
1385 
1386  >>> q0 = random_quaternion()
1387  >>> q1 = quaternion_conjugate(q0)
1388  >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:])
1389  True
1390 
1391  """
1392  q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1393  numpy.negative(q[1:], q[1:])
1394  return q
1395 
1396 
1397 def quaternion_inverse(quaternion):
1398  """Return inverse of quaternion.
1399 
1400  >>> q0 = random_quaternion()
1401  >>> q1 = quaternion_inverse(q0)
1402  >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0])
1403  True
1404 
1405  """
1406  q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
1407  numpy.negative(q[1:], q[1:])
1408  return q / numpy.dot(q, q)
1409 
1410 
1411 def quaternion_real(quaternion):
1412  """Return real part of quaternion.
1413 
1414  >>> quaternion_real([3, 0, 1, 2])
1415  3.0
1416 
1417  """
1418  return float(quaternion[0])
1419 
1420 
1421 def quaternion_imag(quaternion):
1422  """Return imaginary part of quaternion.
1423 
1424  >>> quaternion_imag([3, 0, 1, 2])
1425  array([ 0., 1., 2.])
1426 
1427  """
1428  return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True)
1429 
1430 
1431 def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
1432  """Return spherical linear interpolation between two quaternions.
1433 
1434  >>> q0 = random_quaternion()
1435  >>> q1 = random_quaternion()
1436  >>> q = quaternion_slerp(q0, q1, 0)
1437  >>> numpy.allclose(q, q0)
1438  True
1439  >>> q = quaternion_slerp(q0, q1, 1, 1)
1440  >>> numpy.allclose(q, q1)
1441  True
1442  >>> q = quaternion_slerp(q0, q1, 0.5)
1443  >>> angle = math.acos(numpy.dot(q0, q))
1444  >>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \
1445  numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle)
1446  True
1447 
1448  """
1449  q0 = unit_vector(quat0[:4])
1450  q1 = unit_vector(quat1[:4])
1451  if fraction == 0.0:
1452  return q0
1453  elif fraction == 1.0:
1454  return q1
1455  d = numpy.dot(q0, q1)
1456  if abs(abs(d) - 1.0) < _EPS:
1457  return q0
1458  if shortestpath and d < 0.0:
1459  # invert rotation
1460  d = -d
1461  numpy.negative(q1, q1)
1462  angle = math.acos(d) + spin * math.pi
1463  if abs(angle) < _EPS:
1464  return q0
1465  isin = 1.0 / math.sin(angle)
1466  q0 *= math.sin((1.0 - fraction) * angle) * isin
1467  q1 *= math.sin(fraction * angle) * isin
1468  q0 += q1
1469  return q0
1470 
1471 
1472 def random_quaternion(rand=None):
1473  """Return uniform random unit quaternion.
1474 
1475  rand: array like or None
1476  Three independent random variables that are uniformly distributed
1477  between 0 and 1.
1478 
1479  >>> q = random_quaternion()
1480  >>> numpy.allclose(1, vector_norm(q))
1481  True
1482  >>> q = random_quaternion(numpy.random.random(3))
1483  >>> len(q.shape), q.shape[0]==4
1484  (1, True)
1485 
1486  """
1487  if rand is None:
1488  rand = numpy.random.rand(3)
1489  else:
1490  assert len(rand) == 3
1491  r1 = numpy.sqrt(1.0 - rand[0])
1492  r2 = numpy.sqrt(rand[0])
1493  pi2 = math.pi * 2.0
1494  t1 = pi2 * rand[1]
1495  t2 = pi2 * rand[2]
1496  return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1,
1497  numpy.cos(t1)*r1, numpy.sin(t2)*r2])
1498 
1499 
1500 def random_rotation_matrix(rand=None):
1501  """Return uniform random rotation matrix.
1502 
1503  rand: array like
1504  Three independent random variables that are uniformly distributed
1505  between 0 and 1 for each returned quaternion.
1506 
1507  >>> R = random_rotation_matrix()
1508  >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
1509  True
1510 
1511  """
1512  return quaternion_matrix(random_quaternion(rand))
1513 
1514 
1515 class Arcball(object):
1516  """Virtual Trackball Control.
1517 
1518  >>> ball = Arcball()
1519  >>> ball = Arcball(initial=numpy.identity(4))
1520  >>> ball.place([320, 320], 320)
1521  >>> ball.down([500, 250])
1522  >>> ball.drag([475, 275])
1523  >>> R = ball.matrix()
1524  >>> numpy.allclose(numpy.sum(R), 3.90583455)
1525  True
1526  >>> ball = Arcball(initial=[1, 0, 0, 0])
1527  >>> ball.place([320, 320], 320)
1528  >>> ball.setaxes([1, 1, 0], [-1, 1, 0])
1529  >>> ball.constrain = True
1530  >>> ball.down([400, 200])
1531  >>> ball.drag([200, 400])
1532  >>> R = ball.matrix()
1533  >>> numpy.allclose(numpy.sum(R), 0.2055924)
1534  True
1535  >>> ball.next()
1536 
1537  """
1538 
1539  def __init__(self, initial=None):
1540  """Initialize virtual trackball control.
1541 
1542  initial : quaternion or rotation matrix
1543 
1544  """
1545  self._axis = None
1546  self._axes = None
1547  self._radius = 1.0
1548  self._center = [0.0, 0.0]
1549  self._vdown = numpy.array([0.0, 0.0, 1.0])
1550  self._constrain = False
1551  if initial is None:
1552  self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0])
1553  else:
1554  initial = numpy.array(initial, dtype=numpy.float64)
1555  if initial.shape == (4, 4):
1556  self._qdown = quaternion_from_matrix(initial)
1557  elif initial.shape == (4, ):
1558  initial /= vector_norm(initial)
1559  self._qdown = initial
1560  else:
1561  raise ValueError("initial not a quaternion or matrix")
1562  self._qnow = self._qpre = self._qdown
1563 
1564  def place(self, center, radius):
1565  """Place Arcball, e.g. when window size changes.
1566 
1567  center : sequence[2]
1568  Window coordinates of trackball center.
1569  radius : float
1570  Radius of trackball in window coordinates.
1571 
1572  """
1573  self._radius = float(radius)
1574  self._center[0] = center[0]
1575  self._center[1] = center[1]
1576 
1577  def setaxes(self, *axes):
1578  """Set axes to constrain rotations."""
1579  if axes is None:
1580  self._axes = None
1581  else:
1582  self._axes = [unit_vector(axis) for axis in axes]
1583 
1584  @property
1585  def constrain(self):
1586  """Return state of constrain to axis mode."""
1587  return self._constrain
1588 
1589  @constrain.setter
1590  def constrain(self, value):
1591  """Set state of constrain to axis mode."""
1592  self._constrain = bool(value)
1593 
1594  def down(self, point):
1595  """Set initial cursor window coordinates and pick constrain-axis."""
1596  self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
1597  self._qdown = self._qpre = self._qnow
1598  if self._constrain and self._axes is not None:
1599  self._axis = arcball_nearest_axis(self._vdown, self._axes)
1600  self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
1601  else:
1602  self._axis = None
1603 
1604  def drag(self, point):
1605  """Update current cursor window coordinates."""
1606  vnow = arcball_map_to_sphere(point, self._center, self._radius)
1607  if self._axis is not None:
1608  vnow = arcball_constrain_to_axis(vnow, self._axis)
1609  self._qpre = self._qnow
1610  t = numpy.cross(self._vdown, vnow)
1611  if numpy.dot(t, t) < _EPS:
1612  self._qnow = self._qdown
1613  else:
1614  q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]]
1615  self._qnow = quaternion_multiply(q, self._qdown)
1616 
1617  def next(self, acceleration=0.0):
1618  """Continue rotation in direction of last drag."""
1619  q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False)
1620  self._qpre, self._qnow = self._qnow, q
1621 
1622  def matrix(self):
1623  """Return homogeneous rotation matrix."""
1624  return quaternion_matrix(self._qnow)
1625 
1626 
1627 def arcball_map_to_sphere(point, center, radius):
1628  """Return unit sphere coordinates from window coordinates."""
1629  v0 = (point[0] - center[0]) / radius
1630  v1 = (center[1] - point[1]) / radius
1631  n = v0*v0 + v1*v1
1632  if n > 1.0:
1633  # position outside of sphere
1634  n = math.sqrt(n)
1635  return numpy.array([v0/n, v1/n, 0.0])
1636  else:
1637  return numpy.array([v0, v1, math.sqrt(1.0 - n)])
1638 
1639 
1640 def arcball_constrain_to_axis(point, axis):
1641  """Return sphere point perpendicular to axis."""
1642  v = numpy.array(point, dtype=numpy.float64, copy=True)
1643  a = numpy.array(axis, dtype=numpy.float64, copy=True)
1644  v -= a * numpy.dot(a, v) # on plane
1645  n = vector_norm(v)
1646  if n > _EPS:
1647  if v[2] < 0.0:
1648  numpy.negative(v, v)
1649  v /= n
1650  return v
1651  if a[2] == 1.0:
1652  return numpy.array([1.0, 0.0, 0.0])
1653  return unit_vector([-a[1], a[0], 0.0])
1654 
1655 
1656 def arcball_nearest_axis(point, axes):
1657  """Return axis, which arc is nearest to point."""
1658  point = numpy.array(point, dtype=numpy.float64, copy=False)
1659  nearest = None
1660  mx = -1.0
1661  for axis in axes:
1662  t = numpy.dot(arcball_constrain_to_axis(point, axis), point)
1663  if t > mx:
1664  nearest = axis
1665  mx = t
1666  return nearest
1667 
1668 
1669 # epsilon for testing whether a number is close to zero
1670 _EPS = numpy.finfo(float).eps * 4.0
1671 
1672 # axis sequences for Euler angles
1673 _NEXT_AXIS = [1, 2, 0, 1]
1674 
1675 # map axes strings to/from tuples of inner axis, parity, repetition, frame
1676 _AXES2TUPLE = {
1677  'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
1678  'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
1679  'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
1680  'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
1681  'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
1682  'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
1683  'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
1684  'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
1685 
1686 _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
1687 
1688 
1689 def vector_norm(data, axis=None, out=None):
1690  """Return length, i.e. Euclidean norm, of ndarray along axis.
1691 
1692  >>> v = numpy.random.random(3)
1693  >>> n = vector_norm(v)
1694  >>> numpy.allclose(n, numpy.linalg.norm(v))
1695  True
1696  >>> v = numpy.random.rand(6, 5, 3)
1697  >>> n = vector_norm(v, axis=-1)
1698  >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
1699  True
1700  >>> n = vector_norm(v, axis=1)
1701  >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1702  True
1703  >>> v = numpy.random.rand(5, 4, 3)
1704  >>> n = numpy.empty((5, 3))
1705  >>> vector_norm(v, axis=1, out=n)
1706  >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1707  True
1708  >>> vector_norm([])
1709  0.0
1710  >>> vector_norm([1])
1711  1.0
1712 
1713  """
1714  data = numpy.array(data, dtype=numpy.float64, copy=True)
1715  if out is None:
1716  if data.ndim == 1:
1717  return math.sqrt(numpy.dot(data, data))
1718  data *= data
1719  out = numpy.atleast_1d(numpy.sum(data, axis=axis))
1720  numpy.sqrt(out, out)
1721  return out
1722  else:
1723  data *= data
1724  numpy.sum(data, axis=axis, out=out)
1725  numpy.sqrt(out, out)
1726 
1727 
1728 def unit_vector(data, axis=None, out=None):
1729  """Return ndarray normalized by length, i.e. Euclidean norm, along axis.
1730 
1731  >>> v0 = numpy.random.random(3)
1732  >>> v1 = unit_vector(v0)
1733  >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
1734  True
1735  >>> v0 = numpy.random.rand(5, 4, 3)
1736  >>> v1 = unit_vector(v0, axis=-1)
1737  >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
1738  >>> numpy.allclose(v1, v2)
1739  True
1740  >>> v1 = unit_vector(v0, axis=1)
1741  >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
1742  >>> numpy.allclose(v1, v2)
1743  True
1744  >>> v1 = numpy.empty((5, 4, 3))
1745  >>> unit_vector(v0, axis=1, out=v1)
1746  >>> numpy.allclose(v1, v2)
1747  True
1748  >>> list(unit_vector([]))
1749  []
1750  >>> list(unit_vector([1]))
1751  [1.0]
1752 
1753  """
1754  if out is None:
1755  data = numpy.array(data, dtype=numpy.float64, copy=True)
1756  if data.ndim == 1:
1757  data /= math.sqrt(numpy.dot(data, data))
1758  return data
1759  else:
1760  if out is not data:
1761  out[:] = numpy.array(data, copy=False)
1762  data = out
1763  length = numpy.atleast_1d(numpy.sum(data*data, axis))
1764  numpy.sqrt(length, length)
1765  if axis is not None:
1766  length = numpy.expand_dims(length, axis)
1767  data /= length
1768  if out is None:
1769  return data
1770 
1771 
1772 def random_vector(size):
1773  """Return array of random doubles in the half-open interval [0.0, 1.0).
1774 
1775  >>> v = random_vector(10000)
1776  >>> numpy.all(v >= 0) and numpy.all(v < 1)
1777  True
1778  >>> v0 = random_vector(10)
1779  >>> v1 = random_vector(10)
1780  >>> numpy.any(v0 == v1)
1781  False
1782 
1783  """
1784  return numpy.random.random(size)
1785 
1786 
1787 def vector_product(v0, v1, axis=0):
1788  """Return vector perpendicular to vectors.
1789 
1790  >>> v = vector_product([2, 0, 0], [0, 3, 0])
1791  >>> numpy.allclose(v, [0, 0, 6])
1792  True
1793  >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1794  >>> v1 = [[3], [0], [0]]
1795  >>> v = vector_product(v0, v1)
1796  >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]])
1797  True
1798  >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1799  >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1800  >>> v = vector_product(v0, v1, axis=1)
1801  >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]])
1802  True
1803 
1804  """
1805  return numpy.cross(v0, v1, axis=axis)
1806 
1807 
1808 def angle_between_vectors(v0, v1, directed=True, axis=0):
1809  """Return angle between vectors.
1810 
1811  If directed is False, the input vectors are interpreted as undirected axes,
1812  i.e. the maximum angle is pi/2.
1813 
1814  >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3])
1815  >>> numpy.allclose(a, math.pi)
1816  True
1817  >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False)
1818  >>> numpy.allclose(a, 0)
1819  True
1820  >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]]
1821  >>> v1 = [[3], [0], [0]]
1822  >>> a = angle_between_vectors(v0, v1)
1823  >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532])
1824  True
1825  >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]]
1826  >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]]
1827  >>> a = angle_between_vectors(v0, v1, axis=1)
1828  >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532])
1829  True
1830 
1831  """
1832  v0 = numpy.array(v0, dtype=numpy.float64, copy=False)
1833  v1 = numpy.array(v1, dtype=numpy.float64, copy=False)
1834  dot = numpy.sum(v0 * v1, axis=axis)
1835  dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis)
1836  return numpy.arccos(dot if directed else numpy.fabs(dot))
1837 
1838 
1839 def inverse_matrix(matrix):
1840  """Return inverse of square transformation matrix.
1841 
1842  >>> M0 = random_rotation_matrix()
1843  >>> M1 = inverse_matrix(M0.T)
1844  >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))
1845  True
1846  >>> for size in range(1, 7):
1847  ... M0 = numpy.random.rand(size, size)
1848  ... M1 = inverse_matrix(M0)
1849  ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size)
1850 
1851  """
1852  return numpy.linalg.inv(matrix)
1853 
1854 
1855 def concatenate_matrices(*matrices):
1856  """Return concatenation of series of transformation matrices.
1857 
1858  >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5
1859  >>> numpy.allclose(M, concatenate_matrices(M))
1860  True
1861  >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))
1862  True
1863 
1864  """
1865  M = numpy.identity(4)
1866  for i in matrices:
1867  M = numpy.dot(M, i)
1868  return M
1869 
1870 
1871 def is_same_transform(matrix0, matrix1):
1872  """Return True if two matrices perform same transformation.
1873 
1874  >>> is_same_transform(numpy.identity(4), numpy.identity(4))
1875  True
1876  >>> is_same_transform(numpy.identity(4), random_rotation_matrix())
1877  False
1878 
1879  """
1880  matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True)
1881  matrix0 /= matrix0[3, 3]
1882  matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True)
1883  matrix1 /= matrix1[3, 3]
1884  return numpy.allclose(matrix0, matrix1)
1885 
1886 
1887 def is_same_quaternion(q0, q1):
1888  """Return True if two quaternions are equal."""
1889  q0 = numpy.array(q0)
1890  q1 = numpy.array(q1)
1891  return numpy.allclose(q0, q1) or numpy.allclose(q0, -q1)
1892 
1893 
1894 def _import_module(name, package=None, warn=False, prefix='_py_', ignore='_'):
1895  """Try import all public attributes from module into global namespace.
1896 
1897  Existing attributes with name clashes are renamed with prefix.
1898  Attributes starting with underscore are ignored by default.
1899 
1900  Return True on successful import.
1901 
1902  """
1903  import warnings
1904  from importlib import import_module
1905  try:
1906  if not package:
1907  module = import_module(name)
1908  else:
1909  module = import_module('.' + name, package=package)
1910  except ImportError:
1911  if warn:
1912  warnings.warn("failed to import module %s" % name)
1913  else:
1914  for attr in dir(module):
1915  if ignore and attr.startswith(ignore):
1916  continue
1917  if prefix:
1918  if attr in globals():
1919  globals()[prefix + attr] = globals()[attr]
1920  elif warn:
1921  warnings.warn("no Python implementation of " + attr)
1922  globals()[attr] = getattr(module, attr)
1923  return True
1924 
1925 
1926 _import_module('_transformations')
1927 
1928 if __name__ == "__main__":
1929  import doctest
1930  import random # noqa: used in doctests
1931  numpy.set_printoptions(suppress=True, precision=5)
1932  doctest.testmod()
def clip_matrix(left, right, bottom, top, near, far, perspective=False)
def quaternion_matrix(quaternion)
def euler_from_matrix(matrix, axes='sxyz')
def quaternion_multiply(quaternion1, quaternion0)
def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True)
def euler_matrix(ai, aj, ak, axes='sxyz')
def angle_between_vectors(v0, v1, directed=True, axis=0)
def concatenate_matrices(matrices)
def rotation_matrix(angle, direction, point=None)
def vector_norm(data, axis=None, out=None)
def quaternion_conjugate(quaternion)
def shear_matrix(angle, direction, point, normal)
def quaternion_inverse(quaternion)
def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True)
def reflection_matrix(point, normal)
def quaternion_real(quaternion)
def translation_matrix(direction)
def quaternion_about_axis(angle, axis)
def scale_matrix(factor, origin=None, direction=None)
def orthogonalization_matrix(lengths, angles)
def projection_from_matrix(matrix, pseudo=False)
def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False)
def quaternion_imag(quaternion)
def quaternion_from_euler(ai, aj, ak, axes='sxyz')
def unit_vector(data, axis=None, out=None)
def random_quaternion(rand=None)
def superimposition_matrix(v0, v1, scale=False, usesvd=True)
def euler_from_quaternion(quaternion, axes='sxyz')
def quaternion_from_matrix(matrix, isprecise=False)
def is_same_transform(matrix0, matrix1)
def compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None)
def vector_product(v0, v1, axis=0)
def random_rotation_matrix(rand=None)


exotica_python
Author(s):
autogenerated on Mon Feb 22 2021 03:33:27