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 135namespace 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 142template <class T> 143class 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 343typedef Euler<float> Eulerf; 344typedef Euler<double> Eulerd; 345 346 347//--------------- 348// Implementation 349//--------------- 350 351template<class T> 352inline 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 360template<class T> 361inline 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 374template<class T> 375inline void 376Euler<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 385template<class T> 386inline Vec3<T> 387Euler<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 395template<class T> 396Euler<T>::Euler() : 397 Vec3<T>(0,0,0), 398 _frameStatic(true), 399 _initialRepeated(false), 400 _parityEven(true), 401 _initialAxis(X) 402{} 403 404template<class T> 405Euler<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 415template<class T> 416inline 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 425template<class T> 426inline Euler<T>::Euler(const Euler<T> &euler) 427{ 428 operator=(euler); 429} 430 431template<class T> 432inline Euler<T>::Euler(const Euler<T> &euler,Order p) 433{ 434 setOrder(p); 435 Matrix33<T> M = euler.toMatrix33(); 436 extract(M); 437} 438 439template<class T> 440inline 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 449template<class T> 450inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p ) 451{ 452 setOrder(p); 453 extract(M); 454} 455 456template<class T> 457inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p ) 458{ 459 setOrder(p); 460 extract(M); 461} 462 463template<class T> 464inline void Euler<T>::extract(const Quat<T> &q) 465{ 466 extract(q.toMatrix33()); 467} 468 469template<class T> 470void 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 551template<class T> 552void 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 627template<class T> 628Matrix33<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 670template<class T> 671Matrix44<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 713template<class T> 714Quat<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 764template<class T> 765inline bool 766Euler<T>::legal(typename Euler<T>::Order order) 767{ 768 return (order & ~Legal) ? false : true; 769} 770 771template<class T> 772typename Euler<T>::Order 773Euler<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 784template<class T> 785inline 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 793template<class T> 794void 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 805template<class T> 806const 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 818template<class T> 819const 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 827template<class T> 828std::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 845template <class T> 846float 847Euler<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 857template <class T> 858void 859Euler<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 867template <class T> 868void 869Euler<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 896template <class T> 897void 898Euler<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