PBRT
|
00001 00002 // 00003 // Copyright (c) 2002, Industrial Light & Magic, a division of Lucas 00004 // Digital Ltd. LLC 00005 // 00006 // All rights reserved. 00007 // 00008 // Redistribution and use in source and binary forms, with or without 00009 // modification, are permitted provided that the following conditions are 00010 // met: 00011 // * Redistributions of source code must retain the above copyright 00012 // notice, this list of conditions and the following disclaimer. 00013 // * Redistributions in binary form must reproduce the above 00014 // copyright notice, this list of conditions and the following disclaimer 00015 // in the documentation and/or other materials provided with the 00016 // distribution. 00017 // * Neither the name of Industrial Light & Magic nor the names of 00018 // its contributors may be used to endorse or promote products derived 00019 // from this software without specific prior written permission. 00020 // 00021 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 00024 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 00025 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 00026 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00027 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00028 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 00029 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00030 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00031 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00032 // 00034 00035 00036 00037 #ifndef INCLUDED_IMATHEULER_H 00038 #define INCLUDED_IMATHEULER_H 00039 00040 //---------------------------------------------------------------------- 00041 // 00042 // template class Euler<T> 00043 // 00044 // This class represents euler angle orientations. The class 00045 // inherits from Vec3 to it can be freely cast. The additional 00046 // information is the euler priorities rep. This class is 00047 // essentially a rip off of Ken Shoemake's GemsIV code. It has 00048 // been modified minimally to make it more understandable, but 00049 // hardly enough to make it easy to grok completely. 00050 // 00051 // There are 24 possible combonations of Euler angle 00052 // representations of which 12 are common in CG and you will 00053 // probably only use 6 of these which in this scheme are the 00054 // non-relative-non-repeating types. 00055 // 00056 // The representations can be partitioned according to two 00057 // criteria: 00058 // 00059 // 1) Are the angles measured relative to a set of fixed axis 00060 // or relative to each other (the latter being what happens 00061 // when rotation matrices are multiplied together and is 00062 // almost ubiquitous in the cg community) 00063 // 00064 // 2) Is one of the rotations repeated (ala XYX rotation) 00065 // 00066 // When you construct a given representation from scratch you 00067 // must order the angles according to their priorities. So, the 00068 // easiest is a softimage or aerospace (yaw/pitch/roll) ordering 00069 // of ZYX. 00070 // 00071 // float x_rot = 1; 00072 // float y_rot = 2; 00073 // float z_rot = 3; 00074 // 00075 // Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX); 00076 // -or- 00077 // Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX ); 00078 // 00079 // If instead, the order was YXZ for instance you would have to 00080 // do this: 00081 // 00082 // float x_rot = 1; 00083 // float y_rot = 2; 00084 // float z_rot = 3; 00085 // 00086 // Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ); 00087 // -or- 00088 // Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ ); 00089 // 00090 // Notice how the order you put the angles into the three slots 00091 // should correspond to the enum (YXZ) ordering. The input angle 00092 // vector is called the "ijk" vector -- not an "xyz" vector. The 00093 // ijk vector order is the same as the enum. If you treat the 00094 // Euler<> as a Vec<> (which it inherts from) you will find the 00095 // angles are ordered in the same way, i.e.: 00096 // 00097 // V3f v = angles; 00098 // // v.x == y_rot, v.y == x_rot, v.z == z_rot 00099 // 00100 // If you just want the x, y, and z angles stored in a vector in 00101 // that order, you can do this: 00102 // 00103 // V3f v = angles.toXYZVector() 00104 // // v.x == x_rot, v.y == y_rot, v.z == z_rot 00105 // 00106 // If you want to set the Euler with an XYZVector use the 00107 // optional layout argument: 00108 // 00109 // Eulerf angles(x_rot, y_rot, z_rot, 00110 // Eulerf::YXZ, 00111 // Eulerf::XYZLayout); 00112 // 00113 // This is the same as: 00114 // 00115 // Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ); 00116 // 00117 // Note that this won't do anything intelligent if you have a 00118 // repeated axis in the euler angles (e.g. XYX) 00119 // 00120 // If you need to use the "relative" versions of these, you will 00121 // need to use the "r" enums. 00122 // 00123 // The units of the rotation angles are assumed to be radians. 00124 // 00125 //---------------------------------------------------------------------- 00126 00127 00128 #include "ImathMath.h" 00129 #include "ImathVec.h" 00130 #include "ImathQuat.h" 00131 #include "ImathMatrix.h" 00132 #include "ImathLimits.h" 00133 #include <iostream> 00134 00135 namespace Imath { 00136 00137 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 00138 // Disable MS VC++ warnings about conversion from double to float 00139 #pragma warning(disable:4244) 00140 #endif 00141 00142 template <class T> 00143 class Euler : public Vec3<T> 00144 { 00145 public: 00146 00147 using Vec3<T>::x; 00148 using Vec3<T>::y; 00149 using Vec3<T>::z; 00150 00151 enum Order 00152 { 00153 // 00154 // All 24 possible orderings 00155 // 00156 00157 XYZ = 0x0101, // "usual" orderings 00158 XZY = 0x0001, 00159 YZX = 0x1101, 00160 YXZ = 0x1001, 00161 ZXY = 0x2101, 00162 ZYX = 0x2001, 00163 00164 XZX = 0x0011, // first axis repeated 00165 XYX = 0x0111, 00166 YXY = 0x1011, 00167 YZY = 0x1111, 00168 ZYZ = 0x2011, 00169 ZXZ = 0x2111, 00170 00171 XYZr = 0x2000, // relative orderings -- not common 00172 XZYr = 0x2100, 00173 YZXr = 0x1000, 00174 YXZr = 0x1100, 00175 ZXYr = 0x0000, 00176 ZYXr = 0x0100, 00177 00178 XZXr = 0x2110, // relative first axis repeated 00179 XYXr = 0x2010, 00180 YXYr = 0x1110, 00181 YZYr = 0x1010, 00182 ZYZr = 0x0110, 00183 ZXZr = 0x0010, 00184 // |||| 00185 // VVVV 00186 // Legend: ABCD 00187 // A -> Initial Axis (0==x, 1==y, 2==z) 00188 // B -> Parity Even (1==true) 00189 // C -> Initial Repeated (1==true) 00190 // D -> Frame Static (1==true) 00191 // 00192 00193 Legal = XYZ | XZY | YZX | YXZ | ZXY | ZYX | 00194 XZX | XYX | YXY | YZY | ZYZ | ZXZ | 00195 XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr| 00196 XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr, 00197 00198 Min = 0x0000, 00199 Max = 0x2111, 00200 Default = XYZ 00201 }; 00202 00203 enum Axis { X = 0, Y = 1, Z = 2 }; 00204 00205 enum InputLayout { XYZLayout, IJKLayout }; 00206 00207 //-------------------------------------------------------------------- 00208 // Constructors -- all default to ZYX non-relative ala softimage 00209 // (where there is no argument to specify it) 00210 // 00211 // The Euler-from-matrix constructors assume that the matrix does 00212 // not include shear or non-uniform scaling, but the constructors 00213 // do not examine the matrix to verify this assumption. If necessary, 00214 // you can adjust the matrix by calling the removeScalingAndShear() 00215 // function, defined in ImathMatrixAlgo.h. 00216 //-------------------------------------------------------------------- 00217 00218 Euler(); 00219 Euler(const Euler&); 00220 Euler(Order p); 00221 Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout); 00222 Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout); 00223 Euler(const Euler<T> &euler, Order newp); 00224 Euler(const Matrix33<T> &, Order o = Default); 00225 Euler(const Matrix44<T> &, Order o = Default); 00226 00227 //--------------------------------- 00228 // Algebraic functions/ Operators 00229 //--------------------------------- 00230 00231 const Euler<T>& operator= (const Euler<T>&); 00232 const Euler<T>& operator= (const Vec3<T>&); 00233 00234 //-------------------------------------------------------- 00235 // Set the euler value 00236 // This does NOT convert the angles, but setXYZVector() 00237 // does reorder the input vector. 00238 //-------------------------------------------------------- 00239 00240 static bool legal(Order); 00241 00242 void setXYZVector(const Vec3<T> &); 00243 00244 Order order() const; 00245 void setOrder(Order); 00246 00247 void set(Axis initial, 00248 bool relative, 00249 bool parityEven, 00250 bool firstRepeats); 00251 00252 //------------------------------------------------------------ 00253 // Conversions, toXYZVector() reorders the angles so that 00254 // the X rotation comes first, followed by the Y and Z 00255 // in cases like XYX ordering, the repeated angle will be 00256 // in the "z" component 00257 // 00258 // The Euler-from-matrix extract() functions assume that the 00259 // matrix does not include shear or non-uniform scaling, but 00260 // the extract() functions do not examine the matrix to verify 00261 // this assumption. If necessary, you can adjust the matrix 00262 // by calling the removeScalingAndShear() function, defined 00263 // in ImathMatrixAlgo.h. 00264 //------------------------------------------------------------ 00265 00266 void extract(const Matrix33<T>&); 00267 void extract(const Matrix44<T>&); 00268 void extract(const Quat<T>&); 00269 00270 Matrix33<T> toMatrix33() const; 00271 Matrix44<T> toMatrix44() const; 00272 Quat<T> toQuat() const; 00273 Vec3<T> toXYZVector() const; 00274 00275 //--------------------------------------------------- 00276 // Use this function to unpack angles from ijk form 00277 //--------------------------------------------------- 00278 00279 void angleOrder(int &i, int &j, int &k) const; 00280 00281 //--------------------------------------------------- 00282 // Use this function to determine mapping from xyz to ijk 00283 // - reshuffles the xyz to match the order 00284 //--------------------------------------------------- 00285 00286 void angleMapping(int &i, int &j, int &k) const; 00287 00288 //---------------------------------------------------------------------- 00289 // 00290 // Utility methods for getting continuous rotations. None of these 00291 // methods change the orientation given by its inputs (or at least 00292 // that is the intent). 00293 // 00294 // angleMod() converts an angle to its equivalent in [-PI, PI] 00295 // 00296 // simpleXYZRotation() adjusts xyzRot so that its components differ 00297 // from targetXyzRot by no more than +-PI 00298 // 00299 // nearestRotation() adjusts xyzRot so that its components differ 00300 // from targetXyzRot by as little as possible. 00301 // Note that xyz here really means ijk, because 00302 // the order must be provided. 00303 // 00304 // makeNear() adjusts "this" Euler so that its components differ 00305 // from target by as little as possible. This method 00306 // might not make sense for Eulers with different order 00307 // and it probably doesn't work for repeated axis and 00308 // relative orderings (TODO). 00309 // 00310 //----------------------------------------------------------------------- 00311 00312 static float angleMod (T angle); 00313 static void simpleXYZRotation (Vec3<T> &xyzRot, 00314 const Vec3<T> &targetXyzRot); 00315 static void nearestRotation (Vec3<T> &xyzRot, 00316 const Vec3<T> &targetXyzRot, 00317 Order order = XYZ); 00318 00319 void makeNear (const Euler<T> &target); 00320 00321 bool frameStatic() const { return _frameStatic; } 00322 bool initialRepeated() const { return _initialRepeated; } 00323 bool parityEven() const { return _parityEven; } 00324 Axis initialAxis() const { return _initialAxis; } 00325 00326 protected: 00327 00328 bool _frameStatic : 1; // relative or static rotations 00329 bool _initialRepeated : 1; // init axis repeated as last 00330 bool _parityEven : 1; // "parity of axis permutation" 00331 #if defined _WIN32 || defined _WIN64 00332 Axis _initialAxis ; // First axis of rotation 00333 #else 00334 Axis _initialAxis : 2; // First axis of rotation 00335 #endif 00336 }; 00337 00338 00339 //-------------------- 00340 // Convenient typedefs 00341 //-------------------- 00342 00343 typedef Euler<float> Eulerf; 00344 typedef Euler<double> Eulerd; 00345 00346 00347 //--------------- 00348 // Implementation 00349 //--------------- 00350 00351 template<class T> 00352 inline void 00353 Euler<T>::angleOrder(int &i, int &j, int &k) const 00354 { 00355 i = _initialAxis; 00356 j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2); 00357 k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3; 00358 } 00359 00360 template<class T> 00361 inline void 00362 Euler<T>::angleMapping(int &i, int &j, int &k) const 00363 { 00364 int m[3]; 00365 00366 m[_initialAxis] = 0; 00367 m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2; 00368 m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1; 00369 i = m[0]; 00370 j = m[1]; 00371 k = m[2]; 00372 } 00373 00374 template<class T> 00375 inline void 00376 Euler<T>::setXYZVector(const Vec3<T> &v) 00377 { 00378 int i,j,k; 00379 angleMapping(i,j,k); 00380 (*this)[i] = v.x; 00381 (*this)[j] = v.y; 00382 (*this)[k] = v.z; 00383 } 00384 00385 template<class T> 00386 inline Vec3<T> 00387 Euler<T>::toXYZVector() const 00388 { 00389 int i,j,k; 00390 angleMapping(i,j,k); 00391 return Vec3<T>((*this)[i],(*this)[j],(*this)[k]); 00392 } 00393 00394 00395 template<class T> 00396 Euler<T>::Euler() : 00397 Vec3<T>(0,0,0), 00398 _frameStatic(true), 00399 _initialRepeated(false), 00400 _parityEven(true), 00401 _initialAxis(X) 00402 {} 00403 00404 template<class T> 00405 Euler<T>::Euler(typename Euler<T>::Order p) : 00406 Vec3<T>(0,0,0), 00407 _frameStatic(true), 00408 _initialRepeated(false), 00409 _parityEven(true), 00410 _initialAxis(X) 00411 { 00412 setOrder(p); 00413 } 00414 00415 template<class T> 00416 inline Euler<T>::Euler( const Vec3<T> &v, 00417 typename Euler<T>::Order p, 00418 typename Euler<T>::InputLayout l ) 00419 { 00420 setOrder(p); 00421 if ( l == XYZLayout ) setXYZVector(v); 00422 else { x = v.x; y = v.y; z = v.z; } 00423 } 00424 00425 template<class T> 00426 inline Euler<T>::Euler(const Euler<T> &euler) 00427 { 00428 operator=(euler); 00429 } 00430 00431 template<class T> 00432 inline Euler<T>::Euler(const Euler<T> &euler,Order p) 00433 { 00434 setOrder(p); 00435 Matrix33<T> M = euler.toMatrix33(); 00436 extract(M); 00437 } 00438 00439 template<class T> 00440 inline Euler<T>::Euler( T xi, T yi, T zi, 00441 typename Euler<T>::Order p, 00442 typename Euler<T>::InputLayout l) 00443 { 00444 setOrder(p); 00445 if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi)); 00446 else { x = xi; y = yi; z = zi; } 00447 } 00448 00449 template<class T> 00450 inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p ) 00451 { 00452 setOrder(p); 00453 extract(M); 00454 } 00455 00456 template<class T> 00457 inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p ) 00458 { 00459 setOrder(p); 00460 extract(M); 00461 } 00462 00463 template<class T> 00464 inline void Euler<T>::extract(const Quat<T> &q) 00465 { 00466 extract(q.toMatrix33()); 00467 } 00468 00469 template<class T> 00470 void Euler<T>::extract(const Matrix33<T> &M) 00471 { 00472 int i,j,k; 00473 angleOrder(i,j,k); 00474 00475 if (_initialRepeated) 00476 { 00477 // 00478 // Extract the first angle, x. 00479 // 00480 00481 x = Math<T>::atan2 (M[j][i], M[k][i]); 00482 00483 // 00484 // Remove the x rotation from M, so that the remaining 00485 // rotation, N, is only around two axes, and gimbal lock 00486 // cannot occur. 00487 // 00488 00489 Vec3<T> r (0, 0, 0); 00490 r[i] = (_parityEven? -x: x); 00491 00492 Matrix44<T> N; 00493 N.rotate (r); 00494 00495 N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0, 00496 M[1][0], M[1][1], M[1][2], 0, 00497 M[2][0], M[2][1], M[2][2], 0, 00498 0, 0, 0, 1); 00499 // 00500 // Extract the other two angles, y and z, from N. 00501 // 00502 00503 T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]); 00504 y = Math<T>::atan2 (sy, N[i][i]); 00505 z = Math<T>::atan2 (N[j][k], N[j][j]); 00506 } 00507 else 00508 { 00509 // 00510 // Extract the first angle, x. 00511 // 00512 00513 x = Math<T>::atan2 (M[j][k], M[k][k]); 00514 00515 // 00516 // Remove the x rotation from M, so that the remaining 00517 // rotation, N, is only around two axes, and gimbal lock 00518 // cannot occur. 00519 // 00520 00521 Vec3<T> r (0, 0, 0); 00522 r[i] = (_parityEven? -x: x); 00523 00524 Matrix44<T> N; 00525 N.rotate (r); 00526 00527 N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0, 00528 M[1][0], M[1][1], M[1][2], 0, 00529 M[2][0], M[2][1], M[2][2], 0, 00530 0, 0, 0, 1); 00531 // 00532 // Extract the other two angles, y and z, from N. 00533 // 00534 00535 T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]); 00536 y = Math<T>::atan2 (-N[i][k], cy); 00537 z = Math<T>::atan2 (-N[j][i], N[j][j]); 00538 } 00539 00540 if (!_parityEven) 00541 *this *= -1; 00542 00543 if (!_frameStatic) 00544 { 00545 T t = x; 00546 x = z; 00547 z = t; 00548 } 00549 } 00550 00551 template<class T> 00552 void Euler<T>::extract(const Matrix44<T> &M) 00553 { 00554 int i,j,k; 00555 angleOrder(i,j,k); 00556 00557 if (_initialRepeated) 00558 { 00559 // 00560 // Extract the first angle, x. 00561 // 00562 00563 x = Math<T>::atan2 (M[j][i], M[k][i]); 00564 00565 // 00566 // Remove the x rotation from M, so that the remaining 00567 // rotation, N, is only around two axes, and gimbal lock 00568 // cannot occur. 00569 // 00570 00571 Vec3<T> r (0, 0, 0); 00572 r[i] = (_parityEven? -x: x); 00573 00574 Matrix44<T> N; 00575 N.rotate (r); 00576 N = N * M; 00577 00578 // 00579 // Extract the other two angles, y and z, from N. 00580 // 00581 00582 T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]); 00583 y = Math<T>::atan2 (sy, N[i][i]); 00584 z = Math<T>::atan2 (N[j][k], N[j][j]); 00585 } 00586 else 00587 { 00588 // 00589 // Extract the first angle, x. 00590 // 00591 00592 x = Math<T>::atan2 (M[j][k], M[k][k]); 00593 00594 // 00595 // Remove the x rotation from M, so that the remaining 00596 // rotation, N, is only around two axes, and gimbal lock 00597 // cannot occur. 00598 // 00599 00600 Vec3<T> r (0, 0, 0); 00601 r[i] = (_parityEven? -x: x); 00602 00603 Matrix44<T> N; 00604 N.rotate (r); 00605 N = N * M; 00606 00607 // 00608 // Extract the other two angles, y and z, from N. 00609 // 00610 00611 T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]); 00612 y = Math<T>::atan2 (-N[i][k], cy); 00613 z = Math<T>::atan2 (-N[j][i], N[j][j]); 00614 } 00615 00616 if (!_parityEven) 00617 *this *= -1; 00618 00619 if (!_frameStatic) 00620 { 00621 T t = x; 00622 x = z; 00623 z = t; 00624 } 00625 } 00626 00627 template<class T> 00628 Matrix33<T> Euler<T>::toMatrix33() const 00629 { 00630 int i,j,k; 00631 angleOrder(i,j,k); 00632 00633 Vec3<T> angles; 00634 00635 if ( _frameStatic ) angles = (*this); 00636 else angles = Vec3<T>(z,y,x); 00637 00638 if ( !_parityEven ) angles *= -1.0; 00639 00640 T ci = Math<T>::cos(angles.x); 00641 T cj = Math<T>::cos(angles.y); 00642 T ch = Math<T>::cos(angles.z); 00643 T si = Math<T>::sin(angles.x); 00644 T sj = Math<T>::sin(angles.y); 00645 T sh = Math<T>::sin(angles.z); 00646 00647 T cc = ci*ch; 00648 T cs = ci*sh; 00649 T sc = si*ch; 00650 T ss = si*sh; 00651 00652 Matrix33<T> M; 00653 00654 if ( _initialRepeated ) 00655 { 00656 M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci; 00657 M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc; 00658 M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss; 00659 } 00660 else 00661 { 00662 M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; 00663 M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; 00664 M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci; 00665 } 00666 00667 return M; 00668 } 00669 00670 template<class T> 00671 Matrix44<T> Euler<T>::toMatrix44() const 00672 { 00673 int i,j,k; 00674 angleOrder(i,j,k); 00675 00676 Vec3<T> angles; 00677 00678 if ( _frameStatic ) angles = (*this); 00679 else angles = Vec3<T>(z,y,x); 00680 00681 if ( !_parityEven ) angles *= -1.0; 00682 00683 T ci = Math<T>::cos(angles.x); 00684 T cj = Math<T>::cos(angles.y); 00685 T ch = Math<T>::cos(angles.z); 00686 T si = Math<T>::sin(angles.x); 00687 T sj = Math<T>::sin(angles.y); 00688 T sh = Math<T>::sin(angles.z); 00689 00690 T cc = ci*ch; 00691 T cs = ci*sh; 00692 T sc = si*ch; 00693 T ss = si*sh; 00694 00695 Matrix44<T> M; 00696 00697 if ( _initialRepeated ) 00698 { 00699 M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci; 00700 M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc; 00701 M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss; 00702 } 00703 else 00704 { 00705 M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; 00706 M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; 00707 M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci; 00708 } 00709 00710 return M; 00711 } 00712 00713 template<class T> 00714 Quat<T> Euler<T>::toQuat() const 00715 { 00716 Vec3<T> angles; 00717 int i,j,k; 00718 angleOrder(i,j,k); 00719 00720 if ( _frameStatic ) angles = (*this); 00721 else angles = Vec3<T>(z,y,x); 00722 00723 if ( !_parityEven ) angles.y = -angles.y; 00724 00725 T ti = angles.x*0.5; 00726 T tj = angles.y*0.5; 00727 T th = angles.z*0.5; 00728 T ci = Math<T>::cos(ti); 00729 T cj = Math<T>::cos(tj); 00730 T ch = Math<T>::cos(th); 00731 T si = Math<T>::sin(ti); 00732 T sj = Math<T>::sin(tj); 00733 T sh = Math<T>::sin(th); 00734 T cc = ci*ch; 00735 T cs = ci*sh; 00736 T sc = si*ch; 00737 T ss = si*sh; 00738 00739 T parity = _parityEven ? 1.0 : -1.0; 00740 00741 Quat<T> q; 00742 Vec3<T> a; 00743 00744 if ( _initialRepeated ) 00745 { 00746 a[i] = cj*(cs + sc); 00747 a[j] = sj*(cc + ss) * parity, 00748 a[k] = sj*(cs - sc); 00749 q.r = cj*(cc - ss); 00750 } 00751 else 00752 { 00753 a[i] = cj*sc - sj*cs, 00754 a[j] = (cj*ss + sj*cc) * parity, 00755 a[k] = cj*cs - sj*sc; 00756 q.r = cj*cc + sj*ss; 00757 } 00758 00759 q.v = a; 00760 00761 return q; 00762 } 00763 00764 template<class T> 00765 inline bool 00766 Euler<T>::legal(typename Euler<T>::Order order) 00767 { 00768 return (order & ~Legal) ? false : true; 00769 } 00770 00771 template<class T> 00772 typename Euler<T>::Order 00773 Euler<T>::order() const 00774 { 00775 int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0)); 00776 00777 if (_parityEven) foo |= 0x0100; 00778 if (_initialRepeated) foo |= 0x0010; 00779 if (_frameStatic) foo++; 00780 00781 return (Order)foo; 00782 } 00783 00784 template<class T> 00785 inline void Euler<T>::setOrder(typename Euler<T>::Order p) 00786 { 00787 set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis 00788 !(p & 0x1), // static? 00789 !!(p & 0x100), // permutation even? 00790 !!(p & 0x10)); // initial repeats? 00791 } 00792 00793 template<class T> 00794 void Euler<T>::set(typename Euler<T>::Axis axis, 00795 bool relative, 00796 bool parityEven, 00797 bool firstRepeats) 00798 { 00799 _initialAxis = axis; 00800 _frameStatic = !relative; 00801 _parityEven = parityEven; 00802 _initialRepeated = firstRepeats; 00803 } 00804 00805 template<class T> 00806 const Euler<T>& Euler<T>::operator= (const Euler<T> &euler) 00807 { 00808 x = euler.x; 00809 y = euler.y; 00810 z = euler.z; 00811 _initialAxis = euler._initialAxis; 00812 _frameStatic = euler._frameStatic; 00813 _parityEven = euler._parityEven; 00814 _initialRepeated = euler._initialRepeated; 00815 return *this; 00816 } 00817 00818 template<class T> 00819 const Euler<T>& Euler<T>::operator= (const Vec3<T> &v) 00820 { 00821 x = v.x; 00822 y = v.y; 00823 z = v.z; 00824 return *this; 00825 } 00826 00827 template<class T> 00828 std::ostream& operator << (std::ostream &o, const Euler<T> &euler) 00829 { 00830 char a[3] = { 'X', 'Y', 'Z' }; 00831 00832 const char* r = euler.frameStatic() ? "" : "r"; 00833 int i,j,k; 00834 euler.angleOrder(i,j,k); 00835 00836 if ( euler.initialRepeated() ) k = i; 00837 00838 return o << "(" 00839 << euler.x << " " 00840 << euler.y << " " 00841 << euler.z << " " 00842 << a[i] << a[j] << a[k] << r << ")"; 00843 } 00844 00845 template <class T> 00846 float 00847 Euler<T>::angleMod (T angle) 00848 { 00849 angle = fmod(T (angle), T (2 * M_PI)); 00850 00851 if (angle < -M_PI) angle += 2 * M_PI; 00852 if (angle > +M_PI) angle -= 2 * M_PI; 00853 00854 return angle; 00855 } 00856 00857 template <class T> 00858 void 00859 Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot) 00860 { 00861 Vec3<T> d = xyzRot - targetXyzRot; 00862 xyzRot[0] = targetXyzRot[0] + angleMod(d[0]); 00863 xyzRot[1] = targetXyzRot[1] + angleMod(d[1]); 00864 xyzRot[2] = targetXyzRot[2] + angleMod(d[2]); 00865 } 00866 00867 template <class T> 00868 void 00869 Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot, 00870 Order order) 00871 { 00872 int i,j,k; 00873 Euler<T> e (0,0,0, order); 00874 e.angleOrder(i,j,k); 00875 00876 simpleXYZRotation(xyzRot, targetXyzRot); 00877 00878 Vec3<T> otherXyzRot; 00879 otherXyzRot[i] = M_PI+xyzRot[i]; 00880 otherXyzRot[j] = M_PI-xyzRot[j]; 00881 otherXyzRot[k] = M_PI+xyzRot[k]; 00882 00883 simpleXYZRotation(otherXyzRot, targetXyzRot); 00884 00885 Vec3<T> d = xyzRot - targetXyzRot; 00886 Vec3<T> od = otherXyzRot - targetXyzRot; 00887 T dMag = d.dot(d); 00888 T odMag = od.dot(od); 00889 00890 if (odMag < dMag) 00891 { 00892 xyzRot = otherXyzRot; 00893 } 00894 } 00895 00896 template <class T> 00897 void 00898 Euler<T>::makeNear (const Euler<T> &target) 00899 { 00900 Vec3<T> xyzRot = toXYZVector(); 00901 Euler<T> targetSameOrder = Euler<T>(target, order()); 00902 Vec3<T> targetXyz = targetSameOrder.toXYZVector(); 00903 00904 nearestRotation(xyzRot, targetXyz, order()); 00905 00906 setXYZVector(xyzRot); 00907 } 00908 00909 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 00910 #pragma warning(default:4244) 00911 #endif 00912 00913 } // namespace Imath 00914 00915 00916 #endif