Go to the source code of this file.
Classes | |
| struct | Quat |
| struct | AffineParts |
Typedefs | |
| typedef Quat | HVect |
| typedef float | HMatrix [4][4] |
Enumerations | |
| enum | QuatPart { X, Y, Z, W } |
Functions | |
| float | polar_decomp (HMatrix M, HMatrix Q, HMatrix S) |
| HVect | spect_decomp (HMatrix S, HMatrix U) |
| Quat | snuggle (Quat q, HVect *k) |
| void | decomp_affine (HMatrix A, AffineParts *parts) |
| void | invert_affine (AffineParts *parts, AffineParts *inverse) |
| typedef float HMatrix[4][4] |
Definition at line 7 of file decompose.h.
Definition at line 6 of file decompose.h.
| void decomp_affine | ( | HMatrix | A, | |
| AffineParts * | parts | |||
| ) |
Definition at line 451 of file decompose.cpp.
00452 { 00453 HMatrix Q, S, U; 00454 Quat p; 00455 float det; 00456 parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0); 00457 det = polar_decomp(A, Q, S); 00458 if (det<0.0) { 00459 mat_copy(Q,=,-Q,3); 00460 parts->f = -1; 00461 } else parts->f = 1; 00462 parts->q = Qt_FromMatrix(Q); 00463 parts->k = spect_decomp(S, U); 00464 parts->u = Qt_FromMatrix(U); 00465 p = snuggle(parts->u, &parts->k); 00466 parts->u = Qt_Mul(parts->u, p); 00467 }
| void invert_affine | ( | AffineParts * | parts, | |
| AffineParts * | inverse | |||
| ) |
Definition at line 473 of file decompose.cpp.
00474 { 00475 Quat t, p; 00476 inverse->f = parts->f; 00477 inverse->q = Qt_Conj(parts->q); 00478 inverse->u = Qt_Mul(parts->q, parts->u); 00479 inverse->k.x = (parts->k.x==0.0f || parts->k.x==-0.0f) ? 0.0f : 1.0f/parts->k.x; 00480 inverse->k.y = (parts->k.y==0.0f || parts->k.y==-0.0f) ? 0.0f : 1.0f/parts->k.y; 00481 inverse->k.z = (parts->k.z==0.0f || parts->k.z==-0.0f) ? 0.0f : 1.0f/parts->k.z; 00482 inverse->k.w = parts->k.w; 00483 t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0); 00484 t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u)); 00485 t = Qt_(inverse->k.x*t.x, inverse->k.y*t.y, inverse->k.z*t.z, 0); 00486 p = Qt_Mul(inverse->q, inverse->u); 00487 t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p))); 00488 inverse->t = (inverse->f>0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0); 00489 }
Definition at line 252 of file decompose.cpp.
00253 { 00254 #define TOL 1.0e-6 00255 HMatrix Mk, MadjTk, Ek; 00256 float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2; 00257 int i, j; 00258 mat_tpose(Mk,=,M,3); 00259 M_one = norm_one(Mk); M_inf = norm_inf(Mk); 00260 do { 00261 adjoint_transpose(Mk, MadjTk); 00262 det = vdot(Mk[0], MadjTk[0]); 00263 if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;} 00264 MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk); 00265 gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det)); 00266 g1 = gamma*0.5f; 00267 g2 = 0.5f/(gamma*det); 00268 mat_copy(Ek,=,Mk,3); 00269 mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3); 00270 mat_copy(Ek,-=,Mk,3); 00271 E_one = norm_one(Ek); 00272 M_one = norm_one(Mk); M_inf = norm_inf(Mk); 00273 } while (E_one>(M_one*TOL)); 00274 mat_tpose(Q,=,Mk,3); mat_pad(Q); 00275 mat_mult(Mk, M, S); mat_pad(S); 00276 for (i=0; i<3; i++) for (j=i; j<3; j++) 00277 S[i][j] = S[j][i] = 0.5f*(S[i][j]+S[j][i]); 00278 return (det); 00279 }
Definition at line 346 of file decompose.cpp.
00347 { 00348 #define SQRTHALF (0.7071067811865475244f) 00349 #define sgn(n,v) ((n)?-(v):(v)) 00350 #define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];} 00351 #define cycle(a,p) if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\ 00352 else {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];} 00353 Quat p; 00354 float ka[4]; 00355 int i, turn = -1; 00356 ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z; 00357 if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;} 00358 else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;} 00359 if (turn>=0) { 00360 Quat qtoz, qp; 00361 unsigned neg[3], win; 00362 float mag[3], t; 00363 static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF}; 00364 static Quat qytoz = {SQRTHALF,0,0,SQRTHALF}; 00365 static Quat qppmm = { 0.5f, 0.5f,-0.5f,-0.5f}; 00366 static Quat qpppp = { 0.5f, 0.5f, 0.5f, 0.5f}; 00367 static Quat qmpmm = {-0.5f, 0.5f,-0.5f,-0.5f}; 00368 static Quat qpppm = { 0.5f, 0.5f, 0.5f,-0.5f}; 00369 static Quat q0001 = { 0.0f, 0.0f, 0.0f, 1.0f}; 00370 static Quat q1000 = { 1.0f, 0.0f, 0.0f, 0.0f}; 00371 switch (turn) { 00372 default: return (Qt_Conj(q)); 00373 case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break; 00374 case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break; 00375 case Z: qtoz = q0001; break; 00376 } 00377 q = Qt_Conj(q); 00378 mag[0] = (float)q.z*q.z+(float)q.w*q.w-0.5f; 00379 mag[1] = (float)q.x*q.z-(float)q.y*q.w; 00380 mag[2] = (float)q.y*q.z+(float)q.x*q.w; 00381 for (i=0; i<3; i++) if (neg[i] = (mag[i]<0.0)) mag[i] = -mag[i]; 00382 if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;} 00383 else {if (mag[1]>mag[2]) win = 1; else win = 2;} 00384 switch (win) { 00385 case 0: if (neg[0]) p = q1000; else p = q0001; break; 00386 case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break; 00387 case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break; 00388 } 00389 qp = Qt_Mul(q, p); 00390 t = sqrt(mag[win]+0.5f); 00391 p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t)); 00392 p = Qt_Mul(qtoz, Qt_Conj(p)); 00393 } else { 00394 float qa[4], pa[4]; 00395 unsigned lo, hi, neg[4], par = 0; 00396 float all, big, two; 00397 qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w; 00398 for (i=0; i<4; i++) { 00399 pa[i] = 0.0; 00400 if (neg[i] = (qa[i]<0.0)) qa[i] = -qa[i]; 00401 par ^= neg[i]; 00402 } 00403 /* Find two largest components, indices in hi and lo */ 00404 if (qa[0]>qa[1]) lo = 0; else lo = 1; 00405 if (qa[2]>qa[3]) hi = 2; else hi = 3; 00406 if (qa[lo]>qa[hi]) { 00407 if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;} 00408 else {hi ^= lo; lo ^= hi; hi ^= lo;} 00409 } else {if (qa[hi^1]>qa[lo]) lo = hi^1;} 00410 all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5f; 00411 two = (qa[hi]+qa[lo])*SQRTHALF; 00412 big = qa[hi]; 00413 if (all>two) { 00414 if (all>big) {/*all*/ 00415 {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5f);} 00416 cycle(ka,par) 00417 } else {/*big*/ pa[hi] = sgn(neg[hi],1.0f);} 00418 } else { 00419 if (two>big) {/*two*/ 00420 pa[hi] = sgn(neg[hi],SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF); 00421 if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;} 00422 if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;} 00423 swap(ka,hi,lo) 00424 } else {/*big*/ pa[hi] = sgn(neg[hi],1.0f);} 00425 } 00426 p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3]; 00427 } 00428 k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z]; 00429 return (p); 00430 }
Definition at line 291 of file decompose.cpp.
00292 { 00293 HVect kv; 00294 float Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */ 00295 float g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b; 00296 static char nxt[] = {Y,Z,X}; 00297 int sweep, i, j; 00298 mat_copy(U,=,mat_id,4); 00299 Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z]; 00300 OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y]; 00301 for (sweep=20; sweep>0; sweep--) { 00302 float sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]); 00303 if (sm==0.0) break; 00304 for (i=Z; i>=X; i--) { 00305 int p = nxt[i]; int q = nxt[p]; 00306 fabsOffDi = fabs(OffD[i]); 00307 g = 100.0f*fabsOffDi; 00308 if (fabsOffDi>0.0) { 00309 h = Diag[q] - Diag[p]; 00310 fabsh = fabs(h); 00311 if (fabsh+g==fabsh) { 00312 t = OffD[i]/h; 00313 } else { 00314 theta = 0.5f*h/OffD[i]; 00315 t = 1.0f/(fabs(theta)+sqrt(theta*theta+1.0f)); 00316 if (theta<0.0) t = -t; 00317 } 00318 c = 1.0f/sqrt(t*t+1.0f); s = t*c; 00319 tau = s/(c+1.0f); 00320 ta = t*OffD[i]; OffD[i] = 0.0; 00321 Diag[p] -= ta; Diag[q] += ta; 00322 OffDq = OffD[q]; 00323 OffD[q] -= s*(OffD[p] + tau*OffD[q]); 00324 OffD[p] += s*(OffDq - tau*OffD[p]); 00325 for (j=Z; j>=X; j--) { 00326 a = U[j][p]; b = U[j][q]; 00327 U[j][p] -= s*(b + tau*a); 00328 U[j][q] += s*(a - tau*b); 00329 } 00330 } 00331 } 00332 } 00333 kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0; 00334 return (kv); 00335 }
1.5.5