/cygdrive/d/src/svn/vrut/trunk/modules/iovrml/decompose.cpp File Reference

#include <math.h>
#include "decompose.h"

Go to the source code of this file.

Defines

#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)

#define mat_copy(C, gets, A, n)

#define mat_tpose(AT, gets, A, n)

#define mat_binop(C, gets, A, op, B, n)

#define caseMacro(i, j, k, I, J, K)

#define TOL   1.0e-6

#define SQRTHALF   (0.7071067811865475244f)

#define sgn(n, v)   ((n)?-(v):(v))

#define swap(a, i, j)   {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}

#define cycle(a, p)

Functions

void mat_mult (HMatrix A, HMatrix B, HMatrix AB)

float vdot (float *va, float *vb)

void vcross (float *va, float *vb, float *v)

void adjoint_transpose (HMatrix M, HMatrix MadjT)

Quat Qt_ (float x, float y, float z, float w)

Quat Qt_Conj (Quat q)

Quat Qt_Mul (Quat qL, Quat qR)

Quat Qt_Scale (Quat q, float w)

Quat Qt_FromMatrix (HMatrix mat)

float mat_norm (HMatrix M, int tpose)

float norm_inf (HMatrix M)

float norm_one (HMatrix M)

int find_max_col (HMatrix M)

void make_reflector (float *v, float *u)

void reflect_cols (HMatrix M, float *u)

void reflect_rows (HMatrix M, float *u)

void do_rank1 (HMatrix M, HMatrix Q)

void do_rank2 (HMatrix M, HMatrix MadjT, HMatrix Q)

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)

Variables

static HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}

Define Documentation

#define caseMacro	(	i,
		j,
		k,
		I,
		J,
		K	)

Value:

case I:\
              s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
              qu.i = s*0.5f;\
              s = 0.5f / s;\
              qu.j = (mat[I][J] + mat[J][I]) * s;\
              qu.k = (mat[K][I] + mat[I][K]) * s;\
              qu.w = (mat[K][J] - mat[J][K]) * s;\
              break

#define cycle	(	a,
		p		)

Value:

if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
                  else   {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}

#define mat_binop	(	C,
		gets,
		A,
		op,
		B,
		n	)

Value:

{int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
    C[i][j] gets (A[i][j]) op (B[i][j]);}

Assign nxn matrix C the element-wise combination of A and B using "op"

Definition at line 21 of file decompose.cpp.

#define mat_copy	(	C,
		gets,
		A,
		n	)

Value:

{int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
    C[i][j] gets (A[i][j]);}

Copy nxn matrix A to C using "gets" for assignment

Definition at line 13 of file decompose.cpp.

#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)

Fill out 3x3 matrix to 4x4

Definition at line 10 of file decompose.cpp.

#define mat_tpose	(	AT,
		gets,
		A,
		n	)

Value:

{int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
    AT[i][j] gets (A[j][i]);}

Copy transpose of nxn matrix A to C using "gets" for assignment

Definition at line 17 of file decompose.cpp.

#define sgn	(	n,
		v		)	((n)?-(v):(v))

#define SQRTHALF (0.7071067811865475244f)

#define swap	(	a,
		i,
		j	)	{a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}

#define TOL 1.0e-6

Function Documentation

void adjoint_transpose	(	HMatrix	M,
		HMatrix	MadjT
	)

Set MadjT to transpose of inverse of M times determinant of M

Definition at line 47 of file decompose.cpp.

00048 {
00049     vcross(M[1], M[2], MadjT[0]);
00050     vcross(M[2], M[0], MadjT[1]);
00051     vcross(M[0], M[1], MadjT[2]);
00052 }

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 do_rank1	(	HMatrix	M,
		HMatrix	Q
	)

Find orthogonal factor Q of rank 1 (or less) M

Definition at line 201 of file decompose.cpp.

00202 {
00203     float v1[3], v2[3], s;
00204     int col;
00205     mat_copy(Q,=,mat_id,4);
00206     /* If rank(M) is 1, we should find a non-zero column in M */
00207     col = find_max_col(M);
00208     if (col<0) return; /* Rank is 0 */
00209     v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
00210     make_reflector(v1, v1); reflect_cols(M, v1);
00211     v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
00212     make_reflector(v2, v2); reflect_rows(M, v2);
00213     s = M[2][2];
00214     if (s<0.0) Q[2][2] = -1.0;
00215     reflect_cols(Q, v1); reflect_rows(Q, v2);
00216 }

