1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32 """Homogeneous Transformation Matrices and Quaternions.
33
34 A library for calculating 4x4 matrices for translating, rotating, reflecting,
35 scaling, shearing, projecting, orthogonalizing, and superimposing arrays of
36 3D homogeneous coordinates as well as for converting between rotation matrices,
37 Euler angles, and quaternions. Also includes an Arcball control object and
38 functions to decompose transformation matrices.
39
40 :Authors:
41 `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`__,
42 Laboratory for Fluorescence Dynamics, University of California, Irvine
43
44 :Version: 20090418
45
46 Requirements
47 ------------
48
49 * `Python 2.6 <http://www.python.org>`__
50 * `Numpy 1.3 <http://numpy.scipy.org>`__
51 * `transformations.c 20090418 <http://www.lfd.uci.edu/~gohlke/>`__
52 (optional implementation of some functions in C)
53
54 Notes
55 -----
56
57 Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using
58 numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using
59 numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively
60 numpy.dot(v, M.T) for shape (\*, 4) "array of points".
61
62 Calculations are carried out with numpy.float64 precision.
63
64 This Python implementation is not optimized for speed.
65
66 Vector, point, quaternion, and matrix function arguments are expected to be
67 "array like", i.e. tuple, list, or numpy arrays.
68
69 Return types are numpy arrays unless specified otherwise.
70
71 Angles are in radians unless specified otherwise.
72
73 Quaternions ix+jy+kz+w are represented as [x, y, z, w].
74
75 Use the transpose of transformation matrices for OpenGL glMultMatrixd().
76
77 A triple of Euler angles can be applied/interpreted in 24 ways, which can
78 be specified using a 4 character string or encoded 4-tuple:
79
80 *Axes 4-string*: e.g. 'sxyz' or 'ryxy'
81
82 - first character : rotations are applied to 's'tatic or 'r'otating frame
83 - remaining characters : successive rotation axis 'x', 'y', or 'z'
84
85 *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
86
87 - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
88 - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed
89 by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
90 - repetition : first and last axis are same (1) or different (0).
91 - frame : rotations are applied to static (0) or rotating (1) frame.
92
93 References
94 ----------
95
96 (1) Matrices and transformations. Ronald Goldman.
97 In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
98 (2) More matrices and transformations: shear and pseudo-perspective.
99 Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
100 (3) Decomposing a matrix into simple transformations. Spencer Thomas.
101 In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
102 (4) Recovering the data from the transformation matrix. Ronald Goldman.
103 In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991.
104 (5) Euler angle conversion. Ken Shoemake.
105 In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
106 (6) Arcball rotation control. Ken Shoemake.
107 In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
108 (7) Representing attitude: Euler angles, unit quaternions, and rotation
109 vectors. James Diebel. 2006.
110 (8) A discussion of the solution for the best rotation to relate two sets
111 of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.
112 (9) Closed-form solution of absolute orientation using unit quaternions.
113 BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642.
114 (10) Quaternions. Ken Shoemake.
115 http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf
116 (11) From quaternion to matrix and back. JMP van Waveren. 2005.
117 http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
118 (12) Uniform random rotations. Ken Shoemake.
119 In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992.
120
121
122 Examples
123 --------
124
125 >>> alpha, beta, gamma = 0.123, -1.234, 2.345
126 >>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
127 >>> I = identity_matrix()
128 >>> Rx = rotation_matrix(alpha, xaxis)
129 >>> Ry = rotation_matrix(beta, yaxis)
130 >>> Rz = rotation_matrix(gamma, zaxis)
131 >>> R = concatenate_matrices(Rx, Ry, Rz)
132 >>> euler = euler_from_matrix(R, 'rxyz')
133 >>> numpy.allclose([alpha, beta, gamma], euler)
134 True
135 >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
136 >>> is_same_transform(R, Re)
137 True
138 >>> al, be, ga = euler_from_matrix(Re, 'rxyz')
139 >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
140 True
141 >>> qx = quaternion_about_axis(alpha, xaxis)
142 >>> qy = quaternion_about_axis(beta, yaxis)
143 >>> qz = quaternion_about_axis(gamma, zaxis)
144 >>> q = quaternion_multiply(qx, qy)
145 >>> q = quaternion_multiply(q, qz)
146 >>> Rq = quaternion_matrix(q)
147 >>> is_same_transform(R, Rq)
148 True
149 >>> S = scale_matrix(1.23, origin)
150 >>> T = translation_matrix((1, 2, 3))
151 >>> Z = shear_matrix(beta, xaxis, origin, zaxis)
152 >>> R = random_rotation_matrix(numpy.random.rand(3))
153 >>> M = concatenate_matrices(T, R, Z, S)
154 >>> scale, shear, angles, trans, persp = decompose_matrix(M)
155 >>> numpy.allclose(scale, 1.23)
156 True
157 >>> numpy.allclose(trans, (1, 2, 3))
158 True
159 >>> numpy.allclose(shear, (0, math.tan(beta), 0))
160 True
161 >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
162 True
163 >>> M1 = compose_matrix(scale, shear, angles, trans, persp)
164 >>> is_same_transform(M, M1)
165 True
166
167 """
168
169 from __future__ import division
170
171 import warnings
172 import math
173
174 import numpy
175
176
177 __docformat__ = "restructuredtext en"
178
179
181 """Return 4x4 identity/unit matrix.
182
183 >>> I = identity_matrix()
184 >>> numpy.allclose(I, numpy.dot(I, I))
185 True
186 >>> numpy.sum(I), numpy.trace(I)
187 (4.0, 4.0)
188 >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64))
189 True
190
191 """
192 return numpy.identity(4, dtype=numpy.float64)
193
194
196 """Return matrix to translate by direction vector.
197
198 >>> v = numpy.random.random(3) - 0.5
199 >>> numpy.allclose(v, translation_matrix(v)[:3, 3])
200 True
201
202 """
203 M = numpy.identity(4)
204 M[:3, 3] = direction[:3]
205 return M
206
207
209 """Return translation vector from translation matrix.
210
211 >>> v0 = numpy.random.random(3) - 0.5
212 >>> v1 = translation_from_matrix(translation_matrix(v0))
213 >>> numpy.allclose(v0, v1)
214 True
215
216 """
217 return numpy.array(matrix, copy=False)[:3, 3].copy()
218
219
221 """Return matrix to mirror at plane defined by point and normal vector.
222
223 >>> v0 = numpy.random.random(4) - 0.5
224 >>> v0[3] = 1.0
225 >>> v1 = numpy.random.random(3) - 0.5
226 >>> R = reflection_matrix(v0, v1)
227 >>> numpy.allclose(2., numpy.trace(R))
228 True
229 >>> numpy.allclose(v0, numpy.dot(R, v0))
230 True
231 >>> v2 = v0.copy()
232 >>> v2[:3] += v1
233 >>> v3 = v0.copy()
234 >>> v2[:3] -= v1
235 >>> numpy.allclose(v2, numpy.dot(R, v3))
236 True
237
238 """
239 normal = unit_vector(normal[:3])
240 M = numpy.identity(4)
241 M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
242 M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
243 return M
244
245
247 """Return mirror plane point and normal vector from reflection matrix.
248
249 >>> v0 = numpy.random.random(3) - 0.5
250 >>> v1 = numpy.random.random(3) - 0.5
251 >>> M0 = reflection_matrix(v0, v1)
252 >>> point, normal = reflection_from_matrix(M0)
253 >>> M1 = reflection_matrix(point, normal)
254 >>> is_same_transform(M0, M1)
255 True
256
257 """
258 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
259
260 l, V = numpy.linalg.eig(M[:3, :3])
261 i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0]
262 if not len(i):
263 raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
264 normal = numpy.real(V[:, i[0]]).squeeze()
265
266 l, V = numpy.linalg.eig(M)
267 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
268 if not len(i):
269 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
270 point = numpy.real(V[:, i[-1]]).squeeze()
271 point /= point[3]
272 return point, normal
273
274
276 """Return matrix to rotate about axis defined by point and direction.
277
278 >>> angle = (random.random() - 0.5) * (2*math.pi)
279 >>> direc = numpy.random.random(3) - 0.5
280 >>> point = numpy.random.random(3) - 0.5
281 >>> R0 = rotation_matrix(angle, direc, point)
282 >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
283 >>> is_same_transform(R0, R1)
284 True
285 >>> R0 = rotation_matrix(angle, direc, point)
286 >>> R1 = rotation_matrix(-angle, -direc, point)
287 >>> is_same_transform(R0, R1)
288 True
289 >>> I = numpy.identity(4, numpy.float64)
290 >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
291 True
292 >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2,
293 ... direc, point)))
294 True
295
296 """
297 sina = math.sin(angle)
298 cosa = math.cos(angle)
299 direction = unit_vector(direction[:3])
300
301 R = numpy.array(((cosa, 0.0, 0.0),
302 (0.0, cosa, 0.0),
303 (0.0, 0.0, cosa)), dtype=numpy.float64)
304 R += numpy.outer(direction, direction) * (1.0 - cosa)
305 direction *= sina
306 R += numpy.array((( 0.0, -direction[2], direction[1]),
307 ( direction[2], 0.0, -direction[0]),
308 (-direction[1], direction[0], 0.0)),
309 dtype=numpy.float64)
310 M = numpy.identity(4)
311 M[:3, :3] = R
312 if point is not None:
313
314 point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
315 M[:3, 3] = point - numpy.dot(R, point)
316 return M
317
318
320 """Return rotation angle and axis from rotation matrix.
321
322 >>> angle = (random.random() - 0.5) * (2*math.pi)
323 >>> direc = numpy.random.random(3) - 0.5
324 >>> point = numpy.random.random(3) - 0.5
325 >>> R0 = rotation_matrix(angle, direc, point)
326 >>> angle, direc, point = rotation_from_matrix(R0)
327 >>> R1 = rotation_matrix(angle, direc, point)
328 >>> is_same_transform(R0, R1)
329 True
330
331 """
332 R = numpy.array(matrix, dtype=numpy.float64, copy=False)
333 R33 = R[:3, :3]
334
335 l, W = numpy.linalg.eig(R33.T)
336 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
337 if not len(i):
338 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
339 direction = numpy.real(W[:, i[-1]]).squeeze()
340
341 l, Q = numpy.linalg.eig(R)
342 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
343 if not len(i):
344 raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
345 point = numpy.real(Q[:, i[-1]]).squeeze()
346 point /= point[3]
347
348 cosa = (numpy.trace(R33) - 1.0) / 2.0
349 if abs(direction[2]) > 1e-8:
350 sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
351 elif abs(direction[1]) > 1e-8:
352 sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
353 else:
354 sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
355 angle = math.atan2(sina, cosa)
356 return angle, direction, point
357
358
360 """Return matrix to scale by factor around origin in direction.
361
362 Use factor -1 for point symmetry.
363
364 >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0
365 >>> v[3] = 1.0
366 >>> S = scale_matrix(-1.234)
367 >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3])
368 True
369 >>> factor = random.random() * 10 - 5
370 >>> origin = numpy.random.random(3) - 0.5
371 >>> direct = numpy.random.random(3) - 0.5
372 >>> S = scale_matrix(factor, origin)
373 >>> S = scale_matrix(factor, origin, direct)
374
375 """
376 if direction is None:
377
378 M = numpy.array(((factor, 0.0, 0.0, 0.0),
379 (0.0, factor, 0.0, 0.0),
380 (0.0, 0.0, factor, 0.0),
381 (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64)
382 if origin is not None:
383 M[:3, 3] = origin[:3]
384 M[:3, 3] *= 1.0 - factor
385 else:
386
387 direction = unit_vector(direction[:3])
388 factor = 1.0 - factor
389 M = numpy.identity(4)
390 M[:3, :3] -= factor * numpy.outer(direction, direction)
391 if origin is not None:
392 M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction
393 return M
394
395
397 """Return scaling factor, origin and direction from scaling matrix.
398
399 >>> factor = random.random() * 10 - 5
400 >>> origin = numpy.random.random(3) - 0.5
401 >>> direct = numpy.random.random(3) - 0.5
402 >>> S0 = scale_matrix(factor, origin)
403 >>> factor, origin, direction = scale_from_matrix(S0)
404 >>> S1 = scale_matrix(factor, origin, direction)
405 >>> is_same_transform(S0, S1)
406 True
407 >>> S0 = scale_matrix(factor, origin, direct)
408 >>> factor, origin, direction = scale_from_matrix(S0)
409 >>> S1 = scale_matrix(factor, origin, direction)
410 >>> is_same_transform(S0, S1)
411 True
412
413 """
414 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
415 M33 = M[:3, :3]
416 factor = numpy.trace(M33) - 2.0
417 try:
418
419 l, V = numpy.linalg.eig(M33)
420 i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0]
421 direction = numpy.real(V[:, i]).squeeze()
422 direction /= vector_norm(direction)
423 except IndexError:
424
425 factor = (factor + 2.0) / 3.0
426 direction = None
427
428 l, V = numpy.linalg.eig(M)
429 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
430 if not len(i):
431 raise ValueError("no eigenvector corresponding to eigenvalue 1")
432 origin = numpy.real(V[:, i[-1]]).squeeze()
433 origin /= origin[3]
434 return factor, origin, direction
435
436
437 -def projection_matrix(point, normal, direction=None,
438 perspective=None, pseudo=False):
439 """Return matrix to project onto plane defined by point and normal.
440
441 Using either perspective point, projection direction, or none of both.
442
443 If pseudo is True, perspective projections will preserve relative depth
444 such that Perspective = dot(Orthogonal, PseudoPerspective).
445
446 >>> P = projection_matrix((0, 0, 0), (1, 0, 0))
447 >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:])
448 True
449 >>> point = numpy.random.random(3) - 0.5
450 >>> normal = numpy.random.random(3) - 0.5
451 >>> direct = numpy.random.random(3) - 0.5
452 >>> persp = numpy.random.random(3) - 0.5
453 >>> P0 = projection_matrix(point, normal)
454 >>> P1 = projection_matrix(point, normal, direction=direct)
455 >>> P2 = projection_matrix(point, normal, perspective=persp)
456 >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
457 >>> is_same_transform(P2, numpy.dot(P0, P3))
458 True
459 >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0))
460 >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0
461 >>> v0[3] = 1.0
462 >>> v1 = numpy.dot(P, v0)
463 >>> numpy.allclose(v1[1], v0[1])
464 True
465 >>> numpy.allclose(v1[0], 3.0-v1[1])
466 True
467
468 """
469 M = numpy.identity(4)
470 point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
471 normal = unit_vector(normal[:3])
472 if perspective is not None:
473
474 perspective = numpy.array(perspective[:3], dtype=numpy.float64,
475 copy=False)
476 M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal)
477 M[:3, :3] -= numpy.outer(perspective, normal)
478 if pseudo:
479
480 M[:3, :3] -= numpy.outer(normal, normal)
481 M[:3, 3] = numpy.dot(point, normal) * (perspective+normal)
482 else:
483 M[:3, 3] = numpy.dot(point, normal) * perspective
484 M[3, :3] = -normal
485 M[3, 3] = numpy.dot(perspective, normal)
486 elif direction is not None:
487
488 direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False)
489 scale = numpy.dot(direction, normal)
490 M[:3, :3] -= numpy.outer(direction, normal) / scale
491 M[:3, 3] = direction * (numpy.dot(point, normal) / scale)
492 else:
493
494 M[:3, :3] -= numpy.outer(normal, normal)
495 M[:3, 3] = numpy.dot(point, normal) * normal
496 return M
497
498
500 """Return projection plane and perspective point from projection matrix.
501
502 Return values are same as arguments for projection_matrix function:
503 point, normal, direction, perspective, and pseudo.
504
505 >>> point = numpy.random.random(3) - 0.5
506 >>> normal = numpy.random.random(3) - 0.5
507 >>> direct = numpy.random.random(3) - 0.5
508 >>> persp = numpy.random.random(3) - 0.5
509 >>> P0 = projection_matrix(point, normal)
510 >>> result = projection_from_matrix(P0)
511 >>> P1 = projection_matrix(*result)
512 >>> is_same_transform(P0, P1)
513 True
514 >>> P0 = projection_matrix(point, normal, direct)
515 >>> result = projection_from_matrix(P0)
516 >>> P1 = projection_matrix(*result)
517 >>> is_same_transform(P0, P1)
518 True
519 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
520 >>> result = projection_from_matrix(P0, pseudo=False)
521 >>> P1 = projection_matrix(*result)
522 >>> is_same_transform(P0, P1)
523 True
524 >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
525 >>> result = projection_from_matrix(P0, pseudo=True)
526 >>> P1 = projection_matrix(*result)
527 >>> is_same_transform(P0, P1)
528 True
529
530 """
531 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
532 M33 = M[:3, :3]
533 l, V = numpy.linalg.eig(M)
534 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
535 if not pseudo and len(i):
536
537 point = numpy.real(V[:, i[-1]]).squeeze()
538 point /= point[3]
539
540 l, V = numpy.linalg.eig(M33)
541 i = numpy.where(abs(numpy.real(l)) < 1e-8)[0]
542 if not len(i):
543 raise ValueError("no eigenvector corresponding to eigenvalue 0")
544 direction = numpy.real(V[:, i[0]]).squeeze()
545 direction /= vector_norm(direction)
546
547 l, V = numpy.linalg.eig(M33.T)
548 i = numpy.where(abs(numpy.real(l)) < 1e-8)[0]
549 if len(i):
550
551 normal = numpy.real(V[:, i[0]]).squeeze()
552 normal /= vector_norm(normal)
553 return point, normal, direction, None, False
554 else:
555
556 return point, direction, None, None, False
557 else:
558
559 i = numpy.where(abs(numpy.real(l)) > 1e-8)[0]
560 if not len(i):
561 raise ValueError(
562 "no eigenvector not corresponding to eigenvalue 0")
563 point = numpy.real(V[:, i[-1]]).squeeze()
564 point /= point[3]
565 normal = - M[3, :3]
566 perspective = M[:3, 3] / numpy.dot(point[:3], normal)
567 if pseudo:
568 perspective -= normal
569 return point, normal, None, perspective, pseudo
570
571
572 -def clip_matrix(left, right, bottom, top, near, far, perspective=False):
573 """Return matrix to obtain normalized device coordinates from frustrum.
574
575 The frustrum bounds are axis-aligned along x (left, right),
576 y (bottom, top) and z (near, far).
577
578 Normalized device coordinates are in range [-1, 1] if coordinates are
579 inside the frustrum.
580
581 If perspective is True the frustrum is a truncated pyramid with the
582 perspective point at origin and direction along z axis, otherwise an
583 orthographic canonical view volume (a box).
584
585 Homogeneous coordinates transformed by the perspective clip matrix
586 need to be dehomogenized (devided by w coordinate).
587
588 >>> frustrum = numpy.random.rand(6)
589 >>> frustrum[1] += frustrum[0]
590 >>> frustrum[3] += frustrum[2]
591 >>> frustrum[5] += frustrum[4]
592 >>> M = clip_matrix(*frustrum, perspective=False)
593 >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
594 array([-1., -1., -1., 1.])
595 >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0])
596 array([ 1., 1., 1., 1.])
597 >>> M = clip_matrix(*frustrum, perspective=True)
598 >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
599 >>> v / v[3]
600 array([-1., -1., -1., 1.])
601 >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0])
602 >>> v / v[3]
603 array([ 1., 1., -1., 1.])
604
605 """
606 if left >= right or bottom >= top or near >= far:
607 raise ValueError("invalid frustrum")
608 if perspective:
609 if near <= _EPS:
610 raise ValueError("invalid frustrum: near <= 0")
611 t = 2.0 * near
612 M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0),
613 (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0),
614 (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)),
615 (0.0, 0.0, -1.0, 0.0))
616 else:
617 M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)),
618 (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)),
619 (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)),
620 (0.0, 0.0, 0.0, 1.0))
621 return numpy.array(M, dtype=numpy.float64)
622
623
625 """Return matrix to shear by angle along direction vector on shear plane.
626
627 The shear plane is defined by a point and normal vector. The direction
628 vector must be orthogonal to the plane's normal vector.
629
630 A point P is transformed by the shear matrix into P" such that
631 the vector P-P" is parallel to the direction vector and its extent is
632 given by the angle of P-P'-P", where P' is the orthogonal projection
633 of P onto the shear plane.
634
635 >>> angle = (random.random() - 0.5) * 4*math.pi
636 >>> direct = numpy.random.random(3) - 0.5
637 >>> point = numpy.random.random(3) - 0.5
638 >>> normal = numpy.cross(direct, numpy.random.random(3))
639 >>> S = shear_matrix(angle, direct, point, normal)
640 >>> numpy.allclose(1.0, numpy.linalg.det(S))
641 True
642
643 """
644 normal = unit_vector(normal[:3])
645 direction = unit_vector(direction[:3])
646 if abs(numpy.dot(normal, direction)) > 1e-6:
647 raise ValueError("direction and normal vectors are not orthogonal")
648 angle = math.tan(angle)
649 M = numpy.identity(4)
650 M[:3, :3] += angle * numpy.outer(direction, normal)
651 M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
652 return M
653
654
656 """Return shear angle, direction and plane from shear matrix.
657
658 >>> angle = (random.random() - 0.5) * 4*math.pi
659 >>> direct = numpy.random.random(3) - 0.5
660 >>> point = numpy.random.random(3) - 0.5
661 >>> normal = numpy.cross(direct, numpy.random.random(3))
662 >>> S0 = shear_matrix(angle, direct, point, normal)
663 >>> angle, direct, point, normal = shear_from_matrix(S0)
664 >>> S1 = shear_matrix(angle, direct, point, normal)
665 >>> is_same_transform(S0, S1)
666 True
667
668 """
669 M = numpy.array(matrix, dtype=numpy.float64, copy=False)
670 M33 = M[:3, :3]
671
672 l, V = numpy.linalg.eig(M33)
673 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0]
674 if len(i) < 2:
675 raise ValueError("No two linear independent eigenvectors found %s" % l)
676 V = numpy.real(V[:, i]).squeeze().T
677 lenorm = -1.0
678 for i0, i1 in ((0, 1), (0, 2), (1, 2)):
679 n = numpy.cross(V[i0], V[i1])
680 l = vector_norm(n)
681 if l > lenorm:
682 lenorm = l
683 normal = n
684 normal /= lenorm
685
686 direction = numpy.dot(M33 - numpy.identity(3), normal)
687 angle = vector_norm(direction)
688 direction /= angle
689 angle = math.atan(angle)
690
691 l, V = numpy.linalg.eig(M)
692 i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
693 if not len(i):
694 raise ValueError("no eigenvector corresponding to eigenvalue 1")
695 point = numpy.real(V[:, i[-1]]).squeeze()
696 point /= point[3]
697 return angle, direction, point, normal
698
699
701 """Return sequence of transformations from transformation matrix.
702
703 matrix : array_like
704 Non-degenerative homogeneous transformation matrix
705
706 Return tuple of:
707 scale : vector of 3 scaling factors
708 shear : list of shear factors for x-y, x-z, y-z axes
709 angles : list of Euler angles about static x, y, z axes
710 translate : translation vector along x, y, z axes
711 perspective : perspective partition of matrix
712
713 Raise ValueError if matrix is of wrong type or degenerative.
714
715 >>> T0 = translation_matrix((1, 2, 3))
716 >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
717 >>> T1 = translation_matrix(trans)
718 >>> numpy.allclose(T0, T1)
719 True
720 >>> S = scale_matrix(0.123)
721 >>> scale, shear, angles, trans, persp = decompose_matrix(S)
722 >>> scale[0]
723 0.123
724 >>> R0 = euler_matrix(1, 2, 3)
725 >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
726 >>> R1 = euler_matrix(*angles)
727 >>> numpy.allclose(R0, R1)
728 True
729
730 """
731 M = numpy.array(matrix, dtype=numpy.float64, copy=True).T
732 if abs(M[3, 3]) < _EPS:
733 raise ValueError("M[3, 3] is zero")
734 M /= M[3, 3]
735 P = M.copy()
736 P[:, 3] = 0, 0, 0, 1
737 if not numpy.linalg.det(P):
738 raise ValueError("Matrix is singular")
739
740 scale = numpy.zeros((3, ), dtype=numpy.float64)
741 shear = [0, 0, 0]
742 angles = [0, 0, 0]
743
744 if any(abs(M[:3, 3]) > _EPS):
745 perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T))
746 M[:, 3] = 0, 0, 0, 1
747 else:
748 perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64)
749
750 translate = M[3, :3].copy()
751 M[3, :3] = 0
752
753 row = M[:3, :3].copy()
754 scale[0] = vector_norm(row[0])
755 row[0] /= scale[0]
756 shear[0] = numpy.dot(row[0], row[1])
757 row[1] -= row[0] * shear[0]
758 scale[1] = vector_norm(row[1])
759 row[1] /= scale[1]
760 shear[0] /= scale[1]
761 shear[1] = numpy.dot(row[0], row[2])
762 row[2] -= row[0] * shear[1]
763 shear[2] = numpy.dot(row[1], row[2])
764 row[2] -= row[1] * shear[2]
765 scale[2] = vector_norm(row[2])
766 row[2] /= scale[2]
767 shear[1:] /= scale[2]
768
769 if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0:
770 scale *= -1
771 row *= -1
772
773 angles[1] = math.asin(-row[0, 2])
774 if math.cos(angles[1]):
775 angles[0] = math.atan2(row[1, 2], row[2, 2])
776 angles[2] = math.atan2(row[0, 1], row[0, 0])
777 else:
778
779 angles[0] = math.atan2(-row[2, 1], row[1, 1])
780 angles[2] = 0.0
781
782 return scale, shear, angles, translate, perspective
783
784
785 -def compose_matrix(scale=None, shear=None, angles=None, translate=None,
786 perspective=None):
787 """Return transformation matrix from sequence of transformations.
788
789 This is the inverse of the decompose_matrix function.
790
791 Sequence of transformations:
792 scale : vector of 3 scaling factors
793 shear : list of shear factors for x-y, x-z, y-z axes
794 angles : list of Euler angles about static x, y, z axes
795 translate : translation vector along x, y, z axes
796 perspective : perspective partition of matrix
797
798 >>> scale = numpy.random.random(3) - 0.5
799 >>> shear = numpy.random.random(3) - 0.5
800 >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi)
801 >>> trans = numpy.random.random(3) - 0.5
802 >>> persp = numpy.random.random(4) - 0.5
803 >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
804 >>> result = decompose_matrix(M0)
805 >>> M1 = compose_matrix(*result)
806 >>> is_same_transform(M0, M1)
807 True
808
809 """
810 M = numpy.identity(4)
811 if perspective is not None:
812 P = numpy.identity(4)
813 P[3, :] = perspective[:4]
814 M = numpy.dot(M, P)
815 if translate is not None:
816 T = numpy.identity(4)
817 T[:3, 3] = translate[:3]
818 M = numpy.dot(M, T)
819 if angles is not None:
820 R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz')
821 M = numpy.dot(M, R)
822 if shear is not None:
823 Z = numpy.identity(4)
824 Z[1, 2] = shear[2]
825 Z[0, 2] = shear[1]
826 Z[0, 1] = shear[0]
827 M = numpy.dot(M, Z)
828 if scale is not None:
829 S = numpy.identity(4)
830 S[0, 0] = scale[0]
831 S[1, 1] = scale[1]
832 S[2, 2] = scale[2]
833 M = numpy.dot(M, S)
834 M /= M[3, 3]
835 return M
836
837
839 """Return orthogonalization matrix for crystallographic cell coordinates.
840
841 Angles are expected in degrees.
842
843 The de-orthogonalization matrix is the inverse.
844
845 >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.))
846 >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
847 True
848 >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
849 >>> numpy.allclose(numpy.sum(O), 43.063229)
850 True
851
852 """
853 a, b, c = lengths
854 angles = numpy.radians(angles)
855 sina, sinb, _ = numpy.sin(angles)
856 cosa, cosb, cosg = numpy.cos(angles)
857 co = (cosa * cosb - cosg) / (sina * sinb)
858 return numpy.array((
859 ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0),
860 (-a*sinb*co, b*sina, 0.0, 0.0),
861 ( a*cosb, b*cosa, c, 0.0),
862 ( 0.0, 0.0, 0.0, 1.0)),
863 dtype=numpy.float64)
864
865
867 """Return matrix to transform given vector set into second vector set.
868
869 v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors.
870
871 If usesvd is True, the weighted sum of squared deviations (RMSD) is
872 minimized according to the algorithm by W. Kabsch [8]. Otherwise the
873 quaternion based algorithm by B. Horn [9] is used (slower when using
874 this Python implementation).
875
876 The returned matrix performs rotation, translation and uniform scaling
877 (if specified).
878
879 >>> v0 = numpy.random.rand(3, 10)
880 >>> M = superimposition_matrix(v0, v0)
881 >>> numpy.allclose(M, numpy.identity(4))
882 True
883 >>> R = random_rotation_matrix(numpy.random.random(3))
884 >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1))
885 >>> v1 = numpy.dot(R, v0)
886 >>> M = superimposition_matrix(v0, v1)
887 >>> numpy.allclose(v1, numpy.dot(M, v0))
888 True
889 >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0
890 >>> v0[3] = 1.0
891 >>> v1 = numpy.dot(R, v0)
892 >>> M = superimposition_matrix(v0, v1)
893 >>> numpy.allclose(v1, numpy.dot(M, v0))
894 True
895 >>> S = scale_matrix(random.random())
896 >>> T = translation_matrix(numpy.random.random(3)-0.5)
897 >>> M = concatenate_matrices(T, R, S)
898 >>> v1 = numpy.dot(M, v0)
899 >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1)
900 >>> M = superimposition_matrix(v0, v1, scaling=True)
901 >>> numpy.allclose(v1, numpy.dot(M, v0))
902 True
903 >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
904 >>> numpy.allclose(v1, numpy.dot(M, v0))
905 True
906 >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64)
907 >>> v[:, :, 0] = v0
908 >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
909 >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0]))
910 True
911
912 """
913 v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3]
914 v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3]
915
916 if v0.shape != v1.shape or v0.shape[1] < 3:
917 raise ValueError("Vector sets are of wrong shape or type.")
918
919
920 t0 = numpy.mean(v0, axis=1)
921 t1 = numpy.mean(v1, axis=1)
922 v0 = v0 - t0.reshape(3, 1)
923 v1 = v1 - t1.reshape(3, 1)
924
925 if usesvd:
926
927 u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T))
928
929 R = numpy.dot(u, vh)
930 if numpy.linalg.det(R) < 0.0:
931
932 R -= numpy.outer(u[:, 2], vh[2, :]*2.0)
933 s[-1] *= -1.0
934
935 M = numpy.identity(4)
936 M[:3, :3] = R
937 else:
938
939 xx, yy, zz = numpy.sum(v0 * v1, axis=1)
940 xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1)
941 xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1)
942 N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx),
943 (yz-zy, xx-yy-zz, xy+yx, zx+xz),
944 (zx-xz, xy+yx, -xx+yy-zz, yz+zy),
945 (xy-yx, zx+xz, yz+zy, -xx-yy+zz))
946
947 l, V = numpy.linalg.eig(N)
948 q = V[:, numpy.argmax(l)]
949 q /= vector_norm(q)
950 q = numpy.roll(q, -1)
951
952 M = quaternion_matrix(q)
953
954
955 if scaling:
956 v0 *= v0
957 v1 *= v1
958 M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0))
959
960
961 M[:3, 3] = t1
962 T = numpy.identity(4)
963 T[:3, 3] = -t0
964 M = numpy.dot(M, T)
965 return M
966
967
969 """Return homogeneous rotation matrix from Euler angles and axis sequence.
970
971 ai, aj, ak : Euler's roll, pitch and yaw angles
972 axes : One of 24 axis sequences as string or encoded tuple
973
974 >>> R = euler_matrix(1, 2, 3, 'syxz')
975 >>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
976 True
977 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
978 >>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
979 True
980 >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
981 >>> for axes in _AXES2TUPLE.keys():
982 ... R = euler_matrix(ai, aj, ak, axes)
983 >>> for axes in _TUPLE2AXES.keys():
984 ... R = euler_matrix(ai, aj, ak, axes)
985
986 """
987 try:
988 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
989 except (AttributeError, KeyError):
990 _ = _TUPLE2AXES[axes]
991 firstaxis, parity, repetition, frame = axes
992
993 i = firstaxis
994 j = _NEXT_AXIS[i+parity]
995 k = _NEXT_AXIS[i-parity+1]
996
997 if frame:
998 ai, ak = ak, ai
999 if parity:
1000 ai, aj, ak = -ai, -aj, -ak
1001
1002 si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
1003 ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
1004 cc, cs = ci*ck, ci*sk
1005 sc, ss = si*ck, si*sk
1006
1007 M = numpy.identity(4)
1008 if repetition:
1009 M[i, i] = cj
1010 M[i, j] = sj*si
1011 M[i, k] = sj*ci
1012 M[j, i] = sj*sk
1013 M[j, j] = -cj*ss+cc
1014 M[j, k] = -cj*cs-sc
1015 M[k, i] = -sj*ck
1016 M[k, j] = cj*sc+cs
1017 M[k, k] = cj*cc-ss
1018 else:
1019 M[i, i] = cj*ck
1020 M[i, j] = sj*sc-cs
1021 M[i, k] = sj*cc+ss
1022 M[j, i] = cj*sk
1023 M[j, j] = sj*ss+cc
1024 M[j, k] = sj*cs-sc
1025 M[k, i] = -sj
1026 M[k, j] = cj*si
1027 M[k, k] = cj*ci
1028 return M
1029
1030
1032 """Return Euler angles from rotation matrix for specified axis sequence.
1033
1034 axes : One of 24 axis sequences as string or encoded tuple
1035
1036 Note that many Euler angle triplets can describe one matrix.
1037
1038 >>> R0 = euler_matrix(1, 2, 3, 'syxz')
1039 >>> al, be, ga = euler_from_matrix(R0, 'syxz')
1040 >>> R1 = euler_matrix(al, be, ga, 'syxz')
1041 >>> numpy.allclose(R0, R1)
1042 True
1043 >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
1044 >>> for axes in _AXES2TUPLE.keys():
1045 ... R0 = euler_matrix(axes=axes, *angles)
1046 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
1047 ... if not numpy.allclose(R0, R1): print axes, "failed"
1048
1049 """
1050 try:
1051 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1052 except (AttributeError, KeyError):
1053 _ = _TUPLE2AXES[axes]
1054 firstaxis, parity, repetition, frame = axes
1055
1056 i = firstaxis
1057 j = _NEXT_AXIS[i+parity]
1058 k = _NEXT_AXIS[i-parity+1]
1059
1060 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]
1061 if repetition:
1062 sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
1063 if sy > _EPS:
1064 ax = math.atan2( M[i, j], M[i, k])
1065 ay = math.atan2( sy, M[i, i])
1066 az = math.atan2( M[j, i], -M[k, i])
1067 else:
1068 ax = math.atan2(-M[j, k], M[j, j])
1069 ay = math.atan2( sy, M[i, i])
1070 az = 0.0
1071 else:
1072 cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
1073 if cy > _EPS:
1074 ax = math.atan2( M[k, j], M[k, k])
1075 ay = math.atan2(-M[k, i], cy)
1076 az = math.atan2( M[j, i], M[i, i])
1077 else:
1078 ax = math.atan2(-M[j, k], M[j, j])
1079 ay = math.atan2(-M[k, i], cy)
1080 az = 0.0
1081
1082 if parity:
1083 ax, ay, az = -ax, -ay, -az
1084 if frame:
1085 ax, az = az, ax
1086 return ax, ay, az
1087
1088
1090 """Return Euler angles from quaternion for specified axis sequence.
1091
1092 >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
1093 >>> numpy.allclose(angles, [0.123, 0, 0])
1094 True
1095
1096 """
1097 return euler_from_matrix(quaternion_matrix(quaternion), axes)
1098
1099
1101 """Return quaternion from Euler angles and axis sequence.
1102
1103 ai, aj, ak : Euler's roll, pitch and yaw angles
1104 axes : One of 24 axis sequences as string or encoded tuple
1105
1106 >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
1107 >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953])
1108 True
1109
1110 """
1111 try:
1112 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
1113 except (AttributeError, KeyError):
1114 _ = _TUPLE2AXES[axes]
1115 firstaxis, parity, repetition, frame = axes
1116
1117 i = firstaxis
1118 j = _NEXT_AXIS[i+parity]
1119 k = _NEXT_AXIS[i-parity+1]
1120
1121 if frame:
1122 ai, ak = ak, ai
1123 if parity:
1124 aj = -aj
1125
1126 ai /= 2.0
1127 aj /= 2.0
1128 ak /= 2.0
1129 ci = math.cos(ai)
1130 si = math.sin(ai)
1131 cj = math.cos(aj)
1132 sj = math.sin(aj)
1133 ck = math.cos(ak)
1134 sk = math.sin(ak)
1135 cc = ci*ck
1136 cs = ci*sk
1137 sc = si*ck
1138 ss = si*sk
1139
1140 quaternion = numpy.empty((4, ), dtype=numpy.float64)
1141 if repetition:
1142 quaternion[i] = cj*(cs + sc)
1143 quaternion[j] = sj*(cc + ss)
1144 quaternion[k] = sj*(cs - sc)
1145 quaternion[3] = cj*(cc - ss)
1146 else:
1147 quaternion[i] = cj*sc - sj*cs
1148 quaternion[j] = cj*ss + sj*cc
1149 quaternion[k] = cj*cs - sj*sc
1150 quaternion[3] = cj*cc + sj*ss
1151 if parity:
1152 quaternion[j] *= -1
1153
1154 return quaternion
1155
1156
1158 """Return quaternion for rotation about axis.
1159
1160 >>> q = quaternion_about_axis(0.123, (1, 0, 0))
1161 >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947])
1162 True
1163
1164 """
1165 quaternion = numpy.zeros((4, ), dtype=numpy.float64)
1166 quaternion[:3] = axis[:3]
1167 qlen = vector_norm(quaternion)
1168 if qlen > _EPS:
1169 quaternion *= math.sin(angle/2.0) / qlen
1170 quaternion[3] = math.cos(angle/2.0)
1171 return quaternion
1172
1173
1175 """Return homogeneous rotation matrix from quaternion.
1176
1177 >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
1178 >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
1179 True
1180
1181 """
1182 q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True)
1183 nq = numpy.dot(q, q)
1184 if nq < _EPS:
1185 return numpy.identity(4)
1186 q *= math.sqrt(2.0 / nq)
1187 q = numpy.outer(q, q)
1188 return numpy.array((
1189 (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0),
1190 ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0),
1191 ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
1192 ( 0.0, 0.0, 0.0, 1.0)
1193 ), dtype=numpy.float64)
1194
1195
1197 """Return quaternion from rotation matrix.
1198
1199 >>> R = rotation_matrix(0.123, (1, 2, 3))
1200 >>> q = quaternion_from_matrix(R)
1201 >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095])
1202 True
1203
1204 """
1205 q = numpy.empty((4, ), dtype=numpy.float64)
1206 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
1207 t = numpy.trace(M)
1208 if t > M[3, 3]:
1209 q[3] = t
1210 q[2] = M[1, 0] - M[0, 1]
1211 q[1] = M[0, 2] - M[2, 0]
1212 q[0] = M[2, 1] - M[1, 2]
1213 else:
1214 i, j, k = 0, 1, 2
1215 if M[1, 1] > M[0, 0]:
1216 i, j, k = 1, 2, 0
1217 if M[2, 2] > M[i, i]:
1218 i, j, k = 2, 0, 1
1219 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
1220 q[i] = t
1221 q[j] = M[i, j] + M[j, i]
1222 q[k] = M[k, i] + M[i, k]
1223 q[3] = M[k, j] - M[j, k]
1224 q *= 0.5 / math.sqrt(t * M[3, 3])
1225 return q
1226
1227
1229 """Return multiplication of two quaternions.
1230
1231 >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8])
1232 >>> numpy.allclose(q, [-44, -14, 48, 28])
1233 True
1234
1235 """
1236 x0, y0, z0, w0 = quaternion0
1237 x1, y1, z1, w1 = quaternion1
1238 return numpy.array((
1239 x1*w0 + y1*z0 - z1*y0 + w1*x0,
1240 -x1*z0 + y1*w0 + z1*x0 + w1*y0,
1241 x1*y0 - y1*x0 + z1*w0 + w1*z0,
1242 -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64)
1243
1244
1246 """Return conjugate of quaternion.
1247
1248 >>> q0 = random_quaternion()
1249 >>> q1 = quaternion_conjugate(q0)
1250 >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3])
1251 True
1252
1253 """
1254 return numpy.array((-quaternion[0], -quaternion[1],
1255 -quaternion[2], quaternion[3]), dtype=numpy.float64)
1256
1257
1259 """Return inverse of quaternion.
1260
1261 >>> q0 = random_quaternion()
1262 >>> q1 = quaternion_inverse(q0)
1263 >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1])
1264 True
1265
1266 """
1267 return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion)
1268
1269
1271 """Return spherical linear interpolation between two quaternions.
1272
1273 >>> q0 = random_quaternion()
1274 >>> q1 = random_quaternion()
1275 >>> q = quaternion_slerp(q0, q1, 0.0)
1276 >>> numpy.allclose(q, q0)
1277 True
1278 >>> q = quaternion_slerp(q0, q1, 1.0, 1)
1279 >>> numpy.allclose(q, q1)
1280 True
1281 >>> q = quaternion_slerp(q0, q1, 0.5)
1282 >>> angle = math.acos(numpy.dot(q0, q))
1283 >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \
1284 numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle)
1285 True
1286
1287 """
1288 q0 = unit_vector(quat0[:4])
1289 q1 = unit_vector(quat1[:4])
1290 if fraction == 0.0:
1291 return q0
1292 elif fraction == 1.0:
1293 return q1
1294 d = numpy.dot(q0, q1)
1295 if abs(abs(d) - 1.0) < _EPS:
1296 return q0
1297 if shortestpath and d < 0.0:
1298
1299 d = -d
1300 q1 *= -1.0
1301 angle = math.acos(d) + spin * math.pi
1302 if abs(angle) < _EPS:
1303 return q0
1304 isin = 1.0 / math.sin(angle)
1305 q0 *= math.sin((1.0 - fraction) * angle) * isin
1306 q1 *= math.sin(fraction * angle) * isin
1307 q0 += q1
1308 return q0
1309
1310
1312 """Return uniform random unit quaternion.
1313
1314 rand: array like or None
1315 Three independent random variables that are uniformly distributed
1316 between 0 and 1.
1317
1318 >>> q = random_quaternion()
1319 >>> numpy.allclose(1.0, vector_norm(q))
1320 True
1321 >>> q = random_quaternion(numpy.random.random(3))
1322 >>> q.shape
1323 (4,)
1324
1325 """
1326 if rand is None:
1327 rand = numpy.random.rand(3)
1328 else:
1329 assert len(rand) == 3
1330 r1 = numpy.sqrt(1.0 - rand[0])
1331 r2 = numpy.sqrt(rand[0])
1332 pi2 = math.pi * 2.0
1333 t1 = pi2 * rand[1]
1334 t2 = pi2 * rand[2]
1335 return numpy.array((numpy.sin(t1)*r1,
1336 numpy.cos(t1)*r1,
1337 numpy.sin(t2)*r2,
1338 numpy.cos(t2)*r2), dtype=numpy.float64)
1339
1340
1342 """Return uniform random rotation matrix.
1343
1344 rnd: array like
1345 Three independent random variables that are uniformly distributed
1346 between 0 and 1 for each returned quaternion.
1347
1348 >>> R = random_rotation_matrix()
1349 >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
1350 True
1351
1352 """
1353 return quaternion_matrix(random_quaternion(rand))
1354
1355
1357 """Virtual Trackball Control.
1358
1359 >>> ball = Arcball()
1360 >>> ball = Arcball(initial=numpy.identity(4))
1361 >>> ball.place([320, 320], 320)
1362 >>> ball.down([500, 250])
1363 >>> ball.drag([475, 275])
1364 >>> R = ball.matrix()
1365 >>> numpy.allclose(numpy.sum(R), 3.90583455)
1366 True
1367 >>> ball = Arcball(initial=[0, 0, 0, 1])
1368 >>> ball.place([320, 320], 320)
1369 >>> ball.setaxes([1,1,0], [-1, 1, 0])
1370 >>> ball.setconstrain(True)
1371 >>> ball.down([400, 200])
1372 >>> ball.drag([200, 400])
1373 >>> R = ball.matrix()
1374 >>> numpy.allclose(numpy.sum(R), 0.2055924)
1375 True
1376 >>> ball.next()
1377
1378 """
1379
1381 """Initialize virtual trackball control.
1382
1383 initial : quaternion or rotation matrix
1384
1385 """
1386 self._axis = None
1387 self._axes = None
1388 self._radius = 1.0
1389 self._center = [0.0, 0.0]
1390 self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64)
1391 self._constrain = False
1392
1393 if initial is None:
1394 self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64)
1395 else:
1396 initial = numpy.array(initial, dtype=numpy.float64)
1397 if initial.shape == (4, 4):
1398 self._qdown = quaternion_from_matrix(initial)
1399 elif initial.shape == (4, ):
1400 initial /= vector_norm(initial)
1401 self._qdown = initial
1402 else:
1403 raise ValueError("initial not a quaternion or matrix.")
1404
1405 self._qnow = self._qpre = self._qdown
1406
1407 - def place(self, center, radius):
1408 """Place Arcball, e.g. when window size changes.
1409
1410 center : sequence[2]
1411 Window coordinates of trackball center.
1412 radius : float
1413 Radius of trackball in window coordinates.
1414
1415 """
1416 self._radius = float(radius)
1417 self._center[0] = center[0]
1418 self._center[1] = center[1]
1419
1421 """Set axes to constrain rotations."""
1422 if axes is None:
1423 self._axes = None
1424 else:
1425 self._axes = [unit_vector(axis) for axis in axes]
1426
1428 """Set state of constrain to axis mode."""
1429 self._constrain = constrain == True
1430
1432 """Return state of constrain to axis mode."""
1433 return self._constrain
1434
1435 - def down(self, point):
1436 """Set initial cursor window coordinates and pick constrain-axis."""
1437 self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
1438 self._qdown = self._qpre = self._qnow
1439
1440 if self._constrain and self._axes is not None:
1441 self._axis = arcball_nearest_axis(self._vdown, self._axes)
1442 self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
1443 else:
1444 self._axis = None
1445
1446 - def drag(self, point):
1447 """Update current cursor window coordinates."""
1448 vnow = arcball_map_to_sphere(point, self._center, self._radius)
1449
1450 if self._axis is not None:
1451 vnow = arcball_constrain_to_axis(vnow, self._axis)
1452
1453 self._qpre = self._qnow
1454
1455 t = numpy.cross(self._vdown, vnow)
1456 if numpy.dot(t, t) < _EPS:
1457 self._qnow = self._qdown
1458 else:
1459 q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)]
1460 self._qnow = quaternion_multiply(q, self._qdown)
1461
1462 - def next(self, acceleration=0.0):
1463 """Continue rotation in direction of last drag."""
1464 q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False)
1465 self._qpre, self._qnow = self._qnow, q
1466
1468 """Return homogeneous rotation matrix."""
1469 return quaternion_matrix(self._qnow)
1470
1471
1473 """Return unit sphere coordinates from window coordinates."""
1474 v = numpy.array(((point[0] - center[0]) / radius,
1475 (center[1] - point[1]) / radius,
1476 0.0), dtype=numpy.float64)
1477 n = v[0]*v[0] + v[1]*v[1]
1478 if n > 1.0:
1479 v /= math.sqrt(n)
1480 else:
1481 v[2] = math.sqrt(1.0 - n)
1482 return v
1483
1484
1486 """Return sphere point perpendicular to axis."""
1487 v = numpy.array(point, dtype=numpy.float64, copy=True)
1488 a = numpy.array(axis, dtype=numpy.float64, copy=True)
1489 v -= a * numpy.dot(a, v)
1490 n = vector_norm(v)
1491 if n > _EPS:
1492 if v[2] < 0.0:
1493 v *= -1.0
1494 v /= n
1495 return v
1496 if a[2] == 1.0:
1497 return numpy.array([1, 0, 0], dtype=numpy.float64)
1498 return unit_vector([-a[1], a[0], 0])
1499
1500
1502 """Return axis, which arc is nearest to point."""
1503 point = numpy.array(point, dtype=numpy.float64, copy=False)
1504 nearest = None
1505 mx = -1.0
1506 for axis in axes:
1507 t = numpy.dot(arcball_constrain_to_axis(point, axis), point)
1508 if t > mx:
1509 nearest = axis
1510 mx = t
1511 return nearest
1512
1513
1514
1515 _EPS = numpy.finfo(float).eps * 4.0
1516
1517
1518 _NEXT_AXIS = [1, 2, 0, 1]
1519
1520
1521 _AXES2TUPLE = {
1522 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
1523 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
1524 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
1525 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
1526 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
1527 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
1528 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
1529 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
1530
1531 _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
1532
1533
1534
1536 """Return length, i.e. eucledian norm, of ndarray along axis.
1537
1538 >>> v = numpy.random.random(3)
1539 >>> n = vector_norm(v)
1540 >>> numpy.allclose(n, numpy.linalg.norm(v))
1541 True
1542 >>> v = numpy.random.rand(6, 5, 3)
1543 >>> n = vector_norm(v, axis=-1)
1544 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
1545 True
1546 >>> n = vector_norm(v, axis=1)
1547 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1548 True
1549 >>> v = numpy.random.rand(5, 4, 3)
1550 >>> n = numpy.empty((5, 3), dtype=numpy.float64)
1551 >>> vector_norm(v, axis=1, out=n)
1552 >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
1553 True
1554 >>> vector_norm([])
1555 0.0
1556 >>> vector_norm([1.0])
1557 1.0
1558
1559 """
1560 data = numpy.array(data, dtype=numpy.float64, copy=True)
1561 if out is None:
1562 if data.ndim == 1:
1563 return math.sqrt(numpy.dot(data, data))
1564 data *= data
1565 out = numpy.atleast_1d(numpy.sum(data, axis=axis))
1566 numpy.sqrt(out, out)
1567 return out
1568 else:
1569 data *= data
1570 numpy.sum(data, axis=axis, out=out)
1571 numpy.sqrt(out, out)
1572
1573
1575 """Return ndarray normalized by length, i.e. eucledian norm, along axis.
1576
1577 >>> v0 = numpy.random.random(3)
1578 >>> v1 = unit_vector(v0)
1579 >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
1580 True
1581 >>> v0 = numpy.random.rand(5, 4, 3)
1582 >>> v1 = unit_vector(v0, axis=-1)
1583 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
1584 >>> numpy.allclose(v1, v2)
1585 True
1586 >>> v1 = unit_vector(v0, axis=1)
1587 >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
1588 >>> numpy.allclose(v1, v2)
1589 True
1590 >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64)
1591 >>> unit_vector(v0, axis=1, out=v1)
1592 >>> numpy.allclose(v1, v2)
1593 True
1594 >>> list(unit_vector([]))
1595 []
1596 >>> list(unit_vector([1.0]))
1597 [1.0]
1598
1599 """
1600 if out is None:
1601 data = numpy.array(data, dtype=numpy.float64, copy=True)
1602 if data.ndim == 1:
1603 data /= math.sqrt(numpy.dot(data, data))
1604 return data
1605 else:
1606 if out is not data:
1607 out[:] = numpy.array(data, copy=False)
1608 data = out
1609 length = numpy.atleast_1d(numpy.sum(data*data, axis))
1610 numpy.sqrt(length, length)
1611 if axis is not None:
1612 length = numpy.expand_dims(length, axis)
1613 data /= length
1614 if out is None:
1615 return data
1616
1617
1619 """Return array of random doubles in the half-open interval [0.0, 1.0).
1620
1621 >>> v = random_vector(10000)
1622 >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0)
1623 True
1624 >>> v0 = random_vector(10)
1625 >>> v1 = random_vector(10)
1626 >>> numpy.any(v0 == v1)
1627 False
1628
1629 """
1630 return numpy.random.random(size)
1631
1632
1634 """Return inverse of square transformation matrix.
1635
1636 >>> M0 = random_rotation_matrix()
1637 >>> M1 = inverse_matrix(M0.T)
1638 >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))
1639 True
1640 >>> for size in range(1, 7):
1641 ... M0 = numpy.random.rand(size, size)
1642 ... M1 = inverse_matrix(M0)
1643 ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size
1644
1645 """
1646 return numpy.linalg.inv(matrix)
1647
1648
1650 """Return concatenation of series of transformation matrices.
1651
1652 >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5
1653 >>> numpy.allclose(M, concatenate_matrices(M))
1654 True
1655 >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))
1656 True
1657
1658 """
1659 M = numpy.identity(4)
1660 for i in matrices:
1661 M = numpy.dot(M, i)
1662 return M
1663
1664
1679
1680
1681 -def _import_module(module_name, warn=True, prefix='_py_', ignore='_'):
1682 """Try import all public attributes from module into global namespace.
1683
1684 Existing attributes with name clashes are renamed with prefix.
1685 Attributes starting with underscore are ignored by default.
1686
1687 Return True on successful import.
1688
1689 """
1690 try:
1691 module = __import__(module_name)
1692 except ImportError:
1693 if warn:
1694 warnings.warn("Failed to import module " + module_name)
1695 else:
1696 for attr in dir(module):
1697 if ignore and attr.startswith(ignore):
1698 continue
1699 if prefix:
1700 if attr in globals():
1701 globals()[prefix + attr] = globals()[attr]
1702 elif warn:
1703 warnings.warn("No Python implementation of " + attr)
1704 globals()[attr] = getattr(module, attr)
1705 return True
1706