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 : }
|
