/src/FreeImage/Source/OpenEXR/Imath/ImathEuler.h

https://bitbucket.org/cabalistic/ogredeps/ · C++ Header · 916 lines · 525 code · 149 blank · 242 comment · 38 complexity · bfd9da26088dcc325849834762ea783b MD5 · raw file

  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. #ifndef INCLUDED_IMATHEULER_H
  35. #define INCLUDED_IMATHEULER_H
  36. //----------------------------------------------------------------------
  37. //
  38. // template class Euler<T>
  39. //
  40. // This class represents euler angle orientations. The class
  41. // inherits from Vec3 to it can be freely cast. The additional
  42. // information is the euler priorities rep. This class is
  43. // essentially a rip off of Ken Shoemake's GemsIV code. It has
  44. // been modified minimally to make it more understandable, but
  45. // hardly enough to make it easy to grok completely.
  46. //
  47. // There are 24 possible combonations of Euler angle
  48. // representations of which 12 are common in CG and you will
  49. // probably only use 6 of these which in this scheme are the
  50. // non-relative-non-repeating types.
  51. //
  52. // The representations can be partitioned according to two
  53. // criteria:
  54. //
  55. // 1) Are the angles measured relative to a set of fixed axis
  56. // or relative to each other (the latter being what happens
  57. // when rotation matrices are multiplied together and is
  58. // almost ubiquitous in the cg community)
  59. //
  60. // 2) Is one of the rotations repeated (ala XYX rotation)
  61. //
  62. // When you construct a given representation from scratch you
  63. // must order the angles according to their priorities. So, the
  64. // easiest is a softimage or aerospace (yaw/pitch/roll) ordering
  65. // of ZYX.
  66. //
  67. // float x_rot = 1;
  68. // float y_rot = 2;
  69. // float z_rot = 3;
  70. //
  71. // Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
  72. // -or-
  73. // Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
  74. //
  75. // If instead, the order was YXZ for instance you would have to
  76. // do this:
  77. //
  78. // float x_rot = 1;
  79. // float y_rot = 2;
  80. // float z_rot = 3;
  81. //
  82. // Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
  83. // -or-
  84. // Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
  85. //
  86. // Notice how the order you put the angles into the three slots
  87. // should correspond to the enum (YXZ) ordering. The input angle
  88. // vector is called the "ijk" vector -- not an "xyz" vector. The
  89. // ijk vector order is the same as the enum. If you treat the
  90. // Euler<> as a Vec<> (which it inherts from) you will find the
  91. // angles are ordered in the same way, i.e.:
  92. //
  93. // V3f v = angles;
  94. // // v.x == y_rot, v.y == x_rot, v.z == z_rot
  95. //
  96. // If you just want the x, y, and z angles stored in a vector in
  97. // that order, you can do this:
  98. //
  99. // V3f v = angles.toXYZVector()
  100. // // v.x == x_rot, v.y == y_rot, v.z == z_rot
  101. //
  102. // If you want to set the Euler with an XYZVector use the
  103. // optional layout argument:
  104. //
  105. // Eulerf angles(x_rot, y_rot, z_rot,
  106. // Eulerf::YXZ,
  107. // Eulerf::XYZLayout);
  108. //
  109. // This is the same as:
  110. //
  111. // Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
  112. //
  113. // Note that this won't do anything intelligent if you have a
  114. // repeated axis in the euler angles (e.g. XYX)
  115. //
  116. // If you need to use the "relative" versions of these, you will
  117. // need to use the "r" enums.
  118. //
  119. // The units of the rotation angles are assumed to be radians.
  120. //
  121. //----------------------------------------------------------------------
  122. #include "ImathMath.h"
  123. #include "ImathVec.h"
  124. #include "ImathQuat.h"
  125. #include "ImathMatrix.h"
  126. #include "ImathLimits.h"
  127. #include <iostream>
  128. namespace Imath {
  129. #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
  130. // Disable MS VC++ warnings about conversion from double to float
  131. #pragma warning(disable:4244)
  132. #endif
  133. template <class T>
  134. class Euler : public Vec3<T>
  135. {
  136. public:
  137. using Vec3<T>::x;
  138. using Vec3<T>::y;
  139. using Vec3<T>::z;
  140. enum Order
  141. {
  142. //
  143. // All 24 possible orderings
  144. //
  145. XYZ = 0x0101, // "usual" orderings
  146. XZY = 0x0001,
  147. YZX = 0x1101,
  148. YXZ = 0x1001,
  149. ZXY = 0x2101,
  150. ZYX = 0x2001,
  151. XZX = 0x0011, // first axis repeated
  152. XYX = 0x0111,
  153. YXY = 0x1011,
  154. YZY = 0x1111,
  155. ZYZ = 0x2011,
  156. ZXZ = 0x2111,
  157. XYZr = 0x2000, // relative orderings -- not common
  158. XZYr = 0x2100,
  159. YZXr = 0x1000,
  160. YXZr = 0x1100,
  161. ZXYr = 0x0000,
  162. ZYXr = 0x0100,
  163. XZXr = 0x2110, // relative first axis repeated
  164. XYXr = 0x2010,
  165. YXYr = 0x1110,
  166. YZYr = 0x1010,
  167. ZYZr = 0x0110,
  168. ZXZr = 0x0010,
  169. // ||||
  170. // VVVV
  171. // Legend: ABCD
  172. // A -> Initial Axis (0==x, 1==y, 2==z)
  173. // B -> Parity Even (1==true)
  174. // C -> Initial Repeated (1==true)
  175. // D -> Frame Static (1==true)
  176. //
  177. Legal = XYZ | XZY | YZX | YXZ | ZXY | ZYX |
  178. XZX | XYX | YXY | YZY | ZYZ | ZXZ |
  179. XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
  180. XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,
  181. Min = 0x0000,
  182. Max = 0x2111,
  183. Default = XYZ
  184. };
  185. enum Axis { X = 0, Y = 1, Z = 2 };
  186. enum InputLayout { XYZLayout, IJKLayout };
  187. //--------------------------------------------------------------------
  188. // Constructors -- all default to ZYX non-relative ala softimage
  189. // (where there is no argument to specify it)
  190. //
  191. // The Euler-from-matrix constructors assume that the matrix does
  192. // not include shear or non-uniform scaling, but the constructors
  193. // do not examine the matrix to verify this assumption. If necessary,
  194. // you can adjust the matrix by calling the removeScalingAndShear()
  195. // function, defined in ImathMatrixAlgo.h.
  196. //--------------------------------------------------------------------
  197. Euler();
  198. Euler(const Euler&);
  199. Euler(Order p);
  200. Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
  201. Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
  202. Euler(const Euler<T> &euler, Order newp);
  203. Euler(const Matrix33<T> &, Order o = Default);
  204. Euler(const Matrix44<T> &, Order o = Default);
  205. //---------------------------------
  206. // Algebraic functions/ Operators
  207. //---------------------------------
  208. const Euler<T>& operator= (const Euler<T>&);
  209. const Euler<T>& operator= (const Vec3<T>&);
  210. //--------------------------------------------------------
  211. // Set the euler value
  212. // This does NOT convert the angles, but setXYZVector()
  213. // does reorder the input vector.
  214. //--------------------------------------------------------
  215. static bool legal(Order);
  216. void setXYZVector(const Vec3<T> &);
  217. Order order() const;
  218. void setOrder(Order);
  219. void set(Axis initial,
  220. bool relative,
  221. bool parityEven,
  222. bool firstRepeats);
  223. //------------------------------------------------------------
  224. // Conversions, toXYZVector() reorders the angles so that
  225. // the X rotation comes first, followed by the Y and Z
  226. // in cases like XYX ordering, the repeated angle will be
  227. // in the "z" component
  228. //
  229. // The Euler-from-matrix extract() functions assume that the
  230. // matrix does not include shear or non-uniform scaling, but
  231. // the extract() functions do not examine the matrix to verify
  232. // this assumption. If necessary, you can adjust the matrix
  233. // by calling the removeScalingAndShear() function, defined
  234. // in ImathMatrixAlgo.h.
  235. //------------------------------------------------------------
  236. void extract(const Matrix33<T>&);
  237. void extract(const Matrix44<T>&);
  238. void extract(const Quat<T>&);
  239. Matrix33<T> toMatrix33() const;
  240. Matrix44<T> toMatrix44() const;
  241. Quat<T> toQuat() const;
  242. Vec3<T> toXYZVector() const;
  243. //---------------------------------------------------
  244. // Use this function to unpack angles from ijk form
  245. //---------------------------------------------------
  246. void angleOrder(int &i, int &j, int &k) const;
  247. //---------------------------------------------------
  248. // Use this function to determine mapping from xyz to ijk
  249. // - reshuffles the xyz to match the order
  250. //---------------------------------------------------
  251. void angleMapping(int &i, int &j, int &k) const;
  252. //----------------------------------------------------------------------
  253. //
  254. // Utility methods for getting continuous rotations. None of these
  255. // methods change the orientation given by its inputs (or at least
  256. // that is the intent).
  257. //
  258. // angleMod() converts an angle to its equivalent in [-PI, PI]
  259. //
  260. // simpleXYZRotation() adjusts xyzRot so that its components differ
  261. // from targetXyzRot by no more than +-PI
  262. //
  263. // nearestRotation() adjusts xyzRot so that its components differ
  264. // from targetXyzRot by as little as possible.
  265. // Note that xyz here really means ijk, because
  266. // the order must be provided.
  267. //
  268. // makeNear() adjusts "this" Euler so that its components differ
  269. // from target by as little as possible. This method
  270. // might not make sense for Eulers with different order
  271. // and it probably doesn't work for repeated axis and
  272. // relative orderings (TODO).
  273. //
  274. //-----------------------------------------------------------------------
  275. static float angleMod (T angle);
  276. static void simpleXYZRotation (Vec3<T> &xyzRot,
  277. const Vec3<T> &targetXyzRot);
  278. static void nearestRotation (Vec3<T> &xyzRot,
  279. const Vec3<T> &targetXyzRot,
  280. Order order = XYZ);
  281. void makeNear (const Euler<T> &target);
  282. bool frameStatic() const { return _frameStatic; }
  283. bool initialRepeated() const { return _initialRepeated; }
  284. bool parityEven() const { return _parityEven; }
  285. Axis initialAxis() const { return _initialAxis; }
  286. protected:
  287. bool _frameStatic : 1; // relative or static rotations
  288. bool _initialRepeated : 1; // init axis repeated as last
  289. bool _parityEven : 1; // "parity of axis permutation"
  290. #if defined _WIN32 || defined _WIN64
  291. Axis _initialAxis ; // First axis of rotation
  292. #else
  293. Axis _initialAxis : 2; // First axis of rotation
  294. #endif
  295. };
  296. //--------------------
  297. // Convenient typedefs
  298. //--------------------
  299. typedef Euler<float> Eulerf;
  300. typedef Euler<double> Eulerd;
  301. //---------------
  302. // Implementation
  303. //---------------
  304. template<class T>
  305. inline void
  306. Euler<T>::angleOrder(int &i, int &j, int &k) const
  307. {
  308. i = _initialAxis;
  309. j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
  310. k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
  311. }
  312. template<class T>
  313. inline void
  314. Euler<T>::angleMapping(int &i, int &j, int &k) const
  315. {
  316. int m[3];
  317. m[_initialAxis] = 0;
  318. m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
  319. m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
  320. i = m[0];
  321. j = m[1];
  322. k = m[2];
  323. }
  324. template<class T>
  325. inline void
  326. Euler<T>::setXYZVector(const Vec3<T> &v)
  327. {
  328. int i,j,k;
  329. angleMapping(i,j,k);
  330. (*this)[i] = v.x;
  331. (*this)[j] = v.y;
  332. (*this)[k] = v.z;
  333. }
  334. template<class T>
  335. inline Vec3<T>
  336. Euler<T>::toXYZVector() const
  337. {
  338. int i,j,k;
  339. angleMapping(i,j,k);
  340. return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
  341. }
  342. template<class T>
  343. Euler<T>::Euler() :
  344. Vec3<T>(0,0,0),
  345. _frameStatic(true),
  346. _initialRepeated(false),
  347. _parityEven(true),
  348. _initialAxis(X)
  349. {}
  350. template<class T>
  351. Euler<T>::Euler(typename Euler<T>::Order p) :
  352. Vec3<T>(0,0,0),
  353. _frameStatic(true),
  354. _initialRepeated(false),
  355. _parityEven(true),
  356. _initialAxis(X)
  357. {
  358. setOrder(p);
  359. }
  360. template<class T>
  361. inline Euler<T>::Euler( const Vec3<T> &v,
  362. typename Euler<T>::Order p,
  363. typename Euler<T>::InputLayout l )
  364. {
  365. setOrder(p);
  366. if ( l == XYZLayout ) setXYZVector(v);
  367. else { x = v.x; y = v.y; z = v.z; }
  368. }
  369. template<class T>
  370. inline Euler<T>::Euler(const Euler<T> &euler)
  371. {
  372. operator=(euler);
  373. }
  374. template<class T>
  375. inline Euler<T>::Euler(const Euler<T> &euler,Order p)
  376. {
  377. setOrder(p);
  378. Matrix33<T> M = euler.toMatrix33();
  379. extract(M);
  380. }
  381. template<class T>
  382. inline Euler<T>::Euler( T xi, T yi, T zi,
  383. typename Euler<T>::Order p,
  384. typename Euler<T>::InputLayout l)
  385. {
  386. setOrder(p);
  387. if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
  388. else { x = xi; y = yi; z = zi; }
  389. }
  390. template<class T>
  391. inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
  392. {
  393. setOrder(p);
  394. extract(M);
  395. }
  396. template<class T>
  397. inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
  398. {
  399. setOrder(p);
  400. extract(M);
  401. }
  402. template<class T>
  403. inline void Euler<T>::extract(const Quat<T> &q)
  404. {
  405. extract(q.toMatrix33());
  406. }
  407. template<class T>
  408. void Euler<T>::extract(const Matrix33<T> &M)
  409. {
  410. int i,j,k;
  411. angleOrder(i,j,k);
  412. if (_initialRepeated)
  413. {
  414. //
  415. // Extract the first angle, x.
  416. //
  417. x = Math<T>::atan2 (M[j][i], M[k][i]);
  418. //
  419. // Remove the x rotation from M, so that the remaining
  420. // rotation, N, is only around two axes, and gimbal lock
  421. // cannot occur.
  422. //
  423. Vec3<T> r (0, 0, 0);
  424. r[i] = (_parityEven? -x: x);
  425. Matrix44<T> N;
  426. N.rotate (r);
  427. N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
  428. M[1][0], M[1][1], M[1][2], 0,
  429. M[2][0], M[2][1], M[2][2], 0,
  430. 0, 0, 0, 1);
  431. //
  432. // Extract the other two angles, y and z, from N.
  433. //
  434. T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
  435. y = Math<T>::atan2 (sy, N[i][i]);
  436. z = Math<T>::atan2 (N[j][k], N[j][j]);
  437. }
  438. else
  439. {
  440. //
  441. // Extract the first angle, x.
  442. //
  443. x = Math<T>::atan2 (M[j][k], M[k][k]);
  444. //
  445. // Remove the x rotation from M, so that the remaining
  446. // rotation, N, is only around two axes, and gimbal lock
  447. // cannot occur.
  448. //
  449. Vec3<T> r (0, 0, 0);
  450. r[i] = (_parityEven? -x: x);
  451. Matrix44<T> N;
  452. N.rotate (r);
  453. N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
  454. M[1][0], M[1][1], M[1][2], 0,
  455. M[2][0], M[2][1], M[2][2], 0,
  456. 0, 0, 0, 1);
  457. //
  458. // Extract the other two angles, y and z, from N.
  459. //
  460. T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
  461. y = Math<T>::atan2 (-N[i][k], cy);
  462. z = Math<T>::atan2 (-N[j][i], N[j][j]);
  463. }
  464. if (!_parityEven)
  465. *this *= -1;
  466. if (!_frameStatic)
  467. {
  468. T t = x;
  469. x = z;
  470. z = t;
  471. }
  472. }
  473. template<class T>
  474. void Euler<T>::extract(const Matrix44<T> &M)
  475. {
  476. int i,j,k;
  477. angleOrder(i,j,k);
  478. if (_initialRepeated)
  479. {
  480. //
  481. // Extract the first angle, x.
  482. //
  483. x = Math<T>::atan2 (M[j][i], M[k][i]);
  484. //
  485. // Remove the x rotation from M, so that the remaining
  486. // rotation, N, is only around two axes, and gimbal lock
  487. // cannot occur.
  488. //
  489. Vec3<T> r (0, 0, 0);
  490. r[i] = (_parityEven? -x: x);
  491. Matrix44<T> N;
  492. N.rotate (r);
  493. N = N * M;
  494. //
  495. // Extract the other two angles, y and z, from N.
  496. //
  497. T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
  498. y = Math<T>::atan2 (sy, N[i][i]);
  499. z = Math<T>::atan2 (N[j][k], N[j][j]);
  500. }
  501. else
  502. {
  503. //
  504. // Extract the first angle, x.
  505. //
  506. x = Math<T>::atan2 (M[j][k], M[k][k]);
  507. //
  508. // Remove the x rotation from M, so that the remaining
  509. // rotation, N, is only around two axes, and gimbal lock
  510. // cannot occur.
  511. //
  512. Vec3<T> r (0, 0, 0);
  513. r[i] = (_parityEven? -x: x);
  514. Matrix44<T> N;
  515. N.rotate (r);
  516. N = N * M;
  517. //
  518. // Extract the other two angles, y and z, from N.
  519. //
  520. T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
  521. y = Math<T>::atan2 (-N[i][k], cy);
  522. z = Math<T>::atan2 (-N[j][i], N[j][j]);
  523. }
  524. if (!_parityEven)
  525. *this *= -1;
  526. if (!_frameStatic)
  527. {
  528. T t = x;
  529. x = z;
  530. z = t;
  531. }
  532. }
  533. template<class T>
  534. Matrix33<T> Euler<T>::toMatrix33() const
  535. {
  536. int i,j,k;
  537. angleOrder(i,j,k);
  538. Vec3<T> angles;
  539. if ( _frameStatic ) angles = (*this);
  540. else angles = Vec3<T>(z,y,x);
  541. if ( !_parityEven ) angles *= -1.0;
  542. T ci = Math<T>::cos(angles.x);
  543. T cj = Math<T>::cos(angles.y);
  544. T ch = Math<T>::cos(angles.z);
  545. T si = Math<T>::sin(angles.x);
  546. T sj = Math<T>::sin(angles.y);
  547. T sh = Math<T>::sin(angles.z);
  548. T cc = ci*ch;
  549. T cs = ci*sh;
  550. T sc = si*ch;
  551. T ss = si*sh;
  552. Matrix33<T> M;
  553. if ( _initialRepeated )
  554. {
  555. M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci;
  556. M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
  557. M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss;
  558. }
  559. else
  560. {
  561. M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
  562. M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
  563. M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci;
  564. }
  565. return M;
  566. }
  567. template<class T>
  568. Matrix44<T> Euler<T>::toMatrix44() const
  569. {
  570. int i,j,k;
  571. angleOrder(i,j,k);
  572. Vec3<T> angles;
  573. if ( _frameStatic ) angles = (*this);
  574. else angles = Vec3<T>(z,y,x);
  575. if ( !_parityEven ) angles *= -1.0;
  576. T ci = Math<T>::cos(angles.x);
  577. T cj = Math<T>::cos(angles.y);
  578. T ch = Math<T>::cos(angles.z);
  579. T si = Math<T>::sin(angles.x);
  580. T sj = Math<T>::sin(angles.y);
  581. T sh = Math<T>::sin(angles.z);
  582. T cc = ci*ch;
  583. T cs = ci*sh;
  584. T sc = si*ch;
  585. T ss = si*sh;
  586. Matrix44<T> M;
  587. if ( _initialRepeated )
  588. {
  589. M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci;
  590. M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
  591. M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss;
  592. }
  593. else
  594. {
  595. M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
  596. M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
  597. M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci;
  598. }
  599. return M;
  600. }
  601. template<class T>
  602. Quat<T> Euler<T>::toQuat() const
  603. {
  604. Vec3<T> angles;
  605. int i,j,k;
  606. angleOrder(i,j,k);
  607. if ( _frameStatic ) angles = (*this);
  608. else angles = Vec3<T>(z,y,x);
  609. if ( !_parityEven ) angles.y = -angles.y;
  610. T ti = angles.x*0.5;
  611. T tj = angles.y*0.5;
  612. T th = angles.z*0.5;
  613. T ci = Math<T>::cos(ti);
  614. T cj = Math<T>::cos(tj);
  615. T ch = Math<T>::cos(th);
  616. T si = Math<T>::sin(ti);
  617. T sj = Math<T>::sin(tj);
  618. T sh = Math<T>::sin(th);
  619. T cc = ci*ch;
  620. T cs = ci*sh;
  621. T sc = si*ch;
  622. T ss = si*sh;
  623. T parity = _parityEven ? 1.0 : -1.0;
  624. Quat<T> q;
  625. Vec3<T> a;
  626. if ( _initialRepeated )
  627. {
  628. a[i] = cj*(cs + sc);
  629. a[j] = sj*(cc + ss) * parity,
  630. a[k] = sj*(cs - sc);
  631. q.r = cj*(cc - ss);
  632. }
  633. else
  634. {
  635. a[i] = cj*sc - sj*cs,
  636. a[j] = (cj*ss + sj*cc) * parity,
  637. a[k] = cj*cs - sj*sc;
  638. q.r = cj*cc + sj*ss;
  639. }
  640. q.v = a;
  641. return q;
  642. }
  643. template<class T>
  644. inline bool
  645. Euler<T>::legal(typename Euler<T>::Order order)
  646. {
  647. return (order & ~Legal) ? false : true;
  648. }
  649. template<class T>
  650. typename Euler<T>::Order
  651. Euler<T>::order() const
  652. {
  653. int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
  654. if (_parityEven) foo |= 0x0100;
  655. if (_initialRepeated) foo |= 0x0010;
  656. if (_frameStatic) foo++;
  657. return (Order)foo;
  658. }
  659. template<class T>
  660. inline void Euler<T>::setOrder(typename Euler<T>::Order p)
  661. {
  662. set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
  663. !(p & 0x1), // static?
  664. !!(p & 0x100), // permutation even?
  665. !!(p & 0x10)); // initial repeats?
  666. }
  667. template<class T>
  668. void Euler<T>::set(typename Euler<T>::Axis axis,
  669. bool relative,
  670. bool parityEven,
  671. bool firstRepeats)
  672. {
  673. _initialAxis = axis;
  674. _frameStatic = !relative;
  675. _parityEven = parityEven;
  676. _initialRepeated = firstRepeats;
  677. }
  678. template<class T>
  679. const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
  680. {
  681. x = euler.x;
  682. y = euler.y;
  683. z = euler.z;
  684. _initialAxis = euler._initialAxis;
  685. _frameStatic = euler._frameStatic;
  686. _parityEven = euler._parityEven;
  687. _initialRepeated = euler._initialRepeated;
  688. return *this;
  689. }
  690. template<class T>
  691. const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
  692. {
  693. x = v.x;
  694. y = v.y;
  695. z = v.z;
  696. return *this;
  697. }
  698. template<class T>
  699. std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
  700. {
  701. char a[3] = { 'X', 'Y', 'Z' };
  702. const char* r = euler.frameStatic() ? "" : "r";
  703. int i,j,k;
  704. euler.angleOrder(i,j,k);
  705. if ( euler.initialRepeated() ) k = i;
  706. return o << "("
  707. << euler.x << " "
  708. << euler.y << " "
  709. << euler.z << " "
  710. << a[i] << a[j] << a[k] << r << ")";
  711. }
  712. template <class T>
  713. float
  714. Euler<T>::angleMod (T angle)
  715. {
  716. angle = fmod(T (angle), T (2 * M_PI));
  717. if (angle < -M_PI) angle += 2 * M_PI;
  718. if (angle > +M_PI) angle -= 2 * M_PI;
  719. return angle;
  720. }
  721. template <class T>
  722. void
  723. Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
  724. {
  725. Vec3<T> d = xyzRot - targetXyzRot;
  726. xyzRot[0] = targetXyzRot[0] + angleMod(d[0]);
  727. xyzRot[1] = targetXyzRot[1] + angleMod(d[1]);
  728. xyzRot[2] = targetXyzRot[2] + angleMod(d[2]);
  729. }
  730. template <class T>
  731. void
  732. Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
  733. Order order)
  734. {
  735. int i,j,k;
  736. Euler<T> e (0,0,0, order);
  737. e.angleOrder(i,j,k);
  738. simpleXYZRotation(xyzRot, targetXyzRot);
  739. Vec3<T> otherXyzRot;
  740. otherXyzRot[i] = M_PI+xyzRot[i];
  741. otherXyzRot[j] = M_PI-xyzRot[j];
  742. otherXyzRot[k] = M_PI+xyzRot[k];
  743. simpleXYZRotation(otherXyzRot, targetXyzRot);
  744. Vec3<T> d = xyzRot - targetXyzRot;
  745. Vec3<T> od = otherXyzRot - targetXyzRot;
  746. T dMag = d.dot(d);
  747. T odMag = od.dot(od);
  748. if (odMag < dMag)
  749. {
  750. xyzRot = otherXyzRot;
  751. }
  752. }
  753. template <class T>
  754. void
  755. Euler<T>::makeNear (const Euler<T> &target)
  756. {
  757. Vec3<T> xyzRot = toXYZVector();
  758. Euler<T> targetSameOrder = Euler<T>(target, order());
  759. Vec3<T> targetXyz = targetSameOrder.toXYZVector();
  760. nearestRotation(xyzRot, targetXyz, order());
  761. setXYZVector(xyzRot);
  762. }
  763. #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
  764. #pragma warning(default:4244)
  765. #endif
  766. } // namespace Imath
  767. #endif