zydronium/SBSPSS
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Browse source
This commit is contained in:
Daveo 2000-12-04 16:58:10 +00:00
parent 2a24119501
commit 79caf039ef
19 changed files with 3844 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

527
Utils/Libs/Maths/Decompose.c Normal file
View file

@ -0,0 +1,527 @@
/**** Decompose.c ****/
/* Ken Shoemake, 1993 */
#include <math.h>
#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<n;i++) for(j=0;j<n;j++)\
C[i][j] gets (A[i][j]);}
/** Copy transpose of nxn matrix A to C using "gets" for assignment **/
#define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
AT[i][j] gets (A[j][i]);}
/** Assign nxn matrix C the element-wise combination of A and B using "op" **/
#define mat_binop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
C[i][j] gets (A[i][j]) op (B[i][j]);}
/** Multiply the upper left 3x3 parts of A and B to get AB **/
void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
{
int i, j;
for (i=0; i<3; i++) for (j=0; j<3; j++)
AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
}
/** Return dot product of length 3 vectors va and vb **/
float vdot(float *va, float *vb)
{
return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
}
/** Set v to cross product of length 3 vectors va and vb **/
void vcross(float *va, float *vb, float *v)
{
v[0] = va[1]*vb[2] - va[2]*vb[1];
v[1] = va[2]*vb[0] - va[0]*vb[2];
v[2] = va[0]*vb[1] - va[1]*vb[0];
}
/** Set MadjT to transpose of inverse of M times determinant of M **/
void adjoint_transpose(HMatrix M, HMatrix MadjT)
{
vcross(M[1], M[2], MadjT[0]);
vcross(M[2], M[0], MadjT[1]);
vcross(M[0], M[1], MadjT[2]);
}
/******* Quaternion Preliminaries *******/
/* Construct a (possibly non-unit) quaternion from real components. */
Quat Qt_(float x, float y, float z, float w)
{
Quat qq;
qq.x = x; qq.y = y; qq.z = z; qq.w = w;
return (qq);
}
/* Return conjugate of quaternion. */
Quat Qt_Conj(Quat q)
{
Quat qq;
qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
return (qq);
}
/* Return quaternion product qL * qR. Note: order is important!
* To combine rotations, use the product Mul(qSecond, qFirst),
* which gives the effect of rotating by qFirst then qSecond. */
Quat Qt_Mul(Quat qL, Quat qR)
{
Quat qq;
qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
return (qq);
}
/* Return product of quaternion q by scalar w. */
Quat Qt_Scale(Quat q, float w)
{
Quat qq;
qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
return (qq);
}
/* Construct a unit quaternion from rotation matrix. Assumes matrix is
* used to multiply column vector on the left: vnew = mat vold. Works
* correctly for right-handed coordinate system and right-handed rotations.
* Translation and perspective components ignored. */
Quat Qt_FromMatrix(HMatrix mat)
{
/* This algorithm avoids near-zero divides by looking for a large component
* - first w, then x, y, or z. When the trace is greater than zero,
* |w| is greater than 1/2, which is as small as a largest component can be.
* Otherwise, the largest diagonal entry corresponds to the largest of |x|,
* |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
Quat qu;
register double tr, s;
tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
if (tr >= 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 (max<sum) max = sum;
}
return max;
}
float norm_inf(HMatrix M) {return mat_norm(M, 0);}
float norm_one(HMatrix M) {return mat_norm(M, 1);}
/** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
int find_max_col(HMatrix M)
{
float abs, max;
int i, j, col;
max = 0.0; col = -1;
for (i=0; i<3; i++) for (j=0; j<3; j++) {
abs = M[i][j]; if (abs<0.0) abs = -abs;
if (abs>max) {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)
{
}

604
Utils/Libs/Maths/Decompose.cpp Normal file
View file

@ -0,0 +1,604 @@
/**** Decompose.c ****/
/* Ken Shoemake, 1993 */
#include <math.h>
#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<n;i++) for(j=0;j<n;j++)\
C[i][j] gets (A[i][j]);}
/** Copy transpose of nxn matrix A to C using "gets" for assignment **/
#define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
AT[i][j] gets (A[j][i]);}
/** Assign nxn matrix C the element-wise combination of A and B using "op" **/
#define mat_binop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
C[i][j] gets (A[i][j]) op (B[i][j]);}
/** Multiply the upper left 3x3 parts of A and B to get AB **/
void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
{
int i, j;
for (i=0; i<3; i++) for (j=0; j<3; j++)
AB[i][j] = A[i][0]*B[0][j] + A[i][1]*B[1][j] + A[i][2]*B[2][j];
}
/** Return dot product of length 3 vectors va and vb **/
float vdot(float *va, float *vb)
{
return (va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]);
}
/** Set v to cross product of length 3 vectors va and vb **/
void vcross(float *va, float *vb, float *v)
{
v[0] = va[1]*vb[2] - va[2]*vb[1];
v[1] = va[2]*vb[0] - va[0]*vb[2];
v[2] = va[0]*vb[1] - va[1]*vb[0];
}
/** Set MadjT to transpose of inverse of M times determinant of M **/
void adjoint_transpose(HMatrix M, HMatrix MadjT)
{
vcross(M[1], M[2], MadjT[0]);
vcross(M[2], M[0], MadjT[1]);
vcross(M[0], M[1], MadjT[2]);
}
/******* Quaternion Preliminaries *******/
/* Construct a (possibly non-unit) quaternion from real components. */
Quat Qt_(float x, float y, float z, float w)
{
Quat qq;
qq.x = x; qq.y = y; qq.z = z; qq.w = w;
return (qq);
}
/* Return conjugate of quaternion. */
Quat Qt_Conj(Quat q)
{
Quat qq;
qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
return (qq);
}
/* Return quaternion product qL * qR. Note: order is important!
* To combine rotations, use the product Mul(qSecond, qFirst),
* which gives the effect of rotating by qFirst then qSecond. */
Quat Qt_Mul(Quat qL, Quat qR)
{
Quat qq;
qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
return (qq);
}
/* Return product of quaternion q by scalar w. */
Quat Qt_Scale(Quat q, float w)
{
Quat qq;
qq.w = q.w*w; qq.x = q.x*w; qq.y = q.y*w; qq.z = q.z*w;
return (qq);
}
/* Construct a unit quaternion from rotation matrix. Assumes matrix is
* used to multiply column vector on the left: vnew = mat vold. Works
* correctly for right-handed coordinate system and right-handed rotations.
* Translation and perspective components ignored. */
Quat Qt_FromMatrix(HMatrix mat)
{
/* This algorithm avoids near-zero divides by looking for a large component
* - first w, then x, y, or z. When the trace is greater than zero,
* |w| is greater than 1/2, which is as small as a largest component can be.
* Otherwise, the largest diagonal entry corresponds to the largest of |x|,
* |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
Quat qu;
register double tr, s;
tr = mat[X][X] + mat[Y][Y]+ mat[Z][Z];
if (tr >= 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 (max<sum) max = sum;
}
return max;
}
float norm_inf(HMatrix M) {return mat_norm(M, 0);}
float norm_one(HMatrix M) {return mat_norm(M, 1);}
/** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
int find_max_col(HMatrix M)
{
float abs, max;
int i, j, col;
max = 0.0; col = -1;
for (i=0; i<3; i++) for (j=0; j<3; j++) {
abs = M[i][j]; if (abs<0.0) abs = -abs;
if (abs>max) {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;
}

21
Utils/Libs/Maths/Decompose.h Normal file
View file

@ -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

124
Utils/Libs/Maths/MgcAppr3DLineFit.cpp Normal file
View file

@ -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;
}
//----------------------------------------------------------------------------

33
Utils/Libs/Maths/MgcAppr3DLineFit.h Normal file
View file

@ -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

689
Utils/Libs/Maths/MgcEigen.cpp Normal file
View file

@ -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

112
Utils/Libs/Maths/MgcEigen.h Normal file
View file

@ -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

29
Utils/Libs/Maths/aabox.h Normal file
View file

@ -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

41
Utils/Libs/Maths/axisangle.cpp Normal file
View file

@ -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;
}

22
Utils/Libs/Maths/axisangle.h Normal file
View file

@ -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

162
Utils/Libs/Maths/maths.dsp Normal file
View file

@ -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

54
Utils/Libs/Maths/mathtypes.h Normal file
View file

@ -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

221
Utils/Libs/Maths/matrix4x4.cpp Normal file
View file

@ -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 );
}

490
Utils/Libs/Maths/matrix4x4.h Normal file
View file

@ -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

53
Utils/Libs/Maths/plane.h Normal file
View file

@ -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

117
Utils/Libs/Maths/quat.cpp Normal file
View file

@ -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 );
}

279
Utils/Libs/Maths/quat.h Normal file
View file

@ -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

60
Utils/Libs/Maths/spline.h Normal file
View file

@ -0,0 +1,60 @@
#ifndef __SPLINE_H__
#define __SPLINE_H__
#include "basetypes.h"
template <class T>
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 <class T>
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

206
Utils/Libs/Maths/vector3.h Normal file
View file

@ -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