542 lines
11 KiB
C++
Executable File
542 lines
11 KiB
C++
Executable File
//vectors.cpp
|
|
//vector and matrix math functions
|
|
|
|
#include "vectors.h"
|
|
#include "gamemem.h"
|
|
#include <math.h>
|
|
//Add and Subtract Matrices
|
|
void MatrixAdd(tMatrix3 x,tMatrix3 y,tMatrix3 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
m[i][j]=x[i][j]+y[i][j];
|
|
}
|
|
|
|
void MatrixAdd(tMatrix4 x,tMatrix4 y,tMatrix4 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<4;i++)
|
|
for(j=0;j<4;j++)
|
|
m[i][j]=x[i][j]+y[i][j];
|
|
}
|
|
|
|
void MatrixSub(tMatrix3 x,tMatrix3 y,tMatrix3 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
m[i][j]=x[i][j]-y[i][j];
|
|
}
|
|
|
|
void MatrixSub(tMatrix4 x,tMatrix4 y,tMatrix4 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<4;i++)
|
|
for(j=0;j<4;j++)
|
|
m[i][j]=x[i][j]-y[i][j];
|
|
}
|
|
|
|
//Multiplay Matrices with Scalars
|
|
void MatrixMult(tMatrix3 x,float y,tMatrix3 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
m[i][j]=x[i][j]*y;
|
|
}
|
|
|
|
void MatrixMult(tMatrix4 x,float y,tMatrix4 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<4;i++)
|
|
for(j=0;j<4;j++)
|
|
m[i][j]=x[i][j]*y;
|
|
}
|
|
|
|
void MatrixMult(float y,tMatrix3 x,tMatrix3 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
m[i][j]=x[i][j]*y;
|
|
}
|
|
|
|
void MatrixMult(float y,tMatrix4 x,tMatrix4 m)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<4;i++)
|
|
for(j=0;j<4;j++)
|
|
m[i][j]=x[i][j]*y;
|
|
}
|
|
|
|
//Convert a 3x3 matrix to a 4x4 matrix
|
|
void Matrix3ToMatrix4(tMatrix3 x,tMatrix4 m)
|
|
{
|
|
m[0][0]=x[0][0];m[0][1]=x[0][1];m[0][2]=x[0][2];m[0][3]=0;
|
|
m[1][0]=x[1][0];m[1][1]=x[1][1];m[1][2]=x[1][2];m[1][3]=0;
|
|
m[2][0]=x[2][0];m[2][1]=x[2][1];m[2][2]=x[2][2];m[2][3]=0;
|
|
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
|
|
}
|
|
|
|
//Copy matrices
|
|
void MatrixCopy(tMatrix3 src,tMatrix3 dst)
|
|
{
|
|
MemoryMove(dst,src,sizeof(tMatrix3));
|
|
}
|
|
|
|
void MatrixCopy(tMatrix4 src,tMatrix4 dst)
|
|
{
|
|
MemoryMove(dst,src,sizeof(tMatrix4));
|
|
}
|
|
|
|
void MatrixCopy(tMatrix3 src,tMatrix4 dst)
|
|
{
|
|
Matrix3ToMatrix4(src,dst);
|
|
}
|
|
|
|
//Convert a vector of euler angles to a matrix
|
|
void EulerAnglesToMatrix(tVector3 euler,tMatrix3 dst)
|
|
{
|
|
MatrixIdentity(dst);
|
|
MatrixRotX(dst,euler.x);
|
|
MatrixRotY(dst,euler.y);
|
|
MatrixRotZ(dst,euler.z);
|
|
}
|
|
|
|
void EulerAnglesToMatrix(tVector3 euler,tMatrix4 dst)
|
|
{
|
|
MatrixIdentity(dst);
|
|
MatrixRotX(dst,euler.x);
|
|
MatrixRotY(dst,euler.y);
|
|
MatrixRotZ(dst,euler.z);
|
|
}
|
|
|
|
//Convert a matrix to a vector of euler angles
|
|
tVector3 MatrixToEulerAngles(tMatrix3 m)
|
|
{
|
|
tVector3 euler;
|
|
if(fabs(m[0][2])<=1)
|
|
euler.y=-asin(m[0][2]);
|
|
else
|
|
euler.y=-asin(sign(m[0][2]));
|
|
float C=cos(euler.y);
|
|
float trX,trY;
|
|
if(fabs(C)>0.005)
|
|
{
|
|
trX=m[2][2]/C;
|
|
trY=-m[1][2]/C;
|
|
|
|
euler.x=-atan2(trY,trX);
|
|
|
|
trX=m[0][0]/C;
|
|
trY=-m[0][1]/C;
|
|
|
|
euler.z=-atan2(trY,trX);
|
|
}
|
|
else //Gimbal Lock
|
|
{
|
|
euler.x=0;
|
|
|
|
trX=m[1][1];
|
|
trY=m[1][0];
|
|
|
|
euler.z=atan2(trY,trX);
|
|
}
|
|
return euler;
|
|
}
|
|
|
|
tVector3 MatrixToEulerAngles(tMatrix4 m)
|
|
{
|
|
tVector3 euler;
|
|
euler.y=-asin(m[0][2]);
|
|
float C=cos(euler.y);
|
|
float trX,trY;
|
|
if(fabs(C)>0.005)
|
|
{
|
|
trX=m[2][2]/C;
|
|
trY=-m[1][2]/C;
|
|
|
|
euler.x=-atan2(trY,trX);
|
|
|
|
trX=m[0][0]/C;
|
|
trY=-m[0][1]/C;
|
|
|
|
euler.z=-atan2(trY,trX);
|
|
}
|
|
else //Gimbal Lock
|
|
{
|
|
euler.x=0;
|
|
|
|
trX=m[1][1];
|
|
trY=m[1][0];
|
|
|
|
euler.z=atan2(trY,trX);
|
|
}
|
|
return euler;
|
|
}
|
|
|
|
//Multiply matrices
|
|
void MatrixMult(tMatrix3 x,tMatrix3 y,tMatrix3 m)
|
|
{
|
|
int i,j,k;
|
|
if(x==m||y==m){
|
|
tMatrix3 target;
|
|
for(j=0;j<3;j++)
|
|
{
|
|
target[0][j]=x[0][0]*y[0][j]
|
|
+x[0][1]*y[1][j]
|
|
+x[0][2]*y[2][j];
|
|
target[1][j]=x[1][0]*y[0][j]
|
|
+x[1][1]*y[1][j]
|
|
+x[1][2]*y[2][j];
|
|
target[2][j]=x[2][0]*y[0][j]
|
|
+x[2][1]*y[1][j]
|
|
+x[2][2]*y[2][j];
|
|
}
|
|
MemoryMove(m,target,sizeof(tMatrix3));
|
|
}
|
|
else
|
|
for(j=0;j<3;j++)
|
|
{
|
|
m[0][j]=x[0][0]*y[0][j]
|
|
+x[0][1]*y[1][j]
|
|
+x[0][2]*y[2][j];
|
|
m[1][j]=x[1][0]*y[0][j]
|
|
+x[1][1]*y[1][j]
|
|
+x[1][2]*y[2][j];
|
|
m[2][j]=x[2][0]*y[0][j]
|
|
+x[2][1]*y[1][j]
|
|
+x[2][2]*y[2][j];
|
|
}
|
|
}
|
|
|
|
void MatrixMult(tMatrix3 x,tMatrix4 y,tMatrix4 m)
|
|
{
|
|
tMatrix4 x4;
|
|
Matrix3ToMatrix4(x,x4);
|
|
MatrixMult(x4,y,m);
|
|
}
|
|
|
|
void MatrixMult(tMatrix4 x,tMatrix3 y,tMatrix4 m)
|
|
{
|
|
tMatrix4 y4;
|
|
Matrix3ToMatrix4(y,y4);
|
|
MatrixMult(x,y4,m);
|
|
}
|
|
|
|
void MatrixMult(tMatrix4 x,tMatrix4 y,tMatrix4 m)
|
|
{
|
|
int i,j,k;
|
|
if(x==m||y==m){
|
|
tMatrix4 target;
|
|
for(j=0;j<4;j++)
|
|
{
|
|
target[0][j]=x[0][0]*y[0][j]
|
|
+x[0][1]*y[1][j]
|
|
+x[0][2]*y[2][j]
|
|
+x[0][3]*y[3][j];
|
|
target[1][j]=x[1][0]*y[0][j]
|
|
+x[1][1]*y[1][j]
|
|
+x[1][2]*y[2][j]
|
|
+x[1][3]*y[3][j];
|
|
target[2][j]=x[2][0]*y[0][j]
|
|
+x[2][1]*y[1][j]
|
|
+x[2][2]*y[2][j]
|
|
+x[2][3]*y[3][j];
|
|
target[3][j]=x[3][0]*y[0][j]
|
|
+x[3][1]*y[1][j]
|
|
+x[3][2]*y[2][j]
|
|
+x[3][3]*y[3][j];
|
|
}
|
|
MemoryMove(m,target,sizeof(tMatrix4));
|
|
}
|
|
else
|
|
{
|
|
for(j=0;j<4;j++)
|
|
{
|
|
m[0][j]=x[0][0]*y[0][j]
|
|
+x[0][1]*y[1][j]
|
|
+x[0][2]*y[2][j]
|
|
+x[0][3]*y[3][j];
|
|
m[1][j]=x[1][0]*y[0][j]
|
|
+x[1][1]*y[1][j]
|
|
+x[1][2]*y[2][j]
|
|
+x[1][3]*y[3][j];
|
|
m[2][j]=x[2][0]*y[0][j]
|
|
+x[2][1]*y[1][j]
|
|
+x[2][2]*y[2][j]
|
|
+x[2][3]*y[3][j];
|
|
m[3][j]=x[3][0]*y[0][j]
|
|
+x[3][1]*y[1][j]
|
|
+x[3][2]*y[2][j]
|
|
+x[3][3]*y[3][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
//makes an identity matrix
|
|
void MatrixIdentity(tMatrix3 m)
|
|
{
|
|
m[0][0]=1;m[0][1]=0;m[0][2]=0;
|
|
m[1][0]=0;m[1][1]=1;m[1][2]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=1;
|
|
}
|
|
|
|
void MatrixIdentity(tMatrix4 m)
|
|
{
|
|
m[0][0]=1;m[0][1]=0;m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=1;m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=1;m[2][3]=0;
|
|
m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
|
|
}
|
|
|
|
//Translates a matrix
|
|
void MatrixTranslate(tMatrix4 target,float x,float y,float z)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=1;m[0][1]=0;m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=1;m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=1;m[2][3]=0;
|
|
m[3][0]=x;m[3][1]=y;m[3][2]=z;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixTranslateVector(tMatrix4 target,tVector3 v)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=1;m[0][1]=0;m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=1;m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=1;m[2][3]=0;
|
|
m[3][0]=v.x;m[3][1]=v.y;m[3][2]=v.z;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
//Scales a matrix
|
|
void MatrixScale(tMatrix3 target,float x,float y,float z)
|
|
{
|
|
tMatrix3 m;
|
|
m[0][0]=x;m[0][1]=0;m[0][2]=0;
|
|
m[1][0]=0;m[1][1]=y;m[1][2]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=z;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixScale(tMatrix4 target,float x,float y,float z)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=x;m[0][1]=0;m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=y;m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=z;m[2][3]=0;
|
|
m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixScaleVector(tMatrix3 target,tVector3 v)
|
|
{
|
|
tMatrix3 m;
|
|
m[0][0]=v.x;m[0][1]=0;m[0][2]=0;
|
|
m[1][0]=0;m[1][1]=v.y;m[1][2]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=v.z;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixScaleVector(tMatrix4 target,tVector3 v)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=v.x;m[0][1]=0;m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=v.y;m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=v.z;m[2][3]=0;
|
|
m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
//Rotates a matrix
|
|
void MatrixRotX(tMatrix3 target,float r)
|
|
{
|
|
tMatrix3 m;
|
|
m[0][0]=1;m[0][1]=0;m[0][2]=0;
|
|
m[1][0]=0;m[1][1]=cos(r);m[1][2]=sin(r);
|
|
m[2][0]=0;m[2][1]=-sin(r);m[2][2]=cos(r);
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixRotX(tMatrix4 target,float r)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=1;m[0][1]=0;m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=cos(r);m[1][2]=sin(r);m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=-sin(r);m[2][2]=cos(r);m[2][3]=0;
|
|
m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixRotY(tMatrix3 target,float r)
|
|
{
|
|
tMatrix3 m;
|
|
m[0][0]=cos(r);m[0][1]=0;m[0][2]=-sin(r);
|
|
m[1][0]=0;m[1][1]=1;m[1][2]=0;
|
|
m[2][0]=sin(r);m[2][1]=0;m[2][2]=cos(r);
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixRotY(tMatrix4 target,float r)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=cos(r);m[0][1]=0;m[0][2]=-sin(r);m[0][3]=0;
|
|
m[1][0]=0;m[1][1]=1;m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=sin(r);m[2][1]=0;m[2][2]=cos(r);m[2][3]=0;
|
|
m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixRotZ(tMatrix3 target,float r)
|
|
{
|
|
tMatrix3 m;
|
|
m[0][0]=cos(r);m[0][1]=sin(r);m[0][2]=0;
|
|
m[1][0]=-sin(r);m[1][1]=cos(r);m[1][2]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
void MatrixRotZ(tMatrix4 target,float r)
|
|
{
|
|
tMatrix4 m;
|
|
m[0][0]=cos(r);m[0][1]=sin(r);m[0][2]=0;m[0][3]=0;
|
|
m[1][0]=-sin(r);m[1][1]=cos(r);m[1][2]=0;m[1][3]=0;
|
|
m[2][0]=0;m[2][1]=0;m[2][2]=1;m[2][3]=0;
|
|
m[3][0]=0;m[3][1]=0;m[3][2]=0;m[3][3]=1;
|
|
MatrixMult(target,m,target);
|
|
}
|
|
|
|
//Transposes a matrix
|
|
void MatrixTranspose(tMatrix3 x,tMatrix3 dst)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
dst[i][j]=x[j][i];
|
|
}
|
|
|
|
void MatrixTranspose(tMatrix4 x,tMatrix4 dst)
|
|
{
|
|
int i,j;
|
|
for(i=0;i<4;i++)
|
|
for(j=0;j<4;j++)
|
|
dst[i][j]=x[j][i];
|
|
}
|
|
|
|
//Verifies if a matrix is a valid rotation matrix (Orthonormal base).
|
|
int MatrixVerify(tMatrix3 m)
|
|
{
|
|
tVector3 *m0=(tVector3*)m[0];
|
|
float len=m0->x*m0->x+m0->y*m0->y+m0->z*m0->z;
|
|
if(len>=1+kMatrixTolerance||len<=1-kMatrixTolerance)return false;
|
|
tVector3 v=(*(tVector3*)m[1]%*(tVector3*)m[2])-*(tVector3*)m[0];
|
|
return(v.x*v.x+v.y*v.y+v.z*v.z<=kMatrixTolerance);
|
|
}
|
|
|
|
int MatrixVerify(tMatrix4 m)
|
|
{
|
|
tVector3 v=(*(tVector3*)m[1]%*(tVector3*)m[2])-*(tVector3*)m[0];
|
|
return(v.x*v.x+v.y*v.y+v.z*v.z<=kMatrixTolerance);
|
|
}
|
|
|
|
|
|
//Readjusts matrices to be a valid rotation matrix
|
|
//needed as floating point calculations always carry small errors, so matrices
|
|
//will drift apart more and more
|
|
void MatrixReAdjust(tMatrix3 m)
|
|
{
|
|
*(tVector3*)m[2]=!*(tVector3*)m[2];
|
|
*(tVector3*)m[0]=!(*(tVector3*)m[1]%*(tVector3*)m[2]);
|
|
*(tVector3*)m[1]=!(*(tVector3*)m[2]%*(tVector3*)m[0]);
|
|
}
|
|
|
|
void MatrixReAdjust(tMatrix4 m)
|
|
{
|
|
*(tVector3*)m[2]=!*(tVector3*)m[2];
|
|
*(tVector3*)m[0]=!(*(tVector3*)m[1]%*(tVector3*)m[2]);
|
|
*(tVector3*)m[1]=!(*(tVector3*)m[2]%*(tVector3*)m[0]);
|
|
}
|
|
|
|
//converts a rotation vector to a rotation matrix.
|
|
//a rotation vector defines a rotation by the length of a vector
|
|
//around the axis given by the direction of a vector.
|
|
void RotationVectorToMatrix(tVector3 axis,tMatrix4 mat)
|
|
{
|
|
float angle=~axis;
|
|
if(!angle)
|
|
{
|
|
MatrixIdentity(mat);
|
|
return;
|
|
}
|
|
else axis=axis/angle;
|
|
tMatrix3 axist;
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
axist[i][j]=((float*)&axis)[i]*((float*)&axis)[j];
|
|
tMatrix3 m1,m2,s;
|
|
s[0][0]=0; s[0][1]=-axis.z; s[0][2]=axis.y;
|
|
s[1][0]=axis.z; s[1][1]=0; s[1][2]=-axis.x;
|
|
s[2][0]=-axis.y;s[2][1]=axis.x; s[2][2]=0;
|
|
MatrixIdentity(m1);
|
|
MatrixSub(m1,axist,m1);
|
|
MatrixMult(m1,cos(-angle),m1);
|
|
MatrixAdd(m1,axist,m1);
|
|
MatrixMult(s,sin(-angle),m2);
|
|
MatrixAdd(m1,m2,m1);
|
|
Matrix3ToMatrix4(m1,mat);
|
|
}
|
|
|
|
void RotationVectorToMatrix(tVector3 axis,tMatrix3 mat)
|
|
{
|
|
float angle=~axis;
|
|
if(!angle)
|
|
{
|
|
MatrixIdentity(mat);
|
|
return;
|
|
}
|
|
else axis=axis/angle;
|
|
tMatrix3 axist;
|
|
int i,j;
|
|
for(i=0;i<3;i++)
|
|
for(j=0;j<3;j++)
|
|
axist[i][j]=((float*)&axis)[i]*((float*)&axis)[j];
|
|
tMatrix3 m1,m2,s;
|
|
s[0][0]=0; s[0][1]=-axis.z; s[0][2]=axis.y;
|
|
s[1][0]=axis.z; s[1][1]=0; s[1][2]=-axis.x;
|
|
s[2][0]=-axis.y;s[2][1]=axis.x; s[2][2]=0;
|
|
MatrixIdentity(m1);
|
|
MatrixSub(m1,axist,m1);
|
|
MatrixMult(m1,cos(-angle),m1);
|
|
MatrixAdd(m1,axist,m1);
|
|
MatrixMult(s,sin(-angle),m2);
|
|
MatrixAdd(m1,m2,mat);
|
|
}
|
|
|
|
|
|
//tests if two vectors are equal
|
|
int VectorEqual(tVector3 a,tVector3 b)
|
|
{
|
|
return(a.x==b.x&&a.y==b.y&&a.z==b.z);
|
|
}
|
|
|
|
int VectorEqual(tVector2 a,tVector2 b)
|
|
{
|
|
return(a.x==b.x&&a.y==b.y);
|
|
}
|
|
|
|
//tests if a vector is zero
|
|
int VectorZero(tVector2 v)
|
|
{
|
|
return v.x==0&&v.y==0;
|
|
}
|
|
|
|
int VectorZero(tVector3 v)
|
|
{
|
|
return v.x==0&&v.y==0&&v.z==0;
|
|
}
|
|
|