LCOV - code coverage report
Current view: top level - source/collada - Decompose.cpp (source / functions) Hit Total Coverage
Test: 0 A.D. test coverage report Lines: 0 258 0.0 %
Date: 2023-01-19 00:18:29 Functions: 0 23 0.0 %

          Line data    Source code
       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           0 : void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
      48             : {
      49             :     int i, j;
      50           0 :     for (i=0; i<3; i++) for (j=0; j<3; j++)
      51           0 :     AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
      52           0 : }
      53             : 
      54             : /** Return dot product of length 3 vectors va and vb **/
      55           0 : float vdot(float *va, float *vb)
      56             : {
      57           0 :     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           0 : void vcross(float *va, float *vb, float *v)
      62             : {
      63           0 :     v[0] = va[1]*vb[2] - va[2]*vb[1];
      64           0 :     v[1] = va[2]*vb[0] - va[0]*vb[2];
      65           0 :     v[2] = va[0]*vb[1] - va[1]*vb[0];
      66           0 : }
      67             : 
      68             : /** Set MadjT to transpose of inverse of M times determinant of M **/
      69           0 : void adjoint_transpose(HMatrix M, HMatrix MadjT)
      70             : {
      71           0 :     vcross(M[1], M[2], MadjT[0]);
      72           0 :     vcross(M[2], M[0], MadjT[1]);
      73           0 :     vcross(M[0], M[1], MadjT[2]);
      74           0 : }
      75             : 
      76             : /******* Quaternion Preliminaries *******/
      77             : 
      78             : /* Construct a (possibly non-unit) quaternion from real components. */
      79           0 : Quat Qt_(float x, float y, float z, float w)
      80             : {
      81             :     Quat qq;
      82           0 :     qq.x = x; qq.y = y; qq.z = z; qq.w = w;
      83           0 :     return (qq);
      84             : }
      85             : 
      86             : /* Return conjugate of quaternion. */
      87           0 : Quat Qt_Conj(Quat q)
      88             : {
      89             :     Quat qq;
      90           0 :     qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
      91           0 :     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           0 : Quat Qt_Mul(Quat qL, Quat qR)
      98             : {
      99             :     Quat qq;
     100           0 :     qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
     101           0 :     qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
     102           0 :     qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
     103           0 :     qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
     104           0 :     return (qq);
     105             : }
     106             : 
     107             : /* Return product of quaternion q by scalar w. */
     108           0 : Quat Qt_Scale(Quat q, float w)
     109             : {
     110             :     Quat qq;
     111           0 :     qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
     112           0 :     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           0 : 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           0 :     tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
     130           0 :     if (tr >= 0.0) {
     131           0 :         s = sqrt(tr + mat[W][W]);
     132           0 :         qu.w = s*0.5;
     133           0 :         s = 0.5 / s;
     134           0 :         qu.x = (mat[Z][Y] - mat[Y][Z]) * s;
     135           0 :         qu.y = (mat[X][Z] - mat[Z][X]) * s;
     136           0 :         qu.z = (mat[Y][X] - mat[X][Y]) * s;
     137             :     } else {
     138           0 :         int h = X;
     139           0 :         if (mat[Y][Y] > mat[X][X]) h = Y;
     140           0 :         if (mat[Z][Z] > mat[h][h]) h = Z;
     141           0 :         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           0 :         caseMacro(x,y,z,X,Y,Z);
     152           0 :         caseMacro(y,z,x,Y,Z,X);
     153           0 :         caseMacro(z,x,y,Z,X,Y);
     154             :         }
     155             :     }
     156           0 :     if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W]));
     157           0 :     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           0 : float mat_norm(HMatrix M, int tpose)
     165             : {
     166             :     int i;
     167             :     float sum, max;
     168           0 :     max = 0.0;
     169           0 :     for (i=0; i<3; i++) {
     170           0 :     if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]);
     171           0 :     else       sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]);
     172           0 :     if (max<sum) max = sum;
     173             :     }
     174           0 :     return max;
     175             : }
     176             : 
     177           0 : float norm_inf(HMatrix M) {return mat_norm(M, 0);}
     178           0 : 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           0 : int find_max_col(HMatrix M)
     182             : {
     183             :     float abs, max;
     184             :     int i, j, col;
     185           0 :     max = 0.0; col = -1;
     186           0 :     for (i=0; i<3; i++) for (j=0; j<3; j++) {
     187           0 :     abs = M[i][j]; if (abs<0.0) abs = -abs;
     188           0 :     if (abs>max) {max = abs; col = j;}
     189             :     }
     190           0 :     return col;
     191             : }
     192             : 
     193             : /** Setup u for Household reflection to zero all v components but first **/
     194           0 : void make_reflector(float *v, float *u)
     195             : {
     196           0 :     float s = sqrt(vdot(v, v));
     197           0 :     u[0] = v[0]; u[1] = v[1];
     198           0 :     u[2] = v[2] + ((v[2]<0.0) ? -s : s);
     199           0 :     s = sqrt(2.0/vdot(u, u));
     200           0 :     u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
     201           0 : }
     202             : 
     203             : /** Apply Householder reflection represented by u to column vectors of M **/
     204           0 : void reflect_cols(HMatrix M, float *u)
     205             : {
     206             :     int i, j;
     207           0 :     for (i=0; i<3; i++) {
     208           0 :     float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
     209           0 :     for (j=0; j<3; j++) M[j][i] -= u[j]*s;
     210             :     }
     211           0 : }
     212             : /** Apply Householder reflection represented by u to row vectors of M **/
     213           0 : void reflect_rows(HMatrix M, float *u)
     214             : {
     215             :     int i, j;
     216           0 :     for (i=0; i<3; i++) {
     217           0 :     float s = vdot(u, M[i]);
     218           0 :     for (j=0; j<3; j++) M[i][j] -= u[j]*s;
     219             :     }
     220           0 : }
     221             : 
     222             : /** Find orthogonal factor Q of rank 1 (or less) M **/
     223           0 : void do_rank1(HMatrix M, HMatrix Q)
     224             : {
     225             :     float v1[3], v2[3], s;
     226             :     int col;
     227           0 :     mat_copy(Q,=,mat_id,4);
     228             :     /* If rank(M) is 1, we should find a non-zero column in M */
     229           0 :     col = find_max_col(M);
     230           0 :     if (col<0) return; /* Rank is 0 */
     231           0 :     v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
     232           0 :     make_reflector(v1, v1); reflect_cols(M, v1);
     233           0 :     v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
     234           0 :     make_reflector(v2, v2); reflect_rows(M, v2);
     235           0 :     s = M[2][2];
     236           0 :     if (s<0.0) Q[2][2] = -1.0;
     237           0 :     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           0 : 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           0 :     col = find_max_col(MadjT);
     248           0 :     if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
     249           0 :     v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
     250           0 :     make_reflector(v1, v1); reflect_cols(M, v1);
     251           0 :     vcross(M[0], M[1], v2);
     252           0 :     make_reflector(v2, v2); reflect_rows(M, v2);
     253           0 :     w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
     254           0 :     if (w*z>x*y) {
     255           0 :     c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
     256           0 :     Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
     257             :     } else {
     258           0 :     c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
     259           0 :     Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
     260             :     }
     261           0 :     Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
     262           0 :     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           0 : 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           0 :     mat_tpose(Mk,=,M,3);
     280           0 :     M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
     281           0 :     do {
     282           0 :     adjoint_transpose(Mk, MadjTk);
     283           0 :     det = vdot(Mk[0], MadjTk[0]);
     284           0 :     if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;}
     285           0 :     MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
     286           0 :     gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
     287           0 :     g1 = gamma*0.5;
     288           0 :     g2 = 0.5/(gamma*det);
     289           0 :     mat_copy(Ek,=,Mk,3);
     290           0 :     mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3);
     291           0 :     mat_copy(Ek,-=,Mk,3);
     292           0 :     E_one = norm_one(Ek);
     293           0 :     M_one = norm_one(Mk);  M_inf = norm_inf(Mk);
     294           0 :     } while (E_one>(M_one*TOL));
     295           0 :     mat_tpose(Q,=,Mk,3); mat_pad(Q);
     296           0 :     mat_mult(Mk, M, S);  mat_pad(S);
     297           0 :     for (int i = 0; i < 3; i++) for (int j = i; j < 3; j++)
     298           0 :     S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
     299           0 :     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           0 : 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           0 :     mat_copy(U, =, mat_id, 4);
     332           0 :     Diag[X] = S[X][X];
     333           0 :     Diag[Y] = S[Y][Y];
     334           0 :     Diag[Z] = S[Z][Z];
     335           0 :     OffD[X] = S[Y][Z];
     336           0 :     OffD[Y] = S[Z][X];
     337           0 :     OffD[Z] = S[X][Y];
     338           0 :     for (int sweep = 20; sweep > 0; --sweep)
     339             :     {
     340           0 :         float sm = fabs(OffD[X]) + fabs(OffD[Y]) + fabs(OffD[Z]);
     341           0 :         if (sm == 0.0)
     342           0 :             break;
     343           0 :         for (int i = Z; i >= X; --i)
     344             :         {
     345           0 :             int p = nxt[i];
     346           0 :             int q = nxt[p];
     347           0 :             fabsOffDi = fabs(OffD[i]);
     348           0 :             g = 100.0 * fabsOffDi;
     349           0 :             if (fabsOffDi > 0.0)
     350             :             {
     351           0 :                 h = Diag[q] - Diag[p];
     352           0 :                 fabsh = fabs(h);
     353           0 :                 if (fabsh + g == fabsh)
     354             :                 {
     355           0 :                     t = OffD[i] / h;
     356             :                 }
     357             :                 else
     358             :                 {
     359           0 :                     theta = 0.5 * h / OffD[i];
     360           0 :                     t = 1.0 / (fabs(theta) + sqrt(theta * theta + 1.0));
     361           0 :                     if (theta < 0.0)
     362           0 :                         t = -t;
     363             :                 }
     364           0 :                 c = 1.0 / sqrt(t * t + 1.0);
     365           0 :                 s = t * c;
     366           0 :                 tau = s / (c + 1.0);
     367           0 :                 ta = t * OffD[i];
     368           0 :                 OffD[i] = 0.0;
     369           0 :                 Diag[p] -= ta;
     370           0 :                 Diag[q] += ta;
     371           0 :                 OffDq = OffD[q];
     372           0 :                 OffD[q] -= s * (OffD[p] + tau * OffD[q]);
     373           0 :                 OffD[p] += s * (OffDq - tau * OffD[p]);
     374           0 :                 for (int j = Z; j >= X; --j)
     375             :                 {
     376           0 :                     a = U[j][p];
     377           0 :                     b = U[j][q];
     378           0 :                     U[j][p] -= s * (b + tau * a);
     379           0 :                     U[j][q] += s * (a - tau * b);
     380             :                 }
     381             :             }
     382             :         }
     383             :     }
     384           0 :     kv.x = Diag[X];
     385           0 :     kv.y = Diag[Y];
     386           0 :     kv.z = Diag[Z];
     387           0 :     kv.w = 1.0;
     388           0 :     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           0 : 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           0 :     int turn = -1;
     410           0 :     ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z;
     411           0 :     if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;}
     412           0 :     else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;}
     413           0 :     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           0 :     switch (turn) {
     426           0 :     default: return (Qt_Conj(q));
     427           0 :     case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break;
     428           0 :     case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break;
     429           0 :     case Z: qtoz = q0001; break;
     430             :     }
     431           0 :     q = Qt_Conj(q);
     432           0 :     mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5;
     433           0 :     mag[1] = (double)q.x*q.z-(double)q.y*q.w;
     434           0 :     mag[2] = (double)q.y*q.z+(double)q.x*q.w;
     435           0 :     for (int i = 0; i < 3; ++i) if ((neg[i] = (mag[i] < 0.0)) != 0) mag[i] = -mag[i];
     436           0 :     if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;}
     437           0 :     else           {if (mag[1]>mag[2]) win = 1; else win = 2;}
     438           0 :     switch (win) {
     439           0 :     case 0: if (neg[0]) p = q1000; else p = q0001; break;
     440           0 :     case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break;
     441           0 :     case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break;
     442             :     }
     443           0 :     qp = Qt_Mul(q, p);
     444           0 :     t = sqrt(mag[win]+0.5);
     445           0 :     p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t));
     446           0 :     p = Qt_Mul(qtoz, Qt_Conj(p));
     447             :     } else {
     448             :     float qa[4], pa[4];
     449           0 :     unsigned lo, hi, neg[4], par = 0;
     450             :     double all, big, two;
     451           0 :     qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w;
     452           0 :     for (int i = 0; i < 4; ++i) {
     453           0 :         pa[i] = 0.0;
     454           0 :         if ((neg[i] = (qa[i]<0.0)) != 0) qa[i] = -qa[i];
     455           0 :         par ^= neg[i];
     456             :     }
     457             :     /* Find two largest components, indices in hi and lo */
     458           0 :     if (qa[0]>qa[1]) lo = 0; else lo = 1;
     459           0 :     if (qa[2]>qa[3]) hi = 2; else hi = 3;
     460           0 :     if (qa[lo]>qa[hi]) {
     461           0 :         if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;}
     462           0 :         else {hi ^= lo; lo ^= hi; hi ^= lo;}
     463           0 :     } else {if (qa[hi^1]>qa[lo]) lo = hi^1;}
     464           0 :     all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5;
     465           0 :     two = (qa[hi]+qa[lo])*SQRTHALF;
     466           0 :     big = qa[hi];
     467           0 :     if (all>two) {
     468           0 :         if (all>big) {/*all*/
     469           0 :         {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);}
     470           0 :         cycle(ka,par)
     471           0 :         } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
     472             :     } else {
     473           0 :         if (two>big) {/*two*/
     474           0 :         pa[hi] = sgn(neg[hi],SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF);
     475           0 :         if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;}
     476           0 :         if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;}
     477           0 :         swap(ka,hi,lo)
     478           0 :         } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
     479             :     }
     480           0 :     p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3];
     481             :     }
     482           0 :     k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z];
     483           0 :     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           0 : void decomp_affine(HMatrix A, AffineParts *parts)
     506             : {
     507             :     HMatrix Q, S, U;
     508             :     Quat p;
     509             :     float det;
     510           0 :     parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
     511           0 :     det = polar_decomp(A, Q, S);
     512           0 :     if (det<0.0) {
     513           0 :     mat_copy(Q,=,-Q,3);
     514           0 :     parts->f = -1;
     515           0 :     } else parts->f = 1;
     516           0 :     parts->q = Qt_FromMatrix(Q);
     517           0 :     parts->k = spect_decomp(S, U);
     518           0 :     parts->u = Qt_FromMatrix(U);
     519           0 :     p = snuggle(parts->u, &parts->k);
     520           0 :     parts->u = Qt_Mul(parts->u, p);
     521           0 : }
     522             : 
     523             : /******* Invert Affine Decomposition *******/
     524             : 
     525             : /* Compute inverse of affine decomposition.
     526             :  */
     527           0 : void invert_affine(AffineParts *parts, AffineParts *inverse)
     528             : {
     529             :     Quat t, p;
     530           0 :     inverse->f = parts->f;
     531           0 :     inverse->q = Qt_Conj(parts->q);
     532           0 :     inverse->u = Qt_Mul(parts->q, parts->u);
     533           0 :     inverse->k.x = (parts->k.x==0.0) ? 0.0 : 1.0/parts->k.x;
     534           0 :     inverse->k.y = (parts->k.y==0.0) ? 0.0 : 1.0/parts->k.y;
     535           0 :     inverse->k.z = (parts->k.z==0.0) ? 0.0 : 1.0/parts->k.z;
     536           0 :     inverse->k.w = parts->k.w;
     537           0 :     t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0);
     538           0 :     t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u));
     539           0 :     t = Qt_(inverse->k.x*t.x, inverse->k.y*t.y, inverse->k.z*t.z, 0);
     540           0 :     p = Qt_Mul(inverse->q, inverse->u);
     541           0 :     t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p)));
     542           0 :     inverse->t = (inverse->f>0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0);
     543           0 : }

Generated by: LCOV version 1.13