AffineParts.cpp 23 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include "EditorDefs.h"
  9. /**** Decompose.h - Basic declarations ****/
  10. typedef struct
  11. {
  12. float x, y, z, w;
  13. } Quatern; /* Quaternernion */
  14. enum QuaternPart
  15. {
  16. X, Y, Z, W
  17. };
  18. typedef Quatern HVect; /* Homogeneous 3D vector */
  19. typedef float HMatrix[4][4]; /* Right-handed, for column vectors */
  20. typedef struct
  21. {
  22. HVect t; /* Translation components */
  23. Quatern q; /* Essential rotation */
  24. Quatern u; /* Stretch rotation */
  25. HVect k; /* Stretch factors */
  26. float f; /* Sign of determinant */
  27. } SAffineParts;
  28. float polar_decomp(HMatrix M, HMatrix Q, HMatrix S);
  29. HVect spect_decomp(HMatrix S, HMatrix U);
  30. Quatern snuggle(Quatern q, HVect* k);
  31. /******* Matrix Preliminaries *******/
  32. /** Fill out 3x3 matrix to 4x4 **/
  33. #define mat_pad(A) (A[W][X] = A[X][W] = A[W][Y] = A[Y][W] = A[W][Z] = A[Z][W] = 0, A[W][W] = 1)
  34. /** Copy nxn matrix A to C using "gets" for assignment **/
  35. #define mat_copy(C, gets, A, n) {int i, j; for (i = 0; i < n; i++) {for (j = 0; j < n; j++) { \
  36. C[i][j] gets (A[i][j]); } \
  37. } \
  38. }
  39. /** Copy transpose of nxn matrix A to C using "gets" for assignment **/
  40. #define mat_tpose(AT, gets, A, n) {int i, j; for (i = 0; i < n; i++) {for (j = 0; j < n; j++) { \
  41. AT[i][j] gets (A[j][i]); } \
  42. } \
  43. }
  44. /** Assign nxn matrix C the element-wise combination of A and B using "op" **/
  45. #define mat_binop(C, gets, A, op, B, n) {int i, j; for (i = 0; i < n; i++) {for (j = 0; j < n; j++) { \
  46. C[i][j] gets (A[i][j]) op (B[i][j]); } \
  47. } \
  48. }
  49. /** Multiply the upper left 3x3 parts of A and B to get AB **/
  50. static void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
  51. {
  52. int i, j;
  53. for (i = 0; i < 3; i++)
  54. {
  55. for (j = 0; j < 3; j++)
  56. {
  57. AB[i][j] = A[i][0] * B[0][j] + A[i][1] * B[1][j] + A[i][2] * B[2][j];
  58. }
  59. }
  60. }
  61. /** Return dot product of length 3 vectors va and vb **/
  62. static float vdot(float* va, float* vb)
  63. {
  64. return (va[0] * vb[0] + va[1] * vb[1] + va[2] * vb[2]);
  65. }
  66. /** Set v to cross product of length 3 vectors va and vb **/
  67. static void vcross(float* va, float* vb, float* v)
  68. {
  69. v[0] = va[1] * vb[2] - va[2] * vb[1];
  70. v[1] = va[2] * vb[0] - va[0] * vb[2];
  71. v[2] = va[0] * vb[1] - va[1] * vb[0];
  72. }
  73. /** Set MadjT to transpose of inverse of M times determinant of M **/
  74. static void adjoint_transpose(HMatrix M, HMatrix MadjT)
  75. {
  76. vcross(M[1], M[2], MadjT[0]);
  77. vcross(M[2], M[0], MadjT[1]);
  78. vcross(M[0], M[1], MadjT[2]);
  79. }
  80. /******* Quaternernion Preliminaries *******/
  81. /* Construct a (possibly non-unit) Quaternernion from real components. */
  82. static Quatern Qt_(float x, float y, float z, float w)
  83. {
  84. Quatern qq;
  85. qq.x = x;
  86. qq.y = y;
  87. qq.z = z;
  88. qq.w = w;
  89. return (qq);
  90. }
  91. /* Return conjugate of Quaternernion. */
  92. static Quatern Qt_Conj(Quatern q)
  93. {
  94. Quatern qq;
  95. qq.x = -q.x;
  96. qq.y = -q.y;
  97. qq.z = -q.z;
  98. qq.w = q.w;
  99. return (qq);
  100. }
  101. /* Return Quaternernion product qL * qR. Note: order is important!
  102. * To combine rotations, use the product Mul(qSecond, qFirst),
  103. * which gives the effect of rotating by qFirst then qSecond. */
  104. static Quatern Qt_Mul(Quatern qL, Quatern qR)
  105. {
  106. Quatern qq;
  107. qq.w = qL.w * qR.w - qL.x * qR.x - qL.y * qR.y - qL.z * qR.z;
  108. qq.x = qL.w * qR.x + qL.x * qR.w + qL.y * qR.z - qL.z * qR.y;
  109. qq.y = qL.w * qR.y + qL.y * qR.w + qL.z * qR.x - qL.x * qR.z;
  110. qq.z = qL.w * qR.z + qL.z * qR.w + qL.x * qR.y - qL.y * qR.x;
  111. return (qq);
  112. }
  113. /* Return product of Quaternernion q by scalar w. */
  114. static Quatern Qt_Scale(Quatern q, float w)
  115. {
  116. Quatern qq;
  117. qq.w = q.w * w;
  118. qq.x = q.x * w;
  119. qq.y = q.y * w;
  120. qq.z = q.z * w;
  121. return (qq);
  122. }
  123. /* Construct a unit Quaternernion from rotation matrix. Assumes matrix is
  124. * used to multiply column vector on the left: vnew = mat vold. Works
  125. * correctly for right-handed coordinate system and right-handed rotations.
  126. * Translation and perspective components ignored. */
  127. static Quatern Qt_FromMatrix(HMatrix mat)
  128. {
  129. /* This algorithm avoids near-zero divides by looking for a large component
  130. * - first w, then x, y, or z. When the trace is greater than zero,
  131. * |w| is greater than 1/2, which is as small as a largest component can be.
  132. * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
  133. * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
  134. Quatern qu = { 0.0f, 0.0f, 0.0f, 1.0f };
  135. double tr, s;
  136. tr = mat[X][X] + mat[Y][Y] + mat[Z][Z];
  137. if (tr >= 0.0)
  138. {
  139. s = sqrt(tr + mat[W][W]);
  140. qu.w = static_cast<float>(s * 0.5);
  141. s = 0.5 / s;
  142. qu.x = static_cast<float>((mat[Z][Y] - mat[Y][Z]) * s);
  143. qu.y = static_cast<float>((mat[X][Z] - mat[Z][X]) * s);
  144. qu.z = static_cast<float>((mat[Y][X] - mat[X][Y]) * s);
  145. }
  146. else
  147. {
  148. int h = X;
  149. if (mat[Y][Y] > mat[X][X])
  150. {
  151. h = Y;
  152. }
  153. if (mat[Z][Z] > mat[h][h])
  154. {
  155. h = Z;
  156. }
  157. switch (h)
  158. {
  159. #define caseMacro(i, j, k, I, J, K) \
  160. case I: \
  161. s = sqrt((mat[I][I] - (mat[J][J] + mat[K][K])) + mat[W][W]); \
  162. qu.i = static_cast<float>(s * 0.5); \
  163. s = 0.5 / s; \
  164. qu.j = static_cast<float>((mat[I][J] + mat[J][I]) * s); \
  165. qu.k = static_cast<float>((mat[K][I] + mat[I][K]) * s); \
  166. qu.w = static_cast<float>((mat[K][J] - mat[J][K]) * s); \
  167. break
  168. caseMacro(x, y, z, X, Y, Z);
  169. caseMacro(y, z, x, Y, Z, X);
  170. caseMacro(z, x, y, Z, X, Y);
  171. }
  172. }
  173. if (mat[W][W] != 1.0)
  174. {
  175. qu = Qt_Scale(qu, 1.0f / sqrt(mat[W][W]));
  176. }
  177. return (qu);
  178. }
  179. /******* Decomp Auxiliaries *******/
  180. static HMatrix mat_id = {
  181. {1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}
  182. };
  183. /** Compute either the 1 or infinity norm of M, depending on tpose **/
  184. static float mat_norm(HMatrix M, int tpose)
  185. {
  186. int i;
  187. float sum, max;
  188. max = 0.0;
  189. for (i = 0; i < 3; i++)
  190. {
  191. if (tpose)
  192. {
  193. sum = fabs(M[0][i]) + fabs(M[1][i]) + fabs(M[2][i]);
  194. }
  195. else
  196. {
  197. sum = fabs(M[i][0]) + fabs(M[i][1]) + fabs(M[i][2]);
  198. }
  199. if (max < sum)
  200. {
  201. max = sum;
  202. }
  203. }
  204. return max;
  205. }
  206. static float norm_inf(HMatrix M) {return mat_norm(M, 0); }
  207. static float norm_one(HMatrix M) {return mat_norm(M, 1); }
  208. /** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
  209. static int find_max_col(HMatrix M)
  210. {
  211. float abs, max;
  212. int i, j, col;
  213. max = 0.0;
  214. col = -1;
  215. for (i = 0; i < 3; i++)
  216. {
  217. for (j = 0; j < 3; j++)
  218. {
  219. abs = M[i][j];
  220. if (abs < 0.0)
  221. {
  222. abs = -abs;
  223. }
  224. if (abs > max)
  225. {
  226. max = abs;
  227. col = j;
  228. }
  229. }
  230. }
  231. return col;
  232. }
  233. /** Setup u for Household reflection to zero all v components but first **/
  234. static void make_reflector(float* v, float* u)
  235. {
  236. float s = sqrt(vdot(v, v));
  237. u[0] = v[0];
  238. u[1] = v[1];
  239. u[2] = v[2] + ((v[2] < 0.0) ? -s : s);
  240. s = static_cast<float>(sqrt(2.0f / vdot(u, u)));
  241. u[0] = u[0] * s;
  242. u[1] = u[1] * s;
  243. u[2] = u[2] * s;
  244. }
  245. /** Apply Householder reflection represented by u to column vectors of M **/
  246. static void reflect_cols(HMatrix M, float* u)
  247. {
  248. int i, j;
  249. for (i = 0; i < 3; i++)
  250. {
  251. float s = u[0] * M[0][i] + u[1] * M[1][i] + u[2] * M[2][i];
  252. for (j = 0; j < 3; j++)
  253. {
  254. M[j][i] -= u[j] * s;
  255. }
  256. }
  257. }
  258. /** Apply Householder reflection represented by u to row vectors of M **/
  259. static void reflect_rows(HMatrix M, float* u)
  260. {
  261. int i, j;
  262. for (i = 0; i < 3; i++)
  263. {
  264. float s = vdot(u, M[i]);
  265. for (j = 0; j < 3; j++)
  266. {
  267. M[i][j] -= u[j] * s;
  268. }
  269. }
  270. }
  271. /** Find orthogonal factor Q of rank 1 (or less) M **/
  272. static void do_rank1(HMatrix M, HMatrix Q)
  273. {
  274. float v1[3], v2[3], s;
  275. int col;
  276. mat_copy(Q, =, mat_id, 4);
  277. /* If rank(M) is 1, we should find a non-zero column in M */
  278. col = find_max_col(M);
  279. if (col < 0)
  280. {
  281. return; /* Rank is 0 */
  282. }
  283. v1[0] = M[0][col];
  284. v1[1] = M[1][col];
  285. v1[2] = M[2][col];
  286. make_reflector(v1, v1);
  287. reflect_cols(M, v1);
  288. v2[0] = M[2][0];
  289. v2[1] = M[2][1];
  290. v2[2] = M[2][2];
  291. make_reflector(v2, v2);
  292. reflect_rows(M, v2);
  293. s = M[2][2];
  294. if (s < 0.0)
  295. {
  296. Q[2][2] = -1.0;
  297. }
  298. reflect_cols(Q, v1);
  299. reflect_rows(Q, v2);
  300. }
  301. /** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
  302. static void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q)
  303. {
  304. float v1[3], v2[3];
  305. float w, x, y, z, c, s, d;
  306. int col;
  307. /* If rank(M) is 2, we should find a non-zero column in MadjT */
  308. col = find_max_col(MadjT);
  309. if (col < 0)
  310. {
  311. do_rank1(M, Q);
  312. return;
  313. } /* Rank<2 */
  314. v1[0] = MadjT[0][col];
  315. v1[1] = MadjT[1][col];
  316. v1[2] = MadjT[2][col];
  317. make_reflector(v1, v1);
  318. reflect_cols(M, v1);
  319. vcross(M[0], M[1], v2);
  320. make_reflector(v2, v2);
  321. reflect_rows(M, v2);
  322. w = M[0][0];
  323. x = M[0][1];
  324. y = M[1][0];
  325. z = M[1][1];
  326. if (w * z > x * y)
  327. {
  328. c = z + w;
  329. s = y - x;
  330. d = sqrt(c * c + s * s);
  331. c = c / d;
  332. s = s / d;
  333. Q[0][0] = Q[1][1] = c;
  334. Q[0][1] = -(Q[1][0] = s);
  335. }
  336. else
  337. {
  338. c = z - w;
  339. s = y + x;
  340. d = sqrt(c * c + s * s);
  341. c = c / d;
  342. s = s / d;
  343. Q[0][0] = -(Q[1][1] = c);
  344. Q[0][1] = Q[1][0] = s;
  345. }
  346. Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0;
  347. Q[2][2] = 1.0;
  348. reflect_cols(Q, v1);
  349. reflect_rows(Q, v2);
  350. }
  351. /******* Polar Decomposition *******/
  352. /* Polar Decomposition of 3x3 matrix in 4x4,
  353. * M = QS. See Nicholas Higham and Robert S. Schreiber,
  354. * Fast Polar Decomposition of An Arbitrary Matrix,
  355. * Technical Report 88-942, October 1988,
  356. * Department of Computer Science, Cornell University.
  357. */
  358. float polar_decomp(HMatrix M, HMatrix Q, HMatrix S)
  359. {
  360. #define TOL 1.0e-6
  361. HMatrix Mk, MadjTk, Ek;
  362. float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
  363. mat_tpose(Mk, =, M, 3);
  364. M_one = norm_one(Mk);
  365. M_inf = norm_inf(Mk);
  366. do
  367. {
  368. adjoint_transpose(Mk, MadjTk);
  369. det = vdot(Mk[0], MadjTk[0]);
  370. if (det == 0.0)
  371. {
  372. do_rank2(Mk, MadjTk, Mk);
  373. break;
  374. }
  375. MadjT_one = norm_one(MadjTk);
  376. MadjT_inf = norm_inf(MadjTk);
  377. gamma = sqrt(sqrt((MadjT_one * MadjT_inf) / (M_one * M_inf)) / fabs(det));
  378. g1 = gamma * 0.5f;
  379. g2 = 0.5f / (gamma * det);
  380. mat_copy(Ek, =, Mk, 3);
  381. mat_binop(Mk, =, g1 * Mk, +, g2 * MadjTk, 3);
  382. mat_copy(Ek, -=, Mk, 3);
  383. E_one = norm_one(Ek);
  384. M_one = norm_one(Mk);
  385. M_inf = norm_inf(Mk);
  386. } while (E_one > (M_one * TOL));
  387. mat_tpose(Q, =, Mk, 3);
  388. mat_pad(Q);
  389. mat_mult(Mk, M, S);
  390. mat_pad(S);
  391. for (int i = 0; i < 3; i++)
  392. {
  393. for (int j = i; j < 3; j++)
  394. {
  395. S[i][j] = S[j][i] = 0.5f * (S[i][j] + S[j][i]);
  396. }
  397. }
  398. return (det);
  399. }
  400. /******* Spectral Decomposition *******/
  401. /* Compute the spectral decomposition of symmetric positive semi-definite S.
  402. * Returns rotation in U and scale factors in result, so that if K is a diagonal
  403. * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
  404. * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
  405. */
  406. HVect spect_decomp(HMatrix S, HMatrix U)
  407. {
  408. HVect kv;
  409. double Diag[3], OffD[3]; /* OffD is off-diag (by omitted index) */
  410. double g, h, fabsh, fabsOffDi, t, theta, c, s, tau, ta, OffDq, a, b;
  411. static char nxt[] = {Y, Z, X};
  412. int sweep;
  413. mat_copy(U, =, mat_id, 4);
  414. Diag[X] = S[X][X];
  415. Diag[Y] = S[Y][Y];
  416. Diag[Z] = S[Z][Z];
  417. OffD[X] = S[Y][Z];
  418. OffD[Y] = S[Z][X];
  419. OffD[Z] = S[X][Y];
  420. for (sweep = 20; sweep > 0; sweep--)
  421. {
  422. float sm = static_cast<float>(fabs(OffD[X]) + fabs(OffD[Y]) + fabs(OffD[Z]));
  423. if (sm == 0.0)
  424. {
  425. break;
  426. }
  427. for (int i = Z; i >= X; i--)
  428. {
  429. int p = nxt[i];
  430. int q = nxt[p];
  431. fabsOffDi = fabs(OffD[i]);
  432. g = 100.0 * fabsOffDi;
  433. if (fabsOffDi > AZ::Constants::FloatEpsilon)
  434. {
  435. h = Diag[q] - Diag[p];
  436. fabsh = fabs(h);
  437. if (fabsh + g == fabsh)
  438. {
  439. t = OffD[i] / h;
  440. }
  441. else
  442. {
  443. theta = 0.5 * h / OffD[i];
  444. t = 1.0 / (fabs(theta) + sqrt(theta * theta + 1.0));
  445. if (theta < 0.0)
  446. {
  447. t = -t;
  448. }
  449. }
  450. c = 1.0 / sqrt(t * t + 1.0);
  451. s = t * c;
  452. tau = s / (c + 1.0);
  453. ta = t * OffD[i];
  454. OffD[i] = 0.0;
  455. Diag[p] -= ta;
  456. Diag[q] += ta;
  457. OffDq = OffD[q];
  458. OffD[q] -= s * (OffD[p] + tau * OffD[q]);
  459. OffD[p] += s * (OffDq - tau * OffD[p]);
  460. for (int j = Z; j >= X; j--)
  461. {
  462. a = U[j][p];
  463. b = U[j][q];
  464. U[j][p] -= static_cast<float>(s * (b + tau * a));
  465. U[j][q] += static_cast<float>(s * (a - tau * b));
  466. }
  467. }
  468. }
  469. }
  470. kv.x = static_cast<float>(Diag[X]);
  471. kv.y = static_cast<float>(Diag[Y]);
  472. kv.z = static_cast<float>(Diag[Z]);
  473. kv.w = 1.0f;
  474. return (kv);
  475. }
  476. /******* Spectral Axis Adjustment *******/
  477. /* Given a unit Quaternernion, q, and a scale vector, k, find a unit Quaternernion, p,
  478. * which permutes the axes and turns freely in the plane of duplicate scale
  479. * factors, such that q p has the largest possible w component, i.e. the
  480. * smallest possible angle. Permutes k's components to go with q p instead of q.
  481. * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
  482. * Proceedings of Graphics Interface 1992. Details on p. 262-263.
  483. */
  484. Quatern snuggle(Quatern q, HVect* k)
  485. {
  486. #define SQRTHALF (0.7071067811865475244f)
  487. #define sgn(n, v) ((n) ? -(v) : (v))
  488. #define swap(a, i, j) {a[3] = a[i]; a[i] = a[j]; a[j] = a[3]; }
  489. #define cycle(a, p) if (p) {a[3] = a[0]; a[0] = a[1]; a[1] = a[2]; a[2] = a[3]; } \
  490. else {a[3] = a[2]; a[2] = a[1]; a[1] = a[0]; a[0] = a[3]; }
  491. Quatern p = { 0.0f, 0.0f, 0.0f, 1.0f };
  492. float ka[4];
  493. int i, turn = -1;
  494. ka[X] = k->x;
  495. ka[Y] = k->y;
  496. ka[Z] = k->z;
  497. if (ka[X] == ka[Y])
  498. {
  499. if (ka[X] == ka[Z])
  500. {
  501. turn = W;
  502. }
  503. else
  504. {
  505. turn = Z;
  506. }
  507. }
  508. else
  509. {
  510. if (ka[X] == ka[Z])
  511. {
  512. turn = Y;
  513. }
  514. else if (ka[Y] == ka[Z])
  515. {
  516. turn = X;
  517. }
  518. }
  519. if (turn >= 0)
  520. {
  521. Quatern qtoz, qp;
  522. unsigned neg[3], win;
  523. double mag[3], t;
  524. static Quatern qxtoz = {0, SQRTHALF, 0, SQRTHALF};
  525. static Quatern qytoz = {SQRTHALF, 0, 0, SQRTHALF};
  526. static Quatern qppmm = { 0.5, 0.5, -0.5, -0.5};
  527. static Quatern qpppp = { 0.5, 0.5, 0.5, 0.5};
  528. static Quatern qmpmm = {-0.5, 0.5, -0.5, -0.5};
  529. static Quatern qpppm = { 0.5, 0.5, 0.5, -0.5};
  530. static Quatern q0001 = { 0.0, 0.0, 0.0, 1.0};
  531. static Quatern q1000 = { 1.0, 0.0, 0.0, 0.0};
  532. switch (turn)
  533. {
  534. default:
  535. return (Qt_Conj(q));
  536. case X:
  537. q = Qt_Mul(q, qtoz = qxtoz);
  538. swap(ka, X, Z);
  539. break;
  540. case Y:
  541. q = Qt_Mul(q, qtoz = qytoz);
  542. swap(ka, Y, Z);
  543. break;
  544. case Z:
  545. qtoz = q0001;
  546. break;
  547. }
  548. q = Qt_Conj(q);
  549. mag[0] = (double)q.z * q.z + (double)q.w * q.w - 0.5;
  550. mag[1] = (double)q.x * q.z - (double)q.y * q.w;
  551. mag[2] = (double)q.y * q.z + (double)q.x * q.w;
  552. for (i = 0; i < 3; i++)
  553. {
  554. neg[i] = (mag[i] < 0.0);
  555. if (neg[i])
  556. {
  557. mag[i] = -mag[i];
  558. }
  559. }
  560. if (mag[0] > mag[1])
  561. {
  562. if (mag[0] > mag[2])
  563. {
  564. win = 0;
  565. }
  566. else
  567. {
  568. win = 2;
  569. }
  570. }
  571. else
  572. {
  573. if (mag[1] > mag[2])
  574. {
  575. win = 1;
  576. }
  577. else
  578. {
  579. win = 2;
  580. }
  581. }
  582. switch (win)
  583. {
  584. case 0:
  585. if (neg[0])
  586. {
  587. p = q1000;
  588. }
  589. else
  590. {
  591. p = q0001;
  592. } break;
  593. case 1:
  594. if (neg[1])
  595. {
  596. p = qppmm;
  597. }
  598. else
  599. {
  600. p = qpppp;
  601. } cycle(ka, 0);
  602. break;
  603. case 2:
  604. if (neg[2])
  605. {
  606. p = qmpmm;
  607. }
  608. else
  609. {
  610. p = qpppm;
  611. } cycle(ka, 1);
  612. break;
  613. }
  614. qp = Qt_Mul(q, p);
  615. t = sqrt(mag[win] + 0.5);
  616. p = Qt_Mul(p, Qt_(0.0f, 0.0f, static_cast<float>(-qp.z / t), static_cast<float>(qp.w / t)));
  617. p = Qt_Mul(qtoz, Qt_Conj(p));
  618. }
  619. else
  620. {
  621. float qa[4], pa[4];
  622. unsigned lo, hi, neg[4], par = 0;
  623. double all, big, two;
  624. qa[0] = q.x;
  625. qa[1] = q.y;
  626. qa[2] = q.z;
  627. qa[3] = q.w;
  628. for (i = 0; i < 4; i++)
  629. {
  630. pa[i] = 0.0;
  631. neg[i] = (qa[i] < 0.0);
  632. if (neg[i])
  633. {
  634. qa[i] = -qa[i];
  635. }
  636. par ^= neg[i];
  637. }
  638. /* Find two largest components, indices in hi and lo */
  639. if (qa[0] > qa[1])
  640. {
  641. lo = 0;
  642. }
  643. else
  644. {
  645. lo = 1;
  646. }
  647. if (qa[2] > qa[3])
  648. {
  649. hi = 2;
  650. }
  651. else
  652. {
  653. hi = 3;
  654. }
  655. if (qa[lo] > qa[hi])
  656. {
  657. if (qa[lo ^ 1] > qa[hi])
  658. {
  659. hi = lo;
  660. lo ^= 1;
  661. }
  662. else
  663. {
  664. hi ^= lo;
  665. lo ^= hi;
  666. hi ^= lo;
  667. }
  668. }
  669. else
  670. {
  671. if (qa[hi ^ 1] > qa[lo])
  672. {
  673. lo = hi ^ 1;
  674. }
  675. }
  676. all = (qa[0] + qa[1] + qa[2] + qa[3]) * 0.5;
  677. two = (qa[hi] + qa[lo]) * SQRTHALF;
  678. big = qa[hi];
  679. if (all > two)
  680. {
  681. if (all > big)/*all*/
  682. {
  683. {
  684. int ii;
  685. for (ii = 0; ii < 4; ii++)
  686. {
  687. pa[ii] = static_cast<float>(sgn(neg[ii], 0.5f));
  688. }
  689. }
  690. cycle(ka, par)
  691. }
  692. else
  693. { /*big*/
  694. pa[hi] = static_cast<float>(sgn(neg[hi], 1.0f));
  695. }
  696. }
  697. else
  698. {
  699. if (two > big)/*two*/
  700. {
  701. pa[hi] = sgn(neg[hi], SQRTHALF);
  702. pa[lo] = sgn(neg[lo], SQRTHALF);
  703. if (lo > hi)
  704. {
  705. hi ^= lo;
  706. lo ^= hi;
  707. hi ^= lo;
  708. }
  709. if (hi == W)
  710. {
  711. hi = "\001\002\000"[lo];
  712. lo = 3 - hi - lo;
  713. }
  714. swap(ka, hi, lo)
  715. }
  716. else
  717. { /*big*/
  718. pa[hi] = static_cast<float>(sgn(neg[hi], 1.0f));
  719. }
  720. }
  721. p.x = -pa[0];
  722. p.y = -pa[1];
  723. p.z = -pa[2];
  724. p.w = pa[3];
  725. }
  726. k->x = ka[X];
  727. k->y = ka[Y];
  728. k->z = ka[Z];
  729. return (p);
  730. }
  731. /******* Decompose Affine Matrix *******/
  732. /* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
  733. * translation components, q contains the rotation R, u contains U, k contains
  734. * scale factors, and f contains the sign of the determinant.
  735. * Assumes A transforms column vectors in right-handed coordinates.
  736. * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
  737. * Proceedings of Graphics Interface 1992.
  738. */
  739. static void decomp_affine(HMatrix A, SAffineParts* parts)
  740. {
  741. HMatrix Q, S, U;
  742. Quatern p;
  743. float det;
  744. parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
  745. det = polar_decomp(A, Q, S);
  746. if (det < 0.0)
  747. {
  748. mat_copy(Q, =, -Q, 3);
  749. parts->f = -1;
  750. }
  751. else
  752. {
  753. parts->f = 1;
  754. }
  755. parts->q = Qt_FromMatrix(Q);
  756. parts->k = spect_decomp(S, U);
  757. parts->u = Qt_FromMatrix(U);
  758. p = snuggle(parts->u, &parts->k);
  759. parts->u = Qt_Mul(parts->u, p);
  760. }
  761. static void spectral_decomp_affine(HMatrix A, SAffineParts* parts)
  762. {
  763. HMatrix Q, S, U;
  764. float det;
  765. parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
  766. det = polar_decomp(A, Q, S);
  767. if (det < 0.0)
  768. {
  769. mat_copy(Q, =, -Q, 3);
  770. parts->f = -1;
  771. }
  772. else
  773. {
  774. parts->f = 1;
  775. }
  776. parts->q = Qt_FromMatrix(Q);
  777. parts->k = spect_decomp(S, U);
  778. parts->u = Qt_FromMatrix(U);
  779. }
  780. // Decompose matrix to affine parts.
  781. void AffineParts::Decompose(const Matrix34& tm)
  782. {
  783. SAffineParts parts;
  784. Matrix44 tm44(tm);
  785. HMatrix& H = *((HMatrix*)&tm44); // Treat HMatrix as a Matrix44.
  786. decomp_affine(H, &parts);
  787. rot = Quat(parts.q.w, parts.q.x, parts.q.y, parts.q.z);
  788. rotScale = Quat(parts.u.w, parts.u.x, parts.u.y, parts.u.z);
  789. pos = Vec3(parts.t.x, parts.t.y, parts.t.z);
  790. scale = Vec3(parts.k.x, parts.k.y, parts.k.z);
  791. fDet = parts.f;
  792. }
  793. // Spectral matrix decompostion to affine parts.
  794. void AffineParts::SpectralDecompose(const Matrix34& tm)
  795. {
  796. SAffineParts parts;
  797. Matrix44 tm44(tm);
  798. HMatrix& H = *((HMatrix*)&tm44); // Treat HMatrix as a Matrix44.
  799. spectral_decomp_affine(H, &parts);
  800. rot = Quat(parts.q.w, parts.q.x, parts.q.y, parts.q.z);
  801. rotScale = Quat(parts.u.w, parts.u.x, parts.u.y, parts.u.z);
  802. pos = Vec3(parts.t.x, parts.t.y, parts.t.z);
  803. scale = Vec3(parts.k.x, parts.k.y, parts.k.z);
  804. fDet = parts.f;
  805. }