Debian Codegraph @

src/Imath/ImathEuler.h

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