diff --git a/Utils/Libs/Maths/Decompose.c b/Utils/Libs/Maths/Decompose.c new file mode 100644 index 000000000..ee362ec10 --- /dev/null +++ b/Utils/Libs/Maths/Decompose.c @@ -0,0 +1,527 @@ +/**** Decompose.c ****/ +/* Ken Shoemake, 1993 */ +#include +#include "Decompose.h" + +/******* Matrix Preliminaries *******/ + +typedef struct {float x, y, z, w;} Quat; /* Quaternion */ +enum QuatPart {X, Y, Z, W}; +typedef Quat HVect; /* Homogeneous 3D vector */ +typedef float HMatrix[4][4]; /* Right-handed, for column vectors */ +typedef struct { + HVect t; /* Translation components */ + Quat q; /* Essential rotation */ + Quat u; /* Stretch rotation */ + HVect k; /* Stretch factors */ + float f; /* Sign of determinant */ +} AffinePartsInternal; +float polar_decomp(HMatrix M, HMatrix Q, HMatrix S); +HVect spect_decomp(HMatrix S, HMatrix U); +Quat snuggle(Quat q, HVect *k); + +/** Fill out 3x3 matrix to 4x4 **/ +#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) + +/** Copy nxn matrix A to C using "gets" for assignment **/ +#define mat_copy(C,gets,A,n) {int i,j; for(i=0;i= 0.0) { + s = sqrt(tr + mat[W][W]); + qu.w = s*0.5; + s = 0.5 / s; + qu.x = (mat[Z][Y] - mat[Y][Z]) * s; + qu.y = (mat[X][Z] - mat[Z][X]) * s; + qu.z = (mat[Y][X] - mat[X][Y]) * s; + } else { + int h = X; + if (mat[Y][Y] > mat[X][X]) h = Y; + if (mat[Z][Z] > mat[h][h]) h = Z; + switch (h) { +#define caseMacro(i,j,k,I,J,K) \ + case I:\ + s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\ + qu.i = s*0.5;\ + s = 0.5 / 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 + caseMacro(x,y,z,X,Y,Z); + caseMacro(y,z,x,Y,Z,X); + caseMacro(z,x,y,Z,X,Y); + } + } + if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1/sqrt(mat[W][W])); + return (qu); +} +/******* Decomp Auxiliaries *******/ + +static HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; + +/** Compute either the 1 or infinity norm of M, depending on tpose **/ +float mat_norm(HMatrix M, int tpose) +{ + int i; + float sum, max; + max = 0.0; + for (i=0; i<3; i++) { + if (tpose) sum = fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i]); + else sum = fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2]); + if (maxmax) {max = abs; col = j;} + } + return col; +} + +/** Setup u for Household reflection to zero all v components but first **/ +void make_reflector(float *v, float *u) +{ + float s = sqrt(vdot(v, v)); + u[0] = v[0]; u[1] = v[1]; + u[2] = v[2] + ((v[2]<0.0) ? -s : s); + s = sqrt(2.0/vdot(u, u)); + u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s; +} + +/** Apply Householder reflection represented by u to column vectors of M **/ +void reflect_cols(HMatrix M, float *u) +{ + int i, j; + for (i=0; i<3; i++) { + float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i]; + for (j=0; j<3; j++) M[j][i] -= u[j]*s; + } +} +/** Apply Householder reflection represented by u to row vectors of M **/ +void reflect_rows(HMatrix M, float *u) +{ + int i, j; + for (i=0; i<3; i++) { + float s = vdot(u, M[i]); + for (j=0; j<3; j++) M[i][j] -= u[j]*s; + } +} + +/** Find orthogonal factor Q of rank 1 (or less) M **/ +void do_rank1(HMatrix M, HMatrix Q) +{ + float v1[3], v2[3], s; + int col; + mat_copy(Q,=,mat_id,4); + /* If rank(M) is 1, we should find a non-zero column in M */ + col = find_max_col(M); + if (col<0) return; /* Rank is 0 */ + v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col]; + make_reflector(v1, v1); reflect_cols(M, v1); + v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2]; + make_reflector(v2, v2); reflect_rows(M, v2); + s = M[2][2]; + if (s<0.0) Q[2][2] = -1.0; + reflect_cols(Q, v1); reflect_rows(Q, v2); +} + +/** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/ +void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q) +{ + float v1[3], v2[3]; + float w, x, y, z, c, s, d; + int col; + /* If rank(M) is 2, we should find a non-zero column in MadjT */ + col = find_max_col(MadjT); + if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */ + v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col]; + make_reflector(v1, v1); reflect_cols(M, v1); + vcross(M[0], M[1], v2); + make_reflector(v2, v2); reflect_rows(M, v2); + w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1]; + if (w*z>x*y) { + c = z+w; s = y-x; d = sqrt(c*c+s*s); c = c/d; s = s/d; + Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s); + } else { + c = z-w; s = y+x; d = sqrt(c*c+s*s); c = c/d; s = s/d; + Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s; + } + Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0; + reflect_cols(Q, v1); reflect_rows(Q, v2); +} + + +/******* Polar Decomposition *******/ + +/* Polar Decomposition of 3x3 matrix in 4x4, + * M = QS. See Nicholas Higham and Robert S. Schreiber, + * Fast Polar Decomposition of An Arbitrary Matrix, + * Technical Report 88-942, October 1988, + * Department of Computer Science, Cornell University. + */ +float polar_decomp(HMatrix M, HMatrix Q, HMatrix S) +{ +#define TOL 1.0e-6 + HMatrix Mk, MadjTk, Ek; + float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2; + int i, j; + mat_tpose(Mk,=,M,3); + M_one = norm_one(Mk); M_inf = norm_inf(Mk); + do { + adjoint_transpose(Mk, MadjTk); + det = vdot(Mk[0], MadjTk[0]); + if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;} + MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk); + gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det)); + g1 = gamma*0.5; + g2 = 0.5/(gamma*det); + mat_copy(Ek,=,Mk,3); + mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3); + mat_copy(Ek,-=,Mk,3); + E_one = norm_one(Ek); + M_one = norm_one(Mk); M_inf = norm_inf(Mk); + } while (E_one>(M_one*TOL)); + mat_tpose(Q,=,Mk,3); mat_pad(Q); + mat_mult(Mk, M, S); mat_pad(S); + for (i=0; i<3; i++) for (j=i; j<3; j++) + S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]); + return (det); +} + + + + + + + + + + + + + + + + + +/******* Spectral Decomposition *******/ + +/* Compute the spectral decomposition of symmetric positive semi-definite S. + * Returns rotation in U and scale factors in result, so that if K is a diagonal + * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method. + * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983. + */ +HVect spect_decomp(HMatrix S, HMatrix U) +{ + HVect kv; + double Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */ + double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b; + static char nxt[] = {Y,Z,X}; + int sweep, i, j; + mat_copy(U,=,mat_id,4); + Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z]; + OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y]; + for (sweep=20; sweep>0; sweep--) { + float sm = fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z]); + if (sm==0.0) break; + for (i=Z; i>=X; i--) { + int p = nxt[i]; int q = nxt[p]; + fabsOffDi = fabs(OffD[i]); + g = 100.0*fabsOffDi; + if (fabsOffDi>0.0) { + h = Diag[q] - Diag[p]; + fabsh = fabs(h); + if (fabsh+g==fabsh) { + t = OffD[i]/h; + } else { + theta = 0.5*h/OffD[i]; + t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0)); + if (theta<0.0) t = -t; + } + c = 1.0/sqrt(t*t+1.0); s = t*c; + tau = s/(c+1.0); + ta = t*OffD[i]; OffD[i] = 0.0; + Diag[p] -= ta; Diag[q] += ta; + OffDq = OffD[q]; + OffD[q] -= s*(OffD[p] + tau*OffD[q]); + OffD[p] += s*(OffDq - tau*OffD[p]); + for (j=Z; j>=X; j--) { + a = U[j][p]; b = U[j][q]; + U[j][p] -= s*(b + tau*a); + U[j][q] += s*(a - tau*b); + } + } + } + } + kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0; + return (kv); +} + +/******* Spectral Axis Adjustment *******/ + +/* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p, + * which permutes the axes and turns freely in the plane of duplicate scale + * factors, such that q p has the largest possible w component, i.e. the + * smallest possible angle. Permutes k's components to go with q p instead of q. + * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition. + * Proceedings of Graphics Interface 1992. Details on p. 262-263. + */ +Quat snuggle(Quat q, HVect *k) +{ +#define SQRTHALF (0.7071067811865475244) +#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) 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];} + Quat p; + float ka[4]; + int i, turn = -1; + ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z; + if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;} + else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;} + if (turn>=0) { + Quat qtoz, qp; + unsigned neg[3], win; + double mag[3], t; + static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF}; + static Quat qytoz = {SQRTHALF,0,0,SQRTHALF}; + static Quat qppmm = { 0.5, 0.5,-0.5,-0.5}; + static Quat qpppp = { 0.5, 0.5, 0.5, 0.5}; + static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5}; + static Quat qpppm = { 0.5, 0.5, 0.5,-0.5}; + static Quat q0001 = { 0.0, 0.0, 0.0, 1.0}; + static Quat q1000 = { 1.0, 0.0, 0.0, 0.0}; + switch (turn) { + default: return (Qt_Conj(q)); + case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break; + case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break; + case Z: qtoz = q0001; break; + } + q = Qt_Conj(q); + mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5; + mag[1] = (double)q.x*q.z-(double)q.y*q.w; + mag[2] = (double)q.y*q.z+(double)q.x*q.w; + for (i=0; i<3; i++) if (neg[i] = (mag[i]<0.0)) mag[i] = -mag[i]; + if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;} + else {if (mag[1]>mag[2]) win = 1; else win = 2;} + switch (win) { + case 0: if (neg[0]) p = q1000; else p = q0001; break; + case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break; + case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break; + } + qp = Qt_Mul(q, p); + t = sqrt(mag[win]+0.5); + p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t)); + p = Qt_Mul(qtoz, Qt_Conj(p)); + } else { + float qa[4], pa[4]; + unsigned lo, hi, neg[4], par = 0; + double all, big, two; + qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w; + for (i=0; i<4; i++) { + pa[i] = 0.0; + if (neg[i] = (qa[i]<0.0)) qa[i] = -qa[i]; + par ^= neg[i]; + } + /* Find two largest components, indices in hi and lo */ + if (qa[0]>qa[1]) lo = 0; else lo = 1; + if (qa[2]>qa[3]) hi = 2; else hi = 3; + if (qa[lo]>qa[hi]) { + if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;} + else {hi ^= lo; lo ^= hi; hi ^= lo;} + } else {if (qa[hi^1]>qa[lo]) lo = hi^1;} + all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5; + two = (qa[hi]+qa[lo])*SQRTHALF; + big = qa[hi]; + if (all>two) { + if (all>big) {/*all*/ + {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5);} + cycle(ka,par) + } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);} + } else { + if (two>big) {/*two*/ + pa[hi] = sgn(neg[hi],SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF); + if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;} + if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;} + swap(ka,hi,lo) + } else {/*big*/ pa[hi] = sgn(neg[hi],1.0);} + } + p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3]; + } + k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z]; + return (p); +} + + + + + + + + + + + +/******* Decompose Affine Matrix *******/ + +/* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the + * translation components, q contains the rotation R, u contains U, k contains + * scale factors, and f contains the sign of the determinant. + * Assumes A transforms column vectors in right-handed coordinates. + * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition. + * Proceedings of Graphics Interface 1992. + */ +void decomp_affine(HMatrix A, AffinePartsInternal *parts) +{ + HMatrix Q, S, U; + Quat p; + float det; + parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0); + det = polar_decomp(A, Q, S); + if (det<0.0) { + mat_copy(Q,=,-Q,3); + parts->f = -1; + } else parts->f = 1; + parts->q = Qt_FromMatrix(Q); + parts->k = spect_decomp(S, U); + parts->u = Qt_FromMatrix(U); + p = snuggle(parts->u, &parts->k); + parts->u = Qt_Mul(parts->u, p); +} + +/******* Invert Affine Decomposition *******/ + +/* Compute inverse of affine decomposition. + */ +void invert_affine(AffinePartsInternal *parts, AffinePartsInternal *inverse) +{ + Quat t, p; + inverse->f = parts->f; + inverse->q = Qt_Conj(parts->q); + inverse->u = Qt_Mul(parts->q, parts->u); + inverse->k.x = (parts->k.x==0.0) ? 0.0 : 1.0/parts->k.x; + inverse->k.y = (parts->k.y==0.0) ? 0.0 : 1.0/parts->k.y; + inverse->k.z = (parts->k.z==0.0) ? 0.0 : 1.0/parts->k.z; + inverse->k.w = parts->k.w; + t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0); + t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u)); + t = Qt_(inverse->k.x*t.x, inverse->k.y*t.y, inverse->k.z*t.z, 0); + p = Qt_Mul(inverse->q, inverse->u); + t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p))); + inverse->t = (inverse->f>0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0); +} + +// these are just wrappers to the original graphic gems code + +void decomp_affine(Matrix4x4 const &A, AffineParts *parts) +{ +} + +void invert_affine(AffineParts *parts, AffineParts *inverse) +{ +} + diff --git a/Utils/Libs/Maths/Decompose.cpp b/Utils/Libs/Maths/Decompose.cpp new file mode 100644 index 000000000..f4000078e --- /dev/null +++ b/Utils/Libs/Maths/Decompose.cpp @@ -0,0 +1,604 @@ +/**** Decompose.c ****/ +/* Ken Shoemake, 1993 */ +#include +#include "Decompose.h" + + +typedef struct {float x, y, z, w;} Quat; /* Quaternion */ +enum QuatPart {X, Y, Z, W}; +typedef Quat HVect; /* Homogeneous 3D vector */ +typedef float HMatrix[4][4]; /* Right-handed, for column vectors */ +typedef struct { + HVect t; /* Translation components */ + Quat q; /* Essential rotation */ + Quat u; /* Stretch rotation */ + HVect k; /* Stretch factors */ + float f; /* Sign of determinant */ +} AffinePartsInternal; +float polar_decomp(HMatrix M, HMatrix Q, HMatrix S); +HVect spect_decomp(HMatrix S, HMatrix U); +Quat snuggle(Quat q, HVect *k); + +/******* Matrix Preliminaries *******/ + +/** Fill out 3x3 matrix to 4x4 **/ +#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) + +/** Copy nxn matrix A to C using "gets" for assignment **/ +#define mat_copy(C,gets,A,n) {int i,j; for(i=0;i= 0.0) { + s = (float)sqrt(tr + mat[W][W]); + qu.w = (float)(s*0.5f); + s = 0.5 / s; + qu.x = (float)((mat[Z][Y] - mat[Y][Z]) * s); + qu.y = (float)((mat[X][Z] - mat[Z][X]) * s); + qu.z = (float)((mat[Y][X] - mat[X][Y]) * s); + } else { + int h = X; + if (mat[Y][Y] > mat[X][X]) h = Y; + if (mat[Z][Z] > mat[h][h]) h = Z; + switch (h) { +#define caseMacro(i,j,k,I,J,K) \ + case I:\ + s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\ + qu.i = (float)(s*0.5);\ + s = 0.5 / s;\ + qu.j = (float)((mat[I][J] + mat[J][I]) * s);\ + qu.k = (float)((mat[K][I] + mat[I][K]) * s);\ + qu.w = (float)((mat[K][J] - mat[J][K]) * s);\ + break + caseMacro(x,y,z,X,Y,Z); + caseMacro(y,z,x,Y,Z,X); + caseMacro(z,x,y,Z,X,Y); + } + } + if (mat[W][W] != 1.0f) qu = Qt_Scale(qu, 1.f/(float)sqrt(mat[W][W])); + return (qu); +} +/******* Decomp Auxiliaries *******/ + +static HMatrix mat_id = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; + +/** Compute either the 1 or infinity norm of M, depending on tpose **/ +float mat_norm(HMatrix M, int tpose) +{ + int i; + float sum, max; + max = 0.0; + for (i=0; i<3; i++) { + if (tpose) sum = (float)(fabs(M[0][i])+fabs(M[1][i])+fabs(M[2][i])); + else sum = (float)(fabs(M[i][0])+fabs(M[i][1])+fabs(M[i][2])); + if (maxmax) {max = abs; col = j;} + } + return col; +} + +/** Setup u for Household reflection to zero all v components but first **/ +void make_reflector(float *v, float *u) +{ + float s = (float)sqrt(vdot(v, v)); + u[0] = v[0]; u[1] = v[1]; + u[2] = v[2] + ((v[2]<0.0) ? -s : s); + s = (float)sqrt(2.0/vdot(u, u)); + u[0] = u[0]*s; u[1] = u[1]*s; u[2] = u[2]*s; +} + +/** Apply Householder reflection represented by u to column vectors of M **/ +void reflect_cols(HMatrix M, float *u) +{ + int i, j; + for (i=0; i<3; i++) { + float s = u[0]*M[0][i] + u[1]*M[1][i] + u[2]*M[2][i]; + for (j=0; j<3; j++) M[j][i] -= u[j]*s; + } +} +/** Apply Householder reflection represented by u to row vectors of M **/ +void reflect_rows(HMatrix M, float *u) +{ + int i, j; + for (i=0; i<3; i++) { + float s = vdot(u, M[i]); + for (j=0; j<3; j++) M[i][j] -= u[j]*s; + } +} + +/** Find orthogonal factor Q of rank 1 (or less) M **/ +void do_rank1(HMatrix M, HMatrix Q) +{ + float v1[3], v2[3], s; + int col; + mat_copy(Q,=,mat_id,4); + /* If rank(M) is 1, we should find a non-zero column in M */ + col = find_max_col(M); + if (col<0) return; /* Rank is 0 */ + v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col]; + make_reflector(v1, v1); reflect_cols(M, v1); + v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2]; + make_reflector(v2, v2); reflect_rows(M, v2); + s = M[2][2]; + if (s<0.0) Q[2][2] = -1.0; + reflect_cols(Q, v1); reflect_rows(Q, v2); +} + +/** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/ +void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q) +{ + float v1[3], v2[3]; + float w, x, y, z, c, s, d; + int col; + /* If rank(M) is 2, we should find a non-zero column in MadjT */ + col = find_max_col(MadjT); + if (col<0) {do_rank1(M, Q); return;} /* Rank<2 */ + v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col]; + make_reflector(v1, v1); reflect_cols(M, v1); + vcross(M[0], M[1], v2); + make_reflector(v2, v2); reflect_rows(M, v2); + w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1]; + if (w*z>x*y) { + c = z+w; s = y-x; d = (float)sqrt(c*c+s*s); c = c/d; s = s/d; + Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s); + } else { + c = z-w; s = y+x; d = (float)sqrt(c*c+s*s); c = c/d; s = s/d; + Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s; + } + Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0; + reflect_cols(Q, v1); reflect_rows(Q, v2); +} + + +/******* Polar Decomposition *******/ + +/* Polar Decomposition of 3x3 matrix in 4x4, + * M = QS. See Nicholas Higham and Robert S. Schreiber, + * Fast Polar Decomposition of An Arbitrary Matrix, + * Technical Report 88-942, October 1988, + * Department of Computer Science, Cornell University. + */ +float polar_decomp(HMatrix M, HMatrix Q, HMatrix S) +{ +#define TOL 1.0e-6 + HMatrix Mk, MadjTk, Ek; + float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2; + int i, j; + mat_tpose(Mk,=,M,3); + M_one = norm_one(Mk); M_inf = norm_inf(Mk); + do { + adjoint_transpose(Mk, MadjTk); + det = vdot(Mk[0], MadjTk[0]); + if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;} + MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk); + gamma = (float)sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det)); + g1 = gamma*0.5f; + g2 = 0.5f/(gamma*det); + mat_copy(Ek,=,Mk,3); + mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3); + mat_copy(Ek,-=,Mk,3); + E_one = norm_one(Ek); + M_one = norm_one(Mk); M_inf = norm_inf(Mk); + } while (E_one>(M_one*TOL)); + mat_tpose(Q,=,Mk,3); mat_pad(Q); + mat_mult(Mk, M, S); mat_pad(S); + for (i=0; i<3; i++) for (j=i; j<3; j++) + S[i][j] = S[j][i] = 0.5f*(S[i][j]+S[j][i]); + return (det); +} + + + + + + + + + + + + + + + + + +/******* Spectral Decomposition *******/ + +/* Compute the spectral decomposition of symmetric positive semi-definite S. + * Returns rotation in U and scale factors in result, so that if K is a diagonal + * matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method. + * See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983. + */ +HVect spect_decomp(HMatrix S, HMatrix U) +{ + HVect kv; + double Diag[3],OffD[3]; /* OffD is off-diag (by omitted index) */ + double g,h,fabsh,fabsOffDi,t,theta,c,s,tau,ta,OffDq,a,b; + static char nxt[] = {Y,Z,X}; + int sweep, i, j; + mat_copy(U,=,mat_id,4); + Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z]; + OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y]; + for (sweep=20; sweep>0; sweep--) { + float sm = (float)(fabs(OffD[X])+fabs(OffD[Y])+fabs(OffD[Z])); + if (sm==0.0) break; + for (i=Z; i>=X; i--) { + int p = nxt[i]; int q = nxt[p]; + fabsOffDi = fabs(OffD[i]); + g = 100.0*fabsOffDi; + if (fabsOffDi>0.0) { + h = Diag[q] - Diag[p]; + fabsh = fabs(h); + if (fabsh+g==fabsh) { + t = OffD[i]/h; + } else { + theta = 0.5*h/OffD[i]; + t = 1.0/(fabs(theta)+sqrt(theta*theta+1.0)); + if (theta<0.0) t = -t; + } + c = 1.0/sqrt(t*t+1.0); s = t*c; + tau = s/(c+1.0); + ta = t*OffD[i]; OffD[i] = 0.0; + Diag[p] -= ta; Diag[q] += ta; + OffDq = OffD[q]; + OffD[q] -= s*(OffD[p] + tau*OffD[q]); + OffD[p] += s*(OffDq - tau*OffD[p]); + for (j=Z; j>=X; j--) { + a = U[j][p]; b = U[j][q]; + U[j][p] -= (float)(s*(b + tau*a)); + U[j][q] += (float)(s*(a - tau*b)); + } + } + } + } + kv.x = (float)Diag[X]; kv.y = (float)Diag[Y]; kv.z = (float)Diag[Z]; kv.w = 1.0f; + return (kv); +} + +/******* Spectral Axis Adjustment *******/ + +/* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p, + * which permutes the axes and turns freely in the plane of duplicate scale + * factors, such that q p has the largest possible w component, i.e. the + * smallest possible angle. Permutes k's components to go with q p instead of q. + * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition. + * Proceedings of Graphics Interface 1992. Details on p. 262-263. + */ +Quat snuggle(Quat q, HVect *k) +{ +#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) 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];} + Quat p; + float ka[4]; + int i, turn = -1; + ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z; + if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;} + else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;} + if (turn>=0) { + Quat qtoz, qp; + unsigned neg[3], win; + double mag[3], t; + static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF}; + static Quat qytoz = {SQRTHALF,0,0,SQRTHALF}; + static Quat qppmm = { 0.5, 0.5,-0.5,-0.5}; + static Quat qpppp = { 0.5, 0.5, 0.5, 0.5}; + static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5}; + static Quat qpppm = { 0.5, 0.5, 0.5,-0.5}; + static Quat q0001 = { 0.0, 0.0, 0.0, 1.0}; + static Quat q1000 = { 1.0, 0.0, 0.0, 0.0}; + switch (turn) { + default: return (Qt_Conj(q)); + case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break; + case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break; + case Z: qtoz = q0001; break; + } + q = Qt_Conj(q); + mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5; + mag[1] = (double)q.x*q.z-(double)q.y*q.w; + mag[2] = (double)q.y*q.z+(double)q.x*q.w; + for (i=0; i<3; i++) if (neg[i] = (mag[i]<0.0)) mag[i] = -mag[i]; + if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;} + else {if (mag[1]>mag[2]) win = 1; else win = 2;} + switch (win) { + case 0: if (neg[0]) p = q1000; else p = q0001; break; + case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break; + case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break; + } + qp = Qt_Mul(q, p); + t = sqrt(mag[win]+0.5); + p = Qt_Mul(p, Qt_(0.0f,0.0f,(float)(-qp.z/t),(float)(qp.w/t))); + p = Qt_Mul(qtoz, Qt_Conj(p)); + } else { + float qa[4], pa[4]; + unsigned lo, hi, neg[4], par = 0; + double all, big, two; + qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w; + for (i=0; i<4; i++) { + pa[i] = 0.0; + if (neg[i] = (qa[i]<0.0)) qa[i] = -qa[i]; + par ^= neg[i]; + } + /* Find two largest components, indices in hi and lo */ + if (qa[0]>qa[1]) lo = 0; else lo = 1; + if (qa[2]>qa[3]) hi = 2; else hi = 3; + if (qa[lo]>qa[hi]) { + if (qa[lo^1]>qa[hi]) {hi = lo; lo ^= 1;} + else {hi ^= lo; lo ^= hi; hi ^= lo;} + } else {if (qa[hi^1]>qa[lo]) lo = hi^1;} + all = (qa[0]+qa[1]+qa[2]+qa[3])*0.5; + two = (qa[hi]+qa[lo])*SQRTHALF; + big = qa[hi]; + if (all>two) { + if (all>big) {/*all*/ + {int i; for (i=0; i<4; i++) pa[i] = sgn(neg[i], 0.5f);} + cycle(ka,par) + } else {/*big*/ pa[hi] = sgn(neg[hi],1.0f);} + } else { + if (two>big) {/*two*/ + pa[hi] = sgn(neg[hi],SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF); + if (lo>hi) {hi ^= lo; lo ^= hi; hi ^= lo;} + if (hi==W) {hi = "\001\002\000"[lo]; lo = 3-hi-lo;} + swap(ka,hi,lo) + } else {/*big*/ pa[hi] = sgn(neg[hi],1.0f);} + } + p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3]; + } + k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z]; + return (p); +} + + + + + + + + + + + +/******* Decompose Affine Matrix *******/ + +/* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the + * translation components, q contains the rotation R, u contains U, k contains + * scale factors, and f contains the sign of the determinant. + * Assumes A transforms column vectors in right-handed coordinates. + * See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition. + * Proceedings of Graphics Interface 1992. + */ +void decomp_affine(HMatrix A, AffinePartsInternal *parts) +{ + HMatrix Q, S, U; + Quat p; + float det; + parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0); + det = polar_decomp(A, Q, S); + if (det<0.0) { + mat_copy(Q,=,-Q,3); + parts->f = -1; + } else parts->f = 1; + parts->q = Qt_FromMatrix(Q); + parts->k = spect_decomp(S, U); + parts->u = Qt_FromMatrix(U); + p = snuggle(parts->u, &parts->k); + parts->u = Qt_Mul(parts->u, p); +} + +/******* Invert Affine Decomposition *******/ + +/* Compute inverse of affine decomposition. + */ +void invert_affine(AffinePartsInternal *parts, AffinePartsInternal *inverse) +{ + Quat t, p; + inverse->f = parts->f; + inverse->q = Qt_Conj(parts->q); + inverse->u = Qt_Mul(parts->q, parts->u); + inverse->k.x = (parts->k.x==0.0) ? 0.0f : 1.0f/parts->k.x; + inverse->k.y = (parts->k.y==0.0) ? 0.0f : 1.0f/parts->k.y; + inverse->k.z = (parts->k.z==0.0) ? 0.0f : 1.0f/parts->k.z; + inverse->k.w = parts->k.w; + t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0); + t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u)); + t = Qt_(inverse->k.x*t.x, inverse->k.y*t.y, inverse->k.z*t.z, 0); + p = Qt_Mul(inverse->q, inverse->u); + t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p))); + inverse->t = (inverse->f>0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0); +} + +// these are just wrappers to the original graphic gems code + +void decomp_affine(Matrix4x4 const &A, AffineParts *parts) +{ + HMatrix a; + for (int i=0; i<4;i++) + { + for (int j=0; j<4; j++) + { + a[j][i] = A(i,j); + } + } + + AffinePartsInternal api; + + decomp_affine( a, &api ); + parts->f = api.f; + + parts->k.x = api.k.x; + parts->k.y = api.k.y; + parts->k.z = api.k.z; + + parts->q.x = api.q.x; + parts->q.y = api.q.y; + parts->q.z = api.q.z; + parts->q.w = api.q.w; + + parts->t.x = api.t.x; + parts->t.y = api.t.y; + parts->t.z = api.t.z; + + parts->u.x = api.u.x; + parts->u.y = api.u.y; + parts->u.z = api.u.z; + parts->u.w = api.u.w; +} + +void invert_affine(AffineParts *parts, AffineParts *inverse) +{ + AffinePartsInternal apii, apio; + apii.f = parts->f; + + apii.k.x = parts->k.x; + apii.k.y = parts->k.y; + apii.k.z = parts->k.z; + + apii.q.x = parts->q.x; + apii.q.y = parts->q.y; + apii.q.z = parts->q.z; + apii.q.w = parts->q.w; + + apii.t.x = parts->t.x; + apii.t.y = parts->t.y; + apii.t.z = parts->t.z; + + apii.u.x = parts->u.x; + apii.u.y = parts->u.y; + apii.u.z = parts->u.z; + apii.u.w = parts->u.w; + + + invert_affine( &apii, &apio ); + + + inverse->f = apio.f; + + inverse->k.x = apio.k.x; + inverse->k.y = apio.k.y; + inverse->k.z = apio.k.z; + + inverse->q.x = apio.q.x; + inverse->q.y = apio.q.y; + inverse->q.z = apio.q.z; + inverse->q.w = apio.q.w; + + inverse->t.x = apio.t.x; + inverse->t.y = apio.t.y; + inverse->t.z = apio.t.z; + + inverse->u.x = apio.u.x; + inverse->u.y = apio.u.y; + inverse->u.z = apio.u.z; + inverse->u.w = apio.u.w; +} + + diff --git a/Utils/Libs/Maths/Decompose.h b/Utils/Libs/Maths/Decompose.h new file mode 100644 index 000000000..d5d9bc544 --- /dev/null +++ b/Utils/Libs/Maths/Decompose.h @@ -0,0 +1,21 @@ +/**** Decompose.h - Basic declarations ****/ +#ifndef _H_Decompose +#define _H_Decompose + +#include "matrix4x4.h" +#include "vector3.h" +#include "quat.h" + +struct AffineParts +{ + Vector3 t; + Quaternion q; + Quaternion u; + Vector3 k; + float f; +}; + +void decomp_affine(Matrix4x4 const &A, AffineParts *parts); +void invert_affine(AffineParts *parts, AffineParts *inverse); + +#endif diff --git a/Utils/Libs/Maths/MgcAppr3DLineFit.cpp b/Utils/Libs/Maths/MgcAppr3DLineFit.cpp new file mode 100644 index 000000000..2855af68f --- /dev/null +++ b/Utils/Libs/Maths/MgcAppr3DLineFit.cpp @@ -0,0 +1,124 @@ +// Magic Software, Inc. +// http://www.magic-software.com +// Copyright (c) 2000, All Rights Reserved +// +// Source code from Magic Software is supplied under the terms of a license +// agreement and may not be copied or disclosed except in accordance with the +// terms of that agreement. The various license agreements may be found at +// the Magic Software web site. This file is subject to the license +// +// FREE SOURCE CODE +// http://www.magic-software.com/License/free.pdf + +#include "MgcEigen.h" +#include "vector3.h" +//#include "MgcLinearSystem.h" +#include "MgcAppr3DLineFit.h" + +//---------------------------------------------------------------------------- +void MgcOrthogonalLineFit (int iQuantity, const Vector3* akPoint, + Vector3& rkOffset, Vector3& rkDirection) +{ + // compute average of points + rkOffset = akPoint[0]; + int i; + for (i = 1; i < iQuantity; i++) + rkOffset += akPoint[i]; + real fInvQuantity = 1.0f/iQuantity; + rkOffset *= fInvQuantity; + + // compute sums of products + real fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0; + real fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0; + for (i = 0; i < iQuantity; i++) + { + Vector3 kDiff = akPoint[i] - rkOffset; + fSumXX += kDiff.x*kDiff.x; + fSumXY += kDiff.x*kDiff.y; + fSumXZ += kDiff.x*kDiff.z; + fSumYY += kDiff.y*kDiff.y; + fSumYZ += kDiff.y*kDiff.z; + fSumZZ += kDiff.z*kDiff.z; + } + + // setup the eigensolver + MgcEigen kES(3); + kES.Matrix(0,0) = fSumYY+fSumZZ; + kES.Matrix(0,1) = -fSumXY; + kES.Matrix(0,2) = -fSumXZ; + kES.Matrix(1,0) = kES.Matrix(0,1); + kES.Matrix(1,1) = fSumXX+fSumZZ; + kES.Matrix(1,2) = -fSumYZ; + kES.Matrix(2,0) = kES.Matrix(0,2); + kES.Matrix(2,1) = kES.Matrix(1,2); + kES.Matrix(2,2) = fSumXX+fSumYY; + + // compute eigenstuff, smallest eigenvalue is in last position + kES.DecrSortEigenStuff3(); + + // unit-length direction for best-fit line + rkDirection.x = kES.GetEigenvector(0,2); + rkDirection.y = kES.GetEigenvector(1,2); + rkDirection.z = kES.GetEigenvector(2,2); +} +//---------------------------------------------------------------------------- +bool MgcOrthogonalLineFit (int iQuantity, const Vector3* akPoint, + const bool* abValid, Vector3& rkOffset, Vector3& rkDirection) +{ + // compute average of points + rkOffset.Zero(); + int i, iValidQuantity = 0; + for (i = 0; i < iQuantity; i++) + { + if ( abValid[i] ) + { + rkOffset += akPoint[i]; + iValidQuantity++; + } + } + if ( iValidQuantity == 0 ) + return false; + + real fInvQuantity = 1.0f/iQuantity; + rkOffset *= fInvQuantity; + + // compute sums of products + real fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0; + real fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0; + for (i = 0; i < iQuantity; i++) + { + if ( abValid[i] ) + { + Vector3 kDiff = akPoint[i] - rkOffset; + fSumXX += kDiff.x*kDiff.x; + fSumXY += kDiff.x*kDiff.y; + fSumXZ += kDiff.x*kDiff.z; + fSumYY += kDiff.y*kDiff.y; + fSumYZ += kDiff.y*kDiff.z; + fSumZZ += kDiff.z*kDiff.z; + } + } + + // setup the eigensolver + MgcEigen kES(3); + kES.Matrix(0,0) = fSumYY+fSumZZ; + kES.Matrix(0,1) = -fSumXY; + kES.Matrix(0,2) = -fSumXZ; + kES.Matrix(1,0) = kES.Matrix(0,1); + kES.Matrix(1,1) = fSumXX+fSumZZ; + kES.Matrix(1,2) = -fSumYZ; + kES.Matrix(2,0) = kES.Matrix(0,2); + kES.Matrix(2,1) = kES.Matrix(1,2); + kES.Matrix(2,2) = fSumXX+fSumYY; + + // compute eigenstuff, smallest eigenvalue is in last position + kES.DecrSortEigenStuff3(); + + // unit-length direction for best-fit line + rkDirection.x = kES.GetEigenvector(0,2); + rkDirection.y = kES.GetEigenvector(1,2); + rkDirection.z = kES.GetEigenvector(2,2); + + return true; +} +//---------------------------------------------------------------------------- diff --git a/Utils/Libs/Maths/MgcAppr3DLineFit.h b/Utils/Libs/Maths/MgcAppr3DLineFit.h new file mode 100644 index 000000000..80d543f8f --- /dev/null +++ b/Utils/Libs/Maths/MgcAppr3DLineFit.h @@ -0,0 +1,33 @@ +// Magic Software, Inc. +// http://www.magic-software.com +// Copyright (c) 2000, All Rights Reserved +// +// Source code from Magic Software is supplied under the terms of a license +// agreement and may not be copied or disclosed except in accordance with the +// terms of that agreement. The various license agreements may be found at +// the Magic Software web site. This file is subject to the license +// +// FREE SOURCE CODE +// http://www.magic-software.com/License/free.pdf + +#ifndef MGCAPPR3DLINEFIT_H +#define MGCAPPR3DLINEFIT_H + +#include "Vector3.h" + + +// Least-squares fit of a line to (x,y,z) data by using distance measurements +// orthogonal to the proposed line. The resulting line is represented by +// Offset + t*Direction where the returned direction is a unit-length vector. + +void MgcOrthogonalLineFit (int iQuantity, const Vector3* akPoint, + Vector3& rkOffset, Vector3& rkDirection); + + +// This function allows for selection of vertices from a pool. The return +// value is 'true' if and only if at least one vertex is valid. + +bool MgcOrthogonalLineFit (int iQuantity, const Vector3* akPoint, + const bool* abValid, Vector3& rkOffset, Vector3& rkDirection); + +#endif diff --git a/Utils/Libs/Maths/MgcEigen.cpp b/Utils/Libs/Maths/MgcEigen.cpp new file mode 100644 index 000000000..3bc685810 --- /dev/null +++ b/Utils/Libs/Maths/MgcEigen.cpp @@ -0,0 +1,689 @@ +// Magic Software, Inc. +// http://www.magic-software.com +// Copyright (c) 2000, All Rights Reserved +// +// Source code from Magic Software is supplied under the terms of a license +// agreement and may not be copied or disclosed except in accordance with the +// terms of that agreement. The various license agreements may be found at +// the Magic Software web site. This file is subject to the license +// +// FREE SOURCE CODE +// http://www.magic-software.com/License/free.pdf + +#include "MgcEigen.h" +#include "mathtypes.h" +//#include "debug.h" +#include "math.h" +//#include "MgcRTLib.h" + +//--------------------------------------------------------------------------- +MgcEigen::MgcEigen (int iSize) +{ +// ASSERT( iSize >= 2 ); + m_iSize = iSize; + + m_aafMat = new real*[m_iSize]; + for (int i = 0; i < m_iSize; i++) + m_aafMat[i] = new real[m_iSize]; + + m_afDiag = new real[m_iSize]; + m_afSubd = new real[m_iSize]; +} +//--------------------------------------------------------------------------- +MgcEigen::~MgcEigen () +{ + delete[] m_afSubd; + delete[] m_afDiag; + for (int i = 0; i < m_iSize; i++) + delete[] m_aafMat[i]; + delete[] m_aafMat; +} +//--------------------------------------------------------------------------- +void MgcEigen::Tridiagonal2 (real** m_aafMat, real* m_afDiag, + real* m_afSubd) +{ + // matrix is already tridiagonal + m_afDiag[0] = m_aafMat[0][0]; + m_afDiag[1] = m_aafMat[1][1]; + m_afSubd[0] = m_aafMat[0][1]; + m_afSubd[1] = 0.0; + m_aafMat[0][0] = 1.0; + m_aafMat[0][1] = 0.0; + m_aafMat[1][0] = 0.0; + m_aafMat[1][1] = 1.0; +} +//--------------------------------------------------------------------------- +void MgcEigen::Tridiagonal3 (real** m_aafMat, real* m_afDiag, + real* m_afSubd) +{ + real fM00 = m_aafMat[0][0]; + real fM01 = m_aafMat[0][1]; + real fM02 = m_aafMat[0][2]; + real fM11 = m_aafMat[1][1]; + real fM12 = m_aafMat[1][2]; + real fM22 = m_aafMat[2][2]; + + m_afDiag[0] = fM00; + m_afSubd[2] = 0.0; + if ( fM02 != 0.0 ) + { + real fLength = (real)sqrt(fM01*fM01+fM02*fM02); + real fInvLength = 1.0/fLength; + fM01 *= fInvLength; + fM02 *= fInvLength; + real fQ = 2.0*fM01*fM12+fM02*(fM22-fM11); + m_afDiag[1] = fM11+fM02*fQ; + m_afDiag[2] = fM22-fM02*fQ; + m_afSubd[0] = fLength; + m_afSubd[1] = fM12-fM01*fQ; + m_aafMat[0][0] = 1.0; m_aafMat[0][1] = 0.0; m_aafMat[0][2] = 0.0; + m_aafMat[1][0] = 0.0; m_aafMat[1][1] = fM01; m_aafMat[1][2] = fM02; + m_aafMat[2][0] = 0.0; m_aafMat[2][1] = fM02; m_aafMat[2][2] = -fM01; + } + else + { + m_afDiag[1] = fM11; + m_afDiag[2] = fM22; + m_afSubd[0] = fM01; + m_afSubd[1] = fM12; + m_aafMat[0][0] = 1.0; m_aafMat[0][1] = 0.0; m_aafMat[0][2] = 0.0; + m_aafMat[1][0] = 0.0; m_aafMat[1][1] = 1.0; m_aafMat[1][2] = 0.0; + m_aafMat[2][0] = 0.0; m_aafMat[2][1] = 0.0; m_aafMat[2][2] = 1.0; + } +} +//--------------------------------------------------------------------------- +void MgcEigen::Tridiagonal4 (real** m_aafMat, real* m_afDiag, + real* m_afSubd) +{ + // save matrix M + real fM00 = m_aafMat[0][0]; + real fM01 = m_aafMat[0][1]; + real fM02 = m_aafMat[0][2]; + real fM03 = m_aafMat[0][3]; + real fM11 = m_aafMat[1][1]; + real fM12 = m_aafMat[1][2]; + real fM13 = m_aafMat[1][3]; + real fM22 = m_aafMat[2][2]; + real fM23 = m_aafMat[2][3]; + real fM33 = m_aafMat[3][3]; + + m_afDiag[0] = fM00; + m_afSubd[3] = 0.0; + + m_aafMat[0][0] = 1.0; + m_aafMat[0][1] = 0.0; + m_aafMat[0][2] = 0.0; + m_aafMat[0][3] = 0.0; + m_aafMat[1][0] = 0.0; + m_aafMat[2][0] = 0.0; + m_aafMat[3][0] = 0.0; + + float fLength, fInvLength; + + if ( fM02 != 0.0 || fM03 != 0.0 ) + { + real fQ11, fQ12, fQ13; + real fQ21, fQ22, fQ23; + real fQ31, fQ32, fQ33; + + // build column Q1 + fLength = (real)sqrt(fM01*fM01+fM02*fM02+fM03*fM03); + fInvLength = 1.0/fLength; + fQ11 = fM01*fInvLength; + fQ21 = fM02*fInvLength; + fQ31 = fM03*fInvLength; + + m_afSubd[0] = fLength; + + // compute S*Q1 + real fV0 = fM11*fQ11+fM12*fQ21+fM13*fQ31; + real fV1 = fM12*fQ11+fM22*fQ21+fM23*fQ31; + real fV2 = fM13*fQ11+fM23*fQ21+fM33*fQ31; + + m_afDiag[1] = fQ11*fV0+fQ21*fV1+fQ31*fV2; + + // build column Q3 = Q1x(S*Q1) + fQ13 = fQ21*fV2-fQ31*fV1; + fQ23 = fQ31*fV0-fQ11*fV2; + fQ33 = fQ11*fV1-fQ21*fV0; + fLength = (real)sqrt(fQ13*fQ13+fQ23*fQ23+fQ33*fQ33); + if ( fLength > 0.0 ) + { + fInvLength = 1.0/fLength; + fQ13 *= fInvLength; + fQ23 *= fInvLength; + fQ33 *= fInvLength; + + // build column Q2 = Q3xQ1 + fQ12 = fQ23*fQ31-fQ33*fQ21; + fQ22 = fQ33*fQ11-fQ13*fQ31; + fQ32 = fQ13*fQ21-fQ23*fQ11; + + fV0 = fQ12*fM11+fQ22*fM12+fQ32*fM13; + fV1 = fQ12*fM12+fQ22*fM22+fQ32*fM23; + fV2 = fQ12*fM13+fQ22*fM23+fQ32*fM33; + m_afSubd[1] = fQ11*fV0+fQ21*fV1+fQ31*fV2; + m_afDiag[2] = fQ12*fV0+fQ22*fV1+fQ32*fV2; + m_afSubd[2] = fQ13*fV0+fQ23*fV1+fQ33*fV2; + + fV0 = fQ13*fM11+fQ23*fM12+fQ33*fM13; + fV1 = fQ13*fM12+fQ23*fM22+fQ33*fM23; + fV2 = fQ13*fM13+fQ23*fM23+fQ33*fM33; + m_afDiag[3] = fQ13*fV0+fQ23*fV1+fQ33*fV2; + } + else + { + // S*Q1 parallel to Q1, choose any valid Q2 and Q3 + m_afSubd[1] = 0; + + fLength = fQ21*fQ21+fQ31*fQ31; + if ( fLength > 0.0 ) + { + fInvLength = 1.0/fLength; + real fTmp = fQ11-1.0; + fQ12 = -fQ21; + fQ22 = 1.0+fTmp*fQ21*fQ21*fInvLength; + fQ32 = fTmp*fQ21*fQ31*fInvLength; + + fQ13 = -fQ31; + fQ23 = fQ32; + fQ33 = 1.0+fTmp*fQ31*fQ31*fInvLength; + + fV0 = fQ12*fM11+fQ22*fM12+fQ32*fM13; + fV1 = fQ12*fM12+fQ22*fM22+fQ32*fM23; + fV2 = fQ12*fM13+fQ22*fM23+fQ32*fM33; + m_afDiag[2] = fQ12*fV0+fQ22*fV1+fQ32*fV2; + m_afSubd[2] = fQ13*fV0+fQ23*fV1+fQ33*fV2; + + fV0 = fQ13*fM11+fQ23*fM12+fQ33*fM13; + fV1 = fQ13*fM12+fQ23*fM22+fQ33*fM23; + fV2 = fQ13*fM13+fQ23*fM23+fQ33*fM33; + m_afDiag[3] = fQ13*fV0+fQ23*fV1+fQ33*fV2; + } + else + { + // Q1 = (+-1,0,0) + fQ12 = 0.0; fQ22 = 1.0; fQ32 = 0.0; + fQ13 = 0.0; fQ23 = 0.0; fQ33 = 1.0; + + m_afDiag[2] = fM22; + m_afDiag[3] = fM33; + m_afSubd[2] = fM23; + } + } + + m_aafMat[1][1] = fQ11; m_aafMat[1][2] = fQ12; m_aafMat[1][3] = fQ13; + m_aafMat[2][1] = fQ21; m_aafMat[2][2] = fQ22; m_aafMat[2][3] = fQ23; + m_aafMat[3][1] = fQ31; m_aafMat[3][2] = fQ32; m_aafMat[3][3] = fQ33; + } + else + { + m_afDiag[1] = fM11; + m_afSubd[0] = fM01; + m_aafMat[1][1] = 1.0; + m_aafMat[2][1] = 0.0; + m_aafMat[3][1] = 0.0; + + if ( fM13 != 0.0 ) + { + fLength = (real)sqrt(fM12*fM12+fM13*fM13); + fInvLength = 1.0/fLength; + fM12 *= fInvLength; + fM13 *= fInvLength; + real fQ = 2.0*fM12*fM23+fM13*(fM33-fM22); + + m_afDiag[2] = fM22+fM13*fQ; + m_afDiag[3] = fM33-fM13*fQ; + m_afSubd[1] = fLength; + m_afSubd[2] = fM23-fM12*fQ; + m_aafMat[1][2] = 0.0; + m_aafMat[1][3] = 0.0; + m_aafMat[2][2] = fM12; + m_aafMat[2][3] = fM13; + m_aafMat[3][2] = fM13; + m_aafMat[3][3] = -fM12; + } + else + { + m_afDiag[2] = fM22; + m_afDiag[3] = fM33; + m_afSubd[1] = fM12; + m_afSubd[2] = fM23; + m_aafMat[1][2] = 0.0; + m_aafMat[1][3] = 0.0; + m_aafMat[2][2] = 1.0; + m_aafMat[2][3] = 0.0; + m_aafMat[3][2] = 0.0; + m_aafMat[3][3] = 1.0; + } + } +} +//--------------------------------------------------------------------------- +void MgcEigen::TridiagonalN (int iSize, real** m_aafMat, + real* m_afDiag, real* m_afSubd) +{ + int i0, i1, i2, i3; + + for (i0 = iSize-1, i3 = iSize-2; i0 >= 1; i0--, i3--) + { + real fH = 0.0, fScale = 0.0; + + if ( i3 > 0 ) + { + for (i2 = 0; i2 <= i3; i2++) + fScale += abs(m_aafMat[i0][i2]); + if ( fScale == 0 ) + { + m_afSubd[i0] = m_aafMat[i0][i3]; + } + else + { + float fInvScale = 1.0/fScale; + for (i2 = 0; i2 <= i3; i2++) + { + m_aafMat[i0][i2] *= fInvScale; + fH += m_aafMat[i0][i2]*m_aafMat[i0][i2]; + } + real fF = m_aafMat[i0][i3]; + real fG = (real)sqrt(fH); + if ( fF > 0.0 ) + fG = -fG; + m_afSubd[i0] = fScale*fG; + fH -= fF*fG; + m_aafMat[i0][i3] = fF-fG; + fF = 0.0; + float fInvH = 1.0/fH; + for (i1 = 0; i1 <= i3; i1++) + { + m_aafMat[i1][i0] = m_aafMat[i0][i1]*fInvH; + fG = 0.0; + for (i2 = 0; i2 <= i1; i2++) + fG += m_aafMat[i1][i2]*m_aafMat[i0][i2]; + for (i2 = i1+1; i2 <= i3; i2++) + fG += m_aafMat[i2][i1]*m_aafMat[i0][i2]; + m_afSubd[i1] = fG*fInvH; + fF += m_afSubd[i1]*m_aafMat[i0][i1]; + } + real fHalfFdivH = 0.5*fF*fInvH; + for (i1 = 0; i1 <= i3; i1++) + { + fF = m_aafMat[i0][i1]; + fG = m_afSubd[i1] - fHalfFdivH*fF; + m_afSubd[i1] = fG; + for (i2 = 0; i2 <= i1; i2++) + { + m_aafMat[i1][i2] -= fF*m_afSubd[i2] + + fG*m_aafMat[i0][i2]; + } + } + } + } + else + { + m_afSubd[i0] = m_aafMat[i0][i3]; + } + + m_afDiag[i0] = fH; + } + + m_afDiag[0] = m_afSubd[0] = 0; + for (i0 = 0, i3 = -1; i0 <= iSize-1; i0++, i3++) + { + if ( m_afDiag[i0] ) + { + for (i1 = 0; i1 <= i3; i1++) + { + real fSum = 0; + for (i2 = 0; i2 <= i3; i2++) + fSum += m_aafMat[i0][i2]*m_aafMat[i2][i1]; + for (i2 = 0; i2 <= i3; i2++) + m_aafMat[i2][i1] -= fSum*m_aafMat[i2][i0]; + } + } + m_afDiag[i0] = m_aafMat[i0][i0]; + m_aafMat[i0][i0] = 1; + for (i1 = 0; i1 <= i3; i1++) + m_aafMat[i1][i0] = m_aafMat[i0][i1] = 0; + } + + // re-ordering if MgcEigen::QLAlgorithm is used subsequently + for (i0 = 1, i3 = 0; i0 < iSize; i0++, i3++) + m_afSubd[i3] = m_afSubd[i0]; + m_afSubd[iSize-1] = 0; +} +//--------------------------------------------------------------------------- +bool MgcEigen::QLAlgorithm (int iSize, real* m_afDiag, real* m_afSubd, + real** m_aafMat) +{ + const int iMaxIter = 32; + + for (int i0 = 0; i0 < iSize; i0++) + { + int i1; + for (i1 = 0; i1 < iMaxIter; i1++) + { + int i2; + for (i2 = i0; i2 <= iSize-2; i2++) + { + real fTmp = + abs(m_afDiag[i2])+abs(m_afDiag[i2+1]); + if ( abs(m_afSubd[i2]) + fTmp == fTmp ) + break; + } + if ( i2 == i0 ) + break; + + real fG = (m_afDiag[i0+1]-m_afDiag[i0])/(2.0*m_afSubd[i0]); + real fR = (real)sqrt(fG*fG+1.0); + if ( fG < 0.0 ) + fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR); + else + fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR); + real fSin = 1.0, fCos = 1.0, fP = 0.0; + for (int i3 = i2-1; i3 >= i0; i3--) + { + real fF = fSin*m_afSubd[i3]; + real fB = fCos*m_afSubd[i3]; + if ( abs(fF) >= abs(fG) ) + { + fCos = fG/fF; + fR = (real)sqrt(fCos*fCos+1.0); + m_afSubd[i3+1] = fF*fR; + fSin = 1.0/fR; + fCos *= fSin; + } + else + { + fSin = fF/fG; + fR = (real)sqrt(fSin*fSin+1.0); + m_afSubd[i3+1] = fG*fR; + fCos = 1.0/fR; + fSin *= fCos; + } + fG = m_afDiag[i3+1]-fP; + fR = (m_afDiag[i3]-fG)*fSin+2.0*fB*fCos; + fP = fSin*fR; + m_afDiag[i3+1] = fG+fP; + fG = fCos*fR-fB; + + for (int i4 = 0; i4 < iSize; i4++) + { + fF = m_aafMat[i4][i3+1]; + m_aafMat[i4][i3+1] = fSin*m_aafMat[i4][i3]+fCos*fF; + m_aafMat[i4][i3] = fCos*m_aafMat[i4][i3]-fSin*fF; + } + } + m_afDiag[i0] -= fP; + m_afSubd[i0] = fG; + m_afSubd[i2] = 0.0; + } + if ( i1 == iMaxIter ) + return false; + } + + return true; +} +//--------------------------------------------------------------------------- +void MgcEigen::DecreasingSort (int iSize, real* afEigval, + real** aafEigvec) +{ + // sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1] + for (int i0 = 0, i1; i0 <= iSize-2; i0++) + { + // locate maximum eigenvalue + i1 = i0; + real fMax = afEigval[i1]; + int i2; + for (i2 = i0+1; i2 < iSize; i2++) + { + if ( afEigval[i2] > fMax ) + { + i1 = i2; + fMax = afEigval[i1]; + } + } + + if ( i1 != i0 ) + { + // swap eigenvalues + afEigval[i1] = afEigval[i0]; + afEigval[i0] = fMax; + + // swap eigenvectors + for (i2 = 0; i2 < iSize; i2++) + { + real fTmp = aafEigvec[i2][i0]; + aafEigvec[i2][i0] = aafEigvec[i2][i1]; + aafEigvec[i2][i1] = fTmp; + } + } + } +} +//--------------------------------------------------------------------------- +void MgcEigen::IncreasingSort (int iSize, real* afEigval, + real** aafEigvec) +{ + // sort eigenvalues in increasing order, e[0] <= ... <= e[iSize-1] + for (int i0 = 0, i1; i0 <= iSize-2; i0++) + { + // locate minimum eigenvalue + i1 = i0; + real fMin = afEigval[i1]; + int i2; + for (i2 = i0+1; i2 < iSize; i2++) + { + if ( afEigval[i2] < fMin ) + { + i1 = i2; + fMin = afEigval[i1]; + } + } + + if ( i1 != i0 ) + { + // swap eigenvalues + afEigval[i1] = afEigval[i0]; + afEigval[i0] = fMin; + + // swap eigenvectors + for (i2 = 0; i2 < iSize; i2++) + { + real fTmp = aafEigvec[i2][i0]; + aafEigvec[i2][i0] = aafEigvec[i2][i1]; + aafEigvec[i2][i1] = fTmp; + } + } + } +} +//--------------------------------------------------------------------------- +void MgcEigen::SetMatrix (real** aafMat) +{ + for (int iRow = 0; iRow < m_iSize; iRow++) + { + for (int iCol = 0; iCol < m_iSize; iCol++) + m_aafMat[iRow][iCol] = aafMat[iRow][iCol]; + } +} +//--------------------------------------------------------------------------- +void MgcEigen::EigenStuff2 () +{ + Tridiagonal2(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::EigenStuff3 () +{ + Tridiagonal3(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::EigenStuff4 () +{ + Tridiagonal4(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::EigenStuffN () +{ + TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::EigenStuff () +{ + switch ( m_iSize ) + { + case 2: + Tridiagonal2(m_aafMat,m_afDiag,m_afSubd); + break; + case 3: + Tridiagonal3(m_aafMat,m_afDiag,m_afSubd); + break; + case 4: + Tridiagonal4(m_aafMat,m_afDiag,m_afSubd); + break; + default: + TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd); + break; + } + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::DecrSortEigenStuff2 () +{ + Tridiagonal2(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + DecreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::DecrSortEigenStuff3 () +{ + Tridiagonal3(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + DecreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::DecrSortEigenStuff4 () +{ + Tridiagonal4(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + DecreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::DecrSortEigenStuffN () +{ + TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + DecreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::DecrSortEigenStuff () +{ + switch ( m_iSize ) + { + case 2: + Tridiagonal2(m_aafMat,m_afDiag,m_afSubd); + break; + case 3: + Tridiagonal3(m_aafMat,m_afDiag,m_afSubd); + break; + case 4: + Tridiagonal4(m_aafMat,m_afDiag,m_afSubd); + break; + default: + TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd); + break; + } + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + DecreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::IncrSortEigenStuff2 () +{ + Tridiagonal2(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + IncreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::IncrSortEigenStuff3 () +{ + Tridiagonal3(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + IncreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::IncrSortEigenStuff4 () +{ + Tridiagonal4(m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + IncreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::IncrSortEigenStuffN () +{ + TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd); + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + IncreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- +void MgcEigen::IncrSortEigenStuff () +{ + switch ( m_iSize ) + { + case 2: + Tridiagonal2(m_aafMat,m_afDiag,m_afSubd); + break; + case 3: + Tridiagonal3(m_aafMat,m_afDiag,m_afSubd); + break; + case 4: + Tridiagonal4(m_aafMat,m_afDiag,m_afSubd); + break; + default: + TridiagonalN(m_iSize,m_aafMat,m_afDiag,m_afSubd); + break; + } + QLAlgorithm(m_iSize,m_afDiag,m_afSubd,m_aafMat); + IncreasingSort(m_iSize,m_afDiag,m_aafMat); +} +//--------------------------------------------------------------------------- + +#ifdef EIGEN_TEST + +int main () +{ + MgcEigen kES(3); + + kES.Matrix(0,0) = 2.0; kES.Matrix(0,1) = 1.0; kES.Matrix(0,2) = 1.0; + kES.Matrix(1,0) = 1.0; kES.Matrix(1,1) = 2.0; kES.Matrix(1,2) = 1.0; + kES.Matrix(2,0) = 1.0; kES.Matrix(2,1) = 1.0; kES.Matrix(2,2) = 2.0; + + kES.IncrSortEigenStuff3(); + + cout.setf(ios::fixed); + + cout << "eigenvalues = " << endl; + int iRow; + for (iRow = 0; iRow < 3; iRow++) + cout << kES.GetEigenvalue(iRow) << ' '; + cout << endl; + + cout << "eigenvectors = " << endl; + for (iRow = 0; iRow < 3; iRow++) + { + for (int iCol = 0; iCol < 3; iCol++) + cout << kES.GetEigenvector(iRow,iCol) << ' '; + cout << endl; + } + + // eigenvalues = + // 1.000000 1.000000 4.000000 + // eigenvectors = + // 0.411953 0.704955 0.577350 + // 0.404533 -0.709239 0.577350 + // -0.816485 0.004284 0.577350 + + return 0; +} + +#endif diff --git a/Utils/Libs/Maths/MgcEigen.h b/Utils/Libs/Maths/MgcEigen.h new file mode 100644 index 000000000..52a661fb9 --- /dev/null +++ b/Utils/Libs/Maths/MgcEigen.h @@ -0,0 +1,112 @@ +// Magic Software, Inc. +// http://www.magic-software.com +// Copyright (c) 2000, All Rights Reserved +// +// Source code from Magic Software is supplied under the terms of a license +// agreement and may not be copied or disclosed except in accordance with the +// terms of that agreement. The various license agreements may be found at +// the Magic Software web site. This file is subject to the license +// +// FREE SOURCE CODE +// http://www.magic-software.com/License/free.pdf + +#ifndef MGCEIGEN_H +#define MGCEIGEN_H + +#include "mathtypes.h" + + +class MgcEigen +{ +public: + MgcEigen (int iSize); + ~MgcEigen (); + + // set the matrix for eigensolving + real& Matrix (int iRow, int iCol); + void SetMatrix (real** aafMat); + + // get the results of eigensolving (eigenvectors are columns of matrix) + real GetEigenvalue (int i) const; + real GetEigenvector (int iRow, int iCol) const; + real* GetEigenvalue (); + real** GetEigenvector (); + + // solve eigensystem + void EigenStuff2 (); + void EigenStuff3 (); + void EigenStuff4 (); + void EigenStuffN (); + void EigenStuff (); + + // solve eigensystem, use decreasing sort on eigenvalues + void DecrSortEigenStuff2 (); + void DecrSortEigenStuff3 (); + void DecrSortEigenStuff4 (); + void DecrSortEigenStuffN (); + void DecrSortEigenStuff (); + + // solve eigensystem, use increasing sort on eigenvalues + void IncrSortEigenStuff2 (); + void IncrSortEigenStuff3 (); + void IncrSortEigenStuff4 (); + void IncrSortEigenStuffN (); + void IncrSortEigenStuff (); + +protected: + int m_iSize; + real** m_aafMat; + real* m_afDiag; + real* m_afSubd; + + // Householder reduction to tridiagonal form + static void Tridiagonal2 (real** aafMat, real* afDiag, + real* afSubd); + static void Tridiagonal3 (real** aafMat, real* afDiag, + real* afSubd); + static void Tridiagonal4 (real** aafMat, real* afDiag, + real* afSubd); + static void TridiagonalN (int iSize, real** aafMat, real* afDiag, + real* afSubd); + + // QL algorithm with implicit shifting, applies to tridiagonal matrices + static bool QLAlgorithm (int iSize, real* afDiag, real* afSubd, + real** aafMat); + + // sort eigenvalues from largest to smallest + static void DecreasingSort (int iSize, real* afEigval, + real** aafEigvec); + + // sort eigenvalues from smallest to largest + static void IncreasingSort (int iSize, real* afEigval, + real** aafEigvec); +}; + +//--------------------------------------------------------------------------- +inline real& MgcEigen::Matrix (int iRow, int iCol) +{ + return m_aafMat[iRow][iCol]; +} +//--------------------------------------------------------------------------- +inline real MgcEigen::GetEigenvalue (int i) const +{ + return m_afDiag[i]; +} +//--------------------------------------------------------------------------- +inline real MgcEigen::GetEigenvector (int iRow, int iCol) const +{ + return m_aafMat[iRow][iCol]; +} +//--------------------------------------------------------------------------- +inline real* MgcEigen::GetEigenvalue () +{ + return m_afDiag; +} +//--------------------------------------------------------------------------- +inline real** MgcEigen::GetEigenvector () +{ + return m_aafMat; +} +//--------------------------------------------------------------------------- + +#endif diff --git a/Utils/Libs/Maths/aabox.h b/Utils/Libs/Maths/aabox.h new file mode 100644 index 000000000..b36e9a680 --- /dev/null +++ b/Utils/Libs/Maths/aabox.h @@ -0,0 +1,29 @@ +#ifndef __AABOX_H__ +#define __AABOX_H__ + +#include "vector3.h" + +struct AABox +{ + Vector3 m_Min; + Vector3 m_Max; + + bool QueryInside( Vector3 const &pnt ) const + { + if (pnt.x >= m_Min.x-0.001f && pnt.y >= m_Min.y-0.001f && pnt.z >= m_Min.z-0.001f && + pnt.x < m_Max.x+0.001f && pnt.y < m_Max.y+0.001f && pnt.z < m_Max.z+0.001f) + { + return true; + } else + { + return false; + } + } + + bool QueryInside( AABox const &box ) const + { + return QueryInside( box.m_Min ) && QueryInside( box.m_Max ); + } +}; + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/axisangle.cpp b/Utils/Libs/Maths/axisangle.cpp new file mode 100644 index 000000000..bfb9454d4 --- /dev/null +++ b/Utils/Libs/Maths/axisangle.cpp @@ -0,0 +1,41 @@ +#include "axisangle.h" +#include "mathtypes.h" +#include "quat.h" +#include "matrix4x4.h" + + +void AxisAngle::ToQuaternion( Quaternion &q ) const +{ + real ca = (real)cos(angle*0.5f); + real sa = (real)sin(angle*0.5f); + q.x = sa * x; + q.y = sa * y; + q.z = sa * z; + q.w = ca; +} + +void AxisAngle::ToMatrix( Matrix4x4 &m ) const +{ + real sa=(real)sin(angle); + real ca=(real)cos(angle); + real t = 1.0f - ca; + + real sx = sa * x; + real sy = sa * y; + real sz = sa * z; + real tx = t * x; + real ty = t * y; + real tz = t * z; + + m._11 = (tx * x) + ca; + m._21 = (tx * y) + sz; + m._31 = (tx * z) - sy; + + m._12 = (tx * y) - sz; + m._22 = (ty * y) + ca; + m._32 = (ty * z) + sx; + + m._13 = (tx * z) - sy; + m._23 = (ty * z) + sx; + m._33 = (tz * z) + ca; +} diff --git a/Utils/Libs/Maths/axisangle.h b/Utils/Libs/Maths/axisangle.h new file mode 100644 index 000000000..dbe0108ba --- /dev/null +++ b/Utils/Libs/Maths/axisangle.h @@ -0,0 +1,22 @@ +#ifndef __AXISANGLE_H__ +#define __AXISANGLE_H__ + +#include "mathtypes.h" + +class Quaternion; +struct Matrix4x4; + +class AxisAngle +{ +public: + real x,y,z; + real angle; + + void ToQuaternion( Quaternion &q ) const; + void ToMatrix( Matrix4x4 &m ) const; +}; + + + + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/maths.dsp b/Utils/Libs/Maths/maths.dsp new file mode 100644 index 000000000..a564c51d7 --- /dev/null +++ b/Utils/Libs/Maths/maths.dsp @@ -0,0 +1,162 @@ +# Microsoft Developer Studio Project File - Name="maths" - Package Owner=<4> +# Microsoft Developer Studio Generated Build File, Format Version 6.00 +# ** DO NOT EDIT ** + +# TARGTYPE "Win32 (x86) Static Library" 0x0104 + +CFG=maths - Win32 Debug +!MESSAGE This is not a valid makefile. To build this project using NMAKE, +!MESSAGE use the Export Makefile command and run +!MESSAGE +!MESSAGE NMAKE /f "maths.mak". +!MESSAGE +!MESSAGE You can specify a configuration when running NMAKE +!MESSAGE by defining the macro CFG on the command line. For example: +!MESSAGE +!MESSAGE NMAKE /f "maths.mak" CFG="maths - Win32 Debug" +!MESSAGE +!MESSAGE Possible choices for configuration are: +!MESSAGE +!MESSAGE "maths - Win32 Release" (based on "Win32 (x86) Static Library") +!MESSAGE "maths - Win32 Debug" (based on "Win32 (x86) Static Library") +!MESSAGE + +# Begin Project +# PROP AllowPerConfigDependencies 0 +# PROP Scc_ProjName ""$/src/libs/maths", DTAAAAAA" +# PROP Scc_LocalPath "." +CPP=cl.exe +RSC=rc.exe + +!IF "$(CFG)" == "maths - Win32 Release" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 0 +# PROP BASE Output_Dir "Release" +# PROP BASE Intermediate_Dir "Release" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 0 +# PROP Output_Dir "Release" +# PROP Intermediate_Dir "Release" +# PROP Target_Dir "" +MTL=midl.exe +# ADD BASE CPP /nologo /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c +# ADD CPP /nologo /W3 /GX /O2 /I "..\common" /D "WIN32" /D "NDEBUG" /D "_MBCS" /D "_LIB" /YX /FD /c +# ADD BASE RSC /l 0x809 /d "NDEBUG" +# ADD RSC /l 0x809 /d "NDEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LIB32=link.exe -lib +# ADD BASE LIB32 /nologo +# ADD LIB32 /nologo + +!ELSEIF "$(CFG)" == "maths - Win32 Debug" + +# PROP BASE Use_MFC 0 +# PROP BASE Use_Debug_Libraries 1 +# PROP BASE Output_Dir "Debug" +# PROP BASE Intermediate_Dir "Debug" +# PROP BASE Target_Dir "" +# PROP Use_MFC 0 +# PROP Use_Debug_Libraries 1 +# PROP Output_Dir "Debug" +# PROP Intermediate_Dir "Debug" +# PROP Target_Dir "" +MTL=midl.exe +# ADD BASE CPP /nologo /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /YX /FD /GZ /c +# ADD CPP /nologo /MDd /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_MBCS" /D "_LIB" /FR /YX /FD /GZ /c +# ADD BASE RSC /l 0x809 /d "_DEBUG" +# ADD RSC /l 0x809 /d "_DEBUG" +BSC32=bscmake.exe +# ADD BASE BSC32 /nologo +# ADD BSC32 /nologo +LIB32=link.exe -lib +# ADD BASE LIB32 /nologo +# ADD LIB32 /nologo + +!ENDIF + +# Begin Target + +# Name "maths - Win32 Release" +# Name "maths - Win32 Debug" +# Begin Group "Source Files" + +# PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" +# Begin Source File + +SOURCE=.\axisangle.cpp +# End Source File +# Begin Source File + +SOURCE=.\Decompose.cpp +# End Source File +# Begin Source File + +SOURCE=.\matrix4x4.cpp +# End Source File +# Begin Source File + +SOURCE=.\MgcAppr3DLineFit.cpp +# End Source File +# Begin Source File + +SOURCE=.\MgcEigen.cpp +# End Source File +# Begin Source File + +SOURCE=.\quat.cpp +# End Source File +# End Group +# Begin Group "Header Files" + +# PROP Default_Filter "h;hpp;hxx;hm;inl" +# Begin Source File + +SOURCE=.\aabox.h +# End Source File +# Begin Source File + +SOURCE=.\axisangle.h +# End Source File +# Begin Source File + +SOURCE=.\Decompose.h +# End Source File +# Begin Source File + +SOURCE=.\mathtypes.h +# End Source File +# Begin Source File + +SOURCE=.\matrix4x4.h +# End Source File +# Begin Source File + +SOURCE=.\MgcAppr3DLineFit.h +# End Source File +# Begin Source File + +SOURCE=.\MgcEigen.h +# End Source File +# Begin Source File + +SOURCE=.\plane.h +# End Source File +# Begin Source File + +SOURCE=.\quat.h +# End Source File +# Begin Source File + +SOURCE=.\spline.h +# End Source File +# Begin Source File + +SOURCE=.\vector3.h +# End Source File +# End Group +# End Target +# End Project diff --git a/Utils/Libs/Maths/mathtypes.h b/Utils/Libs/Maths/mathtypes.h new file mode 100644 index 000000000..85593b8b6 --- /dev/null +++ b/Utils/Libs/Maths/mathtypes.h @@ -0,0 +1,54 @@ +#ifndef __BASETYPES_H__ +#define __BASETYPES_H__ + +#ifndef s32 +typedef long s32; +#endif + +#ifndef u32 +typedef unsigned long u32; +#endif + +#ifndef s16 +typedef short s16; +#endif + +#ifndef u16 +typedef unsigned short u16; +#endif + +#ifndef s8 +//typedef char s8; +#endif + +#ifndef u8 +typedef unsigned char u8; +#endif + +#ifndef byte +typedef u8 byte; +#endif + +// Versions with capitals which are equivalent to the above. + +#ifndef U32 +typedef unsigned long U32; +#endif +#ifndef U16 +typedef unsigned short int U16; +#endif +#ifndef U8 +typedef unsigned char U8; +#endif +#ifndef S32 +typedef signed long S32; +#endif + +typedef float real; +typedef double dblreal; + +#ifndef NULL +#define NULL 0 +#endif + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/matrix4x4.cpp b/Utils/Libs/Maths/matrix4x4.cpp new file mode 100644 index 000000000..8180c706b --- /dev/null +++ b/Utils/Libs/Maths/matrix4x4.cpp @@ -0,0 +1,221 @@ +#include "mathtypes.h" +#include "matrix4x4.h" +#include "math.h" + +#include "quat.h" + + +void Matrix4x4::ToQuaternion( Quaternion &q ) const +{ + real s,v; + + v=_11+_22+_33; + if (v>0.0f) + { + s=(float)sqrt(v+1.0f); + q.w=0.5f*s; + s=0.5f/s; + q.x=(_32-_23)*s; + q.y=(_13-_31)*s; + q.z=(_21-_12)*s; + } else + { + if (_22>_11) + { + if (_33>_22) + { + s=(float)sqrt((_33-(_11+_22))+1.0f); + q.z=s*0.5f; + if (s!=0.0f) s=0.5f/s; + q.w=(_21-_12)*s; + q.x=(_13+_31)*s; + q.y=(_23+_32)*s; + } else + { + s=(float)sqrt((_22-(_33+_11))+1.0f); + q.y=s*0.5f; + if (s!=0.0f) s=0.5f/s; + q.w=(_13-_31)*s; + q.z=(_32+_23)*s; + q.x=(_12+_21)*s; + } + } else + if (_33>_11) + { + s=(float)sqrt((_33-(_11+_22))+1.0f); + q.z=s*0.5f; + if (s!=0.0f) s=0.5f/s; + q.w=(_21-_12)*s; + q.x=(_13+_31)*s; + q.y=(_23+_32)*s; + } else + { + s=(float)sqrt((_11-(_22+_33))+1.0f); + q.x=s*0.5f; + if (s!=0.0f) s=0.5f/s; + q.w=(_32-_23)*s; + q.y=(_21+_12)*s; + q.z=(_31+_13)*s; + } + } +} + + + +void MatrixInvert( Matrix4x4 &out, Matrix4x4 const &m ) +{ +#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; } + + out.Identity(); + + float wtmp[4][8]; + float m0, m1, m2, m3, s; + float *r0, *r1, *r2, *r3; + + r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; + + r0[0] = m(0,0), r0[1] = m(0,1), + r0[2] = m(0,2), r0[3] = m(0,3), + r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, + + r1[0] = m(1,0), r1[1] = m(1,1), + r1[2] = m(1,2), r1[3] = m(1,3), + r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, + + r2[0] = m(2,0), r2[1] = m(2,1), + r2[2] = m(2,2), r2[3] = m(2,3), + r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, + + r3[0] = m(3,0), r3[1] = m(3,1), + r3[2] = m(3,2), r3[3] = m(3,3), + r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; + + /* choose pivot - or die */ + if (fabs(r3[0])>fabs(r2[0])) + SWAP_ROWS(r3, r2); + + if (fabs(r2[0])>fabs(r1[0])) + SWAP_ROWS(r2, r1); + + if (fabs(r1[0])>fabs(r0[0])) + SWAP_ROWS(r1, r0); + + if (0.0 == r0[0]) + return; + + /* eliminate first variable */ + m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; + s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; + s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; + s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; + + s = r0[4]; + if (s != 0.0) + { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } + + s = r0[5]; + if (s != 0.0) + { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } + + s = r0[6]; + if (s != 0.0) + { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } + + s = r0[7]; + if (s != 0.0) + { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } + + /* choose pivot - or die */ + if (fabs(r3[1])>fabs(r2[1])) + SWAP_ROWS(r3, r2); + + if (fabs(r2[1])>fabs(r1[1])) + SWAP_ROWS(r2, r1); + + if (0.0 == r1[1]) + return; + + /* eliminate second variable */ + m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; + r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; + r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; + + s = r1[4]; + if (0.0 != s) + { r2[4] -= m2 * s; r3[4] -= m3 * s; } + + s = r1[5]; + if (0.0 != s) + { r2[5] -= m2 * s; r3[5] -= m3 * s; } + + s = r1[6]; + if (0.0 != s) + { r2[6] -= m2 * s; r3[6] -= m3 * s; } + + s = r1[7]; + if (0.0 != s) + { r2[7] -= m2 * s; r3[7] -= m3 * s; } + + /* choose pivot - or die */ + if (fabs(r3[2])>fabs(r2[2])) + SWAP_ROWS(r3, r2); + + if (0.0 == r2[2]) + return; + + /* eliminate third variable */ + m3 = r3[2]/r2[2]; + r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], + r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], + r3[7] -= m3 * r2[7]; + + /* last check */ + if (0.0 == r3[3]) + return; + + s = float(1.0)/r3[3]; /* now back substitute row 3 */ + r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; + + m2 = r2[3]; /* now back substitute row 2 */ + s = float(1.0)/r2[2]; + r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), + r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); + m1 = r1[3]; + r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, + r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; + m0 = r0[3]; + r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, + r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; + + m1 = r1[2]; /* now back substitute row 1 */ + s = float(1.0)/r1[1]; + r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), + r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); + m0 = r0[2]; + r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, + r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; + + m0 = r0[1]; /* now back substitute row 0 */ + s = float(1.0)/r0[0]; + r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), + r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); + + out(0,0) = r0[4]; out(0,1) = r0[5], + out(0,2) = r0[6]; out(0,3) = r0[7], + out(1,0) = r1[4]; out(1,1) = r1[5], + out(1,2) = r1[6]; out(1,3) = r1[7], + out(2,0) = r2[4]; out(2,1) = r2[5], + out(2,2) = r2[6]; out(2,3) = r2[7], + out(3,0) = r3[4]; out(3,1) = r3[5], + out(3,2) = r3[6]; out(3,3) = r3[7]; + + return; + +#undef SWAP_ROWS +} + +void Matrix4x4::Invert( void ) +{ + Matrix4x4 temp = *this; + MatrixInvert( *this, temp ); +} \ No newline at end of file diff --git a/Utils/Libs/Maths/matrix4x4.h b/Utils/Libs/Maths/matrix4x4.h new file mode 100644 index 000000000..d7c9efd0f --- /dev/null +++ b/Utils/Libs/Maths/matrix4x4.h @@ -0,0 +1,490 @@ +#ifndef __MATRIX4X4_H__ +#define __MATRIX4X4_H__ + +#include "mathtypes.h" +#include "vector3.h" +#include "plane.h" +//#include "user.h" + +class Quaternion; + +//AF - added this to suppress warning for nonstandard [unnamed union] +#pragma warning(push) +#pragma warning(disable:4201) + +#if defined(__USER_Mike__) + #define USE_INTRINSICS +#endif + +#if defined(USE_INTRINSICS) +#include "xmmintrin.h" +#endif + +// there should be _NO_ constructor +struct Matrix4x4 +{ + union + { + real m_M[4][4]; + struct + { + real _11, _12, _13, _14; + real _21, _22, _23, _24; + real _31, _32, _33, _34; + real _41, _42, _43, _44; + }; + }; + + inline real & operator()(int ix, int iy) + { + return m_M[ix][iy]; + } + + inline real const & operator()(int ix, int iy) const + { + return m_M[ix][iy]; + } + + inline void Identity( void ) + { + m_M[0][0] = 1.f; + m_M[0][1] = 0.f; + m_M[0][2] = 0.f; + m_M[0][3] = 0.f; + m_M[1][0] = 0.f; + m_M[1][1] = 1.f; + m_M[1][2] = 0.f; + m_M[1][3] = 0.f; + m_M[2][0] = 0.f; + m_M[2][1] = 0.f; + m_M[2][2] = 1.f; + m_M[2][3] = 0.f; + m_M[3][0] = 0.f; + m_M[3][1] = 0.f; + m_M[3][2] = 0.f; + m_M[3][3] = 1.f; + } + + inline void Zero( void ) + { + m_M[0][0] = 0.f; + m_M[0][1] = 0.f; + m_M[0][2] = 0.f; + m_M[0][3] = 0.f; + m_M[1][0] = 0.f; + m_M[1][1] = 0.f; + m_M[1][2] = 0.f; + m_M[1][3] = 0.f; + m_M[2][0] = 0.f; + m_M[2][1] = 0.f; + m_M[2][2] = 0.f; + m_M[2][3] = 0.f; + m_M[3][0] = 0.f; + m_M[3][1] = 0.f; + m_M[3][2] = 0.f; + m_M[3][3] = 0.f; + } + + void ToQuaternion( Quaternion &q ) const; + + void Invert( void ); + inline void Transpose( void ); + + inline void SetRotX( real a ); + inline void SetRotY( real a ); + inline void SetRotZ( real a ); + + inline void SetTranslation( real x, real y, real z ); + inline void SetTranslation( Vector3 const &v ); + + inline void GetTranslation(Vector3 &out) const; + + inline void SetProjection( real fov, real aspect, real nearclip, real farclip ); + + inline void ApplyScale(Vector3 Scl); + + inline Matrix4x4 &operator+=( Matrix4x4 const &other ); + + friend inline Matrix4x4 operator*( Matrix4x4 const &a, Matrix4x4 const &b ); + friend inline Matrix4x4 operator+( Matrix4x4 const &a, Matrix4x4 const &b ); + + friend inline Vector3 operator*( Matrix4x4 const &a, Vector3 const &b ); +}; + +#pragma warning(pop) + +inline Matrix4x4 &Matrix4x4::operator +=( Matrix4x4 const &other ) +{ + for (int i=0; i<4; i++) + { + for (int j=0; j<4; j++) + { + m_M[i][j] += other.m_M[i][j]; + } + } + return *this; +} + +#if 0//defined(USE_INTRINSICS) +inline void Matrix4x4::Transpose( void ) +{ + _MM_TRANSPOSE4_PS(_114, _214, _314, _414); +} +#else +inline void Matrix4x4::Transpose( void ) +{ + Matrix4x4 temp = *this; + for (int i=0; i<4; i++) + { + for (int j=0; j<4; j++) + { + m_M[i][j] = temp(j,i); + } + } +} +#endif + +inline void Matrix4x4::SetRotX( real a ) +{ + real ca = (real)cos( a ); + real sa = (real)sin( a ); + m_M[0][0] = 1.f; + m_M[0][1] = 0.f; + m_M[0][2] = 0.f; + m_M[0][3] = 0.f; + + m_M[1][0] = 0.f; + m_M[1][1] = ca; + m_M[1][2] = sa; + m_M[1][3] = 0.f; + + m_M[2][0] = 0.f; + m_M[2][1] = -sa; + m_M[2][2] = ca; + m_M[2][3] = 0.f; + + m_M[3][0] = 0.f; + m_M[3][1] = 0.f; + m_M[3][2] = 0.f; + m_M[3][3] = 1.f; +} + +inline void Matrix4x4::SetRotY( real a ) +{ + real ca = (real)cos( a ); + real sa = (real)sin( a ); + m_M[0][0] = ca; + m_M[0][1] = 0.f; + m_M[0][2] = -sa; + m_M[0][3] = 0.f; + + m_M[1][0] = 0.f; + m_M[1][1] = 1.f; + m_M[1][2] = 0.f; + m_M[1][3] = 0.f; + + m_M[2][0] = sa; + m_M[2][1] = 0.f; + m_M[2][2] = ca; + m_M[2][3] = 0.f; + + m_M[3][0] = 0.f; + m_M[3][1] = 0.f; + m_M[3][2] = 0.f; + m_M[3][3] = 1.f; +} + +inline void Matrix4x4::SetRotZ( real a ) +{ + real ca = (real)cos( a ); + real sa = (real)sin( a ); + m_M[0][0] = ca; + m_M[0][1] = sa; + m_M[0][2] = 0.f; + m_M[0][3] = 0.f; + + m_M[1][0] = -sa; + m_M[1][1] = ca; + m_M[1][2] = 0.f; + m_M[1][3] = 0.f; + + m_M[2][0] = 0.f; + m_M[2][1] = 0.f; + m_M[2][2] = 1.f; + m_M[2][3] = 0.f; + + m_M[3][0] = 0.f; + m_M[3][1] = 0.f; + m_M[3][2] = 0.f; + m_M[3][3] = 1.f; +} + +inline void Matrix4x4::SetTranslation( real x, real y, real z ) +{ + m_M[3][0] = x; + m_M[3][1] = y; + m_M[3][2] = z; +} + +inline void Matrix4x4::SetTranslation( Vector3 const &v ) +{ + m_M[3][0] = v.x; + m_M[3][1] = v.y; + m_M[3][2] = v.z; +} + +inline void Matrix4x4::GetTranslation(Vector3 &out) const +{ + out.x=m_M[3][0]; + out.y=m_M[3][1]; + out.z=m_M[3][2]; +} + +inline void Matrix4x4::SetProjection( real fovx, real fovy, real nearclip, real farclip ) +{ + real w = 1.f/((real)tan( fovx*0.5f)); + real h = 1.f/((real)tan( fovy*0.5f)); + real q = farclip / (farclip - nearclip); + + m_M[0][0] = w; + m_M[0][1] = 0.f; + m_M[0][2] = 0.f; + m_M[0][3] = 0.f; + m_M[1][0] = 0.f; + m_M[1][1] = h; + m_M[1][2] = 0.f; + m_M[1][3] = 0.f; + m_M[2][0] = 0.f; + m_M[2][1] = 0.f; + m_M[2][2] = q; + m_M[2][3] = 1.f; + m_M[3][0] = 0.f; + m_M[3][1] = 0.f; + m_M[3][2] = -q*nearclip; + m_M[3][3] = 0.f; +} + +inline void Matrix4x4::ApplyScale(Vector3 Scl) +{ + m_M[0][0] *= Scl.x; + m_M[1][0] *= Scl.x; + m_M[2][0] *= Scl.x; + m_M[0][1] *= Scl.y; + m_M[1][1] *= Scl.y; + m_M[2][1] *= Scl.y; + m_M[0][2] *= Scl.z; + m_M[1][2] *= Scl.z; + m_M[2][2] *= Scl.z; +} + + +#if defined(USE_INTRINSICS) +inline void MatrixMult( Matrix4x4 &out, Matrix4x4 const &a, Matrix4x4 const &b ) +{ + __m128 t114 = _mm_loadu_ps( b.m_M[0] ); + __m128 t214 = _mm_loadu_ps( b.m_M[1] ); + __m128 t314 = _mm_loadu_ps( b.m_M[2] ); + __m128 t414 = _mm_loadu_ps( b.m_M[3] ); + __m128 a114 = _mm_loadu_ps( a.m_M[0] ); + __m128 a214 = _mm_loadu_ps( a.m_M[1] ); + __m128 a314 = _mm_loadu_ps( a.m_M[2] ); + __m128 a414 = _mm_loadu_ps( a.m_M[3] ); + + _MM_TRANSPOSE4_PS(t114, t214, t314, t414); + + __m128 o11 = _mm_mul_ps( a114, t114 ); + __m128 o12 = _mm_mul_ps( a114, t214 ); + __m128 o13 = _mm_mul_ps( a114, t314 ); + __m128 o14 = _mm_mul_ps( a114, t414 ); + + __m128 o21 = _mm_mul_ps( a214, t114 ); + __m128 o22 = _mm_mul_ps( a214, t214 ); + __m128 o23 = _mm_mul_ps( a214, t314 ); + __m128 o24 = _mm_mul_ps( a214, t414 ); + + __m128 o31 = _mm_mul_ps( a314, t114 ); + __m128 o32 = _mm_mul_ps( a314, t214 ); + __m128 o33 = _mm_mul_ps( a314, t314 ); + __m128 o34 = _mm_mul_ps( a314, t414 ); + + __m128 o41 = _mm_mul_ps( a414, t114 ); + __m128 o42 = _mm_mul_ps( a414, t214 ); + __m128 o43 = _mm_mul_ps( a414, t314 ); + __m128 o44 = _mm_mul_ps( a414, t414 ); + + + __m128 _11 = _mm_add_ss(o11,_mm_add_ss(_mm_shuffle_ps(o11, o11, 1),_mm_add_ss(_mm_shuffle_ps(o11, o11, 2),_mm_shuffle_ps(o11, o11, 3)))); + __m128 _12 = _mm_add_ss(o12,_mm_add_ss(_mm_shuffle_ps(o12, o12, 1),_mm_add_ss(_mm_shuffle_ps(o12, o12, 2),_mm_shuffle_ps(o12, o12, 3)))); + __m128 _13 = _mm_add_ss(o13,_mm_add_ss(_mm_shuffle_ps(o13, o13, 1),_mm_add_ss(_mm_shuffle_ps(o13, o13, 2),_mm_shuffle_ps(o13, o13, 3)))); + __m128 _14 = _mm_add_ss(o14,_mm_add_ss(_mm_shuffle_ps(o14, o14, 1),_mm_add_ss(_mm_shuffle_ps(o14, o14, 2),_mm_shuffle_ps(o14, o14, 3)))); + + __m128 _21 = _mm_add_ss(o21,_mm_add_ss(_mm_shuffle_ps(o21, o21, 1),_mm_add_ss(_mm_shuffle_ps(o21, o21, 2),_mm_shuffle_ps(o21, o21, 3)))); + __m128 _22 = _mm_add_ss(o22,_mm_add_ss(_mm_shuffle_ps(o22, o22, 1),_mm_add_ss(_mm_shuffle_ps(o22, o22, 2),_mm_shuffle_ps(o22, o22, 3)))); + __m128 _23 = _mm_add_ss(o23,_mm_add_ss(_mm_shuffle_ps(o23, o23, 1),_mm_add_ss(_mm_shuffle_ps(o23, o23, 2),_mm_shuffle_ps(o23, o23, 3)))); + __m128 _24 = _mm_add_ss(o24,_mm_add_ss(_mm_shuffle_ps(o24, o24, 1),_mm_add_ss(_mm_shuffle_ps(o24, o24, 2),_mm_shuffle_ps(o24, o24, 3)))); + + __m128 _31 = _mm_add_ss(o31,_mm_add_ss(_mm_shuffle_ps(o31, o31, 1),_mm_add_ss(_mm_shuffle_ps(o31, o31, 2),_mm_shuffle_ps(o31, o31, 3)))); + __m128 _32 = _mm_add_ss(o32,_mm_add_ss(_mm_shuffle_ps(o32, o32, 1),_mm_add_ss(_mm_shuffle_ps(o32, o32, 2),_mm_shuffle_ps(o32, o32, 3)))); + __m128 _33 = _mm_add_ss(o33,_mm_add_ss(_mm_shuffle_ps(o33, o33, 1),_mm_add_ss(_mm_shuffle_ps(o33, o33, 2),_mm_shuffle_ps(o33, o33, 3)))); + __m128 _34 = _mm_add_ss(o34,_mm_add_ss(_mm_shuffle_ps(o34, o34, 1),_mm_add_ss(_mm_shuffle_ps(o34, o34, 2),_mm_shuffle_ps(o34, o34, 3)))); + + __m128 _41 = _mm_add_ss(o41,_mm_add_ss(_mm_shuffle_ps(o41, o41, 1),_mm_add_ss(_mm_shuffle_ps(o41, o41, 2),_mm_shuffle_ps(o41, o41, 3)))); + __m128 _42 = _mm_add_ss(o42,_mm_add_ss(_mm_shuffle_ps(o42, o42, 1),_mm_add_ss(_mm_shuffle_ps(o42, o42, 2),_mm_shuffle_ps(o42, o42, 3)))); + __m128 _43 = _mm_add_ss(o43,_mm_add_ss(_mm_shuffle_ps(o43, o43, 1),_mm_add_ss(_mm_shuffle_ps(o43, o43, 2),_mm_shuffle_ps(o43, o43, 3)))); + __m128 _44 = _mm_add_ss(o44,_mm_add_ss(_mm_shuffle_ps(o44, o44, 1),_mm_add_ss(_mm_shuffle_ps(o44, o44, 2),_mm_shuffle_ps(o44, o44, 3)))); + + _mm_store_ss(&out._11, _11); + _mm_store_ss(&out._12, _12); + _mm_store_ss(&out._13, _13); + _mm_store_ss(&out._14, _14); + + _mm_store_ss(&out._21, _21); + _mm_store_ss(&out._22, _22); + _mm_store_ss(&out._23, _23); + _mm_store_ss(&out._24, _24); + + _mm_store_ss(&out._31, _31); + _mm_store_ss(&out._32, _32); + _mm_store_ss(&out._33, _33); + _mm_store_ss(&out._34, _34); + + _mm_store_ss(&out._41, _41); + _mm_store_ss(&out._42, _42); + _mm_store_ss(&out._43, _43); + _mm_store_ss(&out._44, _44); + +/* Matrix4x4 testout; + testout(0,0) = a(0,0) * b(0,0) + a(0,1) * b(1,0) + a(0,2) * b(2,0) + a(0,3) * b(3,0); + testout(0,1) = a(0,0) * b(0,1) + a(0,1) * b(1,1) + a(0,2) * b(2,1) + a(0,3) * b(3,1); + testout(0,2) = a(0,0) * b(0,2) + a(0,1) * b(1,2) + a(0,2) * b(2,2) + a(0,3) * b(3,2); + testout(0,3) = a(0,0) * b(0,3) + a(0,1) * b(1,3) + a(0,2) * b(2,3) + a(0,3) * b(3,3); + + testout(1,0) = a(1,0) * b(0,0) + a(1,1) * b(1,0) + a(1,2) * b(2,0) + a(1,3) * b(3,0); + testout(1,1) = a(1,0) * b(0,1) + a(1,1) * b(1,1) + a(1,2) * b(2,1) + a(1,3) * b(3,1); + testout(1,2) = a(1,0) * b(0,2) + a(1,1) * b(1,2) + a(1,2) * b(2,2) + a(1,3) * b(3,2); + testout(1,3) = a(1,0) * b(0,3) + a(1,1) * b(1,3) + a(1,2) * b(2,3) + a(1,3) * b(3,3); + + testout(2,0) = a(2,0) * b(0,0) + a(2,1) * b(1,0) + a(2,2) * b(2,0) + a(2,3) * b(3,0); + testout(2,1) = a(2,0) * b(0,1) + a(2,1) * b(1,1) + a(2,2) * b(2,1) + a(2,3) * b(3,1); + testout(2,2) = a(2,0) * b(0,2) + a(2,1) * b(1,2) + a(2,2) * b(2,2) + a(2,3) * b(3,2); + testout(2,3) = a(2,0) * b(0,3) + a(2,1) * b(1,3) + a(2,2) * b(2,3) + a(2,3) * b(3,3); + + testout(3,0) = a(3,0) * b(0,0) + a(3,1) * b(1,0) + a(3,2) * b(2,0) + a(3,3) * b(3,0); + testout(3,1) = a(3,0) * b(0,1) + a(3,1) * b(1,1) + a(3,2) * b(2,1) + a(3,3) * b(3,1); + testout(3,2) = a(3,0) * b(0,2) + a(3,1) * b(1,2) + a(3,2) * b(2,2) + a(3,3) * b(3,2); + testout(3,3) = a(3,0) * b(0,3) + a(3,1) * b(1,3) + a(3,2) * b(2,3) + a(3,3) * b(3,3);*/ +} +#else +inline void MatrixMult( Matrix4x4 &out, Matrix4x4 const &a, Matrix4x4 const &b ) +{ + out(0,0) = a(0,0) * b(0,0) + a(0,1) * b(1,0) + a(0,2) * b(2,0) + a(0,3) * b(3,0); + out(0,1) = a(0,0) * b(0,1) + a(0,1) * b(1,1) + a(0,2) * b(2,1) + a(0,3) * b(3,1); + out(0,2) = a(0,0) * b(0,2) + a(0,1) * b(1,2) + a(0,2) * b(2,2) + a(0,3) * b(3,2); + out(0,3) = a(0,0) * b(0,3) + a(0,1) * b(1,3) + a(0,2) * b(2,3) + a(0,3) * b(3,3); + + out(1,0) = a(1,0) * b(0,0) + a(1,1) * b(1,0) + a(1,2) * b(2,0) + a(1,3) * b(3,0); + out(1,1) = a(1,0) * b(0,1) + a(1,1) * b(1,1) + a(1,2) * b(2,1) + a(1,3) * b(3,1); + out(1,2) = a(1,0) * b(0,2) + a(1,1) * b(1,2) + a(1,2) * b(2,2) + a(1,3) * b(3,2); + out(1,3) = a(1,0) * b(0,3) + a(1,1) * b(1,3) + a(1,2) * b(2,3) + a(1,3) * b(3,3); + + out(2,0) = a(2,0) * b(0,0) + a(2,1) * b(1,0) + a(2,2) * b(2,0) + a(2,3) * b(3,0); + out(2,1) = a(2,0) * b(0,1) + a(2,1) * b(1,1) + a(2,2) * b(2,1) + a(2,3) * b(3,1); + out(2,2) = a(2,0) * b(0,2) + a(2,1) * b(1,2) + a(2,2) * b(2,2) + a(2,3) * b(3,2); + out(2,3) = a(2,0) * b(0,3) + a(2,1) * b(1,3) + a(2,2) * b(2,3) + a(2,3) * b(3,3); + + out(3,0) = a(3,0) * b(0,0) + a(3,1) * b(1,0) + a(3,2) * b(2,0) + a(3,3) * b(3,0); + out(3,1) = a(3,0) * b(0,1) + a(3,1) * b(1,1) + a(3,2) * b(2,1) + a(3,3) * b(3,1); + out(3,2) = a(3,0) * b(0,2) + a(3,1) * b(1,2) + a(3,2) * b(2,2) + a(3,3) * b(3,2); + out(3,3) = a(3,0) * b(0,3) + a(3,1) * b(1,3) + a(3,2) * b(2,3) + a(3,3) * b(3,3); +} +#endif + +inline void MatrixAdd( Matrix4x4 &out, Matrix4x4 const &a, Matrix4x4 const &b ) +{ + for (int i=0; i<4; i++) + { + for (int j=0; j<4; j++) + { + out(i,j) = a(i,j) + b(i,j); + } + } +} + +inline Matrix4x4 operator*( Matrix4x4 const &a, Matrix4x4 const &b ) +{ + Matrix4x4 temp; + MatrixMult( temp, a, b ); + return temp; +} + +inline Matrix4x4 operator+( Matrix4x4 const &a, Matrix4x4 const &b ) +{ + Matrix4x4 temp; + MatrixAdd( temp, a, b ); + return temp; +} + +#if 0//defined(USE_INTRINSICS) +inline void MatrixMult( Vector3 &out, Matrix4x4 const &a, Vector3 const &b ) +{ + __m128 a114 = _mm_loadu_ps( a.m_M[0] ); + __m128 a214 = _mm_loadu_ps( a.m_M[1] ); + __m128 a314 = _mm_loadu_ps( a.m_M[2] ); + __m128 a414 = _mm_loadu_ps( a.m_M[3] ); + __m128 b114 = _mm_loadu_ps( b.m_Vec ); + + _MM_TRANSPOSE4_PS(a114, a214, a314, a414); + + __m128 o11 = _mm_mul_ps( a114, b114 ); + __m128 o12 = _mm_mul_ps( a214, b114 ); + __m128 o13 = _mm_mul_ps( a314, b114 ); + + __m128 _11 = _mm_add_ss(o11,_mm_add_ss(_mm_shuffle_ps(o11, o11, 1),_mm_add_ss(_mm_shuffle_ps(o11, o11, 2),_mm_shuffle_ps(o11, o11, 3)))); + __m128 _12 = _mm_add_ss(o12,_mm_add_ss(_mm_shuffle_ps(o12, o12, 1),_mm_add_ss(_mm_shuffle_ps(o12, o12, 2),_mm_shuffle_ps(o12, o12, 3)))); + __m128 _13 = _mm_add_ss(o13,_mm_add_ss(_mm_shuffle_ps(o13, o13, 1),_mm_add_ss(_mm_shuffle_ps(o13, o13, 2),_mm_shuffle_ps(o13, o13, 3)))); + +} +#else +inline void MatrixMult( Vector3 &out, Matrix4x4 const &a, Vector3 const &b ) +{ +// out.x = a(0,0) * b.x + a(1,0) * b.y + a(2,0) * b.z + a(3,0); +// out.y = a(0,1) * b.x + a(1,1) * b.y + a(2,1) * b.z + a(3,1); +// out.z = a(0,2) * b.x + a(1,2) * b.y + a(2,2) * b.z + a(3,2); + out.x = a._11 * b.x + a._21 * b.y + a._31 * b.z + a._41; + out.y = a._12 * b.x + a._22 * b.y + a._32 * b.z + a._42; + out.z = a._13 * b.x + a._23 * b.y + a._33 * b.z + a._43; +} +#endif + + +inline Vector3 operator*( Matrix4x4 const &a, Vector3 const &b ) +{ + Vector3 temp; + MatrixMult( temp, a, b ); + return temp; +} + +inline void MatrixMult( Plane &out, Matrix4x4 const &a, Plane const &b ) +{ + Vector3 const &n = b.m_Normal; + Vector3 const &o = out.m_Normal; + float const d = b.m_Distance; + + out.m_Normal.x = a._11 * n.x + a._21 * n.y + a._31 * n.z; + out.m_Normal.y = a._12 * n.x + a._22 * n.y + a._32 * n.z; + out.m_Normal.z = a._13 * n.x + a._23 * n.y + a._33 * n.z; + out.m_Distance =-a._41 * o.x - a._42 * o.y - a._43 * o.z - d; +// out.m_Distance = -b.m_Normal.x*M[3][0] - P.b*M[3][1] - P.c*M[3][2] - P.d +} + +inline Plane operator*( Matrix4x4 const &a, Plane const &b ) +{ + Plane temp; + MatrixMult( temp, a, b ); + return temp; +} + + +void MatrixInvert( Matrix4x4 &out, Matrix4x4 const &m ); + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/plane.h b/Utils/Libs/Maths/plane.h new file mode 100644 index 000000000..ba0a71491 --- /dev/null +++ b/Utils/Libs/Maths/plane.h @@ -0,0 +1,53 @@ +#ifndef __PLANE_H__ +#define __PLANE_H__ + +#include "mathtypes.h" +#include "vector3.h" + + +struct Plane +{ + Vector3 m_Normal; + real m_Distance; + + inline real GetDistance( Vector3 const &pnt ) const; + inline void Normalise( void ); + + inline void SetPlane( Vector3 const &p0, Vector3 const &p1 ); + inline void SetPlane( Vector3 const &p0, Vector3 const &p1, Vector3 const &p2 ); +}; + +inline real Plane::GetDistance( Vector3 const &pnt ) const +{ + real v = (m_Normal * pnt) + m_Distance; + return v; +} + +inline void Plane::Normalise( void ) +{ + real mag = m_Normal.GetLength(); + real oneomag = 1.f / mag; + + m_Normal.x *= oneomag; + m_Normal.y *= oneomag; + m_Normal.z *= oneomag; + m_Distance *= oneomag; +} + +inline void Plane::SetPlane( Vector3 const &v0, Vector3 const &v1 ) +{ + m_Normal = v0 ^ v1; + m_Normal.Normalise(); + m_Distance = 0.f; +} + +inline void Plane::SetPlane( Vector3 const &p0, Vector3 const &p1, Vector3 const &p2 ) +{ + Vector3 v0 = (p1 - p0); + Vector3 v1 = (p2 - p0); + m_Normal = v0 ^ v1; + m_Normal.Normalise(); + m_Distance = -(p0 * m_Normal); +} + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/quat.cpp b/Utils/Libs/Maths/quat.cpp new file mode 100644 index 000000000..b68e1dc80 --- /dev/null +++ b/Utils/Libs/Maths/quat.cpp @@ -0,0 +1,117 @@ +#include "mathtypes.h" +#include "quat.h" + +#include "matrix4x4.h" +#include "axisangle.h" + +void Quaternion::ToMatrix( Matrix4x4 &m ) const +{ + real tx = x * 2.f; + real ty = y * 2.f; + real tz = z * 2.f; + real txx = (tx * x); + real tyx = (ty * x); + real tzx = (tz * x); + real tyy = (ty * y); + real tzy = (tz * y); + real tzz = (tz * z); + real txw = (tx * w); + real tyw = (ty * w); + real tzw = (tz * w); + + m._11 = 1.f - (tyy + tzz); + m._21 = tyx - tzw; + m._31 = tzx + tyw; + m._12 = tyx + tzw; + m._22 = 1.f - (txx + tzz); + m._32 = tzy - txw; + m._13 = tzx - tyw; + m._23 = tzy + txw; + m._33 = 1.f - (txx + tyy); +} + +void Quaternion::ToInverseMatrix( Matrix4x4 &m ) const +{ + real tx = -x * 2.f; + real ty = -y * 2.f; + real tz = -z * 2.f; + real txx = (tx * -x); + real tyx = (ty * -x); + real tzx = (tz * -x); + real tyy = (ty * -y); + real tzy = (tz * -y); + real tzz = (tz * -z); + real txw = (tx * w); + real tyw = (ty * w); + real tzw = (tz * w); + + m._11 = 1.f - (tyy + tzz); + m._21 = tyx - tzw; + m._31 = tzx + tyw; + m._12 = tyx + tzw; + m._22 = 1.f - (txx + tzz); + m._32 = tzy - txw; + m._13 = tzx - tyw; + m._23 = tzy + txw; + m._33 = 1.f - (txx + tyy); +} + + +void Quaternion::ToAxisAngle( AxisAngle & a ) const +{ + real halfang = (real)acos(w); + real angle = 2.f * halfang; + real sa = (real)sin(angle); + real oosa = 1.f/sa; + + a.x = x * oosa; + a.y = y * oosa; + a.z = z * oosa; + a.angle = angle; +} + + +void Squad( Quaternion &out, Quaternion const &a, Quaternion const &i0, Quaternion const &i1, Quaternion const &b, real t ) +{ + Quaternion res0; + Quaternion res1; + + Slerp( res0, a, b, t ); + Slerp( res1, i0, i1, t ); + Slerp( out, res0, res1, 2.f * t * (1.f - t)); +} + +void InnerQuadPoint( Quaternion &out, Quaternion const &a0, Quaternion const &a1, Quaternion const &a2 ) +{ + + Quaternion inva0 = a0; + Quaternion inva1 = a1; + + inva0.UnitInverse(); + inva1.UnitInverse(); + + Quaternion inva0a1 = inva0*a1; + Quaternion inva1a2 = inva1*a2; + + inva0a1.Log(); + inva1a2.Log(); + + inva0a1 += inva1a2; + + inva0a1 /= -4.f; + + inva0a1.Exp(); + + out = a1 * inva0a1; +} + +void CubicInterp( Quaternion &out, Quaternion const &a0, Quaternion const &a1, Quaternion const &a2, Quaternion const &a3, real t ) +{ + Quaternion i0; + Quaternion i1; + + InnerQuadPoint( i0, a0, a1, a2 ); + InnerQuadPoint( i1, a1, a2, a3 ); + + Squad( out, a0, i0, i1, a3, t ); +} diff --git a/Utils/Libs/Maths/quat.h b/Utils/Libs/Maths/quat.h new file mode 100644 index 000000000..face15864 --- /dev/null +++ b/Utils/Libs/Maths/quat.h @@ -0,0 +1,279 @@ +#ifndef __QUAT_H__ +#define __QUAT_H__ + +#include "mathtypes.h" +#include "math.h" + +struct Matrix4x4; +class AxisAngle; + +class Quaternion +{ +public: + union + { + real m_Vec[4]; + + struct + { + real x, y, z, w; + }; + }; + + void ToMatrix( Matrix4x4 &m ) const; + void ToInverseMatrix( Matrix4x4 &m ) const; + void ToAxisAngle( AxisAngle & a) const; + + inline real SquareMagnitude( void ) const; + inline real Magnitude( void ) const; + inline void Normalise( void ); + inline void Inverse( void ); + inline void UnitInverse( void ); + inline void Negate( void ); + inline void Log( void ); + inline void Exp( void ); + + inline void Identity( void ); + + inline real &operator[](int idx); + inline real const &operator[](int idx) const; + + inline Quaternion& operator*=( Quaternion const &b ); + inline Quaternion& operator+=( Quaternion const &b ); + inline Quaternion& operator/=( float const b ); + + inline friend Quaternion operator*( Quaternion const &a, Quaternion const &b ); + inline friend real DotProduct( Quaternion const &a, Quaternion const &b ); +}; + + + + +real Quaternion::SquareMagnitude( void ) const +{ + real magsq = x*x + y*y + z*z + w*w; + + return magsq; +} + +real Quaternion::Magnitude( void ) const +{ + real magsq = x*x + y*y + z*z + w*w; + + if (magsq == 0.0f) + { + return 0.f; + } else + { + real mag = (real)(sqrt( magsq )); + return mag; + } +} + +void Quaternion::Normalise( void ) +{ + float mag = Magnitude(); + float oomag; + if (mag != 0.0f) + { + oomag = 1.f / mag; + } else + { + oomag = 1.f; + } + x*=oomag; + y*=oomag; + z*=oomag; + w*=oomag; +} + +void Quaternion::Inverse( void ) +{ + float d=SquareMagnitude(); + float ood; + if (d != 0.0f) + { + ood = 1.f / d; + } else + { + ood = 1.f; + } + + x=-x*ood; + y=-y*ood; + z=-z*ood; + w*=ood; + +} + +void Quaternion::UnitInverse( void ) +{ + x = -x; + y = -y; + z = -z; +} + +void Quaternion::Negate( void ) +{ + float mag = Magnitude(); + float oomag; + if (mag != 0.0f) + { + oomag = 1.f / mag; + } else + { + oomag = 1.f; + } + + x*=-oomag; + y*=-oomag; + z*=-oomag; + w*=oomag; +} + +void Quaternion::Identity( void ) +{ + x = y = z = 0.f; + w = 1.f; +} + +void Quaternion::Log( void ) +{ + float d=(float)sqrt(x*x+y*y+z*z); + if (w!=0.0f) + { + d=atanf(d/w); + } else + { + d=3.1479f*2.0f; + } + + x *= d; + y *= d; + z *= d; + w = 0.f; +} + + +void Quaternion::Exp( void ) +{ + float d=(float)sqrt(x*x+y*y+z*z); + float d1; + + if (d>0.0f) + { + d1=sinf(d)/d; + } else + { + d1=1.0f; + } + + x *= d1; + y *= d1; + z *= d1; + w = cosf( d ); +} + + +real &Quaternion::operator[](int idx) +{ + return m_Vec[idx]; +} + +real const &Quaternion::operator[](int idx) const +{ + return m_Vec[idx]; +} + +inline void QuaternionMult(Quaternion &out, Quaternion const &a, Quaternion const &b) +{ + out.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; + out.y = a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z; + out.z = a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x; + out.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; +} + +Quaternion& Quaternion::operator*=(Quaternion const &other) +{ + Quaternion copy = *this; + + QuaternionMult( *this, copy, other); + return *this; +} + +Quaternion& Quaternion::operator+=( Quaternion const &b ) +{ + x += b.x; + y += b.y; + z += b.z; + w += b.w; + + return *this; +} + +Quaternion& Quaternion::operator/=( float const b ) +{ + x /= b; + y /= b; + z /= b; + w /= b; + + return *this; +} + + +Quaternion operator*(Quaternion const &a, Quaternion const &b) +{ + Quaternion out; + QuaternionMult( out, a, b); + return out; +} + +inline real DotProduct( Quaternion const &a, Quaternion const &b ) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + + +inline void Slerp( Quaternion &out, Quaternion const &a, Quaternion const &b, real t) +{ + real ca = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; + real flip; + if (ca<0.f) + { + ca = -ca; + flip = -1.f; + } else + { + flip = 1.f; + } + + + real c0; + real c1; + if ( ca > 0.9999f ) + { + c0 = 1.f - t; + c1 = t * flip; + } else + { + real angle = (real)acos( ca ); + real sa = (real)sin( angle ); + real oosa = 1.f / sa; + c0 = (real)(sin( (1.f - t) * angle ) * oosa); + c1 = (real)(sin( t * angle ) * oosa) * flip; + } + + out.x = c0 * a.x + c1 * b.x; + out.y = c0 * a.y + c1 * b.y; + out.z = c0 * a.z + c1 * b.z; + out.w = c0 * a.w + c1 * b.w; +} + +void Squad( Quaternion &out, Quaternion const &a, Quaternion const &i0, Quaternion const &i1, Quaternion const &b, real t ); + +void InnerQuadPoint( Quaternion &out, Quaternion const &a0, Quaternion const &a1, Quaternion const &a2 ); + +void CubicInterp( Quaternion &out, Quaternion const &a0, Quaternion const &a1, Quaternion const &a2, Quaternion const &a3, real t ); + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/spline.h b/Utils/Libs/Maths/spline.h new file mode 100644 index 000000000..b9ac087cf --- /dev/null +++ b/Utils/Libs/Maths/spline.h @@ -0,0 +1,60 @@ +#ifndef __SPLINE_H__ +#define __SPLINE_H__ + +#include "basetypes.h" + +template +void spline(real x, int nknots, T *knot) +{ + const float cr[16]={-0.5, 1.5, -1.5, 0.5, + 1.0, -2.5, 2.0, -0.5, + -0.5, 0.0, 0.5, 0.0, + 0.0, 1.0, 0.0, 0.0}; + + int nspans=nknots-3; + + if (nspans<1) + { + ASSERT("spline has to few knots"); + return 0; + } + + if (x<0) x=0; + if (x>1) x=1; + x*=nspans; + + int span=(int)x; + if (span>=nknots-3) + span=nknots-3; + + x-=span; + knot+=span; + + float c3 = cr[0]*knot[0] + cr[1]*knot[1] + cr[2]*knot[2] + cr[3]*knot[3]; + float c2 = cr[4]*knot[0] + cr[5]*knot[1] + cr[6]*knot[2] + cr[7]*knot[3]; + float c1 = cr[8]*knot[0] + cr[9]*knot[1] + cr[10]*knot[2] + cr[11]*knot[3]; + float c0 = cr[12]*knot[0] + cr[13]*knot[1] + cr[14]*knot[2] + cr[15]*knot[3]; + + return ((c3*x + c2)*x + c1)*x + c0; +} + +template +void hermite(real x, T const &k0, T const &k1, T const &k2, T const &k3, T &out ) +{ + // x of 0 = k1 + // x of 1 = k2 + + T g0 = (k2 - k0) * 0.5f; + T g1 = (k3 - k1) * 0.5f; + + real x2 = x * x; + real x3 = x2 * x; + real c0 = 2 * x3 - 3 * x2 + 1.f; + real c1 = -2 * x3 + 3 * x2; + real c2 = x3 - 2*x2 + x; + real c3 = x3 - x2; + out = (c0 * k1) + (c1 * k2) + (c2 * g0) + (c3 * g1); +} + + +#endif \ No newline at end of file diff --git a/Utils/Libs/Maths/vector3.h b/Utils/Libs/Maths/vector3.h new file mode 100644 index 000000000..a3283da56 --- /dev/null +++ b/Utils/Libs/Maths/vector3.h @@ -0,0 +1,206 @@ +#ifndef __VECTOR3_H__ +#define __VECTOR3_H__ + +#include "mathtypes.h" +#include "math.h" + +//AF - added this to suppress warning for nonstandard [unnamed union] +#pragma warning(push) +#pragma warning(disable:4201) + +// there should be _NO_ constructor +struct Vector3 +{ + union + { + real m_Vec[3]; + + struct + { + real x, y, z; + }; + }; + + inline bool operator==( Vector3 const &other ); + inline Vector3 & operator+=( Vector3 const &other ); + inline Vector3 & operator-=( Vector3 const &other ); + + inline Vector3 & operator/=( real val ); + inline Vector3 & operator*=( real val ); + + inline real &operator[](int idx); + inline real const &operator[](int idx) const; + + + inline real GetLength( void ) const; + inline real GetLengthSquared( void ) const; + inline void Normalise( void ); + inline void Zero(void); //AF - hope mike don't mind me fiddling with his bits... + + + friend inline real operator*( Vector3 const &a, Vector3 const &b ); // dot product + friend inline Vector3 operator^( Vector3 const &a, Vector3 const &b ); // cross product + friend inline Vector3 operator+( Vector3 const &a, Vector3 const &b ); + friend inline Vector3 operator-( Vector3 const &a, Vector3 const &b ); + friend inline Vector3 operator*( Vector3 const &a, real val ); + friend inline Vector3 operator/( Vector3 const &a, real val ); + friend inline Vector3 operator*( real val, Vector3 const &a ); + friend inline Vector3 operator/( real val, Vector3 const &a ); + +}; + +#pragma warning(pop) +//AF - warning no longer suppressed + +inline bool Vector3::operator==( Vector3 const &other ) +{ + return (x==other.x && y==other.y && z==other.z); +} + +inline Vector3 & Vector3::operator+=( Vector3 const &other ) +{ + x += other.x; + y += other.y; + z += other.z; + return *this; +} + +inline Vector3 & Vector3::operator-=( Vector3 const &other ) +{ + x -= other.x; + y -= other.y; + z -= other.z; + return *this; +} + +inline Vector3 & Vector3::operator/=( real val ) +{ + float temp = 1.f / val; + x *= temp; + y *= temp; + z *= temp; + return *this; +} + +inline Vector3 & Vector3::operator*=( real val ) +{ + x *= val; + y *= val; + z *= val; + return *this; +} + +inline real Vector3::GetLength( void ) const +{ + real sqmag = x * x + y * y + z * z; + real mag = (real)sqrt( sqmag ); + return mag; +} + +inline real Vector3::GetLengthSquared( void ) const +{ + real sqmag = x * x + y * y + z * z; + return sqmag; +} + +inline void Vector3::Normalise( void ) +{ + real mag = GetLength(); + real oneomag = 1.f / mag; + x *= oneomag; + y *= oneomag; + z *= oneomag; +} + +inline void Vector3::Zero(void) +{ + x=0; + y=0; + z=0; +} + +inline real &Vector3::operator[](int idx) +{ + return m_Vec[idx]; +} + +inline real const &Vector3::operator[](int idx) const +{ + return m_Vec[idx]; +} + + + +inline real operator*( Vector3 const &a, Vector3 const &b ) // dot product, assumes +{ + real tot = a.x * b.x + a.y * b.y + a.z * b.z; + return tot; +} + + +inline Vector3 operator^( Vector3 const &a, Vector3 const &b ) // cross product +{ + Vector3 temp; + temp.x = a.y * b.z - a.z * b.y; + temp.y = a.z * b.x - a.x * b.z; + temp.z = a.x * b.y - a.y * b.x; + return temp; +} + +inline Vector3 operator+( Vector3 const &a, Vector3 const &b ) +{ + Vector3 temp; + temp.x = a.x + b.x; + temp.y = a.y + b.y; + temp.z = a.z + b.z; + return temp; +} + +inline Vector3 operator-( Vector3 const &a, Vector3 const &b ) +{ + Vector3 temp; + temp.x = a.x - b.x; + temp.y = a.y - b.y; + temp.z = a.z - b.z; + return temp; +} + +inline Vector3 operator*( Vector3 const &a, real val ) +{ + Vector3 temp; + temp.x = a.x * val; + temp.y = a.y * val; + temp.z = a.z * val; + return temp; +} + +inline Vector3 operator/( Vector3 const &a, real val ) +{ + Vector3 temp; + float oneoval = 1.f / val; + temp.x = a.x * oneoval; + temp.y = a.y * oneoval; + temp.z = a.z * oneoval; + return temp; +} + +inline Vector3 operator*( real val, Vector3 const &a ) +{ + Vector3 temp; + temp.x = a.x * val; + temp.y = a.y * val; + temp.z = a.z * val; + return temp; +} + +inline Vector3 operator/( real val, Vector3 const &a ) +{ + Vector3 temp; + float oneoval = 1.f / val; + temp.x = a.x * oneoval; + temp.y = a.y * oneoval; + temp.z = a.z * oneoval; + return temp; +} + +#endif \ No newline at end of file