source/collada/Decompose.cpp

   1 /* Copyright (C) 2018 Wildfire Games.
   2  * This file is part of 0 A.D.
   3  *
   4  * 0 A.D. is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * 0 A.D. is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "precompiled.h"
  19
  20 #ifdef _MSC_VER
  21 # pragma warning(disable: 4244 4305 4127 4701)
  22 #endif
  23
  24 /**** Decompose.c ****/
  25 /* Ken Shoemake, 1993 */
  26 #include <math.h>
  27 #include "Decompose.h"
  28
  29 /******* Matrix Preliminaries *******/
  30
  31 /** Fill out 3x3 matrix to 4x4 **/
  32 #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)
  33
  34 /** Copy nxn matrix A to C using "gets" for assignment **/
  35 #define mat_copy(C,gets,A,n) {for (int i = 0; i < n; ++i) for (int j = 0; j < n; ++j)\
  36     C[i][j] gets (A[i][j]);}
  37
  38 /** Copy transpose of nxn matrix A to C using "gets" for assignment **/
  39 #define mat_tpose(AT,gets,A,n) {for (int i = 0; i < n; ++i) for (int j = 0; j < n; ++j)\
  40     AT[i][j] gets (A[j][i]);}
  41
  42 /** Assign nxn matrix C the element-wise combination of A and B using "op" **/
  43 #define mat_binop(C,gets,A,op,B,n) {for (int i = 0; i < n; ++i) for (int j = 0; j < n; ++j)\
  44     C[i][j] gets (A[i][j]) op (B[i][j]);}
  45
  46 /** Multiply the upper left 3x3 parts of A and B to get AB **/
  47 void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
  48 {
  49     int i, j;
  50     for (i=0; i<3; i++) for (j=0; j<3; j++)
  51         AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
  52 }
  53
  54 /** Return dot product of length 3 vectors va and vb **/
  55 float vdot(float *va, float *vb)
  56 {
  57     return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
  58 }
  59
  60 /** Set v to cross product of length 3 vectors va and vb **/
  61 void vcross(float *va, float *vb, float *v)
  62 {
  63     v[0] = va[1]*vb[2] - va[2]*vb[1];
  64     v[1] = va[2]*vb[0] - va[0]*vb[2];
  65     v[2] = va[0]*vb[1] - va[1]*vb[0];
  66 }
  67
  68 /** Set MadjT to transpose of inverse of M times determinant of M **/
  69 void adjoint_transpose(HMatrix M, HMatrix MadjT)
  70 {
  71     vcross(M[1], M[2], MadjT[0]);
  72     vcross(M[2], M[0], MadjT[1]);
  73     vcross(M[0], M[1], MadjT[2]);
  74 }
  75
  76 /******* Quaternion Preliminaries *******/
  77
  78 /* Construct a (possibly non-unit) quaternion from real components. */
  79 Quat Qt_(float x, float y, float z, float w)
  80 {
  81     Quat qq;
  82     qq.x = x; qq.y = y; qq.z = z; qq.w = w;
  83     return (qq);
  84 }
  85
  86 /* Return conjugate of quaternion. */
  87 Quat Qt_Conj(Quat q)
  88 {
  89     Quat qq;
  90     qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
  91     return (qq);
  92 }
  93
  94 /* Return quaternion product qL * qR.  Note: order is important!
  95  * To combine rotations, use the product Mul(qSecond, qFirst),
  96  * which gives the effect of rotating by qFirst then qSecond. */
  97 Quat Qt_Mul(Quat qL, Quat qR)
  98 {
  99     Quat qq;
 100     qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
 101     qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
 102     qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
 103     qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
 104     return (qq);
 105 }
 106
 107 /* Return product of quaternion q by scalar w. */
 108 Quat Qt_Scale(Quat q, float w)
 109 {
 110     Quat qq;
 111     qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
 112     return (qq);
 113 }
 114
 115 /* Construct a unit quaternion from rotation matrix.  Assumes matrix is
 116  * used to multiply column vector on the left: vnew = mat vold.  Works
 117  * correctly for right-handed coordinate system and right-handed rotations.
 118  * Translation and perspective components ignored. */
 119 Quat Qt_FromMatrix(HMatrix mat)
 120 {
 121     /* This algorithm avoids near-zero divides by looking for a large component
 122      * - first w, then x, y, or z.  When the trace is greater than zero,
 123      * |w| is greater than 1/2, which is as small as a largest component can be.
 124      * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
 125      * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
 126     Quat qu;
 127     double tr, s;
 128
 129     tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
 130     if (tr >= 0.0) {
 131             s = sqrt(tr + mat[W][W]);
 132             qu.w = s*0.5;
 133             s = 0.5 / s;
 134             qu.x = (mat[Z][Y] - mat[Y][Z]) * s;
 135             qu.y = (mat[X][Z] - mat[Z][X]) * s;
 136             qu.z = (mat[Y][X] - mat[X][Y]) * s;
 137         } else {
 138             int h = X;
 139             if (mat[Y][Y] > mat[X][X]) h = Y;
 140             if (mat[Z][Z] > mat[h][h]) h = Z;
 141             switch (h) {
 142 #define caseMacro(i,j,k,I,J,K) \
 143             case I:\
 144                 s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
 145                 qu.i = s*0.5;\
 146                 s = 0.5 / s;\
 147                 qu.j = (mat[I][J] + mat[J][I]) * s;\
 148                 qu.k = (mat[K][I] + mat[I][K]) * s;\
 149                 qu.w = (mat[K][J] - mat[J][K]) * s;\
 150                 break
 151             caseMacro(x,y,z,X,Y,Z);
 152             caseMacro(y,z,x,Y,Z,X);
 153             caseMacro(z,x,y,Z,X,Y);
 154             }
 155         }
 156     if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W]));
 157     return (qu);
 158 }
 159 /******* Decomp Auxiliaries *******/
 160
 161 static HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
 162
 163 /** Compute either the 1 or infinity norm of M, depending on tpose **/
 164 float mat_norm(HMatrix M, int tpose)
 165 {
 166     int i;
 167     float sum, max;
 168     max = 0.0;
 169     for (i=0; i<3; i++) {
 170         if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]);
 171         else       sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]);
 172         if (max<sum) max = sum;
 173     }
 174     return max;
 175 }
 176
 177 float norm_inf(HMatrix M) {return mat_norm(M, 0);}
 178 float norm_one(HMatrix M) {return mat_norm(M, 1);}
 179
 180 /** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
 181 int find_max_col(HMatrix M)
 182 {
 183     float abs, max;
 184     int i, j, col;
 185     max = 0.0; col = -1;
 186     for (i=0; i<3; i++) for (j=0; j<3; j++) {
 187         abs = M[i][j]; if (abs<0.0) abs = -abs;
 188         if (abs>max) {max = abs; col = j;}
 189     }
 190     return col;
 191 }
 192
 193 /** Setup u for Household reflection to zero all v components but first **/
 194 void make_reflector(float *v, float *u)
 195 {
 196     float s = sqrt(vdot(v, v));
 197     u[0] = v[0]; u[1] = v[1];
 198     u[2] = v[2] + ((v[2]<0.0) ? -s : s);
 199     s = sqrt(2.0/vdot(u, u));
 200     u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
 201 }
 202
 203 /** Apply Householder reflection represented by u to column vectors of M **/
 204 void reflect_cols(HMatrix M, float *u)
 205 {
 206     int i, j;
 207     for (i=0; i<3; i++) {
 208         float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
 209         for (j=0; j<3; j++) M[j][i] -= u[j]*s;
 210     }
 211 }
 212 /** Apply Householder reflection represented by u to row vectors of M **/
 213 void reflect_rows(HMatrix M, float *u)
 214 {
 215     int i, j;
 216     for (i=0; i<3; i++) {
 217         float s = vdot(u, M[i]);
 218         for (j=0; j<3; j++) M[i][j] -= u[j]*s;
 219     }
 220 }
 221
 222 /** Find orthogonal factor Q of rank 1 (or less) M **/
 223 void do_rank1(HMatrix M, HMatrix Q)
 224 {
 225     float v1[3], v2[3], s;
 226     int col;
 227     mat_copy(Q,=,mat_id,4);
 228     /* If rank(M) is 1, we should find a non-zero column in M */
 229     col = find_max_col(M);
 230     if (col<0) return; /* Rank is 0 */
 231     v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
 232     make_reflector(v1, v1); reflect_cols(M, v1);
 233     v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
 234     make_reflector(v2, v2); reflect_rows(M, v2);
 235     s = M[2][2];
 236     if (s<0.0) Q[2][2] = -1.0;
 237     reflect_cols(Q, v1); reflect_rows(Q, v2);
 238 }
 239
 240 /** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
 241 void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q)
 242 {
 243     float v1[3], v2[3];
 244     float w, x, y, z, c, s, d;
 245     int col;
 246     /* If rank(M) is 2, we should find a non-zero column in MadjT */
 247     col = find_max_col(MadjT);
 248     if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
 249     v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
 250     make_reflector(v1, v1); reflect_cols(M, v1);
 251     vcross(M[0], M[1], v2);
 252     make_reflector(v2, v2); reflect_rows(M, v2);
 253     w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
 254     if (w*z>x*y) {
 255         c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
 256         Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
 257     } else {
 258         c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
 259         Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
 260     }
 261     Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
 262     reflect_cols(Q, v1); reflect_rows(Q, v2);
 263 }
 264
 265
 266 /******* Polar Decomposition *******/
 267
 268 /* Polar Decomposition of 3x3 matrix in 4x4,
 269  * M = QS.  See Nicholas Higham and Robert S. Schreiber,
 270  * Fast Polar Decomposition of An Arbitrary Matrix,
 271  * Technical Report 88-942, October 1988,
 272  * Department of Computer Science, Cornell University.
 273  */
 274 float polar_decomp(HMatrix M, HMatrix Q, HMatrix S)
 275 {
 276 #define TOL 1.0e-6
 277     HMatrix Mk, MadjTk, Ek;
 278     float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
 279     mat_tpose(Mk,=,M,3);
 280     M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
 281     do {
 282         adjoint_transpose(Mk, MadjTk);
 283         det = vdot(Mk[0], MadjTk[0]);
 284         if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;}
 285         MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
 286         gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
 287         g1 = gamma*0.5;
 288         g2 = 0.5/(gamma*det);
 289         mat_copy(Ek,=,Mk,3);
 290         mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3);
 291         mat_copy(Ek,-=,Mk,3);
 292         E_one = norm_one(Ek);
 293         M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
 294     } while (E_one>(M_one*TOL));
 295     mat_tpose(Q,=,Mk,3); mat_pad(Q);
 296     mat_mult(Mk, M, S);  mat_pad(S);
 297         for (int i = 0; i < 3; i++) for (int j = i; j < 3; j++)
 298         S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
 299     return (det);
 300 }
 301
 302
 303
 304
 305
 306
 307
 308
 309
 310
 311
 312
 313
 314
 315
 316
 317
 318 /******* Spectral Decomposition *******/
 319
 320 /* Compute the spectral decomposition of symmetric positive semi-definite S.
 321  * Returns rotation in U and scale factors in result, so that if K is a diagonal
 322  * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
 323  * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
 324  */
 325 HVect spect_decomp(HMatrix S, HMatrix U)
 326 {
 327         HVect kv;
 328         double Diag[3], OffD[3]; /* OffD is off-diag (by omitted index) */
 329         double g, h, fabsh, fabsOffDi, t, theta, c, s, tau, ta, OffDq, a, b;
 330         static char nxt[] = {Y, Z, X};
 331         mat_copy(U, =, mat_id, 4);
 332         Diag[X] = S[X][X];
 333         Diag[Y] = S[Y][Y];
 334         Diag[Z] = S[Z][Z];
 335         OffD[X] = S[Y][Z];
 336         OffD[Y] = S[Z][X];
 337         OffD[Z] = S[X][Y];
 338         for (int sweep = 20; sweep > 0; --sweep)
 339         {
 340                 float sm = fabs(OffD[X]) + fabs(OffD[Y]) + fabs(OffD[Z]);
 341                 if (sm == 0.0)
 342                         break;
 343                 for (int i = Z; i >= X; --i)
 344                 {
 345                         int p = nxt[i];
 346                         int q = nxt[p];
 347                         fabsOffDi = fabs(OffD[i]);
 348                         g = 100.0 * fabsOffDi;
 349                         if (fabsOffDi > 0.0)
 350                         {
 351                                 h = Diag[q] - Diag[p];
 352                                 fabsh = fabs(h);
 353                                 if (fabsh + g == fabsh)
 354                                 {
 355                                         t = OffD[i] / h;
 356                                 }
 357                                 else
 358                                 {
 359                                         theta = 0.5 * h / OffD[i];
 360                                         t = 1.0 / (fabs(theta) + sqrt(theta * theta + 1.0));
 361                                         if (theta < 0.0)
 362                                                 t = -t;
 363                                 }
 364                                 c = 1.0 / sqrt(t * t + 1.0);
 365                                 s = t * c;
 366                                 tau = s / (c + 1.0);
 367                                 ta = t * OffD[i];
 368                                 OffD[i] = 0.0;
 369                                 Diag[p] -= ta;
 370                                 Diag[q] += ta;
 371                                 OffDq = OffD[q];
 372                                 OffD[q] -= s * (OffD[p] + tau * OffD[q]);
 373                                 OffD[p] += s * (OffDq - tau * OffD[p]);
 374                                 for (int j = Z; j >= X; --j)
 375                                 {
 376                                         a = U[j][p];
 377                                         b = U[j][q];
 378                                         U[j][p] -= s * (b + tau * a);
 379                                         U[j][q] += s * (a - tau * b);
 380                                 }
 381                         }
 382                 }
 383         }
 384         kv.x = Diag[X];
 385         kv.y = Diag[Y];
 386         kv.z = Diag[Z];
 387         kv.w = 1.0;
 388         return kv;
 389 }
 390
 391 /******* Spectral Axis Adjustment *******/
 392
 393 /* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
 394  * which permutes the axes and turns freely in the plane of duplicate scale
 395  * factors, such that q p has the largest possible w component, i.e. the
 396  * smallest possible angle. Permutes k's components to go with q p instead of q.
 397  * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
 398  * Proceedings of Graphics Interface 1992. Details on p. 262-263.
 399  */
 400 Quat snuggle(Quat q, HVect *k)
 401 {
 402 #define SQRTHALF (0.7071067811865475244)
 403 #define sgn(n,v)    ((n)?-(v):(v))
 404 #define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
 405 #define cycle(a,p)  if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
 406                     else   {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}
 407     Quat p;
 408     float ka[4];
 409     int turn = -1;
 410     ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z;
 411     if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;}
 412     else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;}
 413     if (turn>=0) {
 414         Quat qtoz, qp;
 415         unsigned neg[3], win;
 416         double mag[3], t;
 417         static Quat qxtoz = {.0f, static_cast<float>(SQRTHALF), .0f, static_cast<float>(SQRTHALF)};
 418         static Quat qytoz = {static_cast<float>(SQRTHALF), .0f, .0f, static_cast<float>(SQRTHALF)};
 419         static Quat qppmm = { 0.5, 0.5,-0.5,-0.5};
 420         static Quat qpppp = { 0.5, 0.5, 0.5, 0.5};
 421         static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5};
 422         static Quat qpppm = { 0.5, 0.5, 0.5,-0.5};
 423         static Quat q0001 = { 0.0, 0.0, 0.0, 1.0};
 424         static Quat q1000 = { 1.0, 0.0, 0.0, 0.0};
 425         switch (turn) {
 426         default: return (Qt_Conj(q));
 427         case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break;
 428         case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break;
 429         case Z: qtoz = q0001; break;
 430         }
 431         q = Qt_Conj(q);
 432         mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5;
 433         mag[1] = (double)q.x*q.z-(double)q.y*q.w;
 434         mag[2] = (double)q.y*q.z+(double)q.x*q.w;
 435         for (int i = 0; i < 3; ++i) if ((neg[i] = (mag[i] < 0.0)) != 0) mag[i] = -mag[i];
 436         if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;}
 437         else               {if (mag[1]>mag[2]) win = 1; else win = 2;}
 438         switch (win) {
 439         case 0: if (neg[0]) p = q1000; else p = q0001; break;
 440         case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break;
 441         case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break;
 442         }
 443         qp = Qt_Mul(q, p);
 444         t = sqrt(mag[win]+0.5);
 445         p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t));
 446         p = Qt_Mul(qtoz, Qt_Conj(p));
 447     } else {
 448         float qa[4], pa[4];
 449         unsigned lo, hi, neg[4], par = 0;
 450         double all, big, two;
 451         qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w;
 452         for (int i = 0; i < 4; ++i) {
 453             pa[i] = 0.0;
 454             if ((neg[i] = (qa[i]<0.0)) != 0) qa[i] = -qa[i];
 455             par ^= neg[i];
 456         }
 457         /* Find two largest components, indices in hi and lo */
 458         if (qa[0]>qa[1]) lo = 0; else lo = 1;
 459         if (qa[2]>qa[3]) hi = 2; else hi = 3;
 460         if (qa[lo]>qa[hi]) {
 461             if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;}
 462             else {hi ^= lo; lo ^= hi; hi ^= lo;}
 463         } else {if (qa[hi^1]>qa[lo]) lo = hi^1;}
 464         all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5;
 465         two = (qa[hi]+qa[lo])*SQRTHALF;
 466         big = qa[hi];
 467         if (all>two) {
 468             if (all>big) {/*all*/
 469                 {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);}
 470                 cycle(ka,par)
 471             } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
 472         } else {
 473             if (two>big) {/*two*/
 474                 pa[hi] = sgn(neg[hi],SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF);
 475                 if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;}
 476                 if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;}
 477                 swap(ka,hi,lo)
 478             } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
 479         }
 480         p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3];
 481     }
 482     k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z];
 483     return (p);
 484 }
 485
 486
 487
 488
 489
 490
 491
 492
 493
 494
 495
 496 /******* Decompose Affine Matrix *******/
 497
 498 /* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
 499  * translation components, q contains the rotation R, u contains U, k contains
 500  * scale factors, and f contains the sign of the determinant.
 501  * Assumes A transforms column vectors in right-handed coordinates.
 502  * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
 503  * Proceedings of Graphics Interface 1992.
 504  */
 505 void decomp_affine(HMatrix A, AffineParts *parts)
 506 {
 507     HMatrix Q, S, U;
 508     Quat p;
 509     float det;
 510     parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
 511     det = polar_decomp(A, Q, S);
 512     if (det<0.0) {
 513         mat_copy(Q,=,-Q,3);
 514         parts->f = -1;
 515     } else parts->f = 1;
 516     parts->q = Qt_FromMatrix(Q);
 517     parts->k = spect_decomp(S, U);
 518     parts->u = Qt_FromMatrix(U);
 519     p = snuggle(parts->u, &parts->k);
 520     parts->u = Qt_Mul(parts->u, p);
 521 }
 522
 523 /******* Invert Affine Decomposition *******/
 524
 525 /* Compute inverse of affine decomposition.
 526  */
 527 void invert_affine(AffineParts *parts, AffineParts *inverse)
 528 {
 529     Quat t, p;
 530     inverse->f = parts->f;
 531     inverse->q = Qt_Conj(parts->q);
 532     inverse->u = Qt_Mul(parts->q, parts->u);
 533     inverse->k.x = (parts->k.x==0.0) ? 0.0 : 1.0/parts->k.x;
 534     inverse->k.y = (parts->k.y==0.0) ? 0.0 : 1.0/parts->k.y;
 535     inverse->k.z = (parts->k.z==0.0) ? 0.0 : 1.0/parts->k.z;
 536     inverse->k.w = parts->k.w;
 537     t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0);
 538     t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u));
 539     t = Qt_(inverse->k.x*t.x, inverse->k.y*t.y, inverse->k.z*t.z, 0);
 540     p = Qt_Mul(inverse->q, inverse->u);
 541     t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p)));
 542     inverse->t = (inverse->f>0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0);
 543 }