← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/python/test/transformations.py

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

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