Redline/source/vectors.cpp

542 lines
11 KiB
C++
Raw Normal View History

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