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