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