Post by microfarad on Jul 21, 2012 0:30:39 GMT -5
Work with matrices and vectors for 3D graphics using KMAT.h
/*
Contains functions for the manipulation of matrices and vectors
Intended for use in 3D graphics
Copyright 2012 Kyle Gagner
Not to be distributed without permission of author. Permission granted to use for personal use only.
*/
/*
Y axis up
X axis right
Z axis out of screen, not in
Matrices are 16 element arrays
[0 1 2 3 ]
[4 5 6 7 ]
[8 9 10 11]
[12 13 14 15]
Vectors are 3 element arrays, but respresent homogeneous coordinates
w coordinate is always assumed to be equal to 1
[1]
[2]
[3]
[w]
Remember, matrix multiplication is associative but not commutative
multiplying in the fashion (AB)C and A(BC) will give the same result
Transform first by A, then B, then C
But (BA)C will transform by B first, then A, then C
*/
//3D vector operations
void KMAT_Vcopy(double *A, double *B) // B = A
{
int n;
for(n=0;n<3;n++)
{
B[n]=A[n];
}
}
double KMAT_Vdot(double *A, double *B) // return A dot B
{
int n;
double v=0;
for(n=0;n<3;n++)
{
v+=A[n]*B[n];
}
return v;
}
void KMAT_Vcross(double *A, double *B, double *C) // C = A x B
{
int n;
for(n=0;n<3;n++)
{
C[n]=(A[(1+n)%3]*B[(2+n)%3])-(A[(2+n)%3]*B[(1+n)%3]);
}
}
void KMAT_Vadd(double *A, double *B, double *C) // C = A + B
{
int n;
for(n=0;n<3;n++)
{
C[n]=A[n]+B[n];
}
}
void KMAT_Vsub(double *A, double *B, double *C) // C = A - B
{
int n;
for(n=0;n<3;n++)
{
C[n]=A[n]-B[n];
}
}
void KMAT_Vscale(double scale, double *A, double *B) // B = A * scale
{
int n;
for(n=0;n<3;n++)
{
B[n]=A[n]*scale;
}
}
double KMAT_Vmag(double *A) // return magnitude of A
{
int n;
double v;
for(n=0;n<3;n++)
{
v+=pow(A[n],2);
}
return pow(v,.5);
}
void KMAT_Vunit(double *A, double *B) // B = A hat
{
double m;
m=1.0/KMAT_Vmag(A);
KMAT_Vscale(m,A,B);
}
void KMAT_Vtransform(double *matrix, double *A, double *B) // B = (matrix)A
{
int y;
int n;
double v;
double *At;
double *Bt;
At=malloc(4*sizeof(double));
Bt=malloc(4*sizeof(double));
for(n=0;n<3;n++)
{
At[n]=A[n];
}
At[3]=1;
for(y=0;y<4;y++)
{
v=0;
for(n=0;n<4;n++)
{
v+=At[n]*matrix[(4*y)+n];
}
Bt[y]=v;
}
for(n=0;n<3;n++)
{
B[n]=Bt[n]/Bt[3];
}
free(At);
free(Bt);
}
void KMAT_Vprint(double *A) // prints A
{
printf("<%f,%f,%f>\n",A[0],A[1],A[2]);
}
void KMAT_Vxyz(double x, double y, double z, double *A) // A = [x,y,z,1]
{
A[0]=x;
A[1]=y;
A[2]=z;
}
void KMAT_Vray(double *origin,double *dir,double t,double *A) // A = (dir)t + origin
{
double temp[3];
KMAT_Vscale(t,dir,temp);
KMAT_Vadd(origin,temp,A);
}
//4x4 matrix operations
void KMAT_Mcopy(double *A, double *B) // B = A
{
int n;
for(n=0;n<16;n++)
{
B[n]=A[n];
}
}
void KMAT_Mmultiply(double *A, double *B, double *C) // C = AB
{
int x;
int y;
int n;
double v;
for(x=0;x<4;x++)
{
for(y=0;y<4;y++)
{
v=0;
for(n=0;n<4;n++)
{
v+=A[n+(4*y)]*B[x+(4*n)];
}
C[x+(4*y)]=v;
}
}
}
void KMAT_Midentity(double *A) // A = [1 0 0 0][0 1 0 0][0 0 1 0][0 0 0 1]
{
int n;
for(n=0;n<16;n++)
{
A[n]=0;
}
for(n=0;n<4;n++)
{
A[5*n]=1;
}
}
//Axis rotations
void KMAT_MrotX(double angle, double *A)
{
KMAT_Midentity(A);
A[5]=cos(angle);
A[6]=-sin(angle);
A[9]=sin(angle);
A[10]=cos(angle);
}
void KMAT_MrotY(double angle, double *A)
{
KMAT_Midentity(A);
A[0]=cos(angle);
A[2]=sin(angle);
A[8]=-sin(angle);
A[10]=cos(angle);
}
void KMAT_MrotZ(double angle, double *A)
{
KMAT_Midentity(A);
A[0]=cos(angle);
A[1]=-sin(angle);
A[4]=sin(angle);
A[5]=cos(angle);
}
void KMAT_Mtranslate(double *vector, double *A) // Translation matrix
{
int n;
KMAT_Midentity(A);
for(n=0;n<3;n++)
{
A[3+(4*n)]=vector[n];
}
}
void KMAT_Mscale(double *vector, double *A) // Scaling matrix
{
int n;
KMAT_Midentity(A);
for(n=0;n<3;n++)
{
A[5*n]=vector[n];
}
}
void KMAT_Mobject(double *pos, double *look, double *up, double *right, double *A) // Positioning and orientation matrix
{
int n;
KMAT_Midentity(A);
for(n=0;n<3;n++)
{
A[n]=right[n];
A[n+4]=up[n];
A[n+8]=look[n];
A[n+12]=pos[n];
}
}
void KMAT_Mcamera(double *pos, double *look, double *up, double *right, double *A) // Camera transform, inverse of Mobject
{
int n;
KMAT_Midentity(A);
for(n=0;n<3;n++)
{
A[(4*n)]=right[n];
A[(4*n)+1]=up[n];
A[(4*n)+2]=look[n];
}
A[12]=-KMAT_Vdot(pos,right);
A[13]=-KMAT_Vdot(pos,up);
A[14]=-KMAT_Vdot(pos,look);
}
void KMAT_Mperspective(double *A,double top,double bottom,double left,double right,double near,double far) // Perspective transform
{
KMAT_Midentity(A);
A[0]=2.0/(right-left);
A[3]=(right+left)/(left-right);
A[5]=2.0/(top-bottom);
A[7]=(top+bottom)/(bottom-top);
A[10]=2.0/(far-near);
A[11]=(far+near)/(near-far);
A[14]=-1;
A[15]=0;
}
void KMAT_Mscreen(double *A,double width,double height)
{
KMAT_Midentity(A);
A[0]=width/2.0;
A[3]=width/2.0;
A[5]=-height/2.0;
A[7]=height/2.0;
}
void KMAT_Mtranspose(double *A, double *B) // Transpose matrix
{
int x;
int y;
for(x=0;x<4;x++)
{
for(y=0;y<4;y++)
{
B[(4*x)+y]=A[(4*y)+x];
}
}
}
void KMAT_Minvtrans(double *A, double *B) // Inverse transpose matrix
{
int x;
int y;
int xm;
int ym;
double *mat;
double *At;
double *Bt;
double *minors;
double det;
mat=malloc(9*sizeof(double));
minors=malloc(16*sizeof(double));
for(y=0;y<4;y++)
{
for(x=0;x<4;x++)
{
At=A;
for(ym=0;ym<4;ym++)
{
if(ym==y)
{
At+=4;
}
else
{
for(xm=0;xm<4;xm++)
{
if(!(xm==x))
{
*mat=*At;
mat+=1;
}
At+=1;
}
}
}
mat-=9;
*minors=(((*mat)*(*(mat+4))*(*(mat+8)))-((*mat)*(*(mat+5))*(*(mat+7)))-
((*(mat+1))*(*(mat+3))*(*(mat+8)))+((*(mat+1))*(*(mat+5))*(*(mat+6)))+
((*(mat+2))*(*(mat+3))*(*(mat+7)))-((*(mat+2))*(*(mat+4))*(*(mat+6))));
minors+=1;
}
}
minors-=16;
det=(((*A)*(*minors))-((*(A+1))*(*(minors+1)))+((*(A+2))*(*(minors+2)))-((*(A+3))*(*(minors+3))));
Bt=B;
for(y=0;y<4;y++)
{
for(x=0;x<4;x++)
{
*Bt=(*minors)*((-2*((x+y)%2))+1)/det;
Bt+=1;
minors+=1;
}
}
minors-=16;
free(minors);
}
void KMAT_Minverse(double *A,double *B) // Inverse matrix
{
double *temp;
temp=malloc(16*sizeof(double));
KMAT_Minvtrans(A,temp);
KMAT_Mtranspose(temp,B);
}
void KMAT_Mupper3(double *A,double *B) // Retrieve the upper left 3x3 matrix from a 4x4, fill the other spaces with 0s and the lower right with a 1
{
int x;
int y;
for(x=0;x<3;x++)
{
for(y=0;y<3;y++)
{
B[(4*y)+x]=A[(4*y)+x];
}
}
for(x=0;x<3;x++)
{
B[(4*y)+3]=0;
B[12+x]=0;
}
B[15]=1;
}
void KMAT_LUR(double *pos,double *at, double *above, double *look, double *up, double *right) // Useful for getting the vectors on a camera matrix
{
double *temp;
temp=malloc(6*sizeof(double));
KMAT_Vsub(at,pos,temp);
KMAT_Vunit(temp,look);
KMAT_Vunit(above,temp+3);
KMAT_Vcross(look,temp+3,right);
KMAT_Vcross(look,right,temp);
KMAT_Vscale(-1.0,temp,up);
free(temp);
}