Commit: a868d649aa24e4cda858267ae8c355a19b37cfc9 - myprojects-hg-reborn (hg)

Commit MetaInfo

• Révision: a868d649aa24e4cda858267ae8c355a19b37cfc9 (tree)
• l'heure: 2008-09-09 23:53:03
• Auteur: iselllo
• Commiter: iselllo

Message de Log

I added a code which performs a number of manipulations on matrices, mainly to investigate rotations (quaternions, Euler, etc...). I took from here
a routing to be used for post-processing the results of Langevin simulations.

Change Summary

• add: Python-codes/transformations.py

Modification

diff -r ff3d03477f19 -r a868d649aa24 Python-codes/transformations.py
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Python-codes/transformations.py	Tue Sep 09 14:53:03 2008 +0000
@@ -0,0 +1,681 @@
+#! /usr/bin/env python
+
+
+# Have a look at http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
+
+
+
+# -*- coding: utf-8 -*-
+# transformations.py
+
+# Copyright (c) 2006-2007, Christoph Gohlke
+# Copyright (c) 2006-2007, The Regents of the University of California
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in the
+#    documentation and/or other materials provided with the distribution.
+#  * Neither the name of the copyright holders nor the names of any
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Homogeneous Transformation Matrices and Quaternions.
+
+Functions for calculating 4x4 matrices for translating, rotating, mirroring,
+scaling, projecting, orthogonalization, and superimposition of arrays of
+homogenous coordinates as well as for converting between rotation matrices,
+Euler angles, and quaternions.
+
+Matrices can be inverted using numpy.linalg.inv(M), concatenated using
+numpy.dot(M0,M1), or used to transform homogeneous points using
+numpy.dot(v, M) for shape (4,*) "point of arrays", respectively
+numpy.dot(v, M.T) for shape (*,4) "array of points".
+
+Quaternions ix+jy+kz+w are represented as [x, y, z, w].
+
+Angles are in radians unless specified otherwise.
+
+The 24 ways of specifying rotations for a given triple of Euler angles,
+can be represented by a 4 character string or encoded 4-tuple:
+
+  Axes 4-string:
+    first character -- rotations are applied to 's'tatic or 'r'otating frame
+    remaining characters -- successive rotation axis 'x', 'y', or 'z'
+  Axes 4-tuple:
+    inner axis -- code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
+    parity -- even (0) if inner axis 'x' is followed by 'y', 'y' is followed
+        by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
+    repetition -- first and last axis are same (1) or different (0).
+    frame -- rotations are applied to static (0) or rotating (1) frame.
+  Examples of tuple codes:
+    'sxyz' <-> (0, 0, 0, 0)
+    'ryxy' <-> (1, 1, 1, 1)
+
+Author:
+    Christoph Gohlke, http://www.lfd.uci.edu/~gohlke/
+    Laboratory for Fluorescence Dynamics, University of California, Irvine
+
+Requirements:
+    Python 2.5 (http://www.python.org)
+    Numpy 1.0 (http://numpy.scipy.org)
+
+References:
+(1) Matrices and Transformations. Ronald Goldman.
+    In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
+(2) Euler Angle Conversion. Ken Shoemake.
+    In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
+(3) Arcball Rotation Control. Ken Shoemake.
+    In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
+
+"""
+
+from __future__ import division
+
+import math
+import numpy
+
+_EPS = numpy.finfo(float).eps * 4.0
+
+def concatenate_transforms(*args):
+    """Merge multiple transformation matrices into one."""
+    M = numpy.identity(4, dtype=numpy.float64)
+    for i in args:
+        M = numpy.dot(M, i)
+    return M
+
+def translation_matrix(direction):
+    """Return matrix to translate by direction vector."""
+    M = numpy.identity(4, dtype=numpy.float64)
+    M[0:3,3] = direction[0:3]
+    return M
+
+def mirror_matrix(point, normal):
+    """Return matrix to mirror at plane defined by point and normal vector."""
+    M = numpy.identity(4, dtype=numpy.float64)
+    n = numpy.array(normal[0:3], dtype=numpy.float64, copy=True)
+    n /= math.sqrt(numpy.dot(n,n))
+    M[0:3,0:3] -= 2.0 * numpy.outer(n, n)
+    M[0:3,3] = 2.0 * numpy.dot(point[0:3], n) * n
+    return M
+
+def rotation_matrix(angle, direction, point=None):
+    """Return matrix to rotate about axis defined by point and direction."""
+    sa, ca = math.sin(angle), math.cos(angle)
+    # unit vector of direction
+    u = numpy.array(direction[0:3], dtype=numpy.float64, copy=True)
+    u /= math.sqrt(numpy.dot(u, u))
+    # rotation matrix around unit vector
+    R = numpy.identity(3, dtype=numpy.float64)
+    R *= ca
+    R += numpy.outer(u, u) * (1.0 - ca)
+    R += sa * numpy.array(((  0.0,-u[2], u[1]),
+                           ( u[2],  0.0,-u[0]),
+                           (-u[1], u[0],  0.0)), dtype=numpy.float64)
+    M = numpy.identity(4, dtype=numpy.float64)
+    M[0:3,0:3] = R
+    if point is not None:
+        # rotation not around origin
+        M[0:3,3] = point[0:3] - numpy.dot(R, point[0:3])
+    return M
+
+def scaling_matrix(factor, origin=None, direction=None):
+    """Return matrix to scale by factor around origin in direction.
+
+    Point Symmetry: factor = -1.0
+
+    """
+    if origin is None: origin = [0,0,0]
+    o = numpy.array(origin[0:3], dtype=numpy.float64, copy=False)
+    if direction is None:
+        # uniform scaling
+        M = factor*numpy.identity(4, dtype=numpy.float64)
+        M[0:3,3] = (1.0-factor) * o
+        M[3,3] = 1.0
+    else:
+        # nonuniform scaling
+        M = numpy.identity(4, dtype=numpy.float64)
+        u = numpy.array(direction[0:3], dtype=numpy.float64, copy=True)
+        u /= math.sqrt(numpy.dot(u, u)) # unit vector of direction
+        M[0:3,0:3] -= (1.0-factor) * numpy.outer(u, u)
+        M[0:3,3] = ((1.0-factor) * numpy.dot(o, u)) * u
+    return M
+
+def projection_matrix(point, normal, direction=None, perspective=None):
+    """Return matrix to project onto plane defined by point and normal.
+
+    Using either perspective point, projection direction, or none of both.
+
+    """
+    M = numpy.identity(4, dtype=numpy.float64)
+    n = numpy.array(normal[0:3], dtype=numpy.float64, copy=True)
+    n /= math.sqrt(numpy.dot(n,n)) # unit vector of normal
+    if perspective is not None:
+        # perspective projection
+        r = numpy.array(perspective[0:3], dtype=numpy.float64)
+        M[0:3,0:3] *= numpy.dot((r-point), n)
+        M[0:3,0:3] -= numpy.outer(n, r)
+        M[0:3,3] = numpy.dot(point, n) * r
+        M[3,0:3] = -n
+        M[3,3] = numpy.dot(perspective, n)
+    elif direction is not None:
+        # parallel projection
+        w = numpy.array(direction[0:3], dtype=numpy.float64)
+        s = 1.0 / numpy.dot(w, n)
+        M[0:3,0:3] -= numpy.outer(n, w) * s
+        M[0:3,3] = w * (numpy.dot(point[0:3], n) * s)
+    else:
+        # orthogonal projection
+        M[0:3,0:3] -= numpy.outer(n, n)
+        M[0:3,3] = numpy.dot(point[0:3], n) * n
+    return M
+
+def orthogonalization_matrix(a=10.0, b=10.0, c=10.0,
+                             alpha=90.0, beta=90.0, gamma=90.0):
+    """Return orthogonalization matrix for crystallographic cell coordinates.
+
+    Angles are in degrees.
+
+    """
+    al = math.radians(alpha)
+    be = math.radians(beta)
+    ga = math.radians(gamma)
+    sia = math.sin(al)
+    sib = math.sin(be)
+    coa = math.cos(al)
+    cob = math.cos(be)
+    co = (coa * cob - math.cos(ga)) / (sia * sib)
+    return numpy.array((
+        (a*sib*math.sqrt(1.0-co*co), 0.0,   0.0, 0.0),
+        (-a*sib*co,                  b*sia, 0.0, 0.0),
+        (a*cob,                      b*coa, c,   0.0),
+        (0.0,                        0.0,   0.0, 1.0)),
+        dtype=numpy.float64)
+
+def superimpose_matrix(v0, v1, compute_rmsd=False):
+    """Return matrix to transform given vector set into second vector set.
+
+    Minimize weighted sum of squared deviations according to W. Kabsch.
+    v0 and v1 are shape (*,3) or (*,4) arrays of at least 3 vectors.
+
+    """
+    v0 = numpy.array(v0, dtype=numpy.float64)
+    v1 = numpy.array(v1, dtype=numpy.float64)
+
+    assert v0.ndim==2 and v0.ndim==v1.ndim and \
+           v0.shape[0]>2 and v0.shape[1] in (3,4)
+
+    # vectors might be homogeneous coordinates
+    if v0.shape[1] == 4:
+        v0 = v0[:,0:3]
+        v1 = v1[:,0:3]
+
+    # move centroids to origin
+    t0 = numpy.mean(v0, axis=0)
+    t1 = numpy.mean(v1, axis=0)
+    v0 = v0 - t0
+    v1 = v1 - t1
+
+    # Singular Value Decomposition of covariance matrix
+    u, s, vh = numpy.linalg.svd(numpy.dot(v1.T, v0))
+
+    # rotation matrix from SVD orthonormal bases
+    R = numpy.dot(u, vh)
+    if numpy.linalg.det(R) < 0.0:
+        # R does not constitute right handed system
+        rc = vh[2,:] * 2.0
+        R -= numpy.vstack((u[0,2]*rc, u[1,2]*rc, u[2,2]*rc))
+        s[-1] *= -1.0
+
+    # homogeneous transformation matrix
+    M = numpy.identity(4, dtype=numpy.float64)
+    T = numpy.identity(4, dtype=numpy.float64)
+    M[0:3,0:3] = R
+    T[0:3,3] = t1
+    M = numpy.dot(T, M)
+    T[0:3,3] = -t0
+    M = numpy.dot(M, T)
+
+    # compute root mean square error from SVD sigma
+    if compute_rmsd:
+        r = numpy.cumsum(v0*v0) + numpy.cumsum(v1*v1)
+        rmsd = numpy.sqrt(abs(r - (2.0 * sum(s)) / len(v0)))
+        return M, rmsd
+    else:
+        return M
+
+def rotation_matrix_from_euler(ai, aj, ak, axes):
+    """Return homogeneous rotation matrix from Euler angles and axis sequence.
+
+    ai, aj, ak -- Euler's roll, pitch and yaw angles
+    axes -- One of 24 axis sequences as string or encoded tuple
+
+    """
+    try:
+        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
+    except (AttributeError, KeyError):
+        _TUPLE2AXES[axes]
+        firstaxis, parity, repetition, frame = axes
+
+    i = firstaxis
+    j = _NEXT_AXIS[i+parity]
+    k = _NEXT_AXIS[i-parity+1]
+
+    if frame: ai, ak = ak, ai
+    if parity: ai, aj, ak = -ai, -aj, -ak
+
+    si, sj, sh = math.sin(ai), math.sin(aj), math.sin(ak)
+    ci, cj, ch = math.cos(ai), math.cos(aj), math.cos(ak)
+    cc, cs = ci*ch, ci*sh
+    sc, ss = si*ch, si*sh
+
+    M = numpy.identity(4, dtype=numpy.float64)
+    if repetition:
+        M[i,i] =  cj;     M[i,j] =  sj*si;    M[i,k] =  sj*ci
+        M[j,i] =  sj*sh;  M[j,j] = -cj*ss+cc; M[j,k] = -cj*cs-sc
+        M[k,i] = -sj*ch;  M[k,j] =  cj*sc+cs; M[k,k] =  cj*cc-ss
+    else:
+        M[i,i] =  cj*ch;  M[i,j] =  sj*sc-cs; M[i,k] =  sj*cc+ss
+        M[j,i] =  cj*sh;  M[j,j] =  sj*ss+cc; M[j,k] =  sj*cs-sc
+        M[k,i] = -sj;     M[k,j] =  cj*si;    M[k,k] =  cj*ci
+    return M
+
+def euler_from_rotation_matrix(matrix, axes='sxyz'):
+    """Return Euler angles from rotation matrix for specified axis sequence.
+
+    matrix -- 3x3 or 4x4 rotation matrix
+    axes -- One of 24 axis sequences as string or encoded tuple
+
+    Note that many Euler angle triplets can describe one matrix.
+
+    """
+    try:
+        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
+    except (AttributeError, KeyError):
+        _TUPLE2AXES[axes]
+        firstaxis, parity, repetition, frame = axes
+
+    i = firstaxis
+    j = _NEXT_AXIS[i+parity]
+    k = _NEXT_AXIS[i-parity+1]
+
+    M = numpy.array(matrix, dtype=numpy.float64)[0:3, 0:3]
+    if repetition:
+        sy = math.sqrt(M[i,j]*M[i,j] + M[i,k]*M[i,k])
+        if sy > _EPS:
+            ax = math.atan2( M[i,j],  M[i,k])
+            ay = math.atan2( sy,      M[i,i])
+            az = math.atan2( M[j,i], -M[k,i])
+        else:
+            ax = math.atan2(-M[j,k],  M[j,j])
+            ay = math.atan2( sy,      M[i,i])
+            az = 0.0
+    else:
+        cy = math.sqrt(M[i,i]*M[i,i] + M[j,i]*M[j,i])
+        if cy > _EPS:
+            ax = math.atan2( M[k,j],  M[k,k])
+            ay = math.atan2(-M[k,i],  cy)
+            az = math.atan2( M[j,i],  M[i,i])
+        else:
+            ax = math.atan2(-M[j,k],  M[j,j])
+            ay = math.atan2(-M[k,i],  cy)
+            az = 0.0
+
+    if parity: ax, ay, az = -ax, -ay, -az
+    if frame: ax, az = az, ax
+    return ax, ay, az
+
+def quaternion_from_euler(ai, aj, ak, axes):
+    """Return quaternion from Euler angles and axis sequence.
+
+    ai, aj, ak -- Euler's roll, pitch and yaw angles
+    axes -- One of 24 axis sequences as string or encoded tuple
+
+    """
+    try:
+        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
+    except (AttributeError, KeyError):
+        _TUPLE2AXES[axes]
+        firstaxis, parity, repetition, frame = axes
+
+    i = firstaxis
+    j = _NEXT_AXIS[i+parity]
+    k = _NEXT_AXIS[i-parity+1]
+
+    if frame: ai, ak = ak, ai
+    if parity: aj = -aj
+
+    ti = ai*0.5
+    tj = aj*0.5
+    tk = ak*0.5
+    ci = math.cos(ti)
+    si = math.sin(ti)
+    cj = math.cos(tj)
+    sj = math.sin(tj)
+    ck = math.cos(tk)
+    sk = math.sin(tk)
+    cc = ci*ck
+    cs = ci*sk
+    sc = si*ck
+    ss = si*sk
+
+    q = numpy.empty((4,), dtype=numpy.float64)
+    if repetition:
+        q[i] = cj*(cs + sc)
+        q[j] = sj*(cc + ss)
+        q[k] = sj*(cs - sc)
+        q[3] = cj*(cc - ss)
+    else:
+        q[i] = cj*sc - sj*cs
+        q[j] = cj*ss + sj*cc
+        q[k] = cj*cs - sj*sc
+        q[3] = cj*cc + sj*ss
+
+    if parity: q[j] *= -1
+    return q
+
+def euler_from_quaternion(quaternion, axes='sxyz'):
+    """Return Euler angles from quaternion for specified axis sequence.
+
+    quaternion -- Sequence of x, y, z, w
+    axes -- One of 24 valid axis sequences as string or encoded tuple
+
+    """
+    return euler_from_rotation_matrix(
+        rotation_matrix_from_quaternion(quaternion), axes)
+
+def quaternion_about_axis(angle, axis):
+    """Return quaternion for rotation about axis."""
+    u = numpy.zeros((4,), dtype=numpy.float64)
+    u[0:3] = axis[0:3]
+    u *= math.sin(angle/2) / math.sqrt(numpy.dot(u, u))
+    u[3] = math.cos(angle/2)
+    return u
+
+def rotation_matrix_from_quaternion(quaternion):
+    """Return homogeneous rotation matrix from quaternion."""
+    q = numpy.array(quaternion, dtype=numpy.float64)[0:4]
+    nq = numpy.dot(q, q)
+    if nq == 0.0:
+        return numpy.identity(4, dtype=numpy.float64)
+    q *= math.sqrt(2.0 / nq)
+    q = numpy.outer(q, q)
+    return numpy.array((
+        (1.0-q[1,1]-q[2,2],     q[0,1]-q[2,3],     q[0,2]+q[1,3], 0.0),
+        (    q[0,1]+q[2,3], 1.0-q[0,0]-q[2,2],     q[1,2]-q[0,3], 0.0),
+        (    q[0,2]-q[1,3],     q[1,2]+q[0,3], 1.0-q[0,0]-q[1,1], 0.0),
+        (              0.0,               0.0,               0.0, 1.0)
+        ), dtype=numpy.float64)
+
+def quaternion_from_rotation_matrix(matrix):
+    """Return quaternion from rotation matrix."""
+    q = numpy.empty((4,), dtype=numpy.float64)
+    M = numpy.array(matrix, dtype=numpy.float64)[0:4,0:4]
+    t = numpy.trace(M)
+    if t > M[3,3]:
+        q[3] = t
+        q[2] = M[1,0] - M[0,1]
+        q[1] = M[0,2] - M[2,0]
+        q[0] = M[2,1] - M[1,2]
+    else:
+        i,j,k = 0,1,2
+        if M[1,1] > M[0,0]:
+            i,j,k = 1,2,0
+        if M[2,2] > M[i,i]:
+            i,j,k = 2,0,1
+        t = M[i,i] - (M[j,j] + M[k,k]) + M[3,3]
+        q[i] = t
+        q[j] = M[i,j] + M[j,i]
+        q[k] = M[k,i] + M[i,k]
+        q[3] = M[k,j] - M[j,k]
+    q *= 0.5 / math.sqrt(t * M[3,3])
+    return q
+
+def quaternion_multiply(q1, q0):
+    """Multiply two quaternions."""
+    x0, y0, z0, w0 = q0
+    x1, y1, z1, w1 = q1
+    return numpy.array((
+         x1*w0 + y1*z0 - z1*y0 + w1*x0,
+        -x1*z0 + y1*w0 + z1*x0 + w1*y0,
+         x1*y0 - y1*x0 + z1*w0 + w1*z0,
+        -x1*x0 - y1*y0 - z1*z0 + w1*w0))
+
+def quaternion_from_sphere_points(v0, v1):
+    """Return quaternion from two points on unit sphere.
+
+    v0 -- E.g. sphere coordinates of cursor at mouse down
+    v1 -- E.g. current sphere coordinates of cursor
+
+    """
+    x, y, z = numpy.cross(v0, v1)
+    return x, y, z, numpy.dot(v0, v1)
+
+def quaternion_to_sphere_points(q):
+    """Return two points on unit sphere from quaternion."""
+    l = math.sqrt(q[0]*q[0] + q[1]*q[1])
+    v0 = numpy.array((0.0, 1.0, 0.0) if l==0.0 else \
+                     (-q[1]/l, q[0]/l, 0.0), dtype=numpy.float64)
+    v1 = numpy.array((q[3]*v0[0] - q[3]*v0[1],
+                      q[3]*v0[1] + q[3]*v0[0],
+                      q[0]*v0[1] - q[1]*v0[0]), dtype=numpy.float64)
+    if q[3] < 0.0:
+        v0 *= -1.0
+    return v0, v1
+
+
+class Arcball(object):
+    """Virtual Trackball Control."""
+
+    def __init__(self, center=None, radius=1.0, initial=None):
+        """Initializes virtual trackball control.
+
+        center -- Window coordinates of trackball center
+        radius -- Radius of trackball in window coordinates
+        initial -- Initial quaternion or rotation matrix
+
+        """
+        self.axis = None
+        self.center = numpy.zeros((3,), dtype=numpy.float64)
+        self.place(center, radius)
+        self.v0 = numpy.array([0., 0., 1.], dtype=numpy.float64)
+        if initial is None:
+            self.q0 = numpy.array([0., 0., 0., 1.], dtype=numpy.float64)
+        else:
+            try:
+                self.q0 = quaternion_from_rotation_matrix(initial)
+            except:
+                self.q0 = initial
+        self.qnow = self.q0
+
+    def place(self, center=[0., 0.], radius=1.0):
+        """Place Arcball, e.g. when window size changes."""
+        self.radius = float(radius)
+        self.center[0:2] = center[0:2]
+
+    def click(self, position, axis=None):
+        """Set axis constraint and initial window coordinates of cursor."""
+        self.axis = axis
+        self.q0 = self.qnow
+        self.v0 = self._map_to_sphere(position, self.center, self.radius)
+
+    def drag(self, position):
+        """Return rotation matrix from updated window coordinates of cursor."""
+        v0 = self.v0
+        v1 = self._map_to_sphere(position, self.center, self.radius)
+
+        if self.axis is not None:
+            v0 = self._constrain_to_axis(v0, self.axis)
+            v1 = self._constrain_to_axis(v1, self.axis)
+
+        t = numpy.cross(v0, v1)
+        if numpy.dot(t, t) < _EPS:
+            # v0 and v1 coincide. no additional rotation
+            self.qnow = self.q0
+        else:
+            q1 = [t[0], t[1], t[2], numpy.dot(v0, v1)]
+            self.qnow = quaternion_multiply(q1, self.q0)
+
+        return rotation_matrix_from_quaternion(self.qnow)
+
+    def _map_to_sphere(self, position, center, radius):
+        """Map window coordinates to unit sphere coordinates."""
+        v = numpy.array([position[0], position[1], 0.0], dtype=numpy.float64)
+        v -= center
+        v /= radius
+        v[1] *= -1
+        l = numpy.dot(v, v)
+        if l > 1.0:
+            v /= math.sqrt(l) # position outside of sphere
+        else:
+            v[2] = math.sqrt(1.0 - l)
+        return v
+
+    def _constrain_to_axis(self, point, axis):
+        """Return sphere point perpendicular to axis."""
+        v = numpy.array(point, dtype=numpy.float64)
+        a = numpy.array(axis, dtype=numpy.float64, copy=True)
+        a /= numpy.dot(a, v)
+        v -= a # on plane
+        n = numpy.dot(v, v)
+        if n > 0.0:
+            v /= math.sqrt(n)
+            return v
+        if a[2] == 1.0:
+            return numpy.array([1.0, 0.0, 0.0], dtype=numpy.float64)
+        v[:] = -a[1], a[0], 0.0
+        v /= math.sqrt(numpy.dot(v, v))
+        return v
+
+    def _nearest_axis(self, point, *axes):
+        """Return axis, which arc is nearest to point."""
+        nearest = None
+        max = -1.0
+        for axis in axes:
+            t = numpy.dot(self._constrain_to_axis(point, axis), point)
+            if t > max:
+                nearest = axis
+                max = d
+        return nearest
+
+# axis sequences for Euler angles
+_NEXT_AXIS = [1, 2, 0, 1]
+_AXES2TUPLE = { # axes string -> (inner axis, parity, repetition, frame)
+    "sxyz": (0, 0, 0, 0), "sxyx": (0, 0, 1, 0), "sxzy": (0, 1, 0, 0),
+    "sxzx": (0, 1, 1, 0), "syzx": (1, 0, 0, 0), "syzy": (1, 0, 1, 0),
+    "syxz": (1, 1, 0, 0), "syxy": (1, 1, 1, 0), "szxy": (2, 0, 0, 0),
+    "szxz": (2, 0, 1, 0), "szyx": (2, 1, 0, 0), "szyz": (2, 1, 1, 0),
+    "rzyx": (0, 0, 0, 1), "rxyx": (0, 0, 1, 1), "ryzx": (0, 1, 0, 1),
+    "rxzx": (0, 1, 1, 1), "rxzy": (1, 0, 0, 1), "ryzy": (1, 0, 1, 1),
+    "rzxy": (1, 1, 0, 1), "ryxy": (1, 1, 1, 1), "ryxz": (2, 0, 0, 1),
+    "rzxz": (2, 0, 1, 1), "rxyz": (2, 1, 0, 1), "rzyz": (2, 1, 1, 1)}
+_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
+
+try:
+    # import faster functions from C extension module
+    import _vlfdlib
+    rotation_matrix_from_quaternion = _vlfdlib.quaternion_to_matrix
+    quaternion_from_rotation_matrix = _vlfdlib.quaternion_from_matrix
+    quaternion_multiply = _vlfdlib.quaternion_multiply
+except ImportError:
+    pass
+
+def test_transformations_module(*args, **kwargs):
+    """Test transformation module."""
+    v = numpy.array([6.67,3.69,4.82], dtype=numpy.float64)
+    I = numpy.identity(4, dtype=numpy.float64)
+    T = translation_matrix(-v)
+    M = mirror_matrix([0,0,0], v)
+    R = rotation_matrix(math.pi, [1,0,0], point=v)
+    P = projection_matrix(-v, v, perspective=10*v)
+    P = projection_matrix(-v, v, direction=v)
+    P = projection_matrix(-v, v)
+    S = scaling_matrix(0.25, v, direction=None) # reduce size
+    S = scaling_matrix(-1.0) # point symmetry
+    S = scaling_matrix(10.0)
+    O = orthogonalization_matrix(10.0, 10.0, 10.0, 90.0, 90.0, 90.0)
+    assert numpy.allclose(S, O)
+
+    angles = (1, -2, 3)
+    for axes in sorted(_AXES2TUPLE.keys()):
+        assert axes == _TUPLE2AXES[_AXES2TUPLE[axes]]
+        Me = rotation_matrix_from_euler(axes=axes, *angles)
+        em = euler_from_rotation_matrix(Me, axes)
+        Mf = rotation_matrix_from_euler(axes=axes, *em)
+        assert numpy.allclose(Me, Mf)
+
+    axes = 'sxyz'
+    euler = (0.0, 0.2, 0.0)
+    qe = quaternion_from_euler(axes=axes, *euler)
+    Mr = rotation_matrix(0.2, [0,1,0])
+    Me = rotation_matrix_from_euler(axes=axes, *euler)
+    Mq = rotation_matrix_from_quaternion(qe)
+    assert numpy.allclose(Mr, Me)
+    assert numpy.allclose(Mr, Mq)
+    em = euler_from_rotation_matrix(Mr, axes)
+    eq = euler_from_quaternion(qe, axes)
+    assert numpy.allclose(em, eq)
+    qm = quaternion_from_rotation_matrix(Mq)
+    assert numpy.allclose(qe, qm)
+
+    assert numpy.allclose(
+        rotation_matrix_from_quaternion(
+            quaternion_about_axis(1.0, [1,-0.5,1])),
+        rotation_matrix(1.0, [1,-0.5,1]))
+
+    ball = Arcball([320,320], 320)
+    ball.click([500,250])
+    R = ball.drag([475, 275])
+    assert numpy.allclose(R, [[ 0.9787566,  0.03798976, -0.20147528, 0.],
+                              [-0.06854143, 0.98677273, -0.14690692, 0.],
+                              [ 0.19322935, 0.15759552,  0.9684142,  0.],
+                              [ 0.,         0.,          0.,         1.]])
+
+if __name__ == "__main__":
+    test_transformations_module()
+
+
+print "Now my own tests"
+
+rot_mat=numpy.zeros((3,3))
+
+
+rot_mat[0,0]=math.sqrt(2.)/2.
+rot_mat[0,1]=0.
+rot_mat[0,1]=-math.sqrt(2.)/2.
+
+rot_mat[1,0]=0.
+rot_mat[1,1]=1.
+rot_mat[1,2]=0.
+
+
+rot_mat[2,0]=math.sqrt(2.)/2.
+rot_mat[2,1]=0.
+rot_mat[2,2]=math.sqrt(2.)/2.
+
+print "rot_mat is, ", rot_mat
+
+
+my_angles=euler_from_rotation_matrix(rot_mat, (0,0,0,0))
+
+print "my_angles are,  ", my_angles
+
+print "Pi/4 is, ", numpy.pi/4.
+
+print "So far so good"