root/trunk/RBRapier/RBRapier/Tools/Quaternion.py

Revision 732, 19.3 kB (checked in by sholloway, 5 years ago)

*** empty log message ***

Line 
1 #!/usr/bin/env python
2 ##~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
3 ##~ License
4 ##~
5 ##- The RuneBlade Foundation library is intended to ease some
6 ##- aspects of writing intricate Jabber, XML, and User Interface (wxPython, etc.)
7 ##- applications, while providing the flexibility to modularly change the
8 ##- architecture. Enjoy.
9 ##~
10 ##~ Copyright (C) 2002  TechGame Networks, LLC.
11 ##~
12 ##~ This library is free software; you can redistribute it and/or
13 ##~ modify it under the terms of the BSD style License as found in the
14 ##~ LICENSE file included with this distribution.
15 ##~
16 ##~ TechGame Networks, LLC can be reached at:
17 ##~ 3578 E. Hartsel Drive #211
18 ##~ Colorado Springs, Colorado, USA, 80920
19 ##~
20 ##~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
21
22 """
23 Derived from Quaternions text and sources:
24     Jav Savarovsky, "Game Programming Gems", Section 2.7, (c) 2000
25     Jason Shankel, "Game Programming Gems", Section 2.8, (c) 2000
26     Jason Shankel, "Game Programming Gems", Section 2.9, (c) 2000
27     Stan Melax, "Game Programming Gems", Section 2.10, (c) 2000
28     Donald Hearn & M. Pauline Baker, "Computer Graphics", 2nd ed., (c) 1994
29     David H. Eberly, "3D Game Engine Design", (c) 2001
30     Konrad Hinsen <hinsen@cnrs-orleans.fr> from his Scientific.Geometry.Quaternion package
31     OpenGL.quaternion
32
33 """
34
35 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
36 #~ Imports
37 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
38
39 import math
40 import Numeric
41
42 from Transformations import TransformPrimitive3dh
43 from Vector import DotCross3 as _VectorDotCross3
44
45 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
46 #~ Definitions
47 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
48
49 class Quaternion(TransformPrimitive3dh):
50     """
51     >>> q = Quaternion.fromAngleAxis(30., (0., 0., 1.)); q
52     <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
53     >>> r = Quaternion.fromAngleAxis(60., (0., 1., 0.)); r
54     <Quaternion: 0.866025403784 + 0.0i + 0.5j + 0.0k>
55     >>> s = Quaternion(q + r); s
56     <Quaternion: 1.83195123007 + 0.0i + 0.5j + 0.258819045103k>
57     >>> q[0], q[1], q[2], q[3]
58     (0.96592582628906831, 0.0, 0.0, 0.25881904510252074)
59     >>> r[0], r[1], r[2], r[3]
60     (0.86602540378443871, 0.0, 0.49999999999999994, 0.0)
61
62     Algebraic expressions quaternion OP value
63     >>> q + 2.
64     <Quaternion: 2.96592582629 + 2.0i + 2.0j + 2.2588190451k>
65     >>> q - 2.
66     <Quaternion: -1.03407417371 + -2.0i + -2.0j + -1.7411809549k>
67     >>> q / 2.
68     <Quaternion: 0.482962913145 + 0.0i + 0.0j + 0.129409522551k>
69     >>> q * 2.
70     <Quaternion: 1.93185165258 + 0.0i + 0.0j + 0.517638090205k>
71     >>> 2. + q
72     <Quaternion: 2.96592582629 + 2.0i + 2.0j + 2.2588190451k>
73     >>> 2. - q
74     <Quaternion: 1.03407417371 + 2.0i + 2.0j + 1.7411809549k>
75     >>> 2. * q
76     <Quaternion: 1.93185165258 + 0.0i + 0.0j + 0.517638090205k>
77     >>> 2. / q
78     Traceback (most recent call last):
79     TypeError: unsupported operand type(s) for /: 'float' and 'Quaternion'
80
81     Algebraic expressions quaternion OP quaternion
82     >>> q + r
83     <Quaternion: 1.83195123007 + 0.0i + 0.5j + 0.258819045103k>
84     >>> r + q
85     <Quaternion: 1.83195123007 + 0.0i + 0.5j + 0.258819045103k>
86     >>> q - r
87     <Quaternion: 0.0999004225046 + 0.0i + -0.5j + 0.258819045103k>
88     >>> r - q
89     <Quaternion: -0.0999004225046 + 0.0i + 0.5j + -0.258819045103k>
90     >>> q * r
91     <Quaternion: 0.836516303738 + -0.129409522551i + 0.482962913145j + 0.224143868042k>
92     >>> r * q
93     <Quaternion: 0.836516303738 + 0.129409522551i + 0.482962913145j + 0.224143868042k>
94     >>> r / q
95     Traceback (most recent call last):
96     TypeError: can't divide a Quaternion by a Quaternion
97
98     >>> +q
99     <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
100     >>> -q
101     <Quaternion: -0.965925826289 + 0.0i + 0.0j + -0.258819045103k>
102     >>> abs(q), abs(r)
103     (1.0, 1.0)
104     >>> abs(q + r)
105     1.9165157467330176
106     """
107
108     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
109     #~ Constants / Variables / Etc.
110     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
111
112     NumericType = Numeric.Float
113    
114     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
115     #~ Special
116     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
117
118     def __init__(self, *args):
119         if not args:
120             self._array = Numeric.asarray((1., 0., 0., 0.), self.NumericType)
121         elif isinstance(args[0], Quaternion):
122             self._array = args[0]._array.copy()
123         elif isinstance(args[0], (tuple, list, Numeric.ArrayType)):
124             self._array = Numeric.asarray(args[0], self.NumericType)
125         elif isinstance(args[1], (tuple, list, Numeric.ArrayType)):
126             self._array = Numeric.asarray([args[0]] + list(args[1]), self.NumericType)
127         else:
128             self._array = Numeric.asarray(args[:4], self.NumericType)
129
130     def __repr__(self):
131         return "<Quaternion: %s + %si + %sj + %sk>" % tuple(self._array)
132
133     def __len__(self): return 4
134     def __getitem__(self, key): return self._array[key]
135     def __setitem__(self, key, value): self._array[key] = value
136
137     def __pos__(self):
138         return self
139     def __neg__(self):
140         return self.__class__(-1 * self._array)
141     def __abs__(self):
142         return self.Magnitude()
143
144     def __iadd__(self, other):
145         if isinstance(other, Quaternion): other = other._array
146         self._array += other
147         return self
148     def __add__(self, other):
149         if isinstance(other, Quaternion): other = other._array
150         return self.__class__(self._array + other)
151     def __radd__(self, other):
152         if isinstance(other, Quaternion): other = other._array
153         return self.__class__(other + self._array)
154     def __isub__(self, other):
155         if isinstance(other, Quaternion): other = other._array
156         self._array -= other
157         return self
158     def __sub__(self, other):
159         if isinstance(other, Quaternion): other = other._array
160         return self.__class__(self._array - other)
161     def __rsub__(self, other):
162         if isinstance(other, Quaternion): other = other._array
163         return self.__class__(other - self._array)
164        
165     def __idiv__(self, other):
166         if isinstance(other, Quaternion):
167             raise TypeError, "can't divide a Quaternion by a Quaternion"
168         else: self._array /= other
169         return self
170     def __div__(self, other):
171         if isinstance(other, Quaternion):
172             raise TypeError, "can't divide a Quaternion by a Quaternion"
173         else: return self.__class__(self._array / other)
174
175     def __imul__(self, other):
176         if isinstance(other, Quaternion):
177             self._array = self._QuaternionMultiply(other)
178         else: self._array *= other
179         return self
180
181     def __mul__(self, other):
182         if isinstance(other, Quaternion):
183             return self.__class__(self._QuaternionMultiply(other))
184         elif isinstance(other, TransformPrimitive3dh):
185             return Matrix(Numeric.dot(self.asArray4x4(), other.asArray4x4()))
186         elif isinstance(other, Numeric.ArrayType):
187             return Numeric.dot(self.asArray4x4(), other)
188         else: return self.__class__(self._array * other)
189
190     def __rmul__(self, other):
191         if isinstance(other, TransformPrimitive3dh):
192             return Matrix(Numeric.dot(other.asArray4x4(), self.asArray4x4()))
193         elif isinstance(other, Numeric.ArrayType):
194             return Numeric.dot(other, self.asArray4x4())
195         else: return self.__class__(other * self._array)
196
197     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
198     #~ Public Methods
199     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
200
201     def _QuaternionMultiply(self, other):
202         s, a, b, c = self._array
203         s2, a2, b2, c2 = other._array
204         return (s*s2-a*a2-b*b2-c*c2, s*a2+a*s2+b*c2-c*b2, s*b2+b*s2+c*a2-a*c2, s*c2+c*s2+a*b2-b*a2)
205
206     def asArray4x4(self):
207         """
208         >>> Quaternion().asArray4x4()
209         array([[ 1.,  0.,  0.,  0.],
210                [ 0.,  1.,  0.,  0.],
211                [ 0.,  0.,  1.,  0.],
212                [ 0.,  0.,  0.,  1.]])
213         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa
214         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
215         >>> aa.asArray4x4()
216         array([[ 0.8660254, -0.5      ,  0.       ,  0.       ],
217                [ 0.5      ,  0.8660254,  0.       ,  0.       ],
218                [ 0.       ,  0.       ,  1.       ,  0.       ],
219                [ 0.       ,  0.       ,  0.       ,  1.       ]])
220         >>> aa.Inverse().asArray4x4()
221         array([[ 0.8660254,  0.5      ,  0.       ,  0.       ],
222                [-0.5      ,  0.8660254,  0.       ,  0.       ],
223                [ 0.       ,  0.       ,  1.       ,  0.       ],
224                [ 0.       ,  0.       ,  0.       ,  1.       ]])
225         >>> a2 = Quaternion.fromAngleAxis(45., (1., 1., 1.)); a2
226         <Quaternion: 0.923879532511 + 0.127561144122i + 0.127561144122j + 0.127561144122k>
227         >>> a2.asArray4x4()
228         array([[ 0.93491262, -0.20315857,  0.26824595,  0.        ],
229                [ 0.26824595,  0.93491262, -0.20315857,  0.        ],
230                [-0.20315857,  0.26824595,  0.93491262,  0.        ],
231                [ 0.        ,  0.        ,  0.        ,  1.        ]])
232         >>> a2.Inverse().asArray4x4()
233         array([[ 0.92006655,  0.32943134, -0.24949789,  0.        ],
234                [-0.24949789,  0.92006655,  0.32943134,  0.        ],
235                [ 0.32943134, -0.24949789,  0.92006655,  0.        ],
236                [ 0.        ,  0.        ,  0.        ,  1.        ]])
237         >>> a2.asInverse4x4()
238         array([[ 0.92006655,  0.32943134, -0.24949789,  0.        ],
239                [-0.24949789,  0.92006655,  0.32943134,  0.        ],
240                [ 0.32943134, -0.24949789,  0.92006655,  0.        ],
241                [ 0.        ,  0.        ,  0.        ,  1.        ]])
242         """
243         s, a, b, c = self._array
244         result = Numeric.identity(4) + 2. * Numeric.asarray(
245            [[-b*b - c*c, a*b - c*s, c*a + b*s, 0.],
246             [ a*b + c*s,-c*c - a*a, b*c - a*s, 0.],
247             [ c*a - b*s, b*c + a*s,-b*b - a*a, 0.],
248             [0., 0., 0., 0.]], self.NumericType)
249         return result
250
251     def asInverse4x4(self):
252         return self.Inverse().asArray4x4()
253
254     def Magnitude(self, squared=0):
255         """
256         >>> q = Quaternion(2., 3., 4., 5.); q
257         <Quaternion: 2.0 + 3.0i + 4.0j + 5.0k>
258         >>> q.Magnitude()
259         7.3484692283495345
260         >>> q.Magnitude(1)
261         54.0
262         >>> q.Magnitude() * q.Magnitude()
263         54.0
264         >>> Quaternion().Magnitude()
265         1.0
266         """
267         value = Numeric.innerproduct(self._array, self._array)
268         if squared: return value
269         else: return Numeric.sqrt(value)
270
271     def Normalize(self):
272         """
273         >>> q = Quaternion(2., 3., 4., 5.); q
274         <Quaternion: 2.0 + 3.0i + 4.0j + 5.0k>
275         >>> q.Magnitude()
276         7.3484692283495345
277         >>> qn = q.Normalize(); qn
278         <Quaternion: 0.272165526976 + 0.408248290464i + 0.544331053952j + 0.68041381744k>
279         >>> qn.Magnitude()
280         1.0
281         """
282         mag = self.Magnitude()
283         return self / mag
284
285     def iNormalize(self):
286         self /= self.Magnitude()
287
288     def Inverse(self):
289         """
290         >>> Quaternion() # Identity
291         <Quaternion: 1.0 + 0.0i + 0.0j + 0.0k>
292         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.))
293         >>> aa * aa.Inverse()
294         <Quaternion: 1.0 + 0.0i + 0.0j + 0.0k>
295         >>> aa.Inverse() * aa
296         <Quaternion: 1.0 + 0.0i + 0.0j + 0.0k>
297         """
298         inverse = -self
299         inverse[0] = -inverse[0]
300         inverse /= self.Magnitude(squared=1)
301         return inverse
302
303     def DeltaAngle(self, other):
304         """
305         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa
306         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
307         >>> a2 = Quaternion.fromAngleAxis(60., (0., 0., 1.))
308         >>> a2
309         <Quaternion: 0.866025403784 + 0.0i + 0.0j + 0.5k>
310         >>> aa.DeltaAngle(a2)
311         29.999999999999986
312         >>> a2.DeltaAngle(aa)
313         29.999999999999986
314         """
315         return math.degrees(self.DeltaRadians(other))
316
317     def DeltaRadians(self, other):
318         """
319         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.))
320         >>> a2 = Quaternion.fromAngleAxis(60., (0., 0., 1.))
321         >>> aa.DeltaRadians(a2)
322         0.52359877559829859
323         >>> a2.DeltaRadians(aa)
324         0.52359877559829859
325         """
326         return 2. * Numeric.arccos(Numeric.innerproduct(self._array, other._array))
327
328     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
329
330     def getAngle(self):
331         """
332         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa.Angle
333         29.999999999999986
334         """
335         try:
336             return math.degrees(2.*Numeric.arccos(self._array[0]))
337         except ValueError: return 0.
338     def setAngle(self, value):
339         """
340         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa
341         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
342         >>> aa.Angle = 60; aa
343         <Quaternion: 0.866025403784 + 0.0i + 0.0j + 0.5k>
344         >>> aa.Angle
345         59.999999999999986
346         """
347         self.setRadians(math.radians(value))
348     Angle = property(getAngle, setAngle)
349
350     def getRadians(self):
351         """
352         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa.Radians
353         0.52359877559829859
354         """
355         try:
356             return 2. * Numeric.arccos(self._array[0])
357         except ValueError: return 0.
358     def setRadians(self, value):
359         """
360         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa
361         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
362         >>> aa.Radians = Numeric.pi/4.; aa
363         <Quaternion: 0.923879532511 + 0.0i + 0.0j + 0.382683432365k>
364         >>> aa.Radians
365         0.78539816339744839
366         """
367         self._array = self._SVfromRadianAxis(value, self.Axis)
368     Radians = property(getRadians, setRadians)
369
370     def getAxis(self):
371         """
372         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa.Axis
373         array([ 0.,  0.,  1.])
374         """
375         return self.toRadianAxis()[-1]
376     def setAxis(self, value):
377         """
378         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa
379         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
380         >>> aa.Axis = 0., 1., 0.; aa
381         <Quaternion: 0.965925826289 + 0.0i + 0.258819045103j + 0.0k>
382         >>> aa.Axis
383         array([ 0.,  1.,  0.])
384         """
385         self._array = self._SVfromRadianAxis(self.Radians, value)
386     Axis = property(getAxis, setAxis)
387
388     def Log(self):
389         """
390         >>> ra = Quaternion.fromRadianAxis(Numeric.pi/3., (1.,0.,0.)); ra
391         <Quaternion: 0.866025403784 + 0.5i + 0.0j + 0.0k>
392         >>> ra.Log()
393         <Quaternion: 0.0 + 0.523598775598i + 0.0j + 0.0k>
394         >>> ra.Log().Exp()
395         <Quaternion: 0.866025403784 + 0.5i + 0.0j + 0.0k>
396         """
397         Radians = Numeric.arccos(self._array[0])
398         sinR = Numeric.sin(Radians)
399         if sinR > 0:
400             result = (Radians/sinR) * self
401             result[0] = 0.
402         else:
403             result = self.__class__((0.,0.,0.,0.))
404         return result
405
406     def Exp(self, power=1.):
407         """
408         >>> ra = Quaternion.fromRadianAxis(Numeric.pi/3., (1.,0.,0.)); ra
409         <Quaternion: 0.866025403784 + 0.5i + 0.0j + 0.0k>
410         >>> ra.Exp()
411         <Quaternion: 0.87758256189 + 0.479425538604i + 0.0j + 0.0k>
412         >>> ra.Exp().Log()
413         <Quaternion: 0.0 + 0.5i + 0.0j + 0.0k>
414         """
415         Radians = Numeric.sqrt(Numeric.dot(self._array[1:], self._array[1:]))
416         sinR, cosR = Numeric.sin(power * Radians), Numeric.cos(power * Radians)
417         if Radians > 0:
418             result = (sinR/Radians) * self
419             result[0] = cosR
420         else:
421             result = self.__class__((cosR,0.,0.,0.))
422         return result
423
424     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
425     #~ Conversions
426     #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
427
428     def asarray(self, *args):
429         if args: return Numeric.asarray(self._array, *args)
430         else: return self._array
431
432     def tolist(self):
433         return self._array
434
435     def _SVfromRadianAxis(Klass, Radians, Axis):
436         Radians *= .5
437         cosR, sinR = Numeric.cos(Radians), Numeric.sin(Radians)
438         Quat = Numeric.asarray([cosR] + list(Axis), Klass.NumericType)
439         Quat[1:] *= sinR / Numeric.innerproduct(Quat[1:], Quat[1:])
440         return Quat
441     _SVfromRadianAxis = classmethod(_SVfromRadianAxis)
442
443     def fromAngleAxis(Klass, Angle, Axis):
444         """
445         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.)); aa
446         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
447         """
448         return Klass(Klass._SVfromRadianAxis(math.radians(Angle), Axis))
449     fromAngleAxis = classmethod(fromAngleAxis)
450
451     def fromRadianAxis(Klass, Radians, Axis):
452         """
453         >>> ra = Quaternion.fromRadianAxis(Numeric.pi/6., (0., 0., 1.)); ra
454         <Quaternion: 0.965925826289 + 0.0i + 0.0j + 0.258819045103k>
455         """
456         return Klass(Klass._SVfromRadianAxis(Radians, Axis))
457     fromRadianAxis = classmethod(fromRadianAxis)
458
459     def fromUnitRotationArc(Klass, v0, v1):
460         """
461         >>> urarc = Quaternion.fromUnitRotationArc((0., 0., 1.), (1., 0., 0.)); urarc
462         <Quaternion: 0.707106781187 + 0.0i + 0.707106781187j + 0.0k>
463         >>> urarc.Angle
464         90.0
465         """
466         cosR, Axis = _VectorDotCross3(v0, v1)
467         s = Numeric.sqrt(2.*(cosR+1))
468         Quat = Numeric.asarray((s/2.,) + Axis, Klass.NumericType)
469         Quat[1:] /= s
470         return Klass(Quat)
471     fromUnitRotationArc = classmethod(fromUnitRotationArc)
472
473     def fromRotationArc(Klass, v0, v1):
474         """
475         >>> rarc = Quaternion.fromUnitRotationArc((0., 0., 3.), (9., 0., 0.)); rarc
476         <Quaternion: 0.707106781187 + 0.0i + 19.091883092j + 0.0k>
477         >>> rarc.Angle
478         90.0
479         >>> Quaternion.fromRotationArc((0., 1., 0.), (0., 0.99999, 0.))
480         <Quaternion: 1.00000250002 + 0.0i + 0.0j + 0.0k>
481         >>> _.toAngleAxis()
482         (0.0, array([ 0.,  1.,  0.]))
483         """
484         v0 = Numeric.asarray(v0, Klass.NumericType)
485         v0 /= Numeric.dot(v0, v0)
486         v1 = Numeric.asarray(v1, Klass.NumericType)
487         v1 /= Numeric.dot(v1, v1)
488         return Klass.fromUnitRotationArc(v0, v1)
489     fromRotationArc = classmethod(fromRotationArc)
490
491     def toAngleAxis(self):
492         """
493         >>> aa = Quaternion.fromAngleAxis(30., (0., 0., 1.))
494         >>> aa.toAngleAxis()
495         (29.999999999999986, array([ 0.,  0.,  1.]))
496         >>> aa.toRadianAxis()
497         (0.52359877559829859, array([ 0.,  0.,  1.]))
498         """
499         Radian, Axis = self.toRadianAxis()
500         return math.degrees(Radian), Axis
501
502     def toRadianAxis(self):
503         """
504         >>> ra = Quaternion.fromRadianAxis(Numeric.pi/6., (0., 0., 1.))
505         >>> ra.toRadianAxis()
506         (0.52359877559829859, array([ 0.,  0.,  1.]))
507         >>> ra.toAngleAxis()
508         (29.999999999999986, array([ 0.,  0.,  1.]))
509         """
510         try:
511             Radians = Numeric.arccos(self._array[0])
512             RadialFactor = 1./Numeric.sin(Radians)
513             return 2. * Radians, RadialFactor * self._array[1:]
514         except ValueError:
515             return 0., Numeric.asarray((0., 1., 0.), self.NumericType)
516
517
518 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
519 #~ Testing
520 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
521
522 if __name__=='__main__':
523     print "Testing..."
524     import doctest
525     import Quaternion as _test
526     doctest.testmod(_test)
527     print "Test complete."
528
Note: See TracBrowser for help on using the browser.