void do_rank2	(	HMatrix	M,
		HMatrix	MadjT,
		HMatrix	Q
	)

Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose

Definition at line 219 of file decompose.cpp.

00220 {
00221     float v1[3], v2[3];
00222     float w, x, y, z, c, s, d;
00223     int col;
00224     /* If rank(M) is 2, we should find a non-zero column in MadjT */
00225     col = find_max_col(MadjT);
00226     if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */
00227     v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
00228     make_reflector(v1, v1); reflect_cols(M, v1);
00229     vcross(M[0], M[1], v2);
00230     make_reflector(v2, v2); reflect_rows(M, v2);
00231     w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
00232     if (w*z>x*y) {
00233        c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
00234        Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
00235     } else {
00236        c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d;
00237        Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
00238     }
00239     Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
00240     reflect_cols(Q, v1); reflect_rows(Q, v2);
00241 }

int find_max_col ( HMatrix M )

Return index of column of M containing maximum abs entry, or -1 if M=0

Definition at line 159 of file decompose.cpp.

00160 {
00161     float abs, max;
00162     int i, j, col;
00163     max = 0.0; col = -1;
00164     for (i=0; i<3; i++) for (j=0; j<3; j++) {
00165        abs = M[i][j]; if (abs<0.0) abs = -abs;
00166        if (abs>max) {max = abs; col = j;}
00167     }
00168     return col;
00169 }

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 }

void make_reflector	(	float *	v,
		float *	u
	)

Setup u for Household reflection to zero all v components but first

Definition at line 172 of file decompose.cpp.

00173 {
00174     float s = sqrt(vdot(v, v));
00175     u[0] = v[0]; u[1] = v[1];
00176     u[2] = v[2] + ((v[2]<0.0) ? -s : s);
00177     s = sqrt(2.0f/vdot(u, u));
00178     u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s;
00179 }

void mat_mult	(	HMatrix	A,
		HMatrix	B,
		HMatrix	AB
	)

Multiply the upper left 3x3 parts of A and B to get AB

Definition at line 25 of file decompose.cpp.

00026 {
00027     int i, j;
00028     for (i=0; i<3; i++) for (j=0; j<3; j++)
00029        AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
00030 }

float mat_norm	(	HMatrix	M,
		int	tpose
	)

Compute either the 1 or infinity norm of M, depending on tpose

Definition at line 142 of file decompose.cpp.

00143 {
00144     int i;
00145     float sum, max;
00146     max = 0.0;
00147     for (i=0; i<3; i++) {
00148        if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]);
00149        else      sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]);
00150        if (max<sum) max = sum;
00151     }
00152     return max;
00153 }

float norm_inf ( HMatrix M )

Definition at line 155 of file decompose.cpp.

00155 {return mat_norm(M, 0);}

float norm_one ( HMatrix M )

Definition at line 156 of file decompose.cpp.

00156 {return mat_norm(M, 1);}

float polar_decomp	(	HMatrix	M,
		HMatrix	Q,
		HMatrix	S
	)

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 }

Quat Qt_	(	float	x,
		float	y,
		float	z,
		float	w
	)

Definition at line 57 of file decompose.cpp.

00058 {
00059     Quat qq;
00060     qq.x = x; qq.y = y; qq.z = z; qq.w = w;
00061     return (qq);
00062 }

Quat Qt_Conj ( Quat q )

Definition at line 65 of file decompose.cpp.

00066 {
00067     Quat qq;
00068     qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
00069     return (qq);
00070 }

Quat Qt_FromMatrix ( HMatrix mat )

Definition at line 97 of file decompose.cpp.

