1 ///////////////////////////////////////////////////////////////////////////
2 //
3 // Copyright (c) 2002-2012, Industrial Light & Magic, a division of Lucas
4 // Digital Ltd. LLC
5 //
6 // All rights reserved.
7 //
8 // Redistribution and use in source and binary forms, with or without
9 // modification, are permitted provided that the following conditions are
10 // met:
11 // * Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 // * Redistributions in binary form must reproduce the above
14 // copyright notice, this list of conditions and the following disclaimer
15 // in the documentation and/or other materials provided with the
16 // distribution.
17 // * Neither the name of Industrial Light & Magic nor the names of
18 // its contributors may be used to endorse or promote products derived
19 // from this software without specific prior written permission.
20 //
21 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
25 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
26 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
27 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
28 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
29 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
30 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 //
33 ///////////////////////////////////////////////////////////////////////////
34
35
36
37 #ifndef INCLUDED_IMATHEULER_H
38 #define INCLUDED_IMATHEULER_H
39
40 //----------------------------------------------------------------------
41 //
42 // template class Euler<T>
43 //
44 // This class represents euler angle orientations. The class
45 // inherits from Vec3 to it can be freely cast. The additional
46 // information is the euler priorities rep. This class is
47 // essentially a rip off of Ken Shoemake's GemsIV code. It has
48 // been modified minimally to make it more understandable, but
49 // hardly enough to make it easy to grok completely.
50 //
51 // There are 24 possible combonations of Euler angle
52 // representations of which 12 are common in CG and you will
53 // probably only use 6 of these which in this scheme are the
54 // non-relative-non-repeating types.
55 //
56 // The representations can be partitioned according to two
57 // criteria:
58 //
59 // 1) Are the angles measured relative to a set of fixed axis
60 // or relative to each other (the latter being what happens
61 // when rotation matrices are multiplied together and is
62 // almost ubiquitous in the cg community)
63 //
64 // 2) Is one of the rotations repeated (ala XYX rotation)
65 //
66 // When you construct a given representation from scratch you
67 // must order the angles according to their priorities. So, the
68 // easiest is a softimage or aerospace (yaw/pitch/roll) ordering
69 // of ZYX.
70 //
71 // float x_rot = 1;
72 // float y_rot = 2;
73 // float z_rot = 3;
74 //
75 // Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
76 // -or-
77 // Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
78 //
79 // If instead, the order was YXZ for instance you would have to
80 // do this:
81 //
82 // float x_rot = 1;
83 // float y_rot = 2;
84 // float z_rot = 3;
85 //
86 // Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
87 // -or-
88 // Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
89 //
90 // Notice how the order you put the angles into the three slots
91 // should correspond to the enum (YXZ) ordering. The input angle
92 // vector is called the "ijk" vector -- not an "xyz" vector. The
93 // ijk vector order is the same as the enum. If you treat the
94 // Euler<> as a Vec<> (which it inherts from) you will find the
95 // angles are ordered in the same way, i.e.:
96 //
97 // V3f v = angles;
98 // // v.x == y_rot, v.y == x_rot, v.z == z_rot
99 //
100 // If you just want the x, y, and z angles stored in a vector in
101 // that order, you can do this:
102 //
103 // V3f v = angles.toXYZVector()
104 // // v.x == x_rot, v.y == y_rot, v.z == z_rot
105 //
106 // If you want to set the Euler with an XYZVector use the
107 // optional layout argument:
108 //
109 // Eulerf angles(x_rot, y_rot, z_rot,
110 // Eulerf::YXZ,
111 // Eulerf::XYZLayout);
112 //
113 // This is the same as:
114 //
115 // Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
116 //
117 // Note that this won't do anything intelligent if you have a
118 // repeated axis in the euler angles (e.g. XYX)
119 //
120 // If you need to use the "relative" versions of these, you will
121 // need to use the "r" enums.
122 //
123 // The units of the rotation angles are assumed to be radians.
124 //
125 //----------------------------------------------------------------------
126
127
128 #include "ImathMath.h"
129 #include "ImathVec.h"
130 #include "ImathQuat.h"
131 #include "ImathMatrix.h"
132 #include "ImathLimits.h"
133 #include "ImathNamespace.h"
134
135 #include <iostream>
136
137 IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
138
139 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
140 // Disable MS VC++ warnings about conversion from double to float
141 #pragma warning(disable:4244)
142 #endif
143
144 template <class T>
145 class Euler : public Vec3<T>
146 {
147 public:
148
149 using Vec3<T>::x;
150 using Vec3<T>::y;
151 using Vec3<T>::z;
152
153 enum Order
154 {
155 //
156 // All 24 possible orderings
157 //
158
159 XYZ = 0x0101, // "usual" orderings
160 XZY = 0x0001,
161 YZX = 0x1101,
162 YXZ = 0x1001,
163 ZXY = 0x2101,
164 ZYX = 0x2001,
165
166 XZX = 0x0011, // first axis repeated
167 XYX = 0x0111,
168 YXY = 0x1011,
169 YZY = 0x1111,
170 ZYZ = 0x2011,
171 ZXZ = 0x2111,
172
173 XYZr = 0x2000, // relative orderings -- not common
174 XZYr = 0x2100,
175 YZXr = 0x1000,
176 YXZr = 0x1100,
177 ZXYr = 0x0000,
178 ZYXr = 0x0100,
179
180 XZXr = 0x2110, // relative first axis repeated
181 XYXr = 0x2010,
182 YXYr = 0x1110,
183 YZYr = 0x1010,
184 ZYZr = 0x0110,
185 ZXZr = 0x0010,
186 // ||||
187 // VVVV
188 // Legend: ABCD
189 // A -> Initial Axis (0==x, 1==y, 2==z)
190 // B -> Parity Even (1==true)
191 // C -> Initial Repeated (1==true)
192 // D -> Frame Static (1==true)
193 //
194
195 Legal = XYZ | XZY | YZX | YXZ | ZXY | ZYX |
196 XZX | XYX | YXY | YZY | ZYZ | ZXZ |
197 XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
198 XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,
199
200 Min = 0x0000,
201 Max = 0x2111,
202 Default = XYZ
203 };
204
205 enum Axis { X = 0, Y = 1, Z = 2 };
206
207 enum InputLayout { XYZLayout, IJKLayout };
208
209 //--------------------------------------------------------------------
210 // Constructors -- all default to ZYX non-relative ala softimage
211 // (where there is no argument to specify it)
212 //
213 // The Euler-from-matrix constructors assume that the matrix does
214 // not include shear or non-uniform scaling, but the constructors
215 // do not examine the matrix to verify this assumption. If necessary,
216 // you can adjust the matrix by calling the removeScalingAndShear()
217 // function, defined in ImathMatrixAlgo.h.
218 //--------------------------------------------------------------------
219
220 Euler();
221 Euler(const Euler&);
222 Euler(Order p);
223 Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
224 Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
225 Euler(const Euler<T> &euler, Order newp);
226 Euler(const Matrix33<T> &, Order o = Default);
227 Euler(const Matrix44<T> &, Order o = Default);
228
229 //---------------------------------
230 // Algebraic functions/ Operators
231 //---------------------------------
232
233 const Euler<T>& operator= (const Euler<T>&);
234 const Euler<T>& operator= (const Vec3<T>&);
235
236 //--------------------------------------------------------
237 // Set the euler value
238 // This does NOT convert the angles, but setXYZVector()
239 // does reorder the input vector.
240 //--------------------------------------------------------
241
242 static bool legal(Order);
243
244 void setXYZVector(const Vec3<T> &);
245
246 Order order() const;
247 void setOrder(Order);
248
249 void set(Axis initial,
250 bool relative,
251 bool parityEven,
252 bool firstRepeats);
253
254 //------------------------------------------------------------
255 // Conversions, toXYZVector() reorders the angles so that
256 // the X rotation comes first, followed by the Y and Z
257 // in cases like XYX ordering, the repeated angle will be
258 // in the "z" component
259 //
260 // The Euler-from-matrix extract() functions assume that the
261 // matrix does not include shear or non-uniform scaling, but
262 // the extract() functions do not examine the matrix to verify
263 // this assumption. If necessary, you can adjust the matrix
264 // by calling the removeScalingAndShear() function, defined
265 // in ImathMatrixAlgo.h.
266 //------------------------------------------------------------
267
268 void extract(const Matrix33<T>&);
269 void extract(const Matrix44<T>&);
270 void extract(const Quat<T>&);
271
272 Matrix33<T> toMatrix33() const;
273 Matrix44<T> toMatrix44() const;
274 Quat<T> toQuat() const;
275 Vec3<T> toXYZVector() const;
276
277 //---------------------------------------------------
278 // Use this function to unpack angles from ijk form
279 //---------------------------------------------------
280
281 void angleOrder(int &i, int &j, int &k) const;
282
283 //---------------------------------------------------
284 // Use this function to determine mapping from xyz to ijk
285 // - reshuffles the xyz to match the order
286 //---------------------------------------------------
287
288 void angleMapping(int &i, int &j, int &k) const;
289
290 //----------------------------------------------------------------------
291 //
292 // Utility methods for getting continuous rotations. None of these
293 // methods change the orientation given by its inputs (or at least
294 // that is the intent).
295 //
296 // angleMod() converts an angle to its equivalent in [-PI, PI]
297 //
298 // simpleXYZRotation() adjusts xyzRot so that its components differ
299 // from targetXyzRot by no more than +-PI
300 //
301 // nearestRotation() adjusts xyzRot so that its components differ
302 // from targetXyzRot by as little as possible.
303 // Note that xyz here really means ijk, because
304 // the order must be provided.
305 //
306 // makeNear() adjusts "this" Euler so that its components differ
307 // from target by as little as possible. This method
308 // might not make sense for Eulers with different order
309 // and it probably doesn't work for repeated axis and
310 // relative orderings (TODO).
311 //
312 //-----------------------------------------------------------------------
313
314 static float angleMod (T angle);
315 static void simpleXYZRotation (Vec3<T> &xyzRot,
316 const Vec3<T> &targetXyzRot);
317 static void nearestRotation (Vec3<T> &xyzRot,
318 const Vec3<T> &targetXyzRot,
319 Order order = XYZ);
320
321 void makeNear (const Euler<T> &target);
322
frameStatic()323 bool frameStatic() const { return _frameStatic; }
initialRepeated()324 bool initialRepeated() const { return _initialRepeated; }
parityEven()325 bool parityEven() const { return _parityEven; }
initialAxis()326 Axis initialAxis() const { return _initialAxis; }
327
328 protected:
329
330 bool _frameStatic : 1; // relative or static rotations
331 bool _initialRepeated : 1; // init axis repeated as last
332 bool _parityEven : 1; // "parity of axis permutation"
333 #if defined _WIN32 || defined _WIN64
334 Axis _initialAxis ; // First axis of rotation
335 #else
336 Axis _initialAxis : 2; // First axis of rotation
337 #endif
338 };
339
340
341 //--------------------
342 // Convenient typedefs
343 //--------------------
344
345 typedef Euler<float> Eulerf;
346 typedef Euler<double> Eulerd;
347
348
349 //---------------
350 // Implementation
351 //---------------
352
353 template<class T>
354 inline void
angleOrder(int & i,int & j,int & k)355 Euler<T>::angleOrder(int &i, int &j, int &k) const
356 {
357 i = _initialAxis;
358 j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
359 k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
360 }
361
362 template<class T>
363 inline void
angleMapping(int & i,int & j,int & k)364 Euler<T>::angleMapping(int &i, int &j, int &k) const
365 {
366 int m[3];
367
368 m[_initialAxis] = 0;
369 m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
370 m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
371 i = m[0];
372 j = m[1];
373 k = m[2];
374 }
375
376 template<class T>
377 inline void
setXYZVector(const Vec3<T> & v)378 Euler<T>::setXYZVector(const Vec3<T> &v)
379 {
380 int i,j,k;
381 angleMapping(i,j,k);
382 (*this)[i] = v.x;
383 (*this)[j] = v.y;
384 (*this)[k] = v.z;
385 }
386
387 template<class T>
388 inline Vec3<T>
toXYZVector()389 Euler<T>::toXYZVector() const
390 {
391 int i,j,k;
392 angleMapping(i,j,k);
393 return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
394 }
395
396
397 template<class T>
Euler()398 Euler<T>::Euler() :
399 Vec3<T>(0,0,0),
400 _frameStatic(true),
401 _initialRepeated(false),
402 _parityEven(true),
403 _initialAxis(X)
404 {}
405
406 template<class T>
Euler(typename Euler<T>::Order p)407 Euler<T>::Euler(typename Euler<T>::Order p) :
408 Vec3<T>(0,0,0),
409 _frameStatic(true),
410 _initialRepeated(false),
411 _parityEven(true),
412 _initialAxis(X)
413 {
414 setOrder(p);
415 }
416
417 template<class T>
Euler(const Vec3<T> & v,typename Euler<T>::Order p,typename Euler<T>::InputLayout l)418 inline Euler<T>::Euler( const Vec3<T> &v,
419 typename Euler<T>::Order p,
420 typename Euler<T>::InputLayout l )
421 {
422 setOrder(p);
423 if ( l == XYZLayout ) setXYZVector(v);
424 else { x = v.x; y = v.y; z = v.z; }
425 }
426
427 template<class T>
Euler(const Euler<T> & euler)428 inline Euler<T>::Euler(const Euler<T> &euler)
429 {
430 operator=(euler);
431 }
432
433 template<class T>
Euler(const Euler<T> & euler,Order p)434 inline Euler<T>::Euler(const Euler<T> &euler,Order p)
435 {
436 setOrder(p);
437 Matrix33<T> M = euler.toMatrix33();
438 extract(M);
439 }
440
441 template<class T>
Euler(T xi,T yi,T zi,typename Euler<T>::Order p,typename Euler<T>::InputLayout l)442 inline Euler<T>::Euler( T xi, T yi, T zi,
443 typename Euler<T>::Order p,
444 typename Euler<T>::InputLayout l)
445 {
446 setOrder(p);
447 if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
448 else { x = xi; y = yi; z = zi; }
449 }
450
451 template<class T>
Euler(const Matrix33<T> & M,typename Euler::Order p)452 inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
453 {
454 setOrder(p);
455 extract(M);
456 }
457
458 template<class T>
Euler(const Matrix44<T> & M,typename Euler::Order p)459 inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
460 {
461 setOrder(p);
462 extract(M);
463 }
464
465 template<class T>
extract(const Quat<T> & q)466 inline void Euler<T>::extract(const Quat<T> &q)
467 {
468 extract(q.toMatrix33());
469 }
470
471 template<class T>
extract(const Matrix33<T> & M)472 void Euler<T>::extract(const Matrix33<T> &M)
473 {
474 int i,j,k;
475 angleOrder(i,j,k);
476
477 if (_initialRepeated)
478 {
479 //
480 // Extract the first angle, x.
481 //
482
483 x = Math<T>::atan2 (M[j][i], M[k][i]);
484
485 //
486 // Remove the x rotation from M, so that the remaining
487 // rotation, N, is only around two axes, and gimbal lock
488 // cannot occur.
489 //
490
491 Vec3<T> r (0, 0, 0);
492 r[i] = (_parityEven? -x: x);
493
494 Matrix44<T> N;
495 N.rotate (r);
496
497 N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
498 M[1][0], M[1][1], M[1][2], 0,
499 M[2][0], M[2][1], M[2][2], 0,
500 0, 0, 0, 1);
501 //
502 // Extract the other two angles, y and z, from N.
503 //
504
505 T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
506 y = Math<T>::atan2 (sy, N[i][i]);
507 z = Math<T>::atan2 (N[j][k], N[j][j]);
508 }
509 else
510 {
511 //
512 // Extract the first angle, x.
513 //
514
515 x = Math<T>::atan2 (M[j][k], M[k][k]);
516
517 //
518 // Remove the x rotation from M, so that the remaining
519 // rotation, N, is only around two axes, and gimbal lock
520 // cannot occur.
521 //
522
523 Vec3<T> r (0, 0, 0);
524 r[i] = (_parityEven? -x: x);
525
526 Matrix44<T> N;
527 N.rotate (r);
528
529 N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
530 M[1][0], M[1][1], M[1][2], 0,
531 M[2][0], M[2][1], M[2][2], 0,
532 0, 0, 0, 1);
533 //
534 // Extract the other two angles, y and z, from N.
535 //
536
537 T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
538 y = Math<T>::atan2 (-N[i][k], cy);
539 z = Math<T>::atan2 (-N[j][i], N[j][j]);
540 }
541
542 if (!_parityEven)
543 *this *= -1;
544
545 if (!_frameStatic)
546 {
547 T t = x;
548 x = z;
549 z = t;
550 }
551 }
552
553 template<class T>
extract(const Matrix44<T> & M)554 void Euler<T>::extract(const Matrix44<T> &M)
555 {
556 int i,j,k;
557 angleOrder(i,j,k);
558
559 if (_initialRepeated)
560 {
561 //
562 // Extract the first angle, x.
563 //
564
565 x = Math<T>::atan2 (M[j][i], M[k][i]);
566
567 //
568 // Remove the x rotation from M, so that the remaining
569 // rotation, N, is only around two axes, and gimbal lock
570 // cannot occur.
571 //
572
573 Vec3<T> r (0, 0, 0);
574 r[i] = (_parityEven? -x: x);
575
576 Matrix44<T> N;
577 N.rotate (r);
578 N = N * M;
579
580 //
581 // Extract the other two angles, y and z, from N.
582 //
583
584 T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
585 y = Math<T>::atan2 (sy, N[i][i]);
586 z = Math<T>::atan2 (N[j][k], N[j][j]);
587 }
588 else
589 {
590 //
591 // Extract the first angle, x.
592 //
593
594 x = Math<T>::atan2 (M[j][k], M[k][k]);
595
596 //
597 // Remove the x rotation from M, so that the remaining
598 // rotation, N, is only around two axes, and gimbal lock
599 // cannot occur.
600 //
601
602 Vec3<T> r (0, 0, 0);
603 r[i] = (_parityEven? -x: x);
604
605 Matrix44<T> N;
606 N.rotate (r);
607 N = N * M;
608
609 //
610 // Extract the other two angles, y and z, from N.
611 //
612
613 T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
614 y = Math<T>::atan2 (-N[i][k], cy);
615 z = Math<T>::atan2 (-N[j][i], N[j][j]);
616 }
617
618 if (!_parityEven)
619 *this *= -1;
620
621 if (!_frameStatic)
622 {
623 T t = x;
624 x = z;
625 z = t;
626 }
627 }
628
629 template<class T>
toMatrix33()630 Matrix33<T> Euler<T>::toMatrix33() const
631 {
632 int i,j,k;
633 angleOrder(i,j,k);
634
635 Vec3<T> angles;
636
637 if ( _frameStatic ) angles = (*this);
638 else angles = Vec3<T>(z,y,x);
639
640 if ( !_parityEven ) angles *= -1.0;
641
642 T ci = Math<T>::cos(angles.x);
643 T cj = Math<T>::cos(angles.y);
644 T ch = Math<T>::cos(angles.z);
645 T si = Math<T>::sin(angles.x);
646 T sj = Math<T>::sin(angles.y);
647 T sh = Math<T>::sin(angles.z);
648
649 T cc = ci*ch;
650 T cs = ci*sh;
651 T sc = si*ch;
652 T ss = si*sh;
653
654 Matrix33<T> M;
655
656 if ( _initialRepeated )
657 {
658 M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci;
659 M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
660 M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss;
661 }
662 else
663 {
664 M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
665 M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
666 M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci;
667 }
668
669 return M;
670 }
671
672 template<class T>
toMatrix44()673 Matrix44<T> Euler<T>::toMatrix44() const
674 {
675 int i,j,k;
676 angleOrder(i,j,k);
677
678 Vec3<T> angles;
679
680 if ( _frameStatic ) angles = (*this);
681 else angles = Vec3<T>(z,y,x);
682
683 if ( !_parityEven ) angles *= -1.0;
684
685 T ci = Math<T>::cos(angles.x);
686 T cj = Math<T>::cos(angles.y);
687 T ch = Math<T>::cos(angles.z);
688 T si = Math<T>::sin(angles.x);
689 T sj = Math<T>::sin(angles.y);
690 T sh = Math<T>::sin(angles.z);
691
692 T cc = ci*ch;
693 T cs = ci*sh;
694 T sc = si*ch;
695 T ss = si*sh;
696
697 Matrix44<T> M;
698
699 if ( _initialRepeated )
700 {
701 M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci;
702 M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
703 M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss;
704 }
705 else
706 {
707 M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
708 M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
709 M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci;
710 }
711
712 return M;
713 }
714
715 template<class T>
toQuat()716 Quat<T> Euler<T>::toQuat() const
717 {
718 Vec3<T> angles;
719 int i,j,k;
720 angleOrder(i,j,k);
721
722 if ( _frameStatic ) angles = (*this);
723 else angles = Vec3<T>(z,y,x);
724
725 if ( !_parityEven ) angles.y = -angles.y;
726
727 T ti = angles.x*0.5;
728 T tj = angles.y*0.5;
729 T th = angles.z*0.5;
730 T ci = Math<T>::cos(ti);
731 T cj = Math<T>::cos(tj);
732 T ch = Math<T>::cos(th);
733 T si = Math<T>::sin(ti);
734 T sj = Math<T>::sin(tj);
735 T sh = Math<T>::sin(th);
736 T cc = ci*ch;
737 T cs = ci*sh;
738 T sc = si*ch;
739 T ss = si*sh;
740
741 T parity = _parityEven ? 1.0 : -1.0;
742
743 Quat<T> q;
744 Vec3<T> a;
745
746 if ( _initialRepeated )
747 {
748 a[i] = cj*(cs + sc);
749 a[j] = sj*(cc + ss) * parity,
750 a[k] = sj*(cs - sc);
751 q.r = cj*(cc - ss);
752 }
753 else
754 {
755 a[i] = cj*sc - sj*cs,
756 a[j] = (cj*ss + sj*cc) * parity,
757 a[k] = cj*cs - sj*sc;
758 q.r = cj*cc + sj*ss;
759 }
760
761 q.v = a;
762
763 return q;
764 }
765
766 template<class T>
767 inline bool
legal(typename Euler<T>::Order order)768 Euler<T>::legal(typename Euler<T>::Order order)
769 {
770 return (order & ~Legal) ? false : true;
771 }
772
773 template<class T>
774 typename Euler<T>::Order
order()775 Euler<T>::order() const
776 {
777 int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
778
779 if (_parityEven) foo |= 0x0100;
780 if (_initialRepeated) foo |= 0x0010;
781 if (_frameStatic) foo++;
782
783 return (Order)foo;
784 }
785
786 template<class T>
setOrder(typename Euler<T>::Order p)787 inline void Euler<T>::setOrder(typename Euler<T>::Order p)
788 {
789 set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
790 !(p & 0x1), // static?
791 !!(p & 0x100), // permutation even?
792 !!(p & 0x10)); // initial repeats?
793 }
794
795 template<class T>
set(typename Euler<T>::Axis axis,bool relative,bool parityEven,bool firstRepeats)796 void Euler<T>::set(typename Euler<T>::Axis axis,
797 bool relative,
798 bool parityEven,
799 bool firstRepeats)
800 {
801 _initialAxis = axis;
802 _frameStatic = !relative;
803 _parityEven = parityEven;
804 _initialRepeated = firstRepeats;
805 }
806
807 template<class T>
808 const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
809 {
810 x = euler.x;
811 y = euler.y;
812 z = euler.z;
813 _initialAxis = euler._initialAxis;
814 _frameStatic = euler._frameStatic;
815 _parityEven = euler._parityEven;
816 _initialRepeated = euler._initialRepeated;
817 return *this;
818 }
819
820 template<class T>
821 const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
822 {
823 x = v.x;
824 y = v.y;
825 z = v.z;
826 return *this;
827 }
828
829 template<class T>
830 std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
831 {
832 char a[3] = { 'X', 'Y', 'Z' };
833
834 const char* r = euler.frameStatic() ? "" : "r";
835 int i,j,k;
836 euler.angleOrder(i,j,k);
837
838 if ( euler.initialRepeated() ) k = i;
839
840 return o << "("
841 << euler.x << " "
842 << euler.y << " "
843 << euler.z << " "
844 << a[i] << a[j] << a[k] << r << ")";
845 }
846
847 template <class T>
848 float
angleMod(T angle)849 Euler<T>::angleMod (T angle)
850 {
851 angle = fmod(T (angle), T (2 * M_PI));
852
853 if (angle < -M_PI) angle += 2 * M_PI;
854 if (angle > +M_PI) angle -= 2 * M_PI;
855
856 return angle;
857 }
858
859 template <class T>
860 void
simpleXYZRotation(Vec3<T> & xyzRot,const Vec3<T> & targetXyzRot)861 Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
862 {
863 Vec3<T> d = xyzRot - targetXyzRot;
864 xyzRot[0] = targetXyzRot[0] + angleMod(d[0]);
865 xyzRot[1] = targetXyzRot[1] + angleMod(d[1]);
866 xyzRot[2] = targetXyzRot[2] + angleMod(d[2]);
867 }
868
869 template <class T>
870 void
nearestRotation(Vec3<T> & xyzRot,const Vec3<T> & targetXyzRot,Order order)871 Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
872 Order order)
873 {
874 int i,j,k;
875 Euler<T> e (0,0,0, order);
876 e.angleOrder(i,j,k);
877
878 simpleXYZRotation(xyzRot, targetXyzRot);
879
880 Vec3<T> otherXyzRot;
881 otherXyzRot[i] = M_PI+xyzRot[i];
882 otherXyzRot[j] = M_PI-xyzRot[j];
883 otherXyzRot[k] = M_PI+xyzRot[k];
884
885 simpleXYZRotation(otherXyzRot, targetXyzRot);
886
887 Vec3<T> d = xyzRot - targetXyzRot;
888 Vec3<T> od = otherXyzRot - targetXyzRot;
889 T dMag = d.dot(d);
890 T odMag = od.dot(od);
891
892 if (odMag < dMag)
893 {
894 xyzRot = otherXyzRot;
895 }
896 }
897
898 template <class T>
899 void
makeNear(const Euler<T> & target)900 Euler<T>::makeNear (const Euler<T> &target)
901 {
902 Vec3<T> xyzRot = toXYZVector();
903 Vec3<T> targetXyz;
904 if (order() != target.order())
905 {
906 Euler<T> targetSameOrder = Euler<T>(target, order());
907 targetXyz = targetSameOrder.toXYZVector();
908 }
909 else
910 {
911 targetXyz = target.toXYZVector();
912 }
913
914 nearestRotation(xyzRot, targetXyz, order());
915
916 setXYZVector(xyzRot);
917 }
918
919 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
920 #pragma warning(default:4244)
921 #endif
922
923 IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
924
925
926 #endif // INCLUDED_IMATHEULER_H
927