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

https://bitbucket.org/cabalistic/ogredeps/ · C++ Header · 1115 lines · 629 code · 234 blank · 252 comment · 64 complexity · 151e74894f916f81a6dfef581006074c 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_IMATHMATRIXALGO_H
  35. #define INCLUDED_IMATHMATRIXALGO_H
  36. //-------------------------------------------------------------------------
  37. //
  38. // This file contains algorithms applied to or in conjunction with
  39. // transformation matrices (Imath::Matrix33 and Imath::Matrix44).
  40. // The assumption made is that these functions are called much less
  41. // often than the basic point functions or these functions require
  42. // more support classes.
  43. //
  44. // This file also defines a few predefined constant matrices.
  45. //
  46. //-------------------------------------------------------------------------
  47. #include "ImathMatrix.h"
  48. #include "ImathQuat.h"
  49. #include "ImathEuler.h"
  50. #include "ImathExc.h"
  51. #include "ImathVec.h"
  52. #include <math.h>
  53. #ifdef OPENEXR_DLL
  54. #ifdef IMATH_EXPORTS
  55. #define IMATH_EXPORT_CONST extern __declspec(dllexport)
  56. #else
  57. #define IMATH_EXPORT_CONST extern __declspec(dllimport)
  58. #endif
  59. #else
  60. #define IMATH_EXPORT_CONST extern const
  61. #endif
  62. namespace Imath {
  63. //------------------
  64. // Identity matrices
  65. //------------------
  66. IMATH_EXPORT_CONST M33f identity33f;
  67. IMATH_EXPORT_CONST M44f identity44f;
  68. IMATH_EXPORT_CONST M33d identity33d;
  69. IMATH_EXPORT_CONST M44d identity44d;
  70. //----------------------------------------------------------------------
  71. // Extract scale, shear, rotation, and translation values from a matrix:
  72. //
  73. // Notes:
  74. //
  75. // This implementation follows the technique described in the paper by
  76. // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
  77. // Matrix into Simple Transformations", p. 320.
  78. //
  79. // - Some of the functions below have an optional exc parameter
  80. // that determines the functions' behavior when the matrix'
  81. // scaling is very close to zero:
  82. //
  83. // If exc is true, the functions throw an Imath::ZeroScale exception.
  84. //
  85. // If exc is false:
  86. //
  87. // extractScaling (m, s) returns false, s is invalid
  88. // sansScaling (m) returns m
  89. // removeScaling (m) returns false, m is unchanged
  90. // sansScalingAndShear (m) returns m
  91. // removeScalingAndShear (m) returns false, m is unchanged
  92. // extractAndRemoveScalingAndShear (m, s, h)
  93. // returns false, m is unchanged,
  94. // (sh) are invalid
  95. // checkForZeroScaleInRow () returns false
  96. // extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid
  97. //
  98. // - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX()
  99. // assume that the matrix does not include shear or non-uniform scaling,
  100. // but they do not examine the matrix to verify this assumption.
  101. // Matrices with shear or non-uniform scaling are likely to produce
  102. // meaningless results. Therefore, you should use the
  103. // removeScalingAndShear() routine, if necessary, prior to calling
  104. // extractEuler...() .
  105. //
  106. // - All functions assume that the matrix does not include perspective
  107. // transformation(s), but they do not examine the matrix to verify
  108. // this assumption. Matrices with perspective transformations are
  109. // likely to produce meaningless results.
  110. //
  111. //----------------------------------------------------------------------
  112. //
  113. // Declarations for 4x4 matrix.
  114. //
  115. template <class T> bool extractScaling
  116. (const Matrix44<T> &mat,
  117. Vec3<T> &scl,
  118. bool exc = true);
  119. template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat,
  120. bool exc = true);
  121. template <class T> bool removeScaling
  122. (Matrix44<T> &mat,
  123. bool exc = true);
  124. template <class T> bool extractScalingAndShear
  125. (const Matrix44<T> &mat,
  126. Vec3<T> &scl,
  127. Vec3<T> &shr,
  128. bool exc = true);
  129. template <class T> Matrix44<T> sansScalingAndShear
  130. (const Matrix44<T> &mat,
  131. bool exc = true);
  132. template <class T> bool removeScalingAndShear
  133. (Matrix44<T> &mat,
  134. bool exc = true);
  135. template <class T> bool extractAndRemoveScalingAndShear
  136. (Matrix44<T> &mat,
  137. Vec3<T> &scl,
  138. Vec3<T> &shr,
  139. bool exc = true);
  140. template <class T> void extractEulerXYZ
  141. (const Matrix44<T> &mat,
  142. Vec3<T> &rot);
  143. template <class T> void extractEulerZYX
  144. (const Matrix44<T> &mat,
  145. Vec3<T> &rot);
  146. template <class T> Quat<T> extractQuat (const Matrix44<T> &mat);
  147. template <class T> bool extractSHRT
  148. (const Matrix44<T> &mat,
  149. Vec3<T> &s,
  150. Vec3<T> &h,
  151. Vec3<T> &r,
  152. Vec3<T> &t,
  153. bool exc /*= true*/,
  154. typename Euler<T>::Order rOrder);
  155. template <class T> bool extractSHRT
  156. (const Matrix44<T> &mat,
  157. Vec3<T> &s,
  158. Vec3<T> &h,
  159. Vec3<T> &r,
  160. Vec3<T> &t,
  161. bool exc = true);
  162. template <class T> bool extractSHRT
  163. (const Matrix44<T> &mat,
  164. Vec3<T> &s,
  165. Vec3<T> &h,
  166. Euler<T> &r,
  167. Vec3<T> &t,
  168. bool exc = true);
  169. //
  170. // Internal utility function.
  171. //
  172. template <class T> bool checkForZeroScaleInRow
  173. (const T &scl,
  174. const Vec3<T> &row,
  175. bool exc = true);
  176. //
  177. // Returns a matrix that rotates "fromDirection" vector to "toDirection"
  178. // vector.
  179. //
  180. template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection,
  181. const Vec3<T> &toDirection);
  182. //
  183. // Returns a matrix that rotates the "fromDir" vector
  184. // so that it points towards "toDir". You may also
  185. // specify that you want the up vector to be pointing
  186. // in a certain direction "upDir".
  187. //
  188. template <class T> Matrix44<T> rotationMatrixWithUpDir
  189. (const Vec3<T> &fromDir,
  190. const Vec3<T> &toDir,
  191. const Vec3<T> &upDir);
  192. //
  193. // Returns a matrix that rotates the z-axis so that it
  194. // points towards "targetDir". You must also specify
  195. // that you want the up vector to be pointing in a
  196. // certain direction "upDir".
  197. //
  198. // Notes: The following degenerate cases are handled:
  199. // (a) when the directions given by "toDir" and "upDir"
  200. // are parallel or opposite;
  201. // (the direction vectors must have a non-zero cross product)
  202. // (b) when any of the given direction vectors have zero length
  203. //
  204. template <class T> Matrix44<T> alignZAxisWithTargetDir
  205. (Vec3<T> targetDir,
  206. Vec3<T> upDir);
  207. //----------------------------------------------------------------------
  208. //
  209. // Declarations for 3x3 matrix.
  210. //
  211. template <class T> bool extractScaling
  212. (const Matrix33<T> &mat,
  213. Vec2<T> &scl,
  214. bool exc = true);
  215. template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat,
  216. bool exc = true);
  217. template <class T> bool removeScaling
  218. (Matrix33<T> &mat,
  219. bool exc = true);
  220. template <class T> bool extractScalingAndShear
  221. (const Matrix33<T> &mat,
  222. Vec2<T> &scl,
  223. T &h,
  224. bool exc = true);
  225. template <class T> Matrix33<T> sansScalingAndShear
  226. (const Matrix33<T> &mat,
  227. bool exc = true);
  228. template <class T> bool removeScalingAndShear
  229. (Matrix33<T> &mat,
  230. bool exc = true);
  231. template <class T> bool extractAndRemoveScalingAndShear
  232. (Matrix33<T> &mat,
  233. Vec2<T> &scl,
  234. T &shr,
  235. bool exc = true);
  236. template <class T> void extractEuler
  237. (const Matrix33<T> &mat,
  238. T &rot);
  239. template <class T> bool extractSHRT (const Matrix33<T> &mat,
  240. Vec2<T> &s,
  241. T &h,
  242. T &r,
  243. Vec2<T> &t,
  244. bool exc = true);
  245. template <class T> bool checkForZeroScaleInRow
  246. (const T &scl,
  247. const Vec2<T> &row,
  248. bool exc = true);
  249. //-----------------------------------------------------------------------------
  250. // Implementation for 4x4 Matrix
  251. //------------------------------
  252. template <class T>
  253. bool
  254. extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc)
  255. {
  256. Vec3<T> shr;
  257. Matrix44<T> M (mat);
  258. if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
  259. return false;
  260. return true;
  261. }
  262. template <class T>
  263. Matrix44<T>
  264. sansScaling (const Matrix44<T> &mat, bool exc)
  265. {
  266. Vec3<T> scl;
  267. Vec3<T> shr;
  268. Vec3<T> rot;
  269. Vec3<T> tran;
  270. if (! extractSHRT (mat, scl, shr, rot, tran, exc))
  271. return mat;
  272. Matrix44<T> M;
  273. M.translate (tran);
  274. M.rotate (rot);
  275. M.shear (shr);
  276. return M;
  277. }
  278. template <class T>
  279. bool
  280. removeScaling (Matrix44<T> &mat, bool exc)
  281. {
  282. Vec3<T> scl;
  283. Vec3<T> shr;
  284. Vec3<T> rot;
  285. Vec3<T> tran;
  286. if (! extractSHRT (mat, scl, shr, rot, tran, exc))
  287. return false;
  288. mat.makeIdentity ();
  289. mat.translate (tran);
  290. mat.rotate (rot);
  291. mat.shear (shr);
  292. return true;
  293. }
  294. template <class T>
  295. bool
  296. extractScalingAndShear (const Matrix44<T> &mat,
  297. Vec3<T> &scl, Vec3<T> &shr, bool exc)
  298. {
  299. Matrix44<T> M (mat);
  300. if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
  301. return false;
  302. return true;
  303. }
  304. template <class T>
  305. Matrix44<T>
  306. sansScalingAndShear (const Matrix44<T> &mat, bool exc)
  307. {
  308. Vec3<T> scl;
  309. Vec3<T> shr;
  310. Matrix44<T> M (mat);
  311. if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
  312. return mat;
  313. return M;
  314. }
  315. template <class T>
  316. bool
  317. removeScalingAndShear (Matrix44<T> &mat, bool exc)
  318. {
  319. Vec3<T> scl;
  320. Vec3<T> shr;
  321. if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
  322. return false;
  323. return true;
  324. }
  325. template <class T>
  326. bool
  327. extractAndRemoveScalingAndShear (Matrix44<T> &mat,
  328. Vec3<T> &scl, Vec3<T> &shr, bool exc)
  329. {
  330. //
  331. // This implementation follows the technique described in the paper by
  332. // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
  333. // Matrix into Simple Transformations", p. 320.
  334. //
  335. Vec3<T> row[3];
  336. row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]);
  337. row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]);
  338. row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]);
  339. T maxVal = 0;
  340. for (int i=0; i < 3; i++)
  341. for (int j=0; j < 3; j++)
  342. if (Imath::abs (row[i][j]) > maxVal)
  343. maxVal = Imath::abs (row[i][j]);
  344. //
  345. // We normalize the 3x3 matrix here.
  346. // It was noticed that this can improve numerical stability significantly,
  347. // especially when many of the upper 3x3 matrix's coefficients are very
  348. // close to zero; we correct for this step at the end by multiplying the
  349. // scaling factors by maxVal at the end (shear and rotation are not
  350. // affected by the normalization).
  351. if (maxVal != 0)
  352. {
  353. for (int i=0; i < 3; i++)
  354. if (! checkForZeroScaleInRow (maxVal, row[i], exc))
  355. return false;
  356. else
  357. row[i] /= maxVal;
  358. }
  359. // Compute X scale factor.
  360. scl.x = row[0].length ();
  361. if (! checkForZeroScaleInRow (scl.x, row[0], exc))
  362. return false;
  363. // Normalize first row.
  364. row[0] /= scl.x;
  365. // An XY shear factor will shear the X coord. as the Y coord. changes.
  366. // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
  367. // extract the first 3 because we can effect the last 3 by shearing in
  368. // XY, XZ, YZ combined rotations and scales.
  369. //
  370. // shear matrix < 1, YX, ZX, 0,
  371. // XY, 1, ZY, 0,
  372. // XZ, YZ, 1, 0,
  373. // 0, 0, 0, 1 >
  374. // Compute XY shear factor and make 2nd row orthogonal to 1st.
  375. shr[0] = row[0].dot (row[1]);
  376. row[1] -= shr[0] * row[0];
  377. // Now, compute Y scale.
  378. scl.y = row[1].length ();
  379. if (! checkForZeroScaleInRow (scl.y, row[1], exc))
  380. return false;
  381. // Normalize 2nd row and correct the XY shear factor for Y scaling.
  382. row[1] /= scl.y;
  383. shr[0] /= scl.y;
  384. // Compute XZ and YZ shears, orthogonalize 3rd row.
  385. shr[1] = row[0].dot (row[2]);
  386. row[2] -= shr[1] * row[0];
  387. shr[2] = row[1].dot (row[2]);
  388. row[2] -= shr[2] * row[1];
  389. // Next, get Z scale.
  390. scl.z = row[2].length ();
  391. if (! checkForZeroScaleInRow (scl.z, row[2], exc))
  392. return false;
  393. // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
  394. row[2] /= scl.z;
  395. shr[1] /= scl.z;
  396. shr[2] /= scl.z;
  397. // At this point, the upper 3x3 matrix in mat is orthonormal.
  398. // Check for a coordinate system flip. If the determinant
  399. // is less than zero, then negate the matrix and the scaling factors.
  400. if (row[0].dot (row[1].cross (row[2])) < 0)
  401. for (int i=0; i < 3; i++)
  402. {
  403. scl[i] *= -1;
  404. row[i] *= -1;
  405. }
  406. // Copy over the orthonormal rows into the returned matrix.
  407. // The upper 3x3 matrix in mat is now a rotation matrix.
  408. for (int i=0; i < 3; i++)
  409. {
  410. mat[i][0] = row[i][0];
  411. mat[i][1] = row[i][1];
  412. mat[i][2] = row[i][2];
  413. }
  414. // Correct the scaling factors for the normalization step that we
  415. // performed above; shear and rotation are not affected by the
  416. // normalization.
  417. scl *= maxVal;
  418. return true;
  419. }
  420. template <class T>
  421. void
  422. extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot)
  423. {
  424. //
  425. // Normalize the local x, y and z axes to remove scaling.
  426. //
  427. Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
  428. Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
  429. Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
  430. i.normalize();
  431. j.normalize();
  432. k.normalize();
  433. Matrix44<T> M (i[0], i[1], i[2], 0,
  434. j[0], j[1], j[2], 0,
  435. k[0], k[1], k[2], 0,
  436. 0, 0, 0, 1);
  437. //
  438. // Extract the first angle, rot.x.
  439. //
  440. rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
  441. //
  442. // Remove the rot.x rotation from M, so that the remaining
  443. // rotation, N, is only around two axes, and gimbal lock
  444. // cannot occur.
  445. //
  446. Matrix44<T> N;
  447. N.rotate (Vec3<T> (-rot.x, 0, 0));
  448. N = N * M;
  449. // Extract the other two angles, rot.y and rot.z, from N.
  450. //
  451. T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]);
  452. rot.y = Math<T>::atan2 (-N[0][2], cy);
  453. rot.z = Math<T>::atan2 (-N[1][0], N[1][1]);
  454. }
  455. template <class T>
  456. void
  457. extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
  458. {
  459. //
  460. // Normalize the local x, y and z axes to remove scaling.
  461. //
  462. Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
  463. Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
  464. Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
  465. i.normalize();
  466. j.normalize();
  467. k.normalize();
  468. Matrix44<T> M (i[0], i[1], i[2], 0,
  469. j[0], j[1], j[2], 0,
  470. k[0], k[1], k[2], 0,
  471. 0, 0, 0, 1);
  472. //
  473. // Extract the first angle, rot.x.
  474. //
  475. rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
  476. //
  477. // Remove the x rotation from M, so that the remaining
  478. // rotation, N, is only around two axes, and gimbal lock
  479. // cannot occur.
  480. //
  481. Matrix44<T> N;
  482. N.rotate (Vec3<T> (0, 0, -rot.x));
  483. N = N * M;
  484. //
  485. // Extract the other two angles, rot.y and rot.z, from N.
  486. //
  487. T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]);
  488. rot.y = -Math<T>::atan2 (-N[2][0], cy);
  489. rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]);
  490. }
  491. template <class T>
  492. Quat<T>
  493. extractQuat (const Matrix44<T> &mat)
  494. {
  495. Matrix44<T> rot;
  496. T tr, s;
  497. T q[4];
  498. int i, j, k;
  499. Quat<T> quat;
  500. int nxt[3] = {1, 2, 0};
  501. tr = mat[0][0] + mat[1][1] + mat[2][2];
  502. // check the diagonal
  503. if (tr > 0.0) {
  504. s = Math<T>::sqrt (tr + 1.0);
  505. quat.r = s / 2.0;
  506. s = 0.5 / s;
  507. quat.v.x = (mat[1][2] - mat[2][1]) * s;
  508. quat.v.y = (mat[2][0] - mat[0][2]) * s;
  509. quat.v.z = (mat[0][1] - mat[1][0]) * s;
  510. }
  511. else {
  512. // diagonal is negative
  513. i = 0;
  514. if (mat[1][1] > mat[0][0])
  515. i=1;
  516. if (mat[2][2] > mat[i][i])
  517. i=2;
  518. j = nxt[i];
  519. k = nxt[j];
  520. s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
  521. q[i] = s * 0.5;
  522. if (s != 0.0)
  523. s = 0.5 / s;
  524. q[3] = (mat[j][k] - mat[k][j]) * s;
  525. q[j] = (mat[i][j] + mat[j][i]) * s;
  526. q[k] = (mat[i][k] + mat[k][i]) * s;
  527. quat.v.x = q[0];
  528. quat.v.y = q[1];
  529. quat.v.z = q[2];
  530. quat.r = q[3];
  531. }
  532. return quat;
  533. }
  534. template <class T>
  535. bool
  536. extractSHRT (const Matrix44<T> &mat,
  537. Vec3<T> &s,
  538. Vec3<T> &h,
  539. Vec3<T> &r,
  540. Vec3<T> &t,
  541. bool exc /* = true */ ,
  542. typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
  543. {
  544. Matrix44<T> rot;
  545. rot = mat;
  546. if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
  547. return false;
  548. extractEulerXYZ (rot, r);
  549. t.x = mat[3][0];
  550. t.y = mat[3][1];
  551. t.z = mat[3][2];
  552. if (rOrder != Euler<T>::XYZ)
  553. {
  554. Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ);
  555. Imath::Euler<T> e (eXYZ, rOrder);
  556. r = e.toXYZVector ();
  557. }
  558. return true;
  559. }
  560. template <class T>
  561. bool
  562. extractSHRT (const Matrix44<T> &mat,
  563. Vec3<T> &s,
  564. Vec3<T> &h,
  565. Vec3<T> &r,
  566. Vec3<T> &t,
  567. bool exc)
  568. {
  569. return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ);
  570. }
  571. template <class T>
  572. bool
  573. extractSHRT (const Matrix44<T> &mat,
  574. Vec3<T> &s,
  575. Vec3<T> &h,
  576. Euler<T> &r,
  577. Vec3<T> &t,
  578. bool exc /* = true */)
  579. {
  580. return extractSHRT (mat, s, h, r, t, exc, r.order ());
  581. }
  582. template <class T>
  583. bool
  584. checkForZeroScaleInRow (const T& scl,
  585. const Vec3<T> &row,
  586. bool exc /* = true */ )
  587. {
  588. for (int i = 0; i < 3; i++)
  589. {
  590. if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
  591. {
  592. if (exc)
  593. throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
  594. "from matrix.");
  595. else
  596. return false;
  597. }
  598. }
  599. return true;
  600. }
  601. template <class T>
  602. Matrix44<T>
  603. rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
  604. {
  605. Quat<T> q;
  606. q.setRotation(from, to);
  607. return q.toMatrix44();
  608. }
  609. template <class T>
  610. Matrix44<T>
  611. rotationMatrixWithUpDir (const Vec3<T> &fromDir,
  612. const Vec3<T> &toDir,
  613. const Vec3<T> &upDir)
  614. {
  615. //
  616. // The goal is to obtain a rotation matrix that takes
  617. // "fromDir" to "toDir". We do this in two steps and
  618. // compose the resulting rotation matrices;
  619. // (a) rotate "fromDir" into the z-axis
  620. // (b) rotate the z-axis into "toDir"
  621. //
  622. // The from direction must be non-zero; but we allow zero to and up dirs.
  623. if (fromDir.length () == 0)
  624. return Matrix44<T> ();
  625. else
  626. {
  627. Matrix44<T> zAxis2FromDir = alignZAxisWithTargetDir
  628. (fromDir, Vec3<T> (0, 1, 0));
  629. Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed ();
  630. Matrix44<T> zAxis2ToDir = alignZAxisWithTargetDir (toDir, upDir);
  631. return fromDir2zAxis * zAxis2ToDir;
  632. }
  633. }
  634. template <class T>
  635. Matrix44<T>
  636. alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir)
  637. {
  638. //
  639. // Ensure that the target direction is non-zero.
  640. //
  641. if ( targetDir.length () == 0 )
  642. targetDir = Vec3<T> (0, 0, 1);
  643. //
  644. // Ensure that the up direction is non-zero.
  645. //
  646. if ( upDir.length () == 0 )
  647. upDir = Vec3<T> (0, 1, 0);
  648. //
  649. // Check for degeneracies. If the upDir and targetDir are parallel
  650. // or opposite, then compute a new, arbitrary up direction that is
  651. // not parallel or opposite to the targetDir.
  652. //
  653. if (upDir.cross (targetDir).length () == 0)
  654. {
  655. upDir = targetDir.cross (Vec3<T> (1, 0, 0));
  656. if (upDir.length() == 0)
  657. upDir = targetDir.cross(Vec3<T> (0, 0, 1));
  658. }
  659. //
  660. // Compute the x-, y-, and z-axis vectors of the new coordinate system.
  661. //
  662. Vec3<T> targetPerpDir = upDir.cross (targetDir);
  663. Vec3<T> targetUpDir = targetDir.cross (targetPerpDir);
  664. //
  665. // Rotate the x-axis into targetPerpDir (row 0),
  666. // rotate the y-axis into targetUpDir (row 1),
  667. // rotate the z-axis into targetDir (row 2).
  668. //
  669. Vec3<T> row[3];
  670. row[0] = targetPerpDir.normalized ();
  671. row[1] = targetUpDir .normalized ();
  672. row[2] = targetDir .normalized ();
  673. Matrix44<T> mat ( row[0][0], row[0][1], row[0][2], 0,
  674. row[1][0], row[1][1], row[1][2], 0,
  675. row[2][0], row[2][1], row[2][2], 0,
  676. 0, 0, 0, 1 );
  677. return mat;
  678. }
  679. //-----------------------------------------------------------------------------
  680. // Implementation for 3x3 Matrix
  681. //------------------------------
  682. template <class T>
  683. bool
  684. extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
  685. {
  686. T shr;
  687. Matrix33<T> M (mat);
  688. if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
  689. return false;
  690. return true;
  691. }
  692. template <class T>
  693. Matrix33<T>
  694. sansScaling (const Matrix33<T> &mat, bool exc)
  695. {
  696. Vec2<T> scl;
  697. T shr;
  698. T rot;
  699. Vec2<T> tran;
  700. if (! extractSHRT (mat, scl, shr, rot, tran, exc))
  701. return mat;
  702. Matrix33<T> M;
  703. M.translate (tran);
  704. M.rotate (rot);
  705. M.shear (shr);
  706. return M;
  707. }
  708. template <class T>
  709. bool
  710. removeScaling (Matrix33<T> &mat, bool exc)
  711. {
  712. Vec2<T> scl;
  713. T shr;
  714. T rot;
  715. Vec2<T> tran;
  716. if (! extractSHRT (mat, scl, shr, rot, tran, exc))
  717. return false;
  718. mat.makeIdentity ();
  719. mat.translate (tran);
  720. mat.rotate (rot);
  721. mat.shear (shr);
  722. return true;
  723. }
  724. template <class T>
  725. bool
  726. extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
  727. {
  728. Matrix33<T> M (mat);
  729. if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
  730. return false;
  731. return true;
  732. }
  733. template <class T>
  734. Matrix33<T>
  735. sansScalingAndShear (const Matrix33<T> &mat, bool exc)
  736. {
  737. Vec2<T> scl;
  738. T shr;
  739. Matrix33<T> M (mat);
  740. if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
  741. return mat;
  742. return M;
  743. }
  744. template <class T>
  745. bool
  746. removeScalingAndShear (Matrix33<T> &mat, bool exc)
  747. {
  748. Vec2<T> scl;
  749. T shr;
  750. if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
  751. return false;
  752. return true;
  753. }
  754. template <class T>
  755. bool
  756. extractAndRemoveScalingAndShear (Matrix33<T> &mat,
  757. Vec2<T> &scl, T &shr, bool exc)
  758. {
  759. Vec2<T> row[2];
  760. row[0] = Vec2<T> (mat[0][0], mat[0][1]);
  761. row[1] = Vec2<T> (mat[1][0], mat[1][1]);
  762. T maxVal = 0;
  763. for (int i=0; i < 2; i++)
  764. for (int j=0; j < 2; j++)
  765. if (Imath::abs (row[i][j]) > maxVal)
  766. maxVal = Imath::abs (row[i][j]);
  767. //
  768. // We normalize the 2x2 matrix here.
  769. // It was noticed that this can improve numerical stability significantly,
  770. // especially when many of the upper 2x2 matrix's coefficients are very
  771. // close to zero; we correct for this step at the end by multiplying the
  772. // scaling factors by maxVal at the end (shear and rotation are not
  773. // affected by the normalization).
  774. if (maxVal != 0)
  775. {
  776. for (int i=0; i < 2; i++)
  777. if (! checkForZeroScaleInRow (maxVal, row[i], exc))
  778. return false;
  779. else
  780. row[i] /= maxVal;
  781. }
  782. // Compute X scale factor.
  783. scl.x = row[0].length ();
  784. if (! checkForZeroScaleInRow (scl.x, row[0], exc))
  785. return false;
  786. // Normalize first row.
  787. row[0] /= scl.x;
  788. // An XY shear factor will shear the X coord. as the Y coord. changes.
  789. // There are 2 combinations (XY, YX), although we only extract the XY
  790. // shear factor because we can effect the an YX shear factor by
  791. // shearing in XY combined with rotations and scales.
  792. //
  793. // shear matrix < 1, YX, 0,
  794. // XY, 1, 0,
  795. // 0, 0, 1 >
  796. // Compute XY shear factor and make 2nd row orthogonal to 1st.
  797. shr = row[0].dot (row[1]);
  798. row[1] -= shr * row[0];
  799. // Now, compute Y scale.
  800. scl.y = row[1].length ();
  801. if (! checkForZeroScaleInRow (scl.y, row[1], exc))
  802. return false;
  803. // Normalize 2nd row and correct the XY shear factor for Y scaling.
  804. row[1] /= scl.y;
  805. shr /= scl.y;
  806. // At this point, the upper 2x2 matrix in mat is orthonormal.
  807. // Check for a coordinate system flip. If the determinant
  808. // is -1, then flip the rotation matrix and adjust the scale(Y)
  809. // and shear(XY) factors to compensate.
  810. if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
  811. {
  812. row[1][0] *= -1;
  813. row[1][1] *= -1;
  814. scl[1] *= -1;
  815. shr *= -1;
  816. }
  817. // Copy over the orthonormal rows into the returned matrix.
  818. // The upper 2x2 matrix in mat is now a rotation matrix.
  819. for (int i=0; i < 2; i++)
  820. {
  821. mat[i][0] = row[i][0];
  822. mat[i][1] = row[i][1];
  823. }
  824. scl *= maxVal;
  825. return true;
  826. }
  827. template <class T>
  828. void
  829. extractEuler (const Matrix33<T> &mat, T &rot)
  830. {
  831. //
  832. // Normalize the local x and y axes to remove scaling.
  833. //
  834. Vec2<T> i (mat[0][0], mat[0][1]);
  835. Vec2<T> j (mat[1][0], mat[1][1]);
  836. i.normalize();
  837. j.normalize();
  838. //
  839. // Extract the angle, rot.
  840. //
  841. rot = - Math<T>::atan2 (j[0], i[0]);
  842. }
  843. template <class T>
  844. bool
  845. extractSHRT (const Matrix33<T> &mat,
  846. Vec2<T> &s,
  847. T &h,
  848. T &r,
  849. Vec2<T> &t,
  850. bool exc)
  851. {
  852. Matrix33<T> rot;
  853. rot = mat;
  854. if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
  855. return false;
  856. extractEuler (rot, r);
  857. t.x = mat[2][0];
  858. t.y = mat[2][1];
  859. return true;
  860. }
  861. template <class T>
  862. bool
  863. checkForZeroScaleInRow (const T& scl,
  864. const Vec2<T> &row,
  865. bool exc /* = true */ )
  866. {
  867. for (int i = 0; i < 2; i++)
  868. {
  869. if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
  870. {
  871. if (exc)
  872. throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
  873. "from matrix.");
  874. else
  875. return false;
  876. }
  877. }
  878. return true;
  879. }
  880. } // namespace Imath
  881. #endif