00098 {
00099     /* This algorithm avoids near-zero divides by looking for a large component
00100      * - first w, then x, y, or z.  When the trace is greater than zero,
00101      * |w| is greater than 1/2, which is as small as a largest component can be.
00102      * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
00103      * |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
00104        Quat qu = {0,0,0,0};
00105     register float tr, s;
00106 
00107     tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
00108     if (tr >= 0.0) {
00109            s = sqrt(tr + mat[W][W]);
00110            qu.w = s*0.5f;
00111            s = 0.5f / s;
00112            qu.x = (mat[Z][Y] - mat[Y][Z]) * s;
00113            qu.y = (mat[X][Z] - mat[Z][X]) * s;
00114            qu.z = (mat[Y][X] - mat[X][Y]) * s;
00115        } else {
00116            int h = X;
00117            if (mat[Y][Y] > mat[X][X]) h = Y;
00118            if (mat[Z][Z] > mat[h][h]) h = Z;
00119            switch (h) {
00120 #define caseMacro(i,j,k,I,J,K) \
00121            case I:\
00122               s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
00123               qu.i = s*0.5f;\
00124               s = 0.5f / s;\
00125               qu.j = (mat[I][J] + mat[J][I]) * s;\
00126               qu.k = (mat[K][I] + mat[I][K]) * s;\
00127               qu.w = (mat[K][J] - mat[J][K]) * s;\
00128               break
00129            caseMacro(x,y,z,X,Y,Z);
00130            caseMacro(y,z,x,Y,Z,X);
00131            caseMacro(z,x,y,Z,X,Y);
00132            }
00133        }
00134     if (mat[W][W] != 1.0f) qu = Qt_Scale(qu, 1/sqrt(mat[W][W]));
00135     return (qu);
00136 }

Quat Qt_Mul	(	Quat	qL,
		Quat	qR
	)

Definition at line 75 of file decompose.cpp.

00076 {
00077     Quat qq;
00078     qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
00079     qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
00080     qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
00081     qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
00082     return (qq);
00083 }

Quat Qt_Scale	(	Quat	q,
		float	w
	)

Definition at line 86 of file decompose.cpp.

00087 {
00088     Quat qq;
00089     qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
00090     return (qq);
00091 }

void reflect_cols	(	HMatrix	M,
		float *	u
	)

Apply Householder reflection represented by u to column vectors of M

Definition at line 182 of file decompose.cpp.

00183 {
00184     int i, j;
00185     for (i=0; i<3; i++) {
00186        float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i];
00187        for (j=0; j<3; j++) M[j][i] -= u[j]*s;
00188     }
00189 }

void reflect_rows	(	HMatrix	M,
		float *	u
	)

Apply Householder reflection represented by u to row vectors of M

Definition at line 191 of file decompose.cpp.

00192 {
00193     int i, j;
00194     for (i=0; i<3; i++) {
00195        float s = vdot(u, M[i]);
00196        for (j=0; j<3; j++) M[i][j] -= u[j]*s;
00197     }
00198 }

Quat snuggle	(	Quat	q,
		HVect *	k
	)

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 }

HVect spect_decomp	(	HMatrix	S,
		HMatrix	U
	)

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 }

void vcross	(	float *	va,
		float *	vb,
		float *	v
	)

Set v to cross product of length 3 vectors va and vb

Definition at line 39 of file decompose.cpp.

00040 {
00041     v[0] = va[1]*vb[2] - va[2]*vb[1];
00042     v[1] = va[2]*vb[0] - va[0]*vb[2];
00043     v[2] = va[0]*vb[1] - va[1]*vb[0];
00044 }

float vdot	(	float *	va,
		float *	vb
	)

Return dot product of length 3 vectors va and vb

Definition at line 33 of file decompose.cpp.

00034 {
00035     return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
00036 }

Variable Documentation

HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}} [static]

Definition at line 139 of file decompose.cpp.


Defines
#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)
#define	mat_copy(C, gets, A, n)
#define	mat_tpose(AT, gets, A, n)
#define	mat_binop(C, gets, A, op, B, n)
#define	caseMacro(i, j, k, I, J, K)
#define	TOL 1.0e-6
#define	SQRTHALF (0.7071067811865475244f)
#define	sgn(n, v) ((n)?-(v):(v))
#define	swap(a, i, j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
#define	cycle(a, p)
Functions
void	mat_mult (HMatrix A, HMatrix B, HMatrix AB)
float	vdot (float va, float vb)
void	vcross (float va, float vb, float *v)
void	adjoint_transpose (HMatrix M, HMatrix MadjT)
Quat	Qt_ (float x, float y, float z, float w)
Quat	Qt_Conj (Quat q)
Quat	Qt_Mul (Quat qL, Quat qR)
Quat	Qt_Scale (Quat q, float w)
Quat	Qt_FromMatrix (HMatrix mat)
float	mat_norm (HMatrix M, int tpose)
float	norm_inf (HMatrix M)
float	norm_one (HMatrix M)
int	find_max_col (HMatrix M)
void	make_reflector (float v, float u)
void	reflect_cols (HMatrix M, float *u)
void	reflect_rows (HMatrix M, float *u)
void	do_rank1 (HMatrix M, HMatrix Q)
void	do_rank2 (HMatrix M, HMatrix MadjT, HMatrix Q)
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)
Variables
static HMatrix	mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}