1 /////////////////////////////////////////////////////////////////////////// 2 // 3 // Copyright (c) 2002, 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 <iostream> 134 135 namespace Imath { 136 137 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 138 // Disable MS VC++ warnings about conversion from double to float 139 #pragma warning(disable:4244) 140 #endif 141 142 template <class T> 143 class Euler : public Vec3<T> 144 { 145 public: 146 147 using Vec3<T>::x; 148 using Vec3<T>::y; 149 using Vec3<T>::z; 150 151 enum Order 152 { 153 // 154 // All 24 possible orderings 155 // 156 157 XYZ = 0x0101, // "usual" orderings 158 XZY = 0x0001, 159 YZX = 0x1101, 160 YXZ = 0x1001, 161 ZXY = 0x2101, 162 ZYX = 0x2001, 163 164 XZX = 0x0011, // first axis repeated 165 XYX = 0x0111, 166 YXY = 0x1011, 167 YZY = 0x1111, 168 ZYZ = 0x2011, 169 ZXZ = 0x2111, 170 171 XYZr = 0x2000, // relative orderings -- not common 172 XZYr = 0x2100, 173 YZXr = 0x1000, 174 YXZr = 0x1100, 175 ZXYr = 0x0000, 176 ZYXr = 0x0100, 177 178 XZXr = 0x2110, // relative first axis repeated 179 XYXr = 0x2010, 180 YXYr = 0x1110, 181 YZYr = 0x1010, 182 ZYZr = 0x0110, 183 ZXZr = 0x0010, 184 // |||| 185 // VVVV 186 // Legend: ABCD 187 // A -> Initial Axis (0==x, 1==y, 2==z) 188 // B -> Parity Even (1==true) 189 // C -> Initial Repeated (1==true) 190 // D -> Frame Static (1==true) 191 // 192 193 Legal = XYZ | XZY | YZX | YXZ | ZXY | ZYX | 194 XZX | XYX | YXY | YZY | ZYZ | ZXZ | 195 XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr| 196 XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr, 197 198 Min = 0x0000, 199 Max = 0x2111, 200 Default = XYZ 201 }; 202 203 enum Axis { X = 0, Y = 1, Z = 2 }; 204 205 enum InputLayout { XYZLayout, IJKLayout }; 206 207 //-------------------------------------------------------------------- 208 // Constructors -- all default to ZYX non-relative ala softimage 209 // (where there is no argument to specify it) 210 // 211 // The Euler-from-matrix constructors assume that the matrix does 212 // not include shear or non-uniform scaling, but the constructors 213 // do not examine the matrix to verify this assumption. If necessary, 214 // you can adjust the matrix by calling the removeScalingAndShear() 215 // function, defined in ImathMatrixAlgo.h. 216 //-------------------------------------------------------------------- 217 218 Euler(); 219 Euler(const Euler&); 220 Euler(Order p); 221 Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout); 222 Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout); 223 Euler(const Euler<T> &euler, Order newp); 224 Euler(const Matrix33<T> &, Order o = Default); 225 Euler(const Matrix44<T> &, Order o = Default); 226 227 //--------------------------------- 228 // Algebraic functions/ Operators 229 //--------------------------------- 230 231 const Euler<T>& operator= (const Euler<T>&); 232 const Euler<T>& operator= (const Vec3<T>&); 233 234 //-------------------------------------------------------- 235 // Set the euler value 236 // This does NOT convert the angles, but setXYZVector() 237 // does reorder the input vector. 238 //-------------------------------------------------------- 239 240 static bool legal(Order); 241 242 void setXYZVector(const Vec3<T> &); 243 244 Order order() const; 245 void setOrder(Order); 246 247 void set(Axis initial, 248 bool relative, 249 bool parityEven, 250 bool firstRepeats); 251 252 //------------------------------------------------------------ 253 // Conversions, toXYZVector() reorders the angles so that 254 // the X rotation comes first, followed by the Y and Z 255 // in cases like XYX ordering, the repeated angle will be 256 // in the "z" component 257 // 258 // The Euler-from-matrix extract() functions assume that the 259 // matrix does not include shear or non-uniform scaling, but 260 // the extract() functions do not examine the matrix to verify 261 // this assumption. If necessary, you can adjust the matrix 262 // by calling the removeScalingAndShear() function, defined 263 // in ImathMatrixAlgo.h. 264 //------------------------------------------------------------ 265 266 void extract(const Matrix33<T>&); 267 void extract(const Matrix44<T>&); 268 void extract(const Quat<T>&); 269 270 Matrix33<T> toMatrix33() const; 271 Matrix44<T> toMatrix44() const; 272 Quat<T> toQuat() const; 273 Vec3<T> toXYZVector() const; 274 275 //--------------------------------------------------- 276 // Use this function to unpack angles from ijk form 277 //--------------------------------------------------- 278 279 void angleOrder(int &i, int &j, int &k) const; 280 281 //--------------------------------------------------- 282 // Use this function to determine mapping from xyz to ijk 283 // - reshuffles the xyz to match the order 284 //--------------------------------------------------- 285 286 void angleMapping(int &i, int &j, int &k) const; 287 288 //---------------------------------------------------------------------- 289 // 290 // Utility methods for getting continuous rotations. None of these 291 // methods change the orientation given by its inputs (or at least 292 // that is the intent). 293 // 294 // angleMod() converts an angle to its equivalent in [-PI, PI] 295 // 296 // simpleXYZRotation() adjusts xyzRot so that its components differ 297 // from targetXyzRot by no more than +-PI 298 // 299 // nearestRotation() adjusts xyzRot so that its components differ 300 // from targetXyzRot by as little as possible. 301 // Note that xyz here really means ijk, because 302 // the order must be provided. 303 // 304 // makeNear() adjusts "this" Euler so that its components differ 305 // from target by as little as possible. This method 306 // might not make sense for Eulers with different order 307 // and it probably doesn't work for repeated axis and 308 // relative orderings (TODO). 309 // 310 //----------------------------------------------------------------------- 311 312 static float angleMod (T angle); 313 static void simpleXYZRotation (Vec3<T> &xyzRot, 314 const Vec3<T> &targetXyzRot); 315 static void nearestRotation (Vec3<T> &xyzRot, 316 const Vec3<T> &targetXyzRot, 317 Order order = XYZ); 318 319 void makeNear (const Euler<T> &target); 320 321 bool frameStatic() const { return _frameStatic; } 322 bool initialRepeated() const { return _initialRepeated; } 323 bool parityEven() const { return _parityEven; } 324 Axis initialAxis() const { return _initialAxis; } 325 326 protected: 327 328 bool _frameStatic : 1; // relative or static rotations 329 bool _initialRepeated : 1; // init axis repeated as last 330 bool _parityEven : 1; // "parity of axis permutation" 331 #if defined _WIN32 || defined _WIN64 332 Axis _initialAxis ; // First axis of rotation 333 #else 334 Axis _initialAxis : 2; // First axis of rotation 335 #endif 336 }; 337 338 339 //-------------------- 340 // Convenient typedefs 341 //-------------------- 342 343 typedef Euler<float> Eulerf; 344 typedef Euler<double> Eulerd; 345 346 347 //--------------- 348 // Implementation 349 //--------------- 350 351 template<class T> 352 inline void 353 Euler<T>::angleOrder(int &i, int &j, int &k) const 354 { 355 i = _initialAxis; 356 j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2); 357 k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3; 358 } 359 360 template<class T> 361 inline void 362 Euler<T>::angleMapping(int &i, int &j, int &k) const 363 { 364 int m[3]; 365 366 m[_initialAxis] = 0; 367 m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2; 368 m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1; 369 i = m[0]; 370 j = m[1]; 371 k = m[2]; 372 } 373 374 template<class T> 375 inline void 376 Euler<T>::setXYZVector(const Vec3<T> &v) 377 { 378 int i,j,k; 379 angleMapping(i,j,k); 380 (*this)[i] = v.x; 381 (*this)[j] = v.y; 382 (*this)[k] = v.z; 383 } 384 385 template<class T> 386 inline Vec3<T> 387 Euler<T>::toXYZVector() const 388 { 389 int i,j,k; 390 angleMapping(i,j,k); 391 return Vec3<T>((*this)[i],(*this)[j],(*this)[k]); 392 } 393 394 395 template<class T> 396 Euler<T>::Euler() : 397 Vec3<T>(0,0,0), 398 _frameStatic(true), 399 _initialRepeated(false), 400 _parityEven(true), 401 _initialAxis(X) 402 {} 403 404 template<class T> 405 Euler<T>::Euler(typename Euler<T>::Order p) : 406 Vec3<T>(0,0,0), 407 _frameStatic(true), 408 _initialRepeated(false), 409 _parityEven(true), 410 _initialAxis(X) 411 { 412 setOrder(p); 413 } 414 415 template<class T> 416 inline Euler<T>::Euler( const Vec3<T> &v, 417 typename Euler<T>::Order p, 418 typename Euler<T>::InputLayout l ) 419 { 420 setOrder(p); 421 if ( l == XYZLayout ) setXYZVector(v); 422 else { x = v.x; y = v.y; z = v.z; } 423 } 424 425 template<class T> 426 inline Euler<T>::Euler(const Euler<T> &euler) 427 { 428 operator=(euler); 429 } 430 431 template<class T> 432 inline Euler<T>::Euler(const Euler<T> &euler,Order p) 433 { 434 setOrder(p); 435 Matrix33<T> M = euler.toMatrix33(); 436 extract(M); 437 } 438 439 template<class T> 440 inline Euler<T>::Euler( T xi, T yi, T zi, 441 typename Euler<T>::Order p, 442 typename Euler<T>::InputLayout l) 443 { 444 setOrder(p); 445 if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi)); 446 else { x = xi; y = yi; z = zi; } 447 } 448 449 template<class T> 450 inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p ) 451 { 452 setOrder(p); 453 extract(M); 454 } 455 456 template<class T> 457 inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p ) 458 { 459 setOrder(p); 460 extract(M); 461 } 462 463 template<class T> 464 inline void Euler<T>::extract(const Quat<T> &q) 465 { 466 extract(q.toMatrix33()); 467 } 468 469 template<class T> 470 void Euler<T>::extract(const Matrix33<T> &M) 471 { 472 int i,j,k; 473 angleOrder(i,j,k); 474 475 if (_initialRepeated) 476 { 477 // 478 // Extract the first angle, x. 479 // 480 481 x = Math<T>::atan2 (M[j][i], M[k][i]); 482 483 // 484 // Remove the x rotation from M, so that the remaining 485 // rotation, N, is only around two axes, and gimbal lock 486 // cannot occur. 487 // 488 489 Vec3<T> r (0, 0, 0); 490 r[i] = (_parityEven? -x: x); 491 492 Matrix44<T> N; 493 N.rotate (r); 494 495 N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0, 496 M[1][0], M[1][1], M[1][2], 0, 497 M[2][0], M[2][1], M[2][2], 0, 498 0, 0, 0, 1); 499 // 500 // Extract the other two angles, y and z, from N. 501 // 502 503 T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]); 504 y = Math<T>::atan2 (sy, N[i][i]); 505 z = Math<T>::atan2 (N[j][k], N[j][j]); 506 } 507 else 508 { 509 // 510 // Extract the first angle, x. 511 // 512 513 x = Math<T>::atan2 (M[j][k], M[k][k]); 514 515 // 516 // Remove the x rotation from M, so that the remaining 517 // rotation, N, is only around two axes, and gimbal lock 518 // cannot occur. 519 // 520 521 Vec3<T> r (0, 0, 0); 522 r[i] = (_parityEven? -x: x); 523 524 Matrix44<T> N; 525 N.rotate (r); 526 527 N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0, 528 M[1][0], M[1][1], M[1][2], 0, 529 M[2][0], M[2][1], M[2][2], 0, 530 0, 0, 0, 1); 531 // 532 // Extract the other two angles, y and z, from N. 533 // 534 535 T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]); 536 y = Math<T>::atan2 (-N[i][k], cy); 537 z = Math<T>::atan2 (-N[j][i], N[j][j]); 538 } 539 540 if (!_parityEven) 541 *this *= -1; 542 543 if (!_frameStatic) 544 { 545 T t = x; 546 x = z; 547 z = t; 548 } 549 } 550 551 template<class T> 552 void Euler<T>::extract(const Matrix44<T> &M) 553 { 554 int i,j,k; 555 angleOrder(i,j,k); 556 557 if (_initialRepeated) 558 { 559 // 560 // Extract the first angle, x. 561 // 562 563 x = Math<T>::atan2 (M[j][i], M[k][i]); 564 565 // 566 // Remove the x rotation from M, so that the remaining 567 // rotation, N, is only around two axes, and gimbal lock 568 // cannot occur. 569 // 570 571 Vec3<T> r (0, 0, 0); 572 r[i] = (_parityEven? -x: x); 573 574 Matrix44<T> N; 575 N.rotate (r); 576 N = N * M; 577 578 // 579 // Extract the other two angles, y and z, from N. 580 // 581 582 T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]); 583 y = Math<T>::atan2 (sy, N[i][i]); 584 z = Math<T>::atan2 (N[j][k], N[j][j]); 585 } 586 else 587 { 588 // 589 // Extract the first angle, x. 590 // 591 592 x = Math<T>::atan2 (M[j][k], M[k][k]); 593 594 // 595 // Remove the x rotation from M, so that the remaining 596 // rotation, N, is only around two axes, and gimbal lock 597 // cannot occur. 598 // 599 600 Vec3<T> r (0, 0, 0); 601 r[i] = (_parityEven? -x: x); 602 603 Matrix44<T> N; 604 N.rotate (r); 605 N = N * M; 606 607 // 608 // Extract the other two angles, y and z, from N. 609 // 610 611 T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]); 612 y = Math<T>::atan2 (-N[i][k], cy); 613 z = Math<T>::atan2 (-N[j][i], N[j][j]); 614 } 615 616 if (!_parityEven) 617 *this *= -1; 618 619 if (!_frameStatic) 620 { 621 T t = x; 622 x = z; 623 z = t; 624 } 625 } 626 627 template<class T> 628 Matrix33<T> Euler<T>::toMatrix33() const 629 { 630 int i,j,k; 631 angleOrder(i,j,k); 632 633 Vec3<T> angles; 634 635 if ( _frameStatic ) angles = (*this); 636 else angles = Vec3<T>(z,y,x); 637 638 if ( !_parityEven ) angles *= -1.0; 639 640 T ci = Math<T>::cos(angles.x); 641 T cj = Math<T>::cos(angles.y); 642 T ch = Math<T>::cos(angles.z); 643 T si = Math<T>::sin(angles.x); 644 T sj = Math<T>::sin(angles.y); 645 T sh = Math<T>::sin(angles.z); 646 647 T cc = ci*ch; 648 T cs = ci*sh; 649 T sc = si*ch; 650 T ss = si*sh; 651 652 Matrix33<T> M; 653 654 if ( _initialRepeated ) 655 { 656 M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci; 657 M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc; 658 M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss; 659 } 660 else 661 { 662 M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; 663 M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; 664 M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci; 665 } 666 667 return M; 668 } 669 670 template<class T> 671 Matrix44<T> Euler<T>::toMatrix44() const 672 { 673 int i,j,k; 674 angleOrder(i,j,k); 675 676 Vec3<T> angles; 677 678 if ( _frameStatic ) angles = (*this); 679 else angles = Vec3<T>(z,y,x); 680 681 if ( !_parityEven ) angles *= -1.0; 682 683 T ci = Math<T>::cos(angles.x); 684 T cj = Math<T>::cos(angles.y); 685 T ch = Math<T>::cos(angles.z); 686 T si = Math<T>::sin(angles.x); 687 T sj = Math<T>::sin(angles.y); 688 T sh = Math<T>::sin(angles.z); 689 690 T cc = ci*ch; 691 T cs = ci*sh; 692 T sc = si*ch; 693 T ss = si*sh; 694 695 Matrix44<T> M; 696 697 if ( _initialRepeated ) 698 { 699 M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci; 700 M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc; 701 M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss; 702 } 703 else 704 { 705 M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; 706 M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; 707 M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci; 708 } 709 710 return M; 711 } 712 713 template<class T> 714 Quat<T> Euler<T>::toQuat() const 715 { 716 Vec3<T> angles; 717 int i,j,k; 718 angleOrder(i,j,k); 719 720 if ( _frameStatic ) angles = (*this); 721 else angles = Vec3<T>(z,y,x); 722 723 if ( !_parityEven ) angles.y = -angles.y; 724 725 T ti = angles.x*0.5; 726 T tj = angles.y*0.5; 727 T th = angles.z*0.5; 728 T ci = Math<T>::cos(ti); 729 T cj = Math<T>::cos(tj); 730 T ch = Math<T>::cos(th); 731 T si = Math<T>::sin(ti); 732 T sj = Math<T>::sin(tj); 733 T sh = Math<T>::sin(th); 734 T cc = ci*ch; 735 T cs = ci*sh; 736 T sc = si*ch; 737 T ss = si*sh; 738 739 T parity = _parityEven ? 1.0 : -1.0; 740 741 Quat<T> q; 742 Vec3<T> a; 743 744 if ( _initialRepeated ) 745 { 746 a[i] = cj*(cs + sc); 747 a[j] = sj*(cc + ss) * parity, 748 a[k] = sj*(cs - sc); 749 q.r = cj*(cc - ss); 750 } 751 else 752 { 753 a[i] = cj*sc - sj*cs, 754 a[j] = (cj*ss + sj*cc) * parity, 755 a[k] = cj*cs - sj*sc; 756 q.r = cj*cc + sj*ss; 757 } 758 759 q.v = a; 760 761 return q; 762 } 763 764 template<class T> 765 inline bool 766 Euler<T>::legal(typename Euler<T>::Order order) 767 { 768 return (order & ~Legal) ? false : true; 769 } 770 771 template<class T> 772 typename Euler<T>::Order 773 Euler<T>::order() const 774 { 775 int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0)); 776 777 if (_parityEven) foo |= 0x0100; 778 if (_initialRepeated) foo |= 0x0010; 779 if (_frameStatic) foo++; 780 781 return (Order)foo; 782 } 783 784 template<class T> 785 inline void Euler<T>::setOrder(typename Euler<T>::Order p) 786 { 787 set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis 788 !(p & 0x1), // static? 789 !!(p & 0x100), // permutation even? 790 !!(p & 0x10)); // initial repeats? 791 } 792 793 template<class T> 794 void Euler<T>::set(typename Euler<T>::Axis axis, 795 bool relative, 796 bool parityEven, 797 bool firstRepeats) 798 { 799 _initialAxis = axis; 800 _frameStatic = !relative; 801 _parityEven = parityEven; 802 _initialRepeated = firstRepeats; 803 } 804 805 template<class T> 806 const Euler<T>& Euler<T>::operator= (const Euler<T> &euler) 807 { 808 x = euler.x; 809 y = euler.y; 810 z = euler.z; 811 _initialAxis = euler._initialAxis; 812 _frameStatic = euler._frameStatic; 813 _parityEven = euler._parityEven; 814 _initialRepeated = euler._initialRepeated; 815 return *this; 816 } 817 818 template<class T> 819 const Euler<T>& Euler<T>::operator= (const Vec3<T> &v) 820 { 821 x = v.x; 822 y = v.y; 823 z = v.z; 824 return *this; 825 } 826 827 template<class T> 828 std::ostream& operator << (std::ostream &o, const Euler<T> &euler) 829 { 830 char a[3] = { 'X', 'Y', 'Z' }; 831 832 const char* r = euler.frameStatic() ? "" : "r"; 833 int i,j,k; 834 euler.angleOrder(i,j,k); 835 836 if ( euler.initialRepeated() ) k = i; 837 838 return o << "(" 839 << euler.x << " " 840 << euler.y << " " 841 << euler.z << " " 842 << a[i] << a[j] << a[k] << r << ")"; 843 } 844 845 template <class T> 846 float 847 Euler<T>::angleMod (T angle) 848 { 849 angle = fmod(T (angle), T (2 * M_PI)); 850 851 if (angle < -M_PI) angle += 2 * M_PI; 852 if (angle > +M_PI) angle -= 2 * M_PI; 853 854 return angle; 855 } 856 857 template <class T> 858 void 859 Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot) 860 { 861 Vec3<T> d = xyzRot - targetXyzRot; 862 xyzRot[0] = targetXyzRot[0] + angleMod(d[0]); 863 xyzRot[1] = targetXyzRot[1] + angleMod(d[1]); 864 xyzRot[2] = targetXyzRot[2] + angleMod(d[2]); 865 } 866 867 template <class T> 868 void 869 Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot, 870 Order order) 871 { 872 int i,j,k; 873 Euler<T> e (0,0,0, order); 874 e.angleOrder(i,j,k); 875 876 simpleXYZRotation(xyzRot, targetXyzRot); 877 878 Vec3<T> otherXyzRot; 879 otherXyzRot[i] = M_PI+xyzRot[i]; 880 otherXyzRot[j] = M_PI-xyzRot[j]; 881 otherXyzRot[k] = M_PI+xyzRot[k]; 882 883 simpleXYZRotation(otherXyzRot, targetXyzRot); 884 885 Vec3<T> d = xyzRot - targetXyzRot; 886 Vec3<T> od = otherXyzRot - targetXyzRot; 887 T dMag = d.dot(d); 888 T odMag = od.dot(od); 889 890 if (odMag < dMag) 891 { 892 xyzRot = otherXyzRot; 893 } 894 } 895 896 template <class T> 897 void 898 Euler<T>::makeNear (const Euler<T> &target) 899 { 900 Vec3<T> xyzRot = toXYZVector(); 901 Vec3<T> targetXyz; 902 if (order() != target.order()) 903 { 904 Euler<T> targetSameOrder = Euler<T>(target, order()); 905 targetXyz = targetSameOrder.toXYZVector(); 906 } 907 else 908 { 909 targetXyz = target.toXYZVector(); 910 } 911 912 nearestRotation(xyzRot, targetXyz, order()); 913 914 setXYZVector(xyzRot); 915 } 916 917 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 918 #pragma warning(default:4244) 919 #endif 920 921 } // namespace Imath 922 923 924 #endif 925
