• R/O
  • SSH

Commit

Tags
Keine Tags

Frequently used words (click to add to your profile)

javac++androidlinuxc#windowsobjective-ccocoa誰得qtpythonphprubygameguibathyscaphec計画中(planning stage)翻訳omegatframeworktwitterdomtestvb.netdirectxゲームエンジンbtronarduinopreviewer

Commit MetaInfo

Revisiona868d649aa24e4cda858267ae8c355a19b37cfc9 (tree)
Zeit2008-09-09 23:53:03
Autoriselllo
Commiteriselllo

Log Message

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.

Ändern Zusammenfassung

Diff

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 @@
1+#! /usr/bin/env python
2+
3+
4+# Have a look at http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
5+
6+
7+
8+# -*- coding: utf-8 -*-
9+# transformations.py
10+
11+# Copyright (c) 2006-2007, Christoph Gohlke
12+# Copyright (c) 2006-2007, The Regents of the University of California
13+# All rights reserved.
14+#
15+# Redistribution and use in source and binary forms, with or without
16+# modification, are permitted provided that the following conditions are met:
17+#
18+# * Redistributions of source code must retain the above copyright
19+# notice, this list of conditions and the following disclaimer.
20+# * Redistributions in binary form must reproduce the above copyright
21+# notice, this list of conditions and the following disclaimer in the
22+# documentation and/or other materials provided with the distribution.
23+# * Neither the name of the copyright holders nor the names of any
24+# contributors may be used to endorse or promote products derived
25+# from this software without specific prior written permission.
26+#
27+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
28+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
29+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
30+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
31+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
32+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
33+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
34+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
35+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
36+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
37+# POSSIBILITY OF SUCH DAMAGE.
38+
39+"""Homogeneous Transformation Matrices and Quaternions.
40+
41+Functions for calculating 4x4 matrices for translating, rotating, mirroring,
42+scaling, projecting, orthogonalization, and superimposition of arrays of
43+homogenous coordinates as well as for converting between rotation matrices,
44+Euler angles, and quaternions.
45+
46+Matrices can be inverted using numpy.linalg.inv(M), concatenated using
47+numpy.dot(M0,M1), or used to transform homogeneous points using
48+numpy.dot(v, M) for shape (4,*) "point of arrays", respectively
49+numpy.dot(v, M.T) for shape (*,4) "array of points".
50+
51+Quaternions ix+jy+kz+w are represented as [x, y, z, w].
52+
53+Angles are in radians unless specified otherwise.
54+
55+The 24 ways of specifying rotations for a given triple of Euler angles,
56+can be represented by a 4 character string or encoded 4-tuple:
57+
58+ Axes 4-string:
59+ first character -- rotations are applied to 's'tatic or 'r'otating frame
60+ remaining characters -- successive rotation axis 'x', 'y', or 'z'
61+ Axes 4-tuple:
62+ inner axis -- code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix.
63+ parity -- even (0) if inner axis 'x' is followed by 'y', 'y' is followed
64+ by 'z', or 'z' is followed by 'x'. Otherwise odd (1).
65+ repetition -- first and last axis are same (1) or different (0).
66+ frame -- rotations are applied to static (0) or rotating (1) frame.
67+ Examples of tuple codes:
68+ 'sxyz' <-> (0, 0, 0, 0)
69+ 'ryxy' <-> (1, 1, 1, 1)
70+
71+Author:
72+ Christoph Gohlke, http://www.lfd.uci.edu/~gohlke/
73+ Laboratory for Fluorescence Dynamics, University of California, Irvine
74+
75+Requirements:
76+ Python 2.5 (http://www.python.org)
77+ Numpy 1.0 (http://numpy.scipy.org)
78+
79+References:
80+(1) Matrices and Transformations. Ronald Goldman.
81+ In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
82+(2) Euler Angle Conversion. Ken Shoemake.
83+ In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
84+(3) Arcball Rotation Control. Ken Shoemake.
85+ In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
86+
87+"""
88+
89+from __future__ import division
90+
91+import math
92+import numpy
93+
94+_EPS = numpy.finfo(float).eps * 4.0
95+
96+def concatenate_transforms(*args):
97+ """Merge multiple transformation matrices into one."""
98+ M = numpy.identity(4, dtype=numpy.float64)
99+ for i in args:
100+ M = numpy.dot(M, i)
101+ return M
102+
103+def translation_matrix(direction):
104+ """Return matrix to translate by direction vector."""
105+ M = numpy.identity(4, dtype=numpy.float64)
106+ M[0:3,3] = direction[0:3]
107+ return M
108+
109+def mirror_matrix(point, normal):
110+ """Return matrix to mirror at plane defined by point and normal vector."""
111+ M = numpy.identity(4, dtype=numpy.float64)
112+ n = numpy.array(normal[0:3], dtype=numpy.float64, copy=True)
113+ n /= math.sqrt(numpy.dot(n,n))
114+ M[0:3,0:3] -= 2.0 * numpy.outer(n, n)
115+ M[0:3,3] = 2.0 * numpy.dot(point[0:3], n) * n
116+ return M
117+
118+def rotation_matrix(angle, direction, point=None):
119+ """Return matrix to rotate about axis defined by point and direction."""
120+ sa, ca = math.sin(angle), math.cos(angle)
121+ # unit vector of direction
122+ u = numpy.array(direction[0:3], dtype=numpy.float64, copy=True)
123+ u /= math.sqrt(numpy.dot(u, u))
124+ # rotation matrix around unit vector
125+ R = numpy.identity(3, dtype=numpy.float64)
126+ R *= ca
127+ R += numpy.outer(u, u) * (1.0 - ca)
128+ R += sa * numpy.array((( 0.0,-u[2], u[1]),
129+ ( u[2], 0.0,-u[0]),
130+ (-u[1], u[0], 0.0)), dtype=numpy.float64)
131+ M = numpy.identity(4, dtype=numpy.float64)
132+ M[0:3,0:3] = R
133+ if point is not None:
134+ # rotation not around origin
135+ M[0:3,3] = point[0:3] - numpy.dot(R, point[0:3])
136+ return M
137+
138+def scaling_matrix(factor, origin=None, direction=None):
139+ """Return matrix to scale by factor around origin in direction.
140+
141+ Point Symmetry: factor = -1.0
142+
143+ """
144+ if origin is None: origin = [0,0,0]
145+ o = numpy.array(origin[0:3], dtype=numpy.float64, copy=False)
146+ if direction is None:
147+ # uniform scaling
148+ M = factor*numpy.identity(4, dtype=numpy.float64)
149+ M[0:3,3] = (1.0-factor) * o
150+ M[3,3] = 1.0
151+ else:
152+ # nonuniform scaling
153+ M = numpy.identity(4, dtype=numpy.float64)
154+ u = numpy.array(direction[0:3], dtype=numpy.float64, copy=True)
155+ u /= math.sqrt(numpy.dot(u, u)) # unit vector of direction
156+ M[0:3,0:3] -= (1.0-factor) * numpy.outer(u, u)
157+ M[0:3,3] = ((1.0-factor) * numpy.dot(o, u)) * u
158+ return M
159+
160+def projection_matrix(point, normal, direction=None, perspective=None):
161+ """Return matrix to project onto plane defined by point and normal.
162+
163+ Using either perspective point, projection direction, or none of both.
164+
165+ """
166+ M = numpy.identity(4, dtype=numpy.float64)
167+ n = numpy.array(normal[0:3], dtype=numpy.float64, copy=True)
168+ n /= math.sqrt(numpy.dot(n,n)) # unit vector of normal
169+ if perspective is not None:
170+ # perspective projection
171+ r = numpy.array(perspective[0:3], dtype=numpy.float64)
172+ M[0:3,0:3] *= numpy.dot((r-point), n)
173+ M[0:3,0:3] -= numpy.outer(n, r)
174+ M[0:3,3] = numpy.dot(point, n) * r
175+ M[3,0:3] = -n
176+ M[3,3] = numpy.dot(perspective, n)
177+ elif direction is not None:
178+ # parallel projection
179+ w = numpy.array(direction[0:3], dtype=numpy.float64)
180+ s = 1.0 / numpy.dot(w, n)
181+ M[0:3,0:3] -= numpy.outer(n, w) * s
182+ M[0:3,3] = w * (numpy.dot(point[0:3], n) * s)
183+ else:
184+ # orthogonal projection
185+ M[0:3,0:3] -= numpy.outer(n, n)
186+ M[0:3,3] = numpy.dot(point[0:3], n) * n
187+ return M
188+
189+def orthogonalization_matrix(a=10.0, b=10.0, c=10.0,
190+ alpha=90.0, beta=90.0, gamma=90.0):
191+ """Return orthogonalization matrix for crystallographic cell coordinates.
192+
193+ Angles are in degrees.
194+
195+ """
196+ al = math.radians(alpha)
197+ be = math.radians(beta)
198+ ga = math.radians(gamma)
199+ sia = math.sin(al)
200+ sib = math.sin(be)
201+ coa = math.cos(al)
202+ cob = math.cos(be)
203+ co = (coa * cob - math.cos(ga)) / (sia * sib)
204+ return numpy.array((
205+ (a*sib*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0),
206+ (-a*sib*co, b*sia, 0.0, 0.0),
207+ (a*cob, b*coa, c, 0.0),
208+ (0.0, 0.0, 0.0, 1.0)),
209+ dtype=numpy.float64)
210+
211+def superimpose_matrix(v0, v1, compute_rmsd=False):
212+ """Return matrix to transform given vector set into second vector set.
213+
214+ Minimize weighted sum of squared deviations according to W. Kabsch.
215+ v0 and v1 are shape (*,3) or (*,4) arrays of at least 3 vectors.
216+
217+ """
218+ v0 = numpy.array(v0, dtype=numpy.float64)
219+ v1 = numpy.array(v1, dtype=numpy.float64)
220+
221+ assert v0.ndim==2 and v0.ndim==v1.ndim and \
222+ v0.shape[0]>2 and v0.shape[1] in (3,4)
223+
224+ # vectors might be homogeneous coordinates
225+ if v0.shape[1] == 4:
226+ v0 = v0[:,0:3]
227+ v1 = v1[:,0:3]
228+
229+ # move centroids to origin
230+ t0 = numpy.mean(v0, axis=0)
231+ t1 = numpy.mean(v1, axis=0)
232+ v0 = v0 - t0
233+ v1 = v1 - t1
234+
235+ # Singular Value Decomposition of covariance matrix
236+ u, s, vh = numpy.linalg.svd(numpy.dot(v1.T, v0))
237+
238+ # rotation matrix from SVD orthonormal bases
239+ R = numpy.dot(u, vh)
240+ if numpy.linalg.det(R) < 0.0:
241+ # R does not constitute right handed system
242+ rc = vh[2,:] * 2.0
243+ R -= numpy.vstack((u[0,2]*rc, u[1,2]*rc, u[2,2]*rc))
244+ s[-1] *= -1.0
245+
246+ # homogeneous transformation matrix
247+ M = numpy.identity(4, dtype=numpy.float64)
248+ T = numpy.identity(4, dtype=numpy.float64)
249+ M[0:3,0:3] = R
250+ T[0:3,3] = t1
251+ M = numpy.dot(T, M)
252+ T[0:3,3] = -t0
253+ M = numpy.dot(M, T)
254+
255+ # compute root mean square error from SVD sigma
256+ if compute_rmsd:
257+ r = numpy.cumsum(v0*v0) + numpy.cumsum(v1*v1)
258+ rmsd = numpy.sqrt(abs(r - (2.0 * sum(s)) / len(v0)))
259+ return M, rmsd
260+ else:
261+ return M
262+
263+def rotation_matrix_from_euler(ai, aj, ak, axes):
264+ """Return homogeneous rotation matrix from Euler angles and axis sequence.
265+
266+ ai, aj, ak -- Euler's roll, pitch and yaw angles
267+ axes -- One of 24 axis sequences as string or encoded tuple
268+
269+ """
270+ try:
271+ firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
272+ except (AttributeError, KeyError):
273+ _TUPLE2AXES[axes]
274+ firstaxis, parity, repetition, frame = axes
275+
276+ i = firstaxis
277+ j = _NEXT_AXIS[i+parity]
278+ k = _NEXT_AXIS[i-parity+1]
279+
280+ if frame: ai, ak = ak, ai
281+ if parity: ai, aj, ak = -ai, -aj, -ak
282+
283+ si, sj, sh = math.sin(ai), math.sin(aj), math.sin(ak)
284+ ci, cj, ch = math.cos(ai), math.cos(aj), math.cos(ak)
285+ cc, cs = ci*ch, ci*sh
286+ sc, ss = si*ch, si*sh
287+
288+ M = numpy.identity(4, dtype=numpy.float64)
289+ if repetition:
290+ M[i,i] = cj; M[i,j] = sj*si; M[i,k] = sj*ci
291+ M[j,i] = sj*sh; M[j,j] = -cj*ss+cc; M[j,k] = -cj*cs-sc
292+ M[k,i] = -sj*ch; M[k,j] = cj*sc+cs; M[k,k] = cj*cc-ss
293+ else:
294+ M[i,i] = cj*ch; M[i,j] = sj*sc-cs; M[i,k] = sj*cc+ss
295+ M[j,i] = cj*sh; M[j,j] = sj*ss+cc; M[j,k] = sj*cs-sc
296+ M[k,i] = -sj; M[k,j] = cj*si; M[k,k] = cj*ci
297+ return M
298+
299+def euler_from_rotation_matrix(matrix, axes='sxyz'):
300+ """Return Euler angles from rotation matrix for specified axis sequence.
301+
302+ matrix -- 3x3 or 4x4 rotation matrix
303+ axes -- One of 24 axis sequences as string or encoded tuple
304+
305+ Note that many Euler angle triplets can describe one matrix.
306+
307+ """
308+ try:
309+ firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
310+ except (AttributeError, KeyError):
311+ _TUPLE2AXES[axes]
312+ firstaxis, parity, repetition, frame = axes
313+
314+ i = firstaxis
315+ j = _NEXT_AXIS[i+parity]
316+ k = _NEXT_AXIS[i-parity+1]
317+
318+ M = numpy.array(matrix, dtype=numpy.float64)[0:3, 0:3]
319+ if repetition:
320+ sy = math.sqrt(M[i,j]*M[i,j] + M[i,k]*M[i,k])
321+ if sy > _EPS:
322+ ax = math.atan2( M[i,j], M[i,k])
323+ ay = math.atan2( sy, M[i,i])
324+ az = math.atan2( M[j,i], -M[k,i])
325+ else:
326+ ax = math.atan2(-M[j,k], M[j,j])
327+ ay = math.atan2( sy, M[i,i])
328+ az = 0.0
329+ else:
330+ cy = math.sqrt(M[i,i]*M[i,i] + M[j,i]*M[j,i])
331+ if cy > _EPS:
332+ ax = math.atan2( M[k,j], M[k,k])
333+ ay = math.atan2(-M[k,i], cy)
334+ az = math.atan2( M[j,i], M[i,i])
335+ else:
336+ ax = math.atan2(-M[j,k], M[j,j])
337+ ay = math.atan2(-M[k,i], cy)
338+ az = 0.0
339+
340+ if parity: ax, ay, az = -ax, -ay, -az
341+ if frame: ax, az = az, ax
342+ return ax, ay, az
343+
344+def quaternion_from_euler(ai, aj, ak, axes):
345+ """Return quaternion from Euler angles and axis sequence.
346+
347+ ai, aj, ak -- Euler's roll, pitch and yaw angles
348+ axes -- One of 24 axis sequences as string or encoded tuple
349+
350+ """
351+ try:
352+ firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
353+ except (AttributeError, KeyError):
354+ _TUPLE2AXES[axes]
355+ firstaxis, parity, repetition, frame = axes
356+
357+ i = firstaxis
358+ j = _NEXT_AXIS[i+parity]
359+ k = _NEXT_AXIS[i-parity+1]
360+
361+ if frame: ai, ak = ak, ai
362+ if parity: aj = -aj
363+
364+ ti = ai*0.5
365+ tj = aj*0.5
366+ tk = ak*0.5
367+ ci = math.cos(ti)
368+ si = math.sin(ti)
369+ cj = math.cos(tj)
370+ sj = math.sin(tj)
371+ ck = math.cos(tk)
372+ sk = math.sin(tk)
373+ cc = ci*ck
374+ cs = ci*sk
375+ sc = si*ck
376+ ss = si*sk
377+
378+ q = numpy.empty((4,), dtype=numpy.float64)
379+ if repetition:
380+ q[i] = cj*(cs + sc)
381+ q[j] = sj*(cc + ss)
382+ q[k] = sj*(cs - sc)
383+ q[3] = cj*(cc - ss)
384+ else:
385+ q[i] = cj*sc - sj*cs
386+ q[j] = cj*ss + sj*cc
387+ q[k] = cj*cs - sj*sc
388+ q[3] = cj*cc + sj*ss
389+
390+ if parity: q[j] *= -1
391+ return q
392+
393+def euler_from_quaternion(quaternion, axes='sxyz'):
394+ """Return Euler angles from quaternion for specified axis sequence.
395+
396+ quaternion -- Sequence of x, y, z, w
397+ axes -- One of 24 valid axis sequences as string or encoded tuple
398+
399+ """
400+ return euler_from_rotation_matrix(
401+ rotation_matrix_from_quaternion(quaternion), axes)
402+
403+def quaternion_about_axis(angle, axis):
404+ """Return quaternion for rotation about axis."""
405+ u = numpy.zeros((4,), dtype=numpy.float64)
406+ u[0:3] = axis[0:3]
407+ u *= math.sin(angle/2) / math.sqrt(numpy.dot(u, u))
408+ u[3] = math.cos(angle/2)
409+ return u
410+
411+def rotation_matrix_from_quaternion(quaternion):
412+ """Return homogeneous rotation matrix from quaternion."""
413+ q = numpy.array(quaternion, dtype=numpy.float64)[0:4]
414+ nq = numpy.dot(q, q)
415+ if nq == 0.0:
416+ return numpy.identity(4, dtype=numpy.float64)
417+ q *= math.sqrt(2.0 / nq)
418+ q = numpy.outer(q, q)
419+ return numpy.array((
420+ (1.0-q[1,1]-q[2,2], q[0,1]-q[2,3], q[0,2]+q[1,3], 0.0),
421+ ( q[0,1]+q[2,3], 1.0-q[0,0]-q[2,2], q[1,2]-q[0,3], 0.0),
422+ ( q[0,2]-q[1,3], q[1,2]+q[0,3], 1.0-q[0,0]-q[1,1], 0.0),
423+ ( 0.0, 0.0, 0.0, 1.0)
424+ ), dtype=numpy.float64)
425+
426+def quaternion_from_rotation_matrix(matrix):
427+ """Return quaternion from rotation matrix."""
428+ q = numpy.empty((4,), dtype=numpy.float64)
429+ M = numpy.array(matrix, dtype=numpy.float64)[0:4,0:4]
430+ t = numpy.trace(M)
431+ if t > M[3,3]:
432+ q[3] = t
433+ q[2] = M[1,0] - M[0,1]
434+ q[1] = M[0,2] - M[2,0]
435+ q[0] = M[2,1] - M[1,2]
436+ else:
437+ i,j,k = 0,1,2
438+ if M[1,1] > M[0,0]:
439+ i,j,k = 1,2,0
440+ if M[2,2] > M[i,i]:
441+ i,j,k = 2,0,1
442+ t = M[i,i] - (M[j,j] + M[k,k]) + M[3,3]
443+ q[i] = t
444+ q[j] = M[i,j] + M[j,i]
445+ q[k] = M[k,i] + M[i,k]
446+ q[3] = M[k,j] - M[j,k]
447+ q *= 0.5 / math.sqrt(t * M[3,3])
448+ return q
449+
450+def quaternion_multiply(q1, q0):
451+ """Multiply two quaternions."""
452+ x0, y0, z0, w0 = q0
453+ x1, y1, z1, w1 = q1
454+ return numpy.array((
455+ x1*w0 + y1*z0 - z1*y0 + w1*x0,
456+ -x1*z0 + y1*w0 + z1*x0 + w1*y0,
457+ x1*y0 - y1*x0 + z1*w0 + w1*z0,
458+ -x1*x0 - y1*y0 - z1*z0 + w1*w0))
459+
460+def quaternion_from_sphere_points(v0, v1):
461+ """Return quaternion from two points on unit sphere.
462+
463+ v0 -- E.g. sphere coordinates of cursor at mouse down
464+ v1 -- E.g. current sphere coordinates of cursor
465+
466+ """
467+ x, y, z = numpy.cross(v0, v1)
468+ return x, y, z, numpy.dot(v0, v1)
469+
470+def quaternion_to_sphere_points(q):
471+ """Return two points on unit sphere from quaternion."""
472+ l = math.sqrt(q[0]*q[0] + q[1]*q[1])
473+ v0 = numpy.array((0.0, 1.0, 0.0) if l==0.0 else \
474+ (-q[1]/l, q[0]/l, 0.0), dtype=numpy.float64)
475+ v1 = numpy.array((q[3]*v0[0] - q[3]*v0[1],
476+ q[3]*v0[1] + q[3]*v0[0],
477+ q[0]*v0[1] - q[1]*v0[0]), dtype=numpy.float64)
478+ if q[3] < 0.0:
479+ v0 *= -1.0
480+ return v0, v1
481+
482+
483+class Arcball(object):
484+ """Virtual Trackball Control."""
485+
486+ def __init__(self, center=None, radius=1.0, initial=None):
487+ """Initializes virtual trackball control.
488+
489+ center -- Window coordinates of trackball center
490+ radius -- Radius of trackball in window coordinates
491+ initial -- Initial quaternion or rotation matrix
492+
493+ """
494+ self.axis = None
495+ self.center = numpy.zeros((3,), dtype=numpy.float64)
496+ self.place(center, radius)
497+ self.v0 = numpy.array([0., 0., 1.], dtype=numpy.float64)
498+ if initial is None:
499+ self.q0 = numpy.array([0., 0., 0., 1.], dtype=numpy.float64)
500+ else:
501+ try:
502+ self.q0 = quaternion_from_rotation_matrix(initial)
503+ except:
504+ self.q0 = initial
505+ self.qnow = self.q0
506+
507+ def place(self, center=[0., 0.], radius=1.0):
508+ """Place Arcball, e.g. when window size changes."""
509+ self.radius = float(radius)
510+ self.center[0:2] = center[0:2]
511+
512+ def click(self, position, axis=None):
513+ """Set axis constraint and initial window coordinates of cursor."""
514+ self.axis = axis
515+ self.q0 = self.qnow
516+ self.v0 = self._map_to_sphere(position, self.center, self.radius)
517+
518+ def drag(self, position):
519+ """Return rotation matrix from updated window coordinates of cursor."""
520+ v0 = self.v0
521+ v1 = self._map_to_sphere(position, self.center, self.radius)
522+
523+ if self.axis is not None:
524+ v0 = self._constrain_to_axis(v0, self.axis)
525+ v1 = self._constrain_to_axis(v1, self.axis)
526+
527+ t = numpy.cross(v0, v1)
528+ if numpy.dot(t, t) < _EPS:
529+ # v0 and v1 coincide. no additional rotation
530+ self.qnow = self.q0
531+ else:
532+ q1 = [t[0], t[1], t[2], numpy.dot(v0, v1)]
533+ self.qnow = quaternion_multiply(q1, self.q0)
534+
535+ return rotation_matrix_from_quaternion(self.qnow)
536+
537+ def _map_to_sphere(self, position, center, radius):
538+ """Map window coordinates to unit sphere coordinates."""
539+ v = numpy.array([position[0], position[1], 0.0], dtype=numpy.float64)
540+ v -= center
541+ v /= radius
542+ v[1] *= -1
543+ l = numpy.dot(v, v)
544+ if l > 1.0:
545+ v /= math.sqrt(l) # position outside of sphere
546+ else:
547+ v[2] = math.sqrt(1.0 - l)
548+ return v
549+
550+ def _constrain_to_axis(self, point, axis):
551+ """Return sphere point perpendicular to axis."""
552+ v = numpy.array(point, dtype=numpy.float64)
553+ a = numpy.array(axis, dtype=numpy.float64, copy=True)
554+ a /= numpy.dot(a, v)
555+ v -= a # on plane
556+ n = numpy.dot(v, v)
557+ if n > 0.0:
558+ v /= math.sqrt(n)
559+ return v
560+ if a[2] == 1.0:
561+ return numpy.array([1.0, 0.0, 0.0], dtype=numpy.float64)
562+ v[:] = -a[1], a[0], 0.0
563+ v /= math.sqrt(numpy.dot(v, v))
564+ return v
565+
566+ def _nearest_axis(self, point, *axes):
567+ """Return axis, which arc is nearest to point."""
568+ nearest = None
569+ max = -1.0
570+ for axis in axes:
571+ t = numpy.dot(self._constrain_to_axis(point, axis), point)
572+ if t > max:
573+ nearest = axis
574+ max = d
575+ return nearest
576+
577+# axis sequences for Euler angles
578+_NEXT_AXIS = [1, 2, 0, 1]
579+_AXES2TUPLE = { # axes string -> (inner axis, parity, repetition, frame)
580+ "sxyz": (0, 0, 0, 0), "sxyx": (0, 0, 1, 0), "sxzy": (0, 1, 0, 0),
581+ "sxzx": (0, 1, 1, 0), "syzx": (1, 0, 0, 0), "syzy": (1, 0, 1, 0),
582+ "syxz": (1, 1, 0, 0), "syxy": (1, 1, 1, 0), "szxy": (2, 0, 0, 0),
583+ "szxz": (2, 0, 1, 0), "szyx": (2, 1, 0, 0), "szyz": (2, 1, 1, 0),
584+ "rzyx": (0, 0, 0, 1), "rxyx": (0, 0, 1, 1), "ryzx": (0, 1, 0, 1),
585+ "rxzx": (0, 1, 1, 1), "rxzy": (1, 0, 0, 1), "ryzy": (1, 0, 1, 1),
586+ "rzxy": (1, 1, 0, 1), "ryxy": (1, 1, 1, 1), "ryxz": (2, 0, 0, 1),
587+ "rzxz": (2, 0, 1, 1), "rxyz": (2, 1, 0, 1), "rzyz": (2, 1, 1, 1)}
588+_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
589+
590+try:
591+ # import faster functions from C extension module
592+ import _vlfdlib
593+ rotation_matrix_from_quaternion = _vlfdlib.quaternion_to_matrix
594+ quaternion_from_rotation_matrix = _vlfdlib.quaternion_from_matrix
595+ quaternion_multiply = _vlfdlib.quaternion_multiply
596+except ImportError:
597+ pass
598+
599+def test_transformations_module(*args, **kwargs):
600+ """Test transformation module."""
601+ v = numpy.array([6.67,3.69,4.82], dtype=numpy.float64)
602+ I = numpy.identity(4, dtype=numpy.float64)
603+ T = translation_matrix(-v)
604+ M = mirror_matrix([0,0,0], v)
605+ R = rotation_matrix(math.pi, [1,0,0], point=v)
606+ P = projection_matrix(-v, v, perspective=10*v)
607+ P = projection_matrix(-v, v, direction=v)
608+ P = projection_matrix(-v, v)
609+ S = scaling_matrix(0.25, v, direction=None) # reduce size
610+ S = scaling_matrix(-1.0) # point symmetry
611+ S = scaling_matrix(10.0)
612+ O = orthogonalization_matrix(10.0, 10.0, 10.0, 90.0, 90.0, 90.0)
613+ assert numpy.allclose(S, O)
614+
615+ angles = (1, -2, 3)
616+ for axes in sorted(_AXES2TUPLE.keys()):
617+ assert axes == _TUPLE2AXES[_AXES2TUPLE[axes]]
618+ Me = rotation_matrix_from_euler(axes=axes, *angles)
619+ em = euler_from_rotation_matrix(Me, axes)
620+ Mf = rotation_matrix_from_euler(axes=axes, *em)
621+ assert numpy.allclose(Me, Mf)
622+
623+ axes = 'sxyz'
624+ euler = (0.0, 0.2, 0.0)
625+ qe = quaternion_from_euler(axes=axes, *euler)
626+ Mr = rotation_matrix(0.2, [0,1,0])
627+ Me = rotation_matrix_from_euler(axes=axes, *euler)
628+ Mq = rotation_matrix_from_quaternion(qe)
629+ assert numpy.allclose(Mr, Me)
630+ assert numpy.allclose(Mr, Mq)
631+ em = euler_from_rotation_matrix(Mr, axes)
632+ eq = euler_from_quaternion(qe, axes)
633+ assert numpy.allclose(em, eq)
634+ qm = quaternion_from_rotation_matrix(Mq)
635+ assert numpy.allclose(qe, qm)
636+
637+ assert numpy.allclose(
638+ rotation_matrix_from_quaternion(
639+ quaternion_about_axis(1.0, [1,-0.5,1])),
640+ rotation_matrix(1.0, [1,-0.5,1]))
641+
642+ ball = Arcball([320,320], 320)
643+ ball.click([500,250])
644+ R = ball.drag([475, 275])
645+ assert numpy.allclose(R, [[ 0.9787566, 0.03798976, -0.20147528, 0.],
646+ [-0.06854143, 0.98677273, -0.14690692, 0.],
647+ [ 0.19322935, 0.15759552, 0.9684142, 0.],
648+ [ 0., 0., 0., 1.]])
649+
650+if __name__ == "__main__":
651+ test_transformations_module()
652+
653+
654+print "Now my own tests"
655+
656+rot_mat=numpy.zeros((3,3))
657+
658+
659+rot_mat[0,0]=math.sqrt(2.)/2.
660+rot_mat[0,1]=0.
661+rot_mat[0,1]=-math.sqrt(2.)/2.
662+
663+rot_mat[1,0]=0.
664+rot_mat[1,1]=1.
665+rot_mat[1,2]=0.
666+
667+
668+rot_mat[2,0]=math.sqrt(2.)/2.
669+rot_mat[2,1]=0.
670+rot_mat[2,2]=math.sqrt(2.)/2.
671+
672+print "rot_mat is, ", rot_mat
673+
674+
675+my_angles=euler_from_rotation_matrix(rot_mat, (0,0,0,0))
676+
677+print "my_angles are, ", my_angles
678+
679+print "Pi/4 is, ", numpy.pi/4.
680+
681+print "So far so good"