transformations.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 # transformations.py
00003 
00004 # Copyright (c) 2006, Christoph Gohlke
00005 # Copyright (c) 2006-2009, The Regents of the University of California
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions are met:
00010 #
00011 # * Redistributions of source code must retain the above copyright
00012 #   notice, this list of conditions and the following disclaimer.
00013 # * Redistributions in binary form must reproduce the above copyright
00014 #   notice, this list of conditions and the following disclaimer in the
00015 #   documentation and/or other materials provided with the distribution.
00016 # * Neither the name of the copyright holders nor the names of any
00017 #   contributors may be used to endorse or promote products derived
00018 #   from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00021 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00022 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00023 # ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00024 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00025 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00026 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00027 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00028 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00029 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030 # POSSIBILITY OF SUCH DAMAGE.
00031 
00032 """Homogeneous Transformation Matrices and Quaternions.
00033 
00034 A library for calculating 4x4 matrices for translating, rotating, reflecting,
00035 scaling, shearing, projecting, orthogonalizing, and superimposing arrays of
00036 3D homogeneous coordinates as well as for converting between rotation matrices,
00037 Euler angles, and quaternions. Also includes an Arcball control object and
00038 functions to decompose transformation matrices.
00039 
00040 :Authors:
00041   `Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`__,
00042   Laboratory for Fluorescence Dynamics, University of California, Irvine
00043 
00044 :Version: 20090418
00045 
00046 Requirements
00047 ------------
00048 
00049 * `Python 2.6 <http://www.python.org>`__
00050 * `Numpy 1.3 <http://numpy.scipy.org>`__
00051 * `transformations.c 20090418 <http://www.lfd.uci.edu/~gohlke/>`__
00052   (optional implementation of some functions in C)
00053 
00054 Notes
00055 -----
00056 
00057 Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using
00058 numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using
00059 numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively
00060 numpy.dot(v, M.T) for shape (\*, 4) "array of points".
00061 
00062 Calculations are carried out with numpy.float64 precision.
00063 
00064 This Python implementation is not optimized for speed.
00065 
00066 Vector, point, quaternion, and matrix function arguments are expected to be
00067 "array like", i.e. tuple, list, or numpy arrays.
00068 
00069 Return types are numpy arrays unless specified otherwise.
00070 
00071 Angles are in radians unless specified otherwise.
00072 
00073 Quaternions ix+jy+kz+w are represented as [x, y, z, w].
00074 
00075 Use the transpose of transformation matrices for OpenGL glMultMatrixd().
00076 
00077 A triple of Euler angles can be applied/interpreted in 24 ways, which can
00078 be specified using a 4 character string or encoded 4-tuple:
00079 
00080   *Axes 4-string*: e.g. 'sxyz' or 'ryxy'
00081 
00082   - first character : rotations are applied to 's'tatic or 'r'otating frame
00083   - remaining characters : successive rotation axis 'x', 'y', or 'z'
00084 
00085   *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)
00086 
00087   - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
00088   - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed
00089     by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
00090   - repetition : first and last axis are same (1) or different (0).
00091   - frame : rotations are applied to static (0) or rotating (1) frame.
00092 
00093 References
00094 ----------
00095 
00096 (1)  Matrices and transformations. Ronald Goldman.
00097      In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
00098 (2)  More matrices and transformations: shear and pseudo-perspective.
00099      Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
00100 (3)  Decomposing a matrix into simple transformations. Spencer Thomas.
00101      In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
00102 (4)  Recovering the data from the transformation matrix. Ronald Goldman.
00103      In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991.
00104 (5)  Euler angle conversion. Ken Shoemake.
00105      In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
00106 (6)  Arcball rotation control. Ken Shoemake.
00107      In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
00108 (7)  Representing attitude: Euler angles, unit quaternions, and rotation
00109      vectors. James Diebel. 2006.
00110 (8)  A discussion of the solution for the best rotation to relate two sets
00111      of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.
00112 (9)  Closed-form solution of absolute orientation using unit quaternions.
00113      BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642.
00114 (10) Quaternions. Ken Shoemake.
00115      http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf
00116 (11) From quaternion to matrix and back. JMP van Waveren. 2005.
00117      http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
00118 (12) Uniform random rotations. Ken Shoemake.
00119      In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992.
00120 
00121 
00122 Examples
00123 --------
00124 
00125 >>> alpha, beta, gamma = 0.123, -1.234, 2.345
00126 >>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
00127 >>> I = identity_matrix()
00128 >>> Rx = rotation_matrix(alpha, xaxis)
00129 >>> Ry = rotation_matrix(beta, yaxis)
00130 >>> Rz = rotation_matrix(gamma, zaxis)
00131 >>> R = concatenate_matrices(Rx, Ry, Rz)
00132 >>> euler = euler_from_matrix(R, 'rxyz')
00133 >>> numpy.allclose([alpha, beta, gamma], euler)
00134 True
00135 >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
00136 >>> is_same_transform(R, Re)
00137 True
00138 >>> al, be, ga = euler_from_matrix(Re, 'rxyz')
00139 >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
00140 True
00141 >>> qx = quaternion_about_axis(alpha, xaxis)
00142 >>> qy = quaternion_about_axis(beta, yaxis)
00143 >>> qz = quaternion_about_axis(gamma, zaxis)
00144 >>> q = quaternion_multiply(qx, qy)
00145 >>> q = quaternion_multiply(q, qz)
00146 >>> Rq = quaternion_matrix(q)
00147 >>> is_same_transform(R, Rq)
00148 True
00149 >>> S = scale_matrix(1.23, origin)
00150 >>> T = translation_matrix((1, 2, 3))
00151 >>> Z = shear_matrix(beta, xaxis, origin, zaxis)
00152 >>> R = random_rotation_matrix(numpy.random.rand(3))
00153 >>> M = concatenate_matrices(T, R, Z, S)
00154 >>> scale, shear, angles, trans, persp = decompose_matrix(M)
00155 >>> numpy.allclose(scale, 1.23)
00156 True
00157 >>> numpy.allclose(trans, (1, 2, 3))
00158 True
00159 >>> numpy.allclose(shear, (0, math.tan(beta), 0))
00160 True
00161 >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
00162 True
00163 >>> M1 = compose_matrix(scale, shear, angles, trans, persp)
00164 >>> is_same_transform(M, M1)
00165 True
00166 
00167 """
00168 
00169 from __future__ import division
00170 
00171 import warnings
00172 import math
00173 
00174 import numpy
00175 
00176 # Documentation in HTML format can be generated with Epydoc
00177 __docformat__ = "restructuredtext en"
00178 
00179 
00180 def identity_matrix():
00181     """Return 4x4 identity/unit matrix.
00182 
00183     >>> I = identity_matrix()
00184     >>> numpy.allclose(I, numpy.dot(I, I))
00185     True
00186     >>> numpy.sum(I), numpy.trace(I)
00187     (4.0, 4.0)
00188     >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64))
00189     True
00190 
00191     """
00192     return numpy.identity(4, dtype=numpy.float64)
00193 
00194 
00195 def translation_matrix(direction):
00196     """Return matrix to translate by direction vector.
00197 
00198     >>> v = numpy.random.random(3) - 0.5
00199     >>> numpy.allclose(v, translation_matrix(v)[:3, 3])
00200     True
00201 
00202     """
00203     M = numpy.identity(4)
00204     M[:3, 3] = direction[:3]
00205     return M
00206 
00207 
00208 def translation_from_matrix(matrix):
00209     """Return translation vector from translation matrix.
00210 
00211     >>> v0 = numpy.random.random(3) - 0.5
00212     >>> v1 = translation_from_matrix(translation_matrix(v0))
00213     >>> numpy.allclose(v0, v1)
00214     True
00215 
00216     """
00217     return numpy.array(matrix, copy=False)[:3, 3].copy()
00218 
00219 
00220 def reflection_matrix(point, normal):
00221     """Return matrix to mirror at plane defined by point and normal vector.
00222 
00223     >>> v0 = numpy.random.random(4) - 0.5
00224     >>> v0[3] = 1.0
00225     >>> v1 = numpy.random.random(3) - 0.5
00226     >>> R = reflection_matrix(v0, v1)
00227     >>> numpy.allclose(2., numpy.trace(R))
00228     True
00229     >>> numpy.allclose(v0, numpy.dot(R, v0))
00230     True
00231     >>> v2 = v0.copy()
00232     >>> v2[:3] += v1
00233     >>> v3 = v0.copy()
00234     >>> v2[:3] -= v1
00235     >>> numpy.allclose(v2, numpy.dot(R, v3))
00236     True
00237 
00238     """
00239     normal = unit_vector(normal[:3])
00240     M = numpy.identity(4)
00241     M[:3, :3] -= 2.0 * numpy.outer(normal, normal)
00242     M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal
00243     return M
00244 
00245 
00246 def reflection_from_matrix(matrix):
00247     """Return mirror plane point and normal vector from reflection matrix.
00248 
00249     >>> v0 = numpy.random.random(3) - 0.5
00250     >>> v1 = numpy.random.random(3) - 0.5
00251     >>> M0 = reflection_matrix(v0, v1)
00252     >>> point, normal = reflection_from_matrix(M0)
00253     >>> M1 = reflection_matrix(point, normal)
00254     >>> is_same_transform(M0, M1)
00255     True
00256 
00257     """
00258     M = numpy.array(matrix, dtype=numpy.float64, copy=False)
00259     # normal: unit eigenvector corresponding to eigenvalue -1
00260     l, V = numpy.linalg.eig(M[:3, :3])
00261     i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0]
00262     if not len(i):
00263         raise ValueError("no unit eigenvector corresponding to eigenvalue -1")
00264     normal = numpy.real(V[:, i[0]]).squeeze()
00265     # point: any unit eigenvector corresponding to eigenvalue 1
00266     l, V = numpy.linalg.eig(M)
00267     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
00268     if not len(i):
00269         raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
00270     point = numpy.real(V[:, i[-1]]).squeeze()
00271     point /= point[3]
00272     return point, normal
00273 
00274 
00275 def rotation_matrix(angle, direction, point=None):
00276     """Return matrix to rotate about axis defined by point and direction.
00277 
00278     >>> angle = (random.random() - 0.5) * (2*math.pi)
00279     >>> direc = numpy.random.random(3) - 0.5
00280     >>> point = numpy.random.random(3) - 0.5
00281     >>> R0 = rotation_matrix(angle, direc, point)
00282     >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
00283     >>> is_same_transform(R0, R1)
00284     True
00285     >>> R0 = rotation_matrix(angle, direc, point)
00286     >>> R1 = rotation_matrix(-angle, -direc, point)
00287     >>> is_same_transform(R0, R1)
00288     True
00289     >>> I = numpy.identity(4, numpy.float64)
00290     >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
00291     True
00292     >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2,
00293     ...                                                direc, point)))
00294     True
00295 
00296     """
00297     sina = math.sin(angle)
00298     cosa = math.cos(angle)
00299     direction = unit_vector(direction[:3])
00300     # rotation matrix around unit vector
00301     R = numpy.array(((cosa, 0.0,  0.0),
00302                      (0.0,  cosa, 0.0),
00303                      (0.0,  0.0,  cosa)), dtype=numpy.float64)
00304     R += numpy.outer(direction, direction) * (1.0 - cosa)
00305     direction *= sina
00306     R += numpy.array((( 0.0,         -direction[2],  direction[1]),
00307                       ( direction[2], 0.0,          -direction[0]),
00308                       (-direction[1], direction[0],  0.0)),
00309                      dtype=numpy.float64)
00310     M = numpy.identity(4)
00311     M[:3, :3] = R
00312     if point is not None:
00313         # rotation not around origin
00314         point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
00315         M[:3, 3] = point - numpy.dot(R, point)
00316     return M
00317 
00318 
00319 def rotation_from_matrix(matrix):
00320     """Return rotation angle and axis from rotation matrix.
00321 
00322     >>> angle = (random.random() - 0.5) * (2*math.pi)
00323     >>> direc = numpy.random.random(3) - 0.5
00324     >>> point = numpy.random.random(3) - 0.5
00325     >>> R0 = rotation_matrix(angle, direc, point)
00326     >>> angle, direc, point = rotation_from_matrix(R0)
00327     >>> R1 = rotation_matrix(angle, direc, point)
00328     >>> is_same_transform(R0, R1)
00329     True
00330 
00331     """
00332     R = numpy.array(matrix, dtype=numpy.float64, copy=False)
00333     R33 = R[:3, :3]
00334     # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
00335     l, W = numpy.linalg.eig(R33.T)
00336     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
00337     if not len(i):
00338         raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
00339     direction = numpy.real(W[:, i[-1]]).squeeze()
00340     # point: unit eigenvector of R33 corresponding to eigenvalue of 1
00341     l, Q = numpy.linalg.eig(R)
00342     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
00343     if not len(i):
00344         raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
00345     point = numpy.real(Q[:, i[-1]]).squeeze()
00346     point /= point[3]
00347     # rotation angle depending on direction
00348     cosa = (numpy.trace(R33) - 1.0) / 2.0
00349     if abs(direction[2]) > 1e-8:
00350         sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2]
00351     elif abs(direction[1]) > 1e-8:
00352         sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1]
00353     else:
00354         sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0]
00355     angle = math.atan2(sina, cosa)
00356     return angle, direction, point
00357 
00358 
00359 def scale_matrix(factor, origin=None, direction=None):
00360     """Return matrix to scale by factor around origin in direction.
00361 
00362     Use factor -1 for point symmetry.
00363 
00364     >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0
00365     >>> v[3] = 1.0
00366     >>> S = scale_matrix(-1.234)
00367     >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3])
00368     True
00369     >>> factor = random.random() * 10 - 5
00370     >>> origin = numpy.random.random(3) - 0.5
00371     >>> direct = numpy.random.random(3) - 0.5
00372     >>> S = scale_matrix(factor, origin)
00373     >>> S = scale_matrix(factor, origin, direct)
00374 
00375     """
00376     if direction is None:
00377         # uniform scaling
00378         M = numpy.array(((factor, 0.0,    0.0,    0.0),
00379                          (0.0,    factor, 0.0,    0.0),
00380                          (0.0,    0.0,    factor, 0.0),
00381                          (0.0,    0.0,    0.0,    1.0)), dtype=numpy.float64)
00382         if origin is not None:
00383             M[:3, 3] = origin[:3]
00384             M[:3, 3] *= 1.0 - factor
00385     else:
00386         # nonuniform scaling
00387         direction = unit_vector(direction[:3])
00388         factor = 1.0 - factor
00389         M = numpy.identity(4)
00390         M[:3, :3] -= factor * numpy.outer(direction, direction)
00391         if origin is not None:
00392             M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction
00393     return M
00394 
00395 
00396 def scale_from_matrix(matrix):
00397     """Return scaling factor, origin and direction from scaling matrix.
00398 
00399     >>> factor = random.random() * 10 - 5
00400     >>> origin = numpy.random.random(3) - 0.5
00401     >>> direct = numpy.random.random(3) - 0.5
00402     >>> S0 = scale_matrix(factor, origin)
00403     >>> factor, origin, direction = scale_from_matrix(S0)
00404     >>> S1 = scale_matrix(factor, origin, direction)
00405     >>> is_same_transform(S0, S1)
00406     True
00407     >>> S0 = scale_matrix(factor, origin, direct)
00408     >>> factor, origin, direction = scale_from_matrix(S0)
00409     >>> S1 = scale_matrix(factor, origin, direction)
00410     >>> is_same_transform(S0, S1)
00411     True
00412 
00413     """
00414     M = numpy.array(matrix, dtype=numpy.float64, copy=False)
00415     M33 = M[:3, :3]
00416     factor = numpy.trace(M33) - 2.0
00417     try:
00418         # direction: unit eigenvector corresponding to eigenvalue factor
00419         l, V = numpy.linalg.eig(M33)
00420         i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0]
00421         direction = numpy.real(V[:, i]).squeeze()
00422         direction /= vector_norm(direction)
00423     except IndexError:
00424         # uniform scaling
00425         factor = (factor + 2.0) / 3.0
00426         direction = None
00427     # origin: any eigenvector corresponding to eigenvalue 1
00428     l, V = numpy.linalg.eig(M)
00429     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
00430     if not len(i):
00431         raise ValueError("no eigenvector corresponding to eigenvalue 1")
00432     origin = numpy.real(V[:, i[-1]]).squeeze()
00433     origin /= origin[3]
00434     return factor, origin, direction
00435 
00436 
00437 def projection_matrix(point, normal, direction=None,
00438                       perspective=None, pseudo=False):
00439     """Return matrix to project onto plane defined by point and normal.
00440 
00441     Using either perspective point, projection direction, or none of both.
00442 
00443     If pseudo is True, perspective projections will preserve relative depth
00444     such that Perspective = dot(Orthogonal, PseudoPerspective).
00445 
00446     >>> P = projection_matrix((0, 0, 0), (1, 0, 0))
00447     >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:])
00448     True
00449     >>> point = numpy.random.random(3) - 0.5
00450     >>> normal = numpy.random.random(3) - 0.5
00451     >>> direct = numpy.random.random(3) - 0.5
00452     >>> persp = numpy.random.random(3) - 0.5
00453     >>> P0 = projection_matrix(point, normal)
00454     >>> P1 = projection_matrix(point, normal, direction=direct)
00455     >>> P2 = projection_matrix(point, normal, perspective=persp)
00456     >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
00457     >>> is_same_transform(P2, numpy.dot(P0, P3))
00458     True
00459     >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0))
00460     >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0
00461     >>> v0[3] = 1.0
00462     >>> v1 = numpy.dot(P, v0)
00463     >>> numpy.allclose(v1[1], v0[1])
00464     True
00465     >>> numpy.allclose(v1[0], 3.0-v1[1])
00466     True
00467 
00468     """
00469     M = numpy.identity(4)
00470     point = numpy.array(point[:3], dtype=numpy.float64, copy=False)
00471     normal = unit_vector(normal[:3])
00472     if perspective is not None:
00473         # perspective projection
00474         perspective = numpy.array(perspective[:3], dtype=numpy.float64,
00475                                   copy=False)
00476         M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal)
00477         M[:3, :3] -= numpy.outer(perspective, normal)
00478         if pseudo:
00479             # preserve relative depth
00480             M[:3, :3] -= numpy.outer(normal, normal)
00481             M[:3, 3] = numpy.dot(point, normal) * (perspective+normal)
00482         else:
00483             M[:3, 3] = numpy.dot(point, normal) * perspective
00484         M[3, :3] = -normal
00485         M[3, 3] = numpy.dot(perspective, normal)
00486     elif direction is not None:
00487         # parallel projection
00488         direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False)
00489         scale = numpy.dot(direction, normal)
00490         M[:3, :3] -= numpy.outer(direction, normal) / scale
00491         M[:3, 3] = direction * (numpy.dot(point, normal) / scale)
00492     else:
00493         # orthogonal projection
00494         M[:3, :3] -= numpy.outer(normal, normal)
00495         M[:3, 3] = numpy.dot(point, normal) * normal
00496     return M
00497 
00498 
00499 def projection_from_matrix(matrix, pseudo=False):
00500     """Return projection plane and perspective point from projection matrix.
00501 
00502     Return values are same as arguments for projection_matrix function:
00503     point, normal, direction, perspective, and pseudo.
00504 
00505     >>> point = numpy.random.random(3) - 0.5
00506     >>> normal = numpy.random.random(3) - 0.5
00507     >>> direct = numpy.random.random(3) - 0.5
00508     >>> persp = numpy.random.random(3) - 0.5
00509     >>> P0 = projection_matrix(point, normal)
00510     >>> result = projection_from_matrix(P0)
00511     >>> P1 = projection_matrix(*result)
00512     >>> is_same_transform(P0, P1)
00513     True
00514     >>> P0 = projection_matrix(point, normal, direct)
00515     >>> result = projection_from_matrix(P0)
00516     >>> P1 = projection_matrix(*result)
00517     >>> is_same_transform(P0, P1)
00518     True
00519     >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
00520     >>> result = projection_from_matrix(P0, pseudo=False)
00521     >>> P1 = projection_matrix(*result)
00522     >>> is_same_transform(P0, P1)
00523     True
00524     >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
00525     >>> result = projection_from_matrix(P0, pseudo=True)
00526     >>> P1 = projection_matrix(*result)
00527     >>> is_same_transform(P0, P1)
00528     True
00529 
00530     """
00531     M = numpy.array(matrix, dtype=numpy.float64, copy=False)
00532     M33 = M[:3, :3]
00533     l, V = numpy.linalg.eig(M)
00534     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
00535     if not pseudo and len(i):
00536         # point: any eigenvector corresponding to eigenvalue 1
00537         point = numpy.real(V[:, i[-1]]).squeeze()
00538         point /= point[3]
00539         # direction: unit eigenvector corresponding to eigenvalue 0
00540         l, V = numpy.linalg.eig(M33)
00541         i = numpy.where(abs(numpy.real(l)) < 1e-8)[0]
00542         if not len(i):
00543             raise ValueError("no eigenvector corresponding to eigenvalue 0")
00544         direction = numpy.real(V[:, i[0]]).squeeze()
00545         direction /= vector_norm(direction)
00546         # normal: unit eigenvector of M33.T corresponding to eigenvalue 0
00547         l, V = numpy.linalg.eig(M33.T)
00548         i = numpy.where(abs(numpy.real(l)) < 1e-8)[0]
00549         if len(i):
00550             # parallel projection
00551             normal = numpy.real(V[:, i[0]]).squeeze()
00552             normal /= vector_norm(normal)
00553             return point, normal, direction, None, False
00554         else:
00555             # orthogonal projection, where normal equals direction vector
00556             return point, direction, None, None, False
00557     else:
00558         # perspective projection
00559         i = numpy.where(abs(numpy.real(l)) > 1e-8)[0]
00560         if not len(i):
00561             raise ValueError(
00562                 "no eigenvector not corresponding to eigenvalue 0")
00563         point = numpy.real(V[:, i[-1]]).squeeze()
00564         point /= point[3]
00565         normal = - M[3, :3]
00566         perspective = M[:3, 3] / numpy.dot(point[:3], normal)
00567         if pseudo:
00568             perspective -= normal
00569         return point, normal, None, perspective, pseudo
00570 
00571 
00572 def clip_matrix(left, right, bottom, top, near, far, perspective=False):
00573     """Return matrix to obtain normalized device coordinates from frustrum.
00574 
00575     The frustrum bounds are axis-aligned along x (left, right),
00576     y (bottom, top) and z (near, far).
00577 
00578     Normalized device coordinates are in range [-1, 1] if coordinates are
00579     inside the frustrum.
00580 
00581     If perspective is True the frustrum is a truncated pyramid with the
00582     perspective point at origin and direction along z axis, otherwise an
00583     orthographic canonical view volume (a box).
00584 
00585     Homogeneous coordinates transformed by the perspective clip matrix
00586     need to be dehomogenized (devided by w coordinate).
00587 
00588     >>> frustrum = numpy.random.rand(6)
00589     >>> frustrum[1] += frustrum[0]
00590     >>> frustrum[3] += frustrum[2]
00591     >>> frustrum[5] += frustrum[4]
00592     >>> M = clip_matrix(*frustrum, perspective=False)
00593     >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
00594     array([-1., -1., -1.,  1.])
00595     >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0])
00596     array([ 1.,  1.,  1.,  1.])
00597     >>> M = clip_matrix(*frustrum, perspective=True)
00598     >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
00599     >>> v / v[3]
00600     array([-1., -1., -1.,  1.])
00601     >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0])
00602     >>> v / v[3]
00603     array([ 1.,  1., -1.,  1.])
00604 
00605     """
00606     if left >= right or bottom >= top or near >= far:
00607         raise ValueError("invalid frustrum")
00608     if perspective:
00609         if near <= _EPS:
00610             raise ValueError("invalid frustrum: near <= 0")
00611         t = 2.0 * near
00612         M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0),
00613              (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0),
00614              (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)),
00615              (0.0, 0.0, -1.0, 0.0))
00616     else:
00617         M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)),
00618              (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)),
00619              (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)),
00620              (0.0, 0.0, 0.0, 1.0))
00621     return numpy.array(M, dtype=numpy.float64)
00622 
00623 
00624 def shear_matrix(angle, direction, point, normal):
00625     """Return matrix to shear by angle along direction vector on shear plane.
00626 
00627     The shear plane is defined by a point and normal vector. The direction
00628     vector must be orthogonal to the plane's normal vector.
00629 
00630     A point P is transformed by the shear matrix into P" such that
00631     the vector P-P" is parallel to the direction vector and its extent is
00632     given by the angle of P-P'-P", where P' is the orthogonal projection
00633     of P onto the shear plane.
00634 
00635     >>> angle = (random.random() - 0.5) * 4*math.pi
00636     >>> direct = numpy.random.random(3) - 0.5
00637     >>> point = numpy.random.random(3) - 0.5
00638     >>> normal = numpy.cross(direct, numpy.random.random(3))
00639     >>> S = shear_matrix(angle, direct, point, normal)
00640     >>> numpy.allclose(1.0, numpy.linalg.det(S))
00641     True
00642 
00643     """
00644     normal = unit_vector(normal[:3])
00645     direction = unit_vector(direction[:3])
00646     if abs(numpy.dot(normal, direction)) > 1e-6:
00647         raise ValueError("direction and normal vectors are not orthogonal")
00648     angle = math.tan(angle)
00649     M = numpy.identity(4)
00650     M[:3, :3] += angle * numpy.outer(direction, normal)
00651     M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction
00652     return M
00653 
00654 
00655 def shear_from_matrix(matrix):
00656     """Return shear angle, direction and plane from shear matrix.
00657 
00658     >>> angle = (random.random() - 0.5) * 4*math.pi
00659     >>> direct = numpy.random.random(3) - 0.5
00660     >>> point = numpy.random.random(3) - 0.5
00661     >>> normal = numpy.cross(direct, numpy.random.random(3))
00662     >>> S0 = shear_matrix(angle, direct, point, normal)
00663     >>> angle, direct, point, normal = shear_from_matrix(S0)
00664     >>> S1 = shear_matrix(angle, direct, point, normal)
00665     >>> is_same_transform(S0, S1)
00666     True
00667 
00668     """
00669     M = numpy.array(matrix, dtype=numpy.float64, copy=False)
00670     M33 = M[:3, :3]
00671     # normal: cross independent eigenvectors corresponding to the eigenvalue 1
00672     l, V = numpy.linalg.eig(M33)
00673     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0]
00674     if len(i) < 2:
00675         raise ValueError("No two linear independent eigenvectors found %s" % l)
00676     V = numpy.real(V[:, i]).squeeze().T
00677     lenorm = -1.0
00678     for i0, i1 in ((0, 1), (0, 2), (1, 2)):
00679         n = numpy.cross(V[i0], V[i1])
00680         l = vector_norm(n)
00681         if l > lenorm:
00682             lenorm = l
00683             normal = n
00684     normal /= lenorm
00685     # direction and angle
00686     direction = numpy.dot(M33 - numpy.identity(3), normal)
00687     angle = vector_norm(direction)
00688     direction /= angle
00689     angle = math.atan(angle)
00690     # point: eigenvector corresponding to eigenvalue 1
00691     l, V = numpy.linalg.eig(M)
00692     i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0]
00693     if not len(i):
00694         raise ValueError("no eigenvector corresponding to eigenvalue 1")
00695     point = numpy.real(V[:, i[-1]]).squeeze()
00696     point /= point[3]
00697     return angle, direction, point, normal
00698 
00699 
00700 def decompose_matrix(matrix):
00701     """Return sequence of transformations from transformation matrix.
00702 
00703     matrix : array_like
00704         Non-degenerative homogeneous transformation matrix
00705 
00706     Return tuple of:
00707         scale : vector of 3 scaling factors
00708         shear : list of shear factors for x-y, x-z, y-z axes
00709         angles : list of Euler angles about static x, y, z axes
00710         translate : translation vector along x, y, z axes
00711         perspective : perspective partition of matrix
00712 
00713     Raise ValueError if matrix is of wrong type or degenerative.
00714 
00715     >>> T0 = translation_matrix((1, 2, 3))
00716     >>> scale, shear, angles, trans, persp = decompose_matrix(T0)
00717     >>> T1 = translation_matrix(trans)
00718     >>> numpy.allclose(T0, T1)
00719     True
00720     >>> S = scale_matrix(0.123)
00721     >>> scale, shear, angles, trans, persp = decompose_matrix(S)
00722     >>> scale[0]
00723     0.123
00724     >>> R0 = euler_matrix(1, 2, 3)
00725     >>> scale, shear, angles, trans, persp = decompose_matrix(R0)
00726     >>> R1 = euler_matrix(*angles)
00727     >>> numpy.allclose(R0, R1)
00728     True
00729 
00730     """
00731     M = numpy.array(matrix, dtype=numpy.float64, copy=True).T
00732     if abs(M[3, 3]) < _EPS:
00733         raise ValueError("M[3, 3] is zero")
00734     M /= M[3, 3]
00735     P = M.copy()
00736     P[:, 3] = 0, 0, 0, 1
00737     if not numpy.linalg.det(P):
00738         raise ValueError("Matrix is singular")
00739 
00740     scale = numpy.zeros((3, ), dtype=numpy.float64)
00741     shear = [0, 0, 0]
00742     angles = [0, 0, 0]
00743 
00744     if any(abs(M[:3, 3]) > _EPS):
00745         perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T))
00746         M[:, 3] = 0, 0, 0, 1
00747     else:
00748         perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64)
00749 
00750     translate = M[3, :3].copy()
00751     M[3, :3] = 0
00752 
00753     row = M[:3, :3].copy()
00754     scale[0] = vector_norm(row[0])
00755     row[0] /= scale[0]
00756     shear[0] = numpy.dot(row[0], row[1])
00757     row[1] -= row[0] * shear[0]
00758     scale[1] = vector_norm(row[1])
00759     row[1] /= scale[1]
00760     shear[0] /= scale[1]
00761     shear[1] = numpy.dot(row[0], row[2])
00762     row[2] -= row[0] * shear[1]
00763     shear[2] = numpy.dot(row[1], row[2])
00764     row[2] -= row[1] * shear[2]
00765     scale[2] = vector_norm(row[2])
00766     row[2] /= scale[2]
00767     shear[1:] /= scale[2]
00768 
00769     if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0:
00770         scale *= -1
00771         row *= -1
00772 
00773     angles[1] = math.asin(-row[0, 2])
00774     if math.cos(angles[1]):
00775         angles[0] = math.atan2(row[1, 2], row[2, 2])
00776         angles[2] = math.atan2(row[0, 1], row[0, 0])
00777     else:
00778         #angles[0] = math.atan2(row[1, 0], row[1, 1])
00779         angles[0] = math.atan2(-row[2, 1], row[1, 1])
00780         angles[2] = 0.0
00781 
00782     return scale, shear, angles, translate, perspective
00783 
00784 
00785 def compose_matrix(scale=None, shear=None, angles=None, translate=None,
00786                    perspective=None):
00787     """Return transformation matrix from sequence of transformations.
00788 
00789     This is the inverse of the decompose_matrix function.
00790 
00791     Sequence of transformations:
00792         scale : vector of 3 scaling factors
00793         shear : list of shear factors for x-y, x-z, y-z axes
00794         angles : list of Euler angles about static x, y, z axes
00795         translate : translation vector along x, y, z axes
00796         perspective : perspective partition of matrix
00797 
00798     >>> scale = numpy.random.random(3) - 0.5
00799     >>> shear = numpy.random.random(3) - 0.5
00800     >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi)
00801     >>> trans = numpy.random.random(3) - 0.5
00802     >>> persp = numpy.random.random(4) - 0.5
00803     >>> M0 = compose_matrix(scale, shear, angles, trans, persp)
00804     >>> result = decompose_matrix(M0)
00805     >>> M1 = compose_matrix(*result)
00806     >>> is_same_transform(M0, M1)
00807     True
00808 
00809     """
00810     M = numpy.identity(4)
00811     if perspective is not None:
00812         P = numpy.identity(4)
00813         P[3, :] = perspective[:4]
00814         M = numpy.dot(M, P)
00815     if translate is not None:
00816         T = numpy.identity(4)
00817         T[:3, 3] = translate[:3]
00818         M = numpy.dot(M, T)
00819     if angles is not None:
00820         R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz')
00821         M = numpy.dot(M, R)
00822     if shear is not None:
00823         Z = numpy.identity(4)
00824         Z[1, 2] = shear[2]
00825         Z[0, 2] = shear[1]
00826         Z[0, 1] = shear[0]
00827         M = numpy.dot(M, Z)
00828     if scale is not None:
00829         S = numpy.identity(4)
00830         S[0, 0] = scale[0]
00831         S[1, 1] = scale[1]
00832         S[2, 2] = scale[2]
00833         M = numpy.dot(M, S)
00834     M /= M[3, 3]
00835     return M
00836 
00837 
00838 def orthogonalization_matrix(lengths, angles):
00839     """Return orthogonalization matrix for crystallographic cell coordinates.
00840 
00841     Angles are expected in degrees.
00842 
00843     The de-orthogonalization matrix is the inverse.
00844 
00845     >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.))
00846     >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
00847     True
00848     >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
00849     >>> numpy.allclose(numpy.sum(O), 43.063229)
00850     True
00851 
00852     """
00853     a, b, c = lengths
00854     angles = numpy.radians(angles)
00855     sina, sinb, _ = numpy.sin(angles)
00856     cosa, cosb, cosg = numpy.cos(angles)
00857     co = (cosa * cosb - cosg) / (sina * sinb)
00858     return numpy.array((
00859         ( a*sinb*math.sqrt(1.0-co*co),  0.0,    0.0, 0.0),
00860         (-a*sinb*co,                    b*sina, 0.0, 0.0),
00861         ( a*cosb,                       b*cosa, c,   0.0),
00862         ( 0.0,                          0.0,    0.0, 1.0)),
00863         dtype=numpy.float64)
00864 
00865 
00866 def superimposition_matrix(v0, v1, scaling=False, usesvd=True):
00867     """Return matrix to transform given vector set into second vector set.
00868 
00869     v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors.
00870 
00871     If usesvd is True, the weighted sum of squared deviations (RMSD) is
00872     minimized according to the algorithm by W. Kabsch [8]. Otherwise the
00873     quaternion based algorithm by B. Horn [9] is used (slower when using
00874     this Python implementation).
00875 
00876     The returned matrix performs rotation, translation and uniform scaling
00877     (if specified).
00878 
00879     >>> v0 = numpy.random.rand(3, 10)
00880     >>> M = superimposition_matrix(v0, v0)
00881     >>> numpy.allclose(M, numpy.identity(4))
00882     True
00883     >>> R = random_rotation_matrix(numpy.random.random(3))
00884     >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1))
00885     >>> v1 = numpy.dot(R, v0)
00886     >>> M = superimposition_matrix(v0, v1)
00887     >>> numpy.allclose(v1, numpy.dot(M, v0))
00888     True
00889     >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0
00890     >>> v0[3] = 1.0
00891     >>> v1 = numpy.dot(R, v0)
00892     >>> M = superimposition_matrix(v0, v1)
00893     >>> numpy.allclose(v1, numpy.dot(M, v0))
00894     True
00895     >>> S = scale_matrix(random.random())
00896     >>> T = translation_matrix(numpy.random.random(3)-0.5)
00897     >>> M = concatenate_matrices(T, R, S)
00898     >>> v1 = numpy.dot(M, v0)
00899     >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1)
00900     >>> M = superimposition_matrix(v0, v1, scaling=True)
00901     >>> numpy.allclose(v1, numpy.dot(M, v0))
00902     True
00903     >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
00904     >>> numpy.allclose(v1, numpy.dot(M, v0))
00905     True
00906     >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64)
00907     >>> v[:, :, 0] = v0
00908     >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
00909     >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0]))
00910     True
00911 
00912     """
00913     v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3]
00914     v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3]
00915 
00916     if v0.shape != v1.shape or v0.shape[1] < 3:
00917         raise ValueError("Vector sets are of wrong shape or type.")
00918 
00919     # move centroids to origin
00920     t0 = numpy.mean(v0, axis=1)
00921     t1 = numpy.mean(v1, axis=1)
00922     v0 = v0 - t0.reshape(3, 1)
00923     v1 = v1 - t1.reshape(3, 1)
00924 
00925     if usesvd:
00926         # Singular Value Decomposition of covariance matrix
00927         u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T))
00928         # rotation matrix from SVD orthonormal bases
00929         R = numpy.dot(u, vh)
00930         if numpy.linalg.det(R) < 0.0:
00931             # R does not constitute right handed system
00932             R -= numpy.outer(u[:, 2], vh[2, :]*2.0)
00933             s[-1] *= -1.0
00934         # homogeneous transformation matrix
00935         M = numpy.identity(4)
00936         M[:3, :3] = R
00937     else:
00938         # compute symmetric matrix N
00939         xx, yy, zz = numpy.sum(v0 * v1, axis=1)
00940         xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1)
00941         xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1)
00942         N = ((xx+yy+zz, yz-zy,    zx-xz,    xy-yx),
00943              (yz-zy,    xx-yy-zz, xy+yx,    zx+xz),
00944              (zx-xz,    xy+yx,   -xx+yy-zz, yz+zy),
00945              (xy-yx,    zx+xz,    yz+zy,   -xx-yy+zz))
00946         # quaternion: eigenvector corresponding to most positive eigenvalue
00947         l, V = numpy.linalg.eig(N)
00948         q = V[:, numpy.argmax(l)]
00949         q /= vector_norm(q) # unit quaternion
00950         q = numpy.roll(q, -1) # move w component to end
00951         # homogeneous transformation matrix
00952         M = quaternion_matrix(q)
00953 
00954     # scale: ratio of rms deviations from centroid
00955     if scaling:
00956         v0 *= v0
00957         v1 *= v1
00958         M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0))
00959 
00960     # translation
00961     M[:3, 3] = t1
00962     T = numpy.identity(4)
00963     T[:3, 3] = -t0
00964     M = numpy.dot(M, T)
00965     return M
00966 
00967 
00968 def euler_matrix(ai, aj, ak, axes='sxyz'):
00969     """Return homogeneous rotation matrix from Euler angles and axis sequence.
00970 
00971     ai, aj, ak : Euler's roll, pitch and yaw angles
00972     axes : One of 24 axis sequences as string or encoded tuple
00973 
00974     >>> R = euler_matrix(1, 2, 3, 'syxz')
00975     >>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
00976     True
00977     >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
00978     >>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
00979     True
00980     >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
00981     >>> for axes in _AXES2TUPLE.keys():
00982     ...    R = euler_matrix(ai, aj, ak, axes)
00983     >>> for axes in _TUPLE2AXES.keys():
00984     ...    R = euler_matrix(ai, aj, ak, axes)
00985 
00986     """
00987     try:
00988         firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
00989     except (AttributeError, KeyError):
00990         _ = _TUPLE2AXES[axes]
00991         firstaxis, parity, repetition, frame = axes
00992 
00993     i = firstaxis
00994     j = _NEXT_AXIS[i+parity]
00995     k = _NEXT_AXIS[i-parity+1]
00996 
00997     if frame:
00998         ai, ak = ak, ai
00999     if parity:
01000         ai, aj, ak = -ai, -aj, -ak
01001 
01002     si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
01003     ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
01004     cc, cs = ci*ck, ci*sk
01005     sc, ss = si*ck, si*sk
01006 
01007     M = numpy.identity(4)
01008     if repetition:
01009         M[i, i] = cj
01010         M[i, j] = sj*si
01011         M[i, k] = sj*ci
01012         M[j, i] = sj*sk
01013         M[j, j] = -cj*ss+cc
01014         M[j, k] = -cj*cs-sc
01015         M[k, i] = -sj*ck
01016         M[k, j] = cj*sc+cs
01017         M[k, k] = cj*cc-ss
01018     else:
01019         M[i, i] = cj*ck
01020         M[i, j] = sj*sc-cs
01021         M[i, k] = sj*cc+ss
01022         M[j, i] = cj*sk
01023         M[j, j] = sj*ss+cc
01024         M[j, k] = sj*cs-sc
01025         M[k, i] = -sj
01026         M[k, j] = cj*si
01027         M[k, k] = cj*ci
01028     return M
01029 
01030 
01031 def euler_from_matrix(matrix, axes='sxyz'):
01032     """Return Euler angles from rotation matrix for specified axis sequence.
01033 
01034     axes : One of 24 axis sequences as string or encoded tuple
01035 
01036     Note that many Euler angle triplets can describe one matrix.
01037 
01038     >>> R0 = euler_matrix(1, 2, 3, 'syxz')
01039     >>> al, be, ga = euler_from_matrix(R0, 'syxz')
01040     >>> R1 = euler_matrix(al, be, ga, 'syxz')
01041     >>> numpy.allclose(R0, R1)
01042     True
01043     >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
01044     >>> for axes in _AXES2TUPLE.keys():
01045     ...    R0 = euler_matrix(axes=axes, *angles)
01046     ...    R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
01047     ...    if not numpy.allclose(R0, R1): print axes, "failed"
01048 
01049     """
01050     try:
01051         firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
01052     except (AttributeError, KeyError):
01053         _ = _TUPLE2AXES[axes]
01054         firstaxis, parity, repetition, frame = axes
01055 
01056     i = firstaxis
01057     j = _NEXT_AXIS[i+parity]
01058     k = _NEXT_AXIS[i-parity+1]
01059 
01060     M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]
01061     if repetition:
01062         sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
01063         if sy > _EPS:
01064             ax = math.atan2( M[i, j],  M[i, k])
01065             ay = math.atan2( sy,       M[i, i])
01066             az = math.atan2( M[j, i], -M[k, i])
01067         else:
01068             ax = math.atan2(-M[j, k],  M[j, j])
01069             ay = math.atan2( sy,       M[i, i])
01070             az = 0.0
01071     else:
01072         cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
01073         if cy > _EPS:
01074             ax = math.atan2( M[k, j],  M[k, k])
01075             ay = math.atan2(-M[k, i],  cy)
01076             az = math.atan2( M[j, i],  M[i, i])
01077         else:
01078             ax = math.atan2(-M[j, k],  M[j, j])
01079             ay = math.atan2(-M[k, i],  cy)
01080             az = 0.0
01081 
01082     if parity:
01083         ax, ay, az = -ax, -ay, -az
01084     if frame:
01085         ax, az = az, ax
01086     return ax, ay, az
01087 
01088 
01089 def euler_from_quaternion(quaternion, axes='sxyz'):
01090     """Return Euler angles from quaternion for specified axis sequence.
01091 
01092     >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
01093     >>> numpy.allclose(angles, [0.123, 0, 0])
01094     True
01095 
01096     """
01097     return euler_from_matrix(quaternion_matrix(quaternion), axes)
01098 
01099 
01100 def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
01101     """Return quaternion from Euler angles and axis sequence.
01102 
01103     ai, aj, ak : Euler's roll, pitch and yaw angles
01104     axes : One of 24 axis sequences as string or encoded tuple
01105 
01106     >>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
01107     >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953])
01108     True
01109 
01110     """
01111     try:
01112         firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
01113     except (AttributeError, KeyError):
01114         _ = _TUPLE2AXES[axes]
01115         firstaxis, parity, repetition, frame = axes
01116 
01117     i = firstaxis
01118     j = _NEXT_AXIS[i+parity]
01119     k = _NEXT_AXIS[i-parity+1]
01120 
01121     if frame:
01122         ai, ak = ak, ai
01123     if parity:
01124         aj = -aj
01125 
01126     ai /= 2.0
01127     aj /= 2.0
01128     ak /= 2.0
01129     ci = math.cos(ai)
01130     si = math.sin(ai)
01131     cj = math.cos(aj)
01132     sj = math.sin(aj)
01133     ck = math.cos(ak)
01134     sk = math.sin(ak)
01135     cc = ci*ck
01136     cs = ci*sk
01137     sc = si*ck
01138     ss = si*sk
01139 
01140     quaternion = numpy.empty((4, ), dtype=numpy.float64)
01141     if repetition:
01142         quaternion[i] = cj*(cs + sc)
01143         quaternion[j] = sj*(cc + ss)
01144         quaternion[k] = sj*(cs - sc)
01145         quaternion[3] = cj*(cc - ss)
01146     else:
01147         quaternion[i] = cj*sc - sj*cs
01148         quaternion[j] = cj*ss + sj*cc
01149         quaternion[k] = cj*cs - sj*sc
01150         quaternion[3] = cj*cc + sj*ss
01151     if parity:
01152         quaternion[j] *= -1
01153 
01154     return quaternion
01155 
01156 
01157 def quaternion_about_axis(angle, axis):
01158     """Return quaternion for rotation about axis.
01159 
01160     >>> q = quaternion_about_axis(0.123, (1, 0, 0))
01161     >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947])
01162     True
01163 
01164     """
01165     quaternion = numpy.zeros((4, ), dtype=numpy.float64)
01166     quaternion[:3] = axis[:3]
01167     qlen = vector_norm(quaternion)
01168     if qlen > _EPS:
01169         quaternion *= math.sin(angle/2.0) / qlen
01170     quaternion[3] = math.cos(angle/2.0)
01171     return quaternion
01172 
01173 
01174 def quaternion_matrix(quaternion):
01175     """Return homogeneous rotation matrix from quaternion.
01176 
01177     >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
01178     >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
01179     True
01180 
01181     """
01182     q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True)
01183     nq = numpy.dot(q, q)
01184     if nq < _EPS:
01185         return numpy.identity(4)
01186     q *= math.sqrt(2.0 / nq)
01187     q = numpy.outer(q, q)
01188     return numpy.array((
01189         (1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3], 0.0),
01190         (    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3], 0.0),
01191         (    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
01192         (                0.0,                 0.0,                 0.0, 1.0)
01193         ), dtype=numpy.float64)
01194 
01195 
01196 def quaternion_from_matrix(matrix):
01197     """Return quaternion from rotation matrix.
01198 
01199     >>> R = rotation_matrix(0.123, (1, 2, 3))
01200     >>> q = quaternion_from_matrix(R)
01201     >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095])
01202     True
01203 
01204     """
01205     q = numpy.empty((4, ), dtype=numpy.float64)
01206     M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4]
01207     t = numpy.trace(M)
01208     if t > M[3, 3]:
01209         q[3] = t
01210         q[2] = M[1, 0] - M[0, 1]
01211         q[1] = M[0, 2] - M[2, 0]
01212         q[0] = M[2, 1] - M[1, 2]
01213     else:
01214         i, j, k = 0, 1, 2
01215         if M[1, 1] > M[0, 0]:
01216             i, j, k = 1, 2, 0
01217         if M[2, 2] > M[i, i]:
01218             i, j, k = 2, 0, 1
01219         t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
01220         q[i] = t
01221         q[j] = M[i, j] + M[j, i]
01222         q[k] = M[k, i] + M[i, k]
01223         q[3] = M[k, j] - M[j, k]
01224     q *= 0.5 / math.sqrt(t * M[3, 3])
01225     return q
01226 
01227 
01228 def quaternion_multiply(quaternion1, quaternion0):
01229     """Return multiplication of two quaternions.
01230 
01231     >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8])
01232     >>> numpy.allclose(q, [-44, -14, 48, 28])
01233     True
01234 
01235     """
01236     x0, y0, z0, w0 = quaternion0
01237     x1, y1, z1, w1 = quaternion1
01238     return numpy.array((
01239          x1*w0 + y1*z0 - z1*y0 + w1*x0,
01240         -x1*z0 + y1*w0 + z1*x0 + w1*y0,
01241          x1*y0 - y1*x0 + z1*w0 + w1*z0,
01242         -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64)
01243 
01244 
01245 def quaternion_conjugate(quaternion):
01246     """Return conjugate of quaternion.
01247 
01248     >>> q0 = random_quaternion()
01249     >>> q1 = quaternion_conjugate(q0)
01250     >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3])
01251     True
01252 
01253     """
01254     return numpy.array((-quaternion[0], -quaternion[1],
01255                         -quaternion[2], quaternion[3]), dtype=numpy.float64)
01256 
01257 
01258 def quaternion_inverse(quaternion):
01259     """Return inverse of quaternion.
01260 
01261     >>> q0 = random_quaternion()
01262     >>> q1 = quaternion_inverse(q0)
01263     >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1])
01264     True
01265 
01266     """
01267     return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion)
01268 
01269 
01270 def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
01271     """Return spherical linear interpolation between two quaternions.
01272 
01273     >>> q0 = random_quaternion()
01274     >>> q1 = random_quaternion()
01275     >>> q = quaternion_slerp(q0, q1, 0.0)
01276     >>> numpy.allclose(q, q0)
01277     True
01278     >>> q = quaternion_slerp(q0, q1, 1.0, 1)
01279     >>> numpy.allclose(q, q1)
01280     True
01281     >>> q = quaternion_slerp(q0, q1, 0.5)
01282     >>> angle = math.acos(numpy.dot(q0, q))
01283     >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \
01284         numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle)
01285     True
01286 
01287     """
01288     q0 = unit_vector(quat0[:4])
01289     q1 = unit_vector(quat1[:4])
01290     if fraction == 0.0:
01291         return q0
01292     elif fraction == 1.0:
01293         return q1
01294     d = numpy.dot(q0, q1)
01295     if abs(abs(d) - 1.0) < _EPS:
01296         return q0
01297     if shortestpath and d < 0.0:
01298         # invert rotation
01299         d = -d
01300         q1 *= -1.0
01301     angle = math.acos(d) + spin * math.pi
01302     if abs(angle) < _EPS:
01303         return q0
01304     isin = 1.0 / math.sin(angle)
01305     q0 *= math.sin((1.0 - fraction) * angle) * isin
01306     q1 *= math.sin(fraction * angle) * isin
01307     q0 += q1
01308     return q0
01309 
01310 
01311 def random_quaternion(rand=None):
01312     """Return uniform random unit quaternion.
01313 
01314     rand: array like or None
01315         Three independent random variables that are uniformly distributed
01316         between 0 and 1.
01317 
01318     >>> q = random_quaternion()
01319     >>> numpy.allclose(1.0, vector_norm(q))
01320     True
01321     >>> q = random_quaternion(numpy.random.random(3))
01322     >>> q.shape
01323     (4,)
01324 
01325     """
01326     if rand is None:
01327         rand = numpy.random.rand(3)
01328     else:
01329         assert len(rand) == 3
01330     r1 = numpy.sqrt(1.0 - rand[0])
01331     r2 = numpy.sqrt(rand[0])
01332     pi2 = math.pi * 2.0
01333     t1 = pi2 * rand[1]
01334     t2 = pi2 * rand[2]
01335     return numpy.array((numpy.sin(t1)*r1,
01336                         numpy.cos(t1)*r1,
01337                         numpy.sin(t2)*r2,
01338                         numpy.cos(t2)*r2), dtype=numpy.float64)
01339 
01340 
01341 def random_rotation_matrix(rand=None):
01342     """Return uniform random rotation matrix.
01343 
01344     rnd: array like
01345         Three independent random variables that are uniformly distributed
01346         between 0 and 1 for each returned quaternion.
01347 
01348     >>> R = random_rotation_matrix()
01349     >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
01350     True
01351 
01352     """
01353     return quaternion_matrix(random_quaternion(rand))
01354 
01355 
01356 class Arcball(object):
01357     """Virtual Trackball Control.
01358 
01359     >>> ball = Arcball()
01360     >>> ball = Arcball(initial=numpy.identity(4))
01361     >>> ball.place([320, 320], 320)
01362     >>> ball.down([500, 250])
01363     >>> ball.drag([475, 275])
01364     >>> R = ball.matrix()
01365     >>> numpy.allclose(numpy.sum(R), 3.90583455)
01366     True
01367     >>> ball = Arcball(initial=[0, 0, 0, 1])
01368     >>> ball.place([320, 320], 320)
01369     >>> ball.setaxes([1,1,0], [-1, 1, 0])
01370     >>> ball.setconstrain(True)
01371     >>> ball.down([400, 200])
01372     >>> ball.drag([200, 400])
01373     >>> R = ball.matrix()
01374     >>> numpy.allclose(numpy.sum(R), 0.2055924)
01375     True
01376     >>> ball.next()
01377 
01378     """
01379 
01380     def __init__(self, initial=None):
01381         """Initialize virtual trackball control.
01382 
01383         initial : quaternion or rotation matrix
01384 
01385         """
01386         self._axis = None
01387         self._axes = None
01388         self._radius = 1.0
01389         self._center = [0.0, 0.0]
01390         self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64)
01391         self._constrain = False
01392 
01393         if initial is None:
01394             self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64)
01395         else:
01396             initial = numpy.array(initial, dtype=numpy.float64)
01397             if initial.shape == (4, 4):
01398                 self._qdown = quaternion_from_matrix(initial)
01399             elif initial.shape == (4, ):
01400                 initial /= vector_norm(initial)
01401                 self._qdown = initial
01402             else:
01403                 raise ValueError("initial not a quaternion or matrix.")
01404 
01405         self._qnow = self._qpre = self._qdown
01406 
01407     def place(self, center, radius):
01408         """Place Arcball, e.g. when window size changes.
01409 
01410         center : sequence[2]
01411             Window coordinates of trackball center.
01412         radius : float
01413             Radius of trackball in window coordinates.
01414 
01415         """
01416         self._radius = float(radius)
01417         self._center[0] = center[0]
01418         self._center[1] = center[1]
01419 
01420     def setaxes(self, *axes):
01421         """Set axes to constrain rotations."""
01422         if axes is None:
01423             self._axes = None
01424         else:
01425             self._axes = [unit_vector(axis) for axis in axes]
01426 
01427     def setconstrain(self, constrain):
01428         """Set state of constrain to axis mode."""
01429         self._constrain = constrain == True
01430 
01431     def getconstrain(self):
01432         """Return state of constrain to axis mode."""
01433         return self._constrain
01434 
01435     def down(self, point):
01436         """Set initial cursor window coordinates and pick constrain-axis."""
01437         self._vdown = arcball_map_to_sphere(point, self._center, self._radius)
01438         self._qdown = self._qpre = self._qnow
01439 
01440         if self._constrain and self._axes is not None:
01441             self._axis = arcball_nearest_axis(self._vdown, self._axes)
01442             self._vdown = arcball_constrain_to_axis(self._vdown, self._axis)
01443         else:
01444             self._axis = None
01445 
01446     def drag(self, point):
01447         """Update current cursor window coordinates."""
01448         vnow = arcball_map_to_sphere(point, self._center, self._radius)
01449 
01450         if self._axis is not None:
01451             vnow = arcball_constrain_to_axis(vnow, self._axis)
01452 
01453         self._qpre = self._qnow
01454 
01455         t = numpy.cross(self._vdown, vnow)
01456         if numpy.dot(t, t) < _EPS:
01457             self._qnow = self._qdown
01458         else:
01459             q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)]
01460             self._qnow = quaternion_multiply(q, self._qdown)
01461 
01462     def next(self, acceleration=0.0):
01463         """Continue rotation in direction of last drag."""
01464         q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False)
01465         self._qpre, self._qnow = self._qnow, q
01466 
01467     def matrix(self):
01468         """Return homogeneous rotation matrix."""
01469         return quaternion_matrix(self._qnow)
01470 
01471 
01472 def arcball_map_to_sphere(point, center, radius):
01473     """Return unit sphere coordinates from window coordinates."""
01474     v = numpy.array(((point[0] - center[0]) / radius,
01475                      (center[1] - point[1]) / radius,
01476                      0.0), dtype=numpy.float64)
01477     n = v[0]*v[0] + v[1]*v[1]
01478     if n > 1.0:
01479         v /= math.sqrt(n) # position outside of sphere
01480     else:
01481         v[2] = math.sqrt(1.0 - n)
01482     return v
01483 
01484 
01485 def arcball_constrain_to_axis(point, axis):
01486     """Return sphere point perpendicular to axis."""
01487     v = numpy.array(point, dtype=numpy.float64, copy=True)
01488     a = numpy.array(axis, dtype=numpy.float64, copy=True)
01489     v -= a * numpy.dot(a, v) # on plane
01490     n = vector_norm(v)
01491     if n > _EPS:
01492         if v[2] < 0.0:
01493             v *= -1.0
01494         v /= n
01495         return v
01496     if a[2] == 1.0:
01497         return numpy.array([1, 0, 0], dtype=numpy.float64)
01498     return unit_vector([-a[1], a[0], 0])
01499 
01500 
01501 def arcball_nearest_axis(point, axes):
01502     """Return axis, which arc is nearest to point."""
01503     point = numpy.array(point, dtype=numpy.float64, copy=False)
01504     nearest = None
01505     mx = -1.0
01506     for axis in axes:
01507         t = numpy.dot(arcball_constrain_to_axis(point, axis), point)
01508         if t > mx:
01509             nearest = axis
01510             mx = t
01511     return nearest
01512 
01513 
01514 # epsilon for testing whether a number is close to zero
01515 _EPS = numpy.finfo(float).eps * 4.0
01516 
01517 # axis sequences for Euler angles
01518 _NEXT_AXIS = [1, 2, 0, 1]
01519 
01520 # map axes strings to/from tuples of inner axis, parity, repetition, frame
01521 _AXES2TUPLE = {
01522     'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
01523     'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
01524     'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
01525     'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
01526     'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
01527     'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
01528     'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
01529     'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
01530 
01531 _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
01532 
01533 # helper functions
01534 
01535 def vector_norm(data, axis=None, out=None):
01536     """Return length, i.e. eucledian norm, of ndarray along axis.
01537 
01538     >>> v = numpy.random.random(3)
01539     >>> n = vector_norm(v)
01540     >>> numpy.allclose(n, numpy.linalg.norm(v))
01541     True
01542     >>> v = numpy.random.rand(6, 5, 3)
01543     >>> n = vector_norm(v, axis=-1)
01544     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
01545     True
01546     >>> n = vector_norm(v, axis=1)
01547     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
01548     True
01549     >>> v = numpy.random.rand(5, 4, 3)
01550     >>> n = numpy.empty((5, 3), dtype=numpy.float64)
01551     >>> vector_norm(v, axis=1, out=n)
01552     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
01553     True
01554     >>> vector_norm([])
01555     0.0
01556     >>> vector_norm([1.0])
01557     1.0
01558 
01559     """
01560     data = numpy.array(data, dtype=numpy.float64, copy=True)
01561     if out is None:
01562         if data.ndim == 1:
01563             return math.sqrt(numpy.dot(data, data))
01564         data *= data
01565         out = numpy.atleast_1d(numpy.sum(data, axis=axis))
01566         numpy.sqrt(out, out)
01567         return out
01568     else:
01569         data *= data
01570         numpy.sum(data, axis=axis, out=out)
01571         numpy.sqrt(out, out)
01572 
01573 
01574 def unit_vector(data, axis=None, out=None):
01575     """Return ndarray normalized by length, i.e. eucledian norm, along axis.
01576 
01577     >>> v0 = numpy.random.random(3)
01578     >>> v1 = unit_vector(v0)
01579     >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
01580     True
01581     >>> v0 = numpy.random.rand(5, 4, 3)
01582     >>> v1 = unit_vector(v0, axis=-1)
01583     >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
01584     >>> numpy.allclose(v1, v2)
01585     True
01586     >>> v1 = unit_vector(v0, axis=1)
01587     >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
01588     >>> numpy.allclose(v1, v2)
01589     True
01590     >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64)
01591     >>> unit_vector(v0, axis=1, out=v1)
01592     >>> numpy.allclose(v1, v2)
01593     True
01594     >>> list(unit_vector([]))
01595     []
01596     >>> list(unit_vector([1.0]))
01597     [1.0]
01598 
01599     """
01600     if out is None:
01601         data = numpy.array(data, dtype=numpy.float64, copy=True)
01602         if data.ndim == 1:
01603             data /= math.sqrt(numpy.dot(data, data))
01604             return data
01605     else:
01606         if out is not data:
01607             out[:] = numpy.array(data, copy=False)
01608         data = out
01609     length = numpy.atleast_1d(numpy.sum(data*data, axis))
01610     numpy.sqrt(length, length)
01611     if axis is not None:
01612         length = numpy.expand_dims(length, axis)
01613     data /= length
01614     if out is None:
01615         return data
01616 
01617 
01618 def random_vector(size):
01619     """Return array of random doubles in the half-open interval [0.0, 1.0).
01620 
01621     >>> v = random_vector(10000)
01622     >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0)
01623     True
01624     >>> v0 = random_vector(10)
01625     >>> v1 = random_vector(10)
01626     >>> numpy.any(v0 == v1)
01627     False
01628 
01629     """
01630     return numpy.random.random(size)
01631 
01632 
01633 def inverse_matrix(matrix):
01634     """Return inverse of square transformation matrix.
01635 
01636     >>> M0 = random_rotation_matrix()
01637     >>> M1 = inverse_matrix(M0.T)
01638     >>> numpy.allclose(M1, numpy.linalg.inv(M0.T))
01639     True
01640     >>> for size in range(1, 7):
01641     ...     M0 = numpy.random.rand(size, size)
01642     ...     M1 = inverse_matrix(M0)
01643     ...     if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size
01644 
01645     """
01646     return numpy.linalg.inv(matrix)
01647 
01648 
01649 def concatenate_matrices(*matrices):
01650     """Return concatenation of series of transformation matrices.
01651 
01652     >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5
01653     >>> numpy.allclose(M, concatenate_matrices(M))
01654     True
01655     >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T))
01656     True
01657 
01658     """
01659     M = numpy.identity(4)
01660     for i in matrices:
01661         M = numpy.dot(M, i)
01662     return M
01663 
01664 
01665 def is_same_transform(matrix0, matrix1):
01666     """Return True if two matrices perform same transformation.
01667 
01668     >>> is_same_transform(numpy.identity(4), numpy.identity(4))
01669     True
01670     >>> is_same_transform(numpy.identity(4), random_rotation_matrix())
01671     False
01672 
01673     """
01674     matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True)
01675     matrix0 /= matrix0[3, 3]
01676     matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True)
01677     matrix1 /= matrix1[3, 3]
01678     return numpy.allclose(matrix0, matrix1)
01679 
01680 
01681 def _import_module(module_name, warn=True, prefix='_py_', ignore='_'):
01682     """Try import all public attributes from module into global namespace.
01683 
01684     Existing attributes with name clashes are renamed with prefix.
01685     Attributes starting with underscore are ignored by default.
01686 
01687     Return True on successful import.
01688 
01689     """
01690     try:
01691         module = __import__(module_name)
01692     except ImportError:
01693         if warn:
01694             warnings.warn("Failed to import module " + module_name)
01695     else:
01696         for attr in dir(module):
01697             if ignore and attr.startswith(ignore):
01698                 continue
01699             if prefix:
01700                 if attr in globals():
01701                     globals()[prefix + attr] = globals()[attr]
01702                 elif warn:
01703                     warnings.warn("No Python implementation of " + attr)
01704             globals()[attr] = getattr(module, attr)
01705         return True


hrl_geom
Author(s): Kelsey Hawkins / kphawkins@gatech.edu, Advisor: Prof. Charlie Kemp (Healthcare Robotics Lab at Georgia Tech)
autogenerated on Wed Nov 27 2013 11:37:25