1 | |
---|
2 | |
---|
3 | |
---|
4 | |
---|
5 | |
---|
6 | |
---|
7 | |
---|
8 | |
---|
9 | |
---|
10 | |
---|
11 | |
---|
12 | |
---|
13 | |
---|
14 | |
---|
15 | |
---|
16 | |
---|
17 | |
---|
18 | |
---|
19 | #include <osg/Matrixf> |
---|
20 | #include <osg/Matrixd> |
---|
21 | |
---|
22 | |
---|
23 | #define matrixCopy(C, gets, A, n) {int i, j; for (i=0;i<n;i++) for (j=0;j<n;j++)\ |
---|
24 | C[i][j] gets (A[i][j]);} |
---|
25 | |
---|
26 | |
---|
27 | #define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\ |
---|
28 | AT[i][j] gets (A[j][i]);} |
---|
29 | |
---|
30 | |
---|
31 | #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) |
---|
32 | |
---|
33 | |
---|
34 | |
---|
35 | #define matBinop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\ |
---|
36 | C[i][j] gets (A[i][j]) op (B[i][j]);} |
---|
37 | |
---|
38 | |
---|
39 | #define mat_copy(C,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\ |
---|
40 | C[i][j] gets (A[i][j]);} |
---|
41 | |
---|
42 | namespace MatrixDecomposition |
---|
43 | { |
---|
44 | |
---|
45 | typedef struct {double x, y, z, w;} Quat; |
---|
46 | enum QuatPart {X, Y, Z, W}; |
---|
47 | typedef double _HMatrix[4][4]; |
---|
48 | typedef Quat HVect; |
---|
49 | typedef struct |
---|
50 | { |
---|
51 | osg::Vec4d t; |
---|
52 | Quat q; |
---|
53 | Quat u; |
---|
54 | HVect k; |
---|
55 | double f; |
---|
56 | } _affineParts; |
---|
57 | |
---|
58 | HVect spectDecomp(_HMatrix S, _HMatrix U); |
---|
59 | Quat snuggle(Quat q, HVect* k); |
---|
60 | double polarDecomp(_HMatrix M, _HMatrix Q, _HMatrix S); |
---|
61 | |
---|
62 | static _HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; |
---|
63 | |
---|
64 | #define SQRTHALF (0.7071067811865475244) |
---|
65 | static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF}; |
---|
66 | static Quat qytoz = {SQRTHALF,0,0,SQRTHALF}; |
---|
67 | static Quat qppmm = { 0.5, 0.5,-0.5,-0.5}; |
---|
68 | static Quat qpppp = { 0.5, 0.5, 0.5, 0.5}; |
---|
69 | static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5}; |
---|
70 | static Quat qpppm = { 0.5, 0.5, 0.5,-0.5}; |
---|
71 | static Quat q0001 = { 0.0, 0.0, 0.0, 1.0}; |
---|
72 | static Quat q1000 = { 1.0, 0.0, 0.0, 0.0}; |
---|
73 | |
---|
74 | |
---|
75 | |
---|
76 | Quat Qt_Scale(Quat q, double w) |
---|
77 | { |
---|
78 | Quat qq; |
---|
79 | qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w; |
---|
80 | return (qq); |
---|
81 | } |
---|
82 | |
---|
83 | |
---|
84 | |
---|
85 | |
---|
86 | Quat Qt_Mul(Quat qL, Quat qR) |
---|
87 | { |
---|
88 | Quat qq; |
---|
89 | qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z; |
---|
90 | qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y; |
---|
91 | qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z; |
---|
92 | qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x; |
---|
93 | return (qq); |
---|
94 | } |
---|
95 | |
---|
96 | |
---|
97 | Quat Qt_Conj(Quat q) |
---|
98 | { |
---|
99 | Quat qq; |
---|
100 | qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w; |
---|
101 | return (qq); |
---|
102 | } |
---|
103 | |
---|
104 | |
---|
105 | Quat Qt_(double x, double y, double z, double w) |
---|
106 | { |
---|
107 | Quat qq; |
---|
108 | qq.x = x; qq.y = y; qq.z = z; qq.w = w; |
---|
109 | return (qq); |
---|
110 | } |
---|
111 | |
---|
112 | |
---|
113 | void mat_mult(_HMatrix A, _HMatrix B, _HMatrix AB) |
---|
114 | { |
---|
115 | int i, j; |
---|
116 | for (i=0; i<3; i++) for (j=0; j<3; j++) |
---|
117 | AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j]; |
---|
118 | } |
---|
119 | |
---|
120 | |
---|
121 | void vcross(double *va, double *vb, double *v) |
---|
122 | { |
---|
123 | v[0] = va[1]*vb[2] - va[2]*vb[1]; |
---|
124 | v[1] = va[2]*vb[0] - va[0]*vb[2]; |
---|
125 | v[2] = va[0]*vb[1] - va[1]*vb[0]; |
---|
126 | } |
---|
127 | |
---|
128 | |
---|
129 | double vdot(double *va, double *vb) |
---|
130 | { |
---|
131 | return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]); |
---|
132 | } |
---|
133 | |
---|
134 | |
---|
135 | |
---|
136 | void adjoint_transpose(_HMatrix M, _HMatrix MadjT) |
---|
137 | { |
---|
138 | vcross(M[1], M[2], MadjT[0]); |
---|
139 | vcross(M[2], M[0], MadjT[1]); |
---|
140 | vcross(M[0], M[1], MadjT[2]); |
---|
141 | } |
---|
142 | |
---|
143 | |
---|
144 | int find_max_col(_HMatrix M) |
---|
145 | { |
---|
146 | double abs, max; |
---|
147 | int i, j, col; |
---|
148 | max = 0.0; col = -1; |
---|
149 | for (i=0; i<3; i++) for (j=0; j<3; j++) { |
---|
150 | abs = M[i][j]; if (abs<0.0) abs = -abs; |
---|
151 | if (abs>max) {max = abs; col = j;} |
---|
152 | } |
---|
153 | return col; |
---|
154 | } |
---|
155 | |
---|
156 | |
---|
157 | void make_reflector(double *v, double *u) |
---|
158 | { |
---|
159 | double s = sqrt(vdot(v, v)); |
---|
160 | u[0] = v[0]; u[1] = v[1]; |
---|
161 | u[2] = v[2] + ((v[2]<0.0) ? -s : s); |
---|
162 | s = sqrt(2.0/vdot(u, u)); |
---|
163 | u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s; |
---|
164 | } |
---|
165 | |
---|
166 | |
---|
167 | void reflect_cols(_HMatrix M, double *u) |
---|
168 | { |
---|
169 | int i, j; |
---|
170 | for (i=0; i<3; i++) { |
---|
171 | double s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i]; |
---|
172 | for (j=0; j<3; j++) M[j][i] -= u[j]*s; |
---|
173 | } |
---|
174 | } |
---|
175 | |
---|
176 | |
---|
177 | void reflect_rows(_HMatrix M, double *u) |
---|
178 | { |
---|
179 | int i, j; |
---|
180 | for (i=0; i<3; i++) { |
---|
181 | double s = vdot(u, M[i]); |
---|
182 | for (j=0; j<3; j++) M[i][j] -= u[j]*s; |
---|
183 | } |
---|
184 | } |
---|
185 | |
---|
186 | |
---|
187 | void do_rank1(_HMatrix M, _HMatrix Q) |
---|
188 | { |
---|
189 | double v1[3], v2[3], s; |
---|
190 | int col; |
---|
191 | mat_copy(Q,=,mat_id,4); |
---|
192 | |
---|
193 | col = find_max_col(M); |
---|
194 | if (col<0) return; |
---|
195 | v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col]; |
---|
196 | make_reflector(v1, v1); reflect_cols(M, v1); |
---|
197 | v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2]; |
---|
198 | make_reflector(v2, v2); reflect_rows(M, v2); |
---|
199 | s = M[2][2]; |
---|
200 | if (s<0.0) Q[2][2] = -1.0; |
---|
201 | reflect_cols(Q, v1); reflect_rows(Q, v2); |
---|
202 | } |
---|
203 | |
---|
204 | |
---|
205 | |
---|
206 | void do_rank2(_HMatrix M, _HMatrix MadjT, _HMatrix Q) |
---|
207 | { |
---|
208 | double v1[3], v2[3]; |
---|
209 | double w, x, y, z, c, s, d; |
---|
210 | int col; |
---|
211 | |
---|
212 | col = find_max_col(MadjT); |
---|
213 | if (col<0) {do_rank1(M, Q); return;} |
---|
214 | v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col]; |
---|
215 | make_reflector(v1, v1); reflect_cols(M, v1); |
---|
216 | vcross(M[0], M[1], v2); |
---|
217 | make_reflector(v2, v2); reflect_rows(M, v2); |
---|
218 | w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1]; |
---|
219 | if (w*z>x*y) { |
---|
220 | c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d; |
---|
221 | Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s); |
---|
222 | } else { |
---|
223 | c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d; |
---|
224 | Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s; |
---|
225 | } |
---|
226 | Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0; |
---|
227 | reflect_cols(Q, v1); reflect_rows(Q, v2); |
---|
228 | } |
---|
229 | |
---|
230 | |
---|
231 | double mat_norm(_HMatrix M, int tpose) |
---|
232 | { |
---|
233 | int i; |
---|
234 | double sum, max; |
---|
235 | max = 0.0; |
---|
236 | for (i=0; i<3; i++) { |
---|
237 | if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]); |
---|
238 | else sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]); |
---|
239 | if (max<sum) max = sum; |
---|
240 | } |
---|
241 | return max; |
---|
242 | } |
---|
243 | |
---|
244 | double norm_inf(_HMatrix M) {return mat_norm(M, 0);} |
---|
245 | double norm_one(_HMatrix M) {return mat_norm(M, 1);} |
---|
246 | |
---|
247 | |
---|
248 | |
---|
249 | |
---|
250 | |
---|
251 | |
---|
252 | Quat quatFromMatrix(_HMatrix mat) |
---|
253 | { |
---|
254 | |
---|
255 | |
---|
256 | |
---|
257 | |
---|
258 | |
---|
259 | Quat qu = q0001; |
---|
260 | double tr, s; |
---|
261 | |
---|
262 | tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z]; |
---|
263 | if (tr >= 0.0) |
---|
264 | { |
---|
265 | s = sqrt(tr + mat[W][W]); |
---|
266 | qu.w = s*0.5; |
---|
267 | s = 0.5 / s; |
---|
268 | qu.x = (mat[Z][Y] - mat[Y][Z]) * s; |
---|
269 | qu.y = (mat[X][Z] - mat[Z][X]) * s; |
---|
270 | qu.z = (mat[Y][X] - mat[X][Y]) * s; |
---|
271 | } |
---|
272 | else |
---|
273 | { |
---|
274 | int h = X; |
---|
275 | if (mat[Y][Y] > mat[X][X]) h = Y; |
---|
276 | if (mat[Z][Z] > mat[h][h]) h = Z; |
---|
277 | switch (h) { |
---|
278 | #define caseMacro(i,j,k,I,J,K) \ |
---|
279 | case I:\ |
---|
280 | s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\ |
---|
281 | qu.i = s*0.5;\ |
---|
282 | s = 0.5 / s;\ |
---|
283 | qu.j = (mat[I][J] + mat[J][I]) * s;\ |
---|
284 | qu.k = (mat[K][I] + mat[I][K]) * s;\ |
---|
285 | qu.w = (mat[K][J] - mat[J][K]) * s;\ |
---|
286 | break |
---|
287 | caseMacro(x,y,z,X,Y,Z); |
---|
288 | caseMacro(y,z,x,Y,Z,X); |
---|
289 | caseMacro(z,x,y,Z,X,Y); |
---|
290 | } |
---|
291 | } |
---|
292 | if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W])); |
---|
293 | return (qu); |
---|
294 | } |
---|
295 | |
---|
296 | |
---|
297 | |
---|
298 | |
---|
299 | |
---|
300 | |
---|
301 | |
---|
302 | |
---|
303 | |
---|
304 | |
---|
305 | |
---|
306 | void decompAffine(_HMatrix A, _affineParts * parts) |
---|
307 | { |
---|
308 | _HMatrix Q, S, U; |
---|
309 | Quat p; |
---|
310 | |
---|
311 | |
---|
312 | parts->t = osg::Vec4d(A[X][W], A[Y][W], A[Z][W], 0); |
---|
313 | double det = polarDecomp(A, Q, S); |
---|
314 | if (det<0.0) |
---|
315 | { |
---|
316 | matrixCopy(Q, =, -Q, 3); |
---|
317 | parts->f = -1; |
---|
318 | } |
---|
319 | else |
---|
320 | parts->f = 1; |
---|
321 | |
---|
322 | parts->q = quatFromMatrix(Q); |
---|
323 | parts->k = spectDecomp(S, U); |
---|
324 | parts->u = quatFromMatrix(U); |
---|
325 | p = snuggle(parts->u, &parts->k); |
---|
326 | parts->u = Qt_Mul(parts->u, p); |
---|
327 | } |
---|
328 | |
---|
329 | |
---|
330 | |
---|
331 | |
---|
332 | |
---|
333 | |
---|
334 | |
---|
335 | |
---|
336 | |
---|
337 | |
---|
338 | double polarDecomp( _HMatrix M, _HMatrix Q, _HMatrix S) |
---|
339 | { |
---|
340 | |
---|
341 | #define TOL 1.0e-6 |
---|
342 | _HMatrix Mk, MadjTk, Ek; |
---|
343 | double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2; |
---|
344 | int i, j; |
---|
345 | |
---|
346 | mat_tpose(Mk,=,M,3); |
---|
347 | M_one = norm_one(Mk); M_inf = norm_inf(Mk); |
---|
348 | |
---|
349 | do |
---|
350 | { |
---|
351 | adjoint_transpose(Mk, MadjTk); |
---|
352 | det = vdot(Mk[0], MadjTk[0]); |
---|
353 | if (det==0.0) |
---|
354 | { |
---|
355 | do_rank2(Mk, MadjTk, Mk); |
---|
356 | break; |
---|
357 | } |
---|
358 | |
---|
359 | MadjT_one = norm_one(MadjTk); |
---|
360 | MadjT_inf = norm_inf(MadjTk); |
---|
361 | |
---|
362 | gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det)); |
---|
363 | g1 = gamma*0.5; |
---|
364 | g2 = 0.5/(gamma*det); |
---|
365 | matrixCopy(Ek,=,Mk,3); |
---|
366 | matBinop(Mk,=,g1*Mk,+,g2*MadjTk,3); |
---|
367 | mat_copy(Ek,-=,Mk,3); |
---|
368 | E_one = norm_one(Ek); |
---|
369 | M_one = norm_one(Mk); |
---|
370 | M_inf = norm_inf(Mk); |
---|
371 | |
---|
372 | } while(E_one>(M_one*TOL)); |
---|
373 | |
---|
374 | mat_tpose(Q,=,Mk,3); mat_pad(Q); |
---|
375 | mat_mult(Mk, M, S); mat_pad(S); |
---|
376 | |
---|
377 | for (i=0; i<3; i++) |
---|
378 | for (j=i; j<3; j++) |
---|
379 | S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]); |
---|
380 | return (det); |
---|
381 | } |
---|
382 | |
---|
383 | |
---|
384 | |
---|
385 | |
---|
386 | |
---|
387 | |
---|
388 | |
---|
389 | |
---|
390 | HVect spectDecomp(_HMatrix S, _HMatrix U) |
---|
391 | { |
---|
392 | HVect kv; |
---|
393 | double Diag[3],OffD[3]; |
---|
394 | double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b; |
---|
395 | static char nxt[] = {Y,Z,X}; |
---|
396 | int sweep, i, j; |
---|
397 | mat_copy(U,=,mat_id,4); |
---|
398 | Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z]; |
---|
399 | OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y]; |
---|
400 | for (sweep=20; sweep>0; sweep--) { |
---|
401 | double sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]); |
---|
402 | if (sm==0.0) break; |
---|
403 | for (i=Z; i>=X; i--) { |
---|
404 | int p = nxt[i]; int q = nxt[p]; |
---|
405 | fabsOffDi = fabs(OffD[i]); |
---|
406 | g = 100.0*fabsOffDi; |
---|
407 | if (fabsOffDi>0.0) { |
---|
408 | h = Diag[q] - Diag[p]; |
---|
409 | fabsh = fabs(h); |
---|
410 | if (fabsh+g==fabsh) { |
---|
411 | t = OffD[i]/h; |
---|
412 | } else { |
---|
413 | theta = 0.5*h/OffD[i]; |
---|
414 | t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0)); |
---|
415 | if (theta<0.0) t = -t; |
---|
416 | } |
---|
417 | c = 1.0/sqrt(t*t+1.0); s = t*c; |
---|
418 | tau = s/(c+1.0); |
---|
419 | ta = t*OffD[i]; OffD[i] = 0.0; |
---|
420 | Diag[p] -= ta; Diag[q] += ta; |
---|
421 | OffDq = OffD[q]; |
---|
422 | OffD[q] -= s*(OffD[p] + tau*OffD[q]); |
---|
423 | OffD[p] += s*(OffDq - tau*OffD[p]); |
---|
424 | for (j=Z; j>=X; j--) { |
---|
425 | a = U[j][p]; b = U[j][q]; |
---|
426 | U[j][p] -= s*(b + tau*a); |
---|
427 | U[j][q] += s*(a - tau*b); |
---|
428 | } |
---|
429 | } |
---|
430 | } |
---|
431 | } |
---|
432 | kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0; |
---|
433 | return (kv); |
---|
434 | } |
---|
435 | |
---|
436 | |
---|
437 | |
---|
438 | |
---|
439 | |
---|
440 | |
---|
441 | |
---|
442 | |
---|
443 | |
---|
444 | |
---|
445 | |
---|
446 | Quat snuggle(Quat q, HVect *k) |
---|
447 | { |
---|
448 | #define sgn(n,v) ((n)?-(v):(v)) |
---|
449 | #define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];} |
---|
450 | #define cycle(a,p) if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\ |
---|
451 | else {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];} |
---|
452 | |
---|
453 | Quat p = q0001; |
---|
454 | double ka[4]; |
---|
455 | int i, turn = -1; |
---|
456 | ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z; |
---|
457 | |
---|
458 | if (ka[X]==ka[Y]) { |
---|
459 | if (ka[X]==ka[Z]) |
---|
460 | turn = W; |
---|
461 | else turn = Z; |
---|
462 | } |
---|
463 | else { |
---|
464 | if (ka[X]==ka[Z]) |
---|
465 | turn = Y; |
---|
466 | else if (ka[Y]==ka[Z]) |
---|
467 | turn = X; |
---|
468 | } |
---|
469 | if (turn>=0) { |
---|
470 | Quat qtoz, qp; |
---|
471 | unsigned int win; |
---|
472 | double mag[3], t; |
---|
473 | switch (turn) { |
---|
474 | default: return (Qt_Conj(q)); |
---|
475 | case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break; |
---|
476 | case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break; |
---|
477 | case Z: qtoz = q0001; break; |
---|
478 | } |
---|
479 | q = Qt_Conj(q); |
---|
480 | mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5; |
---|
481 | mag[1] = (double)q.x*q.z-(double)q.y*q.w; |
---|
482 | mag[2] = (double)q.y*q.z+(double)q.x*q.w; |
---|
483 | |
---|
484 | bool neg[3]; |
---|
485 | for (i=0; i<3; i++) |
---|
486 | { |
---|
487 | neg[i] = (mag[i]<0.0); |
---|
488 | if (neg[i]) mag[i] = -mag[i]; |
---|
489 | } |
---|
490 | |
---|
491 | if (mag[0]>mag[1]) { |
---|
492 | if (mag[0]>mag[2]) |
---|
493 | win = 0; |
---|
494 | else win = 2; |
---|
495 | } |
---|
496 | else { |
---|
497 | if (mag[1]>mag[2]) win = 1; |
---|
498 | else win = 2; |
---|
499 | } |
---|
500 | |
---|
501 | switch (win) { |
---|
502 | case 0: if (neg[0]) p = q1000; else p = q0001; break; |
---|
503 | case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break; |
---|
504 | case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break; |
---|
505 | } |
---|
506 | |
---|
507 | qp = Qt_Mul(q, p); |
---|
508 | t = sqrt(mag[win]+0.5); |
---|
509 | p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t)); |
---|
510 | p = Qt_Mul(qtoz, Qt_Conj(p)); |
---|
511 | } |
---|
512 | else { |
---|
513 | double qa[4], pa[4]; |
---|
514 | unsigned int lo, hi; |
---|
515 | bool par = false; |
---|
516 | bool neg[4]; |
---|
517 | double all, big, two; |
---|
518 | qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w; |
---|
519 | for (i=0; i<4; i++) { |
---|
520 | pa[i] = 0.0; |
---|
521 | neg[i] = (qa[i]<0.0); |
---|
522 | if (neg[i]) qa[i] = -qa[i]; |
---|
523 | par ^= neg[i]; |
---|
524 | } |
---|
525 | |
---|
526 | |
---|
527 | if (qa[0]>qa[1]) lo = 0; |
---|
528 | else lo = 1; |
---|
529 | |
---|
530 | if (qa[2]>qa[3]) hi = 2; |
---|
531 | else hi = 3; |
---|
532 | |
---|
533 | if (qa[lo]>qa[hi]) { |
---|
534 | if (qa[lo^1]>qa[hi]) { |
---|
535 | hi = lo; lo ^= 1; |
---|
536 | } |
---|
537 | else { |
---|
538 | hi ^= lo; lo ^= hi; hi ^= lo; |
---|
539 | } |
---|
540 | } |
---|
541 | else { |
---|
542 | if (qa[hi^1]>qa[lo]) lo = hi^1; |
---|
543 | } |
---|
544 | |
---|
545 | all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5; |
---|
546 | two = (qa[hi]+qa[lo])*SQRTHALF; |
---|
547 | big = qa[hi]; |
---|
548 | if (all>two) { |
---|
549 | if (all>big) { |
---|
550 | {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);} |
---|
551 | cycle(ka,par); |
---|
552 | } |
---|
553 | else { pa[hi] = sgn(neg[hi],1.0);} |
---|
554 | } else { |
---|
555 | if (two>big) { |
---|
556 | pa[hi] = sgn(neg[hi],SQRTHALF); |
---|
557 | pa[lo] = sgn(neg[lo], SQRTHALF); |
---|
558 | if (lo>hi) { |
---|
559 | hi ^= lo; lo ^= hi; hi ^= lo; |
---|
560 | } |
---|
561 | if (hi==W) { |
---|
562 | hi = "\001\002\000"[lo]; |
---|
563 | lo = 3-hi-lo; |
---|
564 | } |
---|
565 | swap(ka,hi,lo); |
---|
566 | } |
---|
567 | else { |
---|
568 | pa[hi] = sgn(neg[hi],1.0); |
---|
569 | } |
---|
570 | } |
---|
571 | p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3]; |
---|
572 | } |
---|
573 | k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z]; |
---|
574 | return (p); |
---|
575 | } |
---|
576 | |
---|
577 | } |
---|
578 | |
---|
579 | void osg::Matrixf::decompose(osg::Vec3f& t, |
---|
580 | osg::Quat& r, |
---|
581 | osg::Vec3f& s, |
---|
582 | osg::Quat& so) const |
---|
583 | { |
---|
584 | Vec3d temp_trans; |
---|
585 | Vec3d temp_scale; |
---|
586 | decompose(temp_trans, r, temp_scale, so); |
---|
587 | t = temp_trans; |
---|
588 | s = temp_scale; |
---|
589 | } |
---|
590 | |
---|
591 | |
---|
592 | void osg::Matrixf::decompose(osg::Vec3d& t, |
---|
593 | osg::Quat& r, |
---|
594 | osg::Vec3d& s, |
---|
595 | osg::Quat& so) const |
---|
596 | { |
---|
597 | MatrixDecomposition::_affineParts parts; |
---|
598 | MatrixDecomposition::_HMatrix hmatrix; |
---|
599 | |
---|
600 | |
---|
601 | for ( int i =0; i<4; i++) |
---|
602 | { |
---|
603 | for ( int j=0; j<4; j++) |
---|
604 | { |
---|
605 | hmatrix[i][j] = (*this)(j,i); |
---|
606 | } |
---|
607 | } |
---|
608 | |
---|
609 | MatrixDecomposition::decompAffine(hmatrix, &parts); |
---|
610 | |
---|
611 | double mul = 1.0; |
---|
612 | if (parts.t[MatrixDecomposition::W] != 0.0) |
---|
613 | mul = 1.0 / parts.t[MatrixDecomposition::W]; |
---|
614 | |
---|
615 | t[0] = parts.t[MatrixDecomposition::X] * mul; |
---|
616 | t[1] = parts.t[MatrixDecomposition::Y] * mul; |
---|
617 | t[2] = parts.t[MatrixDecomposition::Z] * mul; |
---|
618 | |
---|
619 | r.set(parts.q.x, parts.q.y, parts.q.z, parts.q.w); |
---|
620 | |
---|
621 | mul = 1.0; |
---|
622 | if (parts.k.w != 0.0) |
---|
623 | mul = 1.0 / parts.k.w; |
---|
624 | |
---|
625 | |
---|
626 | mul *= parts.f; |
---|
627 | s[0] = parts.k.x * mul; |
---|
628 | s[1] = parts.k.y * mul; |
---|
629 | s[2] = parts.k.z * mul; |
---|
630 | |
---|
631 | so.set(parts.u.x, parts.u.y, parts.u.z, parts.u.w); |
---|
632 | } |
---|
633 | |
---|
634 | void osg::Matrixd::decompose(osg::Vec3f& t, |
---|
635 | osg::Quat& r, |
---|
636 | osg::Vec3f& s, |
---|
637 | osg::Quat& so) const |
---|
638 | { |
---|
639 | Vec3d temp_trans; |
---|
640 | Vec3d temp_scale; |
---|
641 | decompose(temp_trans, r, temp_scale, so); |
---|
642 | t = temp_trans; |
---|
643 | s = temp_scale; |
---|
644 | } |
---|
645 | |
---|
646 | void osg::Matrixd::decompose(osg::Vec3d& t, |
---|
647 | osg::Quat& r, |
---|
648 | osg::Vec3d& s, |
---|
649 | osg::Quat& so) const |
---|
650 | { |
---|
651 | MatrixDecomposition::_affineParts parts; |
---|
652 | MatrixDecomposition::_HMatrix hmatrix; |
---|
653 | |
---|
654 | |
---|
655 | for ( int i =0; i<4; i++) |
---|
656 | { |
---|
657 | for ( int j=0; j<4; j++) |
---|
658 | { |
---|
659 | hmatrix[i][j] = (*this)(j,i); |
---|
660 | } |
---|
661 | } |
---|
662 | |
---|
663 | MatrixDecomposition::decompAffine(hmatrix, &parts); |
---|
664 | |
---|
665 | double mul = 1.0; |
---|
666 | if (parts.t[MatrixDecomposition::W] != 0.0) |
---|
667 | mul = 1.0 / parts.t[MatrixDecomposition::W]; |
---|
668 | |
---|
669 | t[0] = parts.t[MatrixDecomposition::X] * mul; |
---|
670 | t[1] = parts.t[MatrixDecomposition::Y] * mul; |
---|
671 | t[2] = parts.t[MatrixDecomposition::Z] * mul; |
---|
672 | |
---|
673 | r.set(parts.q.x, parts.q.y, parts.q.z, parts.q.w); |
---|
674 | |
---|
675 | mul = 1.0; |
---|
676 | if (parts.k.w != 0.0) |
---|
677 | mul = 1.0 / parts.k.w; |
---|
678 | |
---|
679 | |
---|
680 | mul *= parts.f; |
---|
681 | s[0] = parts.k.x * mul; |
---|
682 | s[1] = parts.k.y * mul; |
---|
683 | s[2] = parts.k.z * mul; |
---|
684 | |
---|
685 | so.set(parts.u.x, parts.u.y, parts.u.z, parts.u.w); |
---|
686 | } |
---|