/*
Matrix.cpp
Written by Matthew Fisher
a 4x4 matrix structure. Used very often for affine vector transformations.
For the full definition of the Matrix functions, see Matrix.h
Most of the functions are straightforward, and thus uncommented.
Although the repeated use of (*this)[Row][Col] is inefficent, it is more straightforward,
and is optimized away because the operator is inlined.
*/
Matrix::Matrix()
{
}
Matrix::Matrix(const Matrix &M)
{
memcpy(Data,M.Data,sizeof(float)*16);
}
#ifdef USE_DIRECTX
Matrix::Matrix(const D3DXMATRIX &M)
{
int i,i2;
for(i=0;i<4;i++)
for(i2=0;i2<4;i2++)
{
(*this)[i][i2] = M(i,i2);
}
}
#endif
Matrix& Matrix::operator = (const Matrix &M)
{
memcpy(Data,M.Data,sizeof(float)*16);
return (*this);
}
void Matrix::SetProduct(const Matrix &Left, const Matrix &Right)
{
int i,i2,i3;
for(i=0;i<4;i++)
for(i2=0;i2<4;i2++)
{
float Total = 0.0f;
for(i3=0;i3<4;i3++)
{
Total += Left.Data[i][i3] * Right.Data[i3][i2];
}
Data[i][i2] = Total;
}
}
void Matrix::Invert()
{
//Inversion by LU decomposition
int i, j, k;
for (i=1; i < 4; i++)
Data[0][i] /= Data[0][0];
for (i=1; i < 4; i++)
{
for (j=i; j < 4; j++)
{
float sum = 0.0;
for (k = 0; k < i; k++)
sum += Data[j][k] * Data[k][i];
Data[j][i] -= sum;
}
if (i == 4-1) continue;
for (j=i+1; j < 4; j++)
{
float sum = 0.0;
for (int k = 0; k < i; k++)
sum += Data[i][k]*Data[k][j];
Data[i][j] =
(Data[i][j]-sum) / Data[i][i];
}
}
for ( i = 0; i < 4; i++ ) // invert L
for ( int j = i; j < 4; j++ )
{
float x = 1.0;
if ( i != j )
{
x = 0.0;
for ( int k = i; k < j; k++ )
x -= Data[j][k]*Data[k][i];
}
Data[j][i] = x / Data[j][j];
}
for ( i = 0; i < 4; i++ ) // invert U
for ( j = i; j < 4; j++ )
{
if ( i == j ) continue;
float sum = 0.0;
for ( int k = i; k < j; k++ )
sum += Data[k][j]*( (i==k) ? 1.0f : Data[i][k] );
Data[i][j] = -sum;
}
for ( i = 0; i < 4; i++ ) // final inversion
for ( int j = 0; j < 4; j++ )
{
float sum = 0.0;
for ( int k = ((i>j)?i:j); k < 4; k++ )
sum += ((j==k)?1.0f:Data[j][k])*Data[k][i];
Data[j][i] = sum;
}
}
ostream& operator << (ostream &os, const Matrix &m)
{
os << setprecision(8) << setiosflags(ios::right | ios::showpoint);
for(UINT i=0;i<4;i++)
{
for(UINT i2=0;i2<4;i2++)
{
os << setw(12) << m[i][i2];
}
os << endl;
}
return os;
}
istream& operator >> (istream &is, Matrix &m)
{
for(UINT i=0;i<4;i++)
{
for(UINT i2=0;i2<4;i2++)
{
is >> m[i][i2];
}
}
return is;
}
Matrix operator * (const Matrix &Left, const Matrix &Right)
{
Matrix Return;
Return.SetProduct(Left, Right);
return Return;
}
Matrix operator * (const Matrix &Left, float &Right)
{
Matrix Return;
int i, i2;
for(i=0; i<4; i++)
for(i2=0; i2<4; i2++)
Return[i][i2] = Left[i][i2]*Right;
return Return;
}
Matrix operator * (float &Left, const Matrix &Right)
{
Matrix Return;
int i, i2;
for(i=0; i<4; i++)
for(i2=0; i2<4; i2++)
Return[i][i2] = Right[i][i2]*Left;
return Return;
}
Matrix operator + (Matrix &Left, Matrix &Right)
{
Matrix Return;
int i, i2;
for(i=0; i<4; i++)
for(i2=0; i2<4; i2++)
Return[i][i2] = Left[i][i2] + Right[i][i2];
return Return;
}
Matrix operator - (Matrix &Left, Matrix &Right)
{
Matrix Return;
int i, i2;
for(i=0; i<4; i++)
for(i2=0; i2<4; i2++)
Return[i][i2] = Left[i][i2] - Right[i][i2];
return Return;
}
void Matrix::Identity()
{
int i,i2;
for(i=0;i<4;i++)
for(i2=0;i2<4;i2++)
{
if(i == i2) Data[i][i2] = 1.0f;
else Data[i][i2] = 0.0f;
}
}
void Matrix::LookAt(const Vec3 &Eye, const Vec3 &At, const Vec3 &Up)
{
Vec3 XAxis, YAxis, ZAxis;
ZAxis = Eye - At;
Vec3Cross(XAxis,Up,ZAxis);
XAxis.Normalize();
Vec3Cross(YAxis,ZAxis,XAxis);
(*this)[0][0] = XAxis.x;
(*this)[1][0] = XAxis.y;
(*this)[2][0] = XAxis.z;
(*this)[3][0] = -Vec3Dot(XAxis,Eye);
(*this)[0][1] = YAxis.x;
(*this)[1][1] = YAxis.y;
(*this)[2][1] = YAxis.z;
(*this)[3][1] = -Vec3Dot(YAxis,Eye);
(*this)[0][2] = ZAxis.x;
(*this)[1][2] = ZAxis.y;
(*this)[2][2] = ZAxis.z;
(*this)[3][2] = -Vec3Dot(ZAxis,Eye);
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = 0.0f;
(*this)[3][3] = 1.0f;
}
void Matrix::Orthogonal(float Width, float Height, float ZNear, float ZFar)
{
(*this)[0][0] = 2.0f / Width;
(*this)[1][0] = 0.0f;
(*this)[2][0] = 0.0f;
(*this)[3][0] = 0.0f;
(*this)[0][1] = 0.0f;
(*this)[1][1] = 2.0f / Height;
(*this)[2][1] = 0.0f;
(*this)[3][1] = 0.0f;
(*this)[0][2] = 0.0f;
(*this)[1][2] = 0.0f;
(*this)[2][2] = 1.0f / (ZNear - ZFar);
(*this)[3][2] = ZNear / (ZNear - ZFar);
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = 0.0f;
(*this)[3][3] = 1.0f;
}
void Matrix::Perspective(float Width, float Height, float ZNear, float ZFar)
{
(*this)[0][0] = 2.0f * ZNear / Width;
(*this)[1][0] = 0.0f;
(*this)[2][0] = 0.0f;
(*this)[3][0] = 0.0f;
(*this)[0][1] = 0.0f;
(*this)[1][1] = 2.0f * ZNear / Height;
(*this)[2][1] = 0.0f;
(*this)[3][1] = 0.0f;
(*this)[0][2] = 0.0f;
(*this)[1][2] = 0.0f;
(*this)[2][2] = ZFar / (ZNear - ZFar);
(*this)[3][2] = ZFar * ZNear / (ZNear - ZFar);
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = -1.0f;
(*this)[3][3] = 0.0f;
}
void Matrix::PerspectiveFov(float FOV, float Aspect, float ZNear, float ZFar)
{
float Width = 1.0f/tanf(FOV/2.0f), Height = Aspect/tanf(FOV/2.0f);
(*this)[0][0] = Width;
(*this)[1][0] = 0.0f;
(*this)[2][0] = 0.0f;
(*this)[3][0] = 0.0f;
(*this)[0][1] = 0.0f;
(*this)[1][1] = Height;
(*this)[2][1] = 0.0f;
(*this)[3][1] = 0.0f;
(*this)[0][2] = 0.0f;
(*this)[1][2] = 0.0f;
(*this)[2][2] = ZFar / (ZNear - ZFar);
(*this)[3][2] = ZFar * ZNear / (ZNear - ZFar);
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = -1.0f;
(*this)[3][3] = 0.0f;
}
void Matrix::Transpose()
{
//Not the most efficent method, but good enough.
float TempData[4][4];
int i,i2;
for(i=0;i<4;i++)
for(i2=0;i2<4;i2++)
TempData[i2][i] = (*this)[i][i2];
for(i=0;i<4;i++)
for(i2=0;i2<4;i2++)
(*this)[i][i2] = TempData[i][i2];
}
void Matrix::Rotation(const Vec3 &Axis, float Angle)
{
float c = cosf(Angle);
float s = sinf(Angle);
float t = 1.0f - c;
Vec3 Axis2 = Axis;
Axis2.Normalize();
float x = Axis2.x;
float y = Axis2.y;
float z = Axis2.z;
(*this)[0][0] = 1 + t*(x*x-1);
(*this)[0][1] = z*s+t*x*y;
(*this)[0][2] = -y*s+t*x*z;
(*this)[0][3] = 0.0f;
(*this)[1][0] = -z*s+t*x*y;
(*this)[1][1] = 1+t*(y*y-1);
(*this)[1][2] = x*s+t*y*z;
(*this)[1][3] = 0.0f;
(*this)[2][0] = y*s+t*x*z;
(*this)[2][1] = -x*s+t*y*z;
(*this)[2][2] = 1+t*(z*z-1);
(*this)[2][3] = 0.0f;
(*this)[3][0] = 0.0f;
(*this)[3][1] = 0.0f;
(*this)[3][2] = 0.0f;
(*this)[3][3] = 1.0f;
}
void Matrix::Rotation(float Yaw, float Pitch, float Roll)
{
Matrix X, Y, Z;
Y.RotationY(Yaw);
X.RotationX(Pitch);
Z.RotationZ(Roll);
(*this) = Y * X * Z;
}
void Matrix::Rotation(const Vec3 &Axis, float Angle, const Vec3 &Center)
{
Matrix T1,R,T2;
T1.Translation(-Center);
R.Rotation(Axis, Angle);
T2.Translation(Center);
(*this) = T1 * R * T2;
}
void Matrix::RotationX(float Theta)
{
float CosT = cosf(Theta);
float SinT = sinf(Theta);
Identity();
(*this)[1][1] = CosT;
(*this)[1][2] = SinT;
(*this)[2][1] = -SinT;
(*this)[2][2] = CosT;
}
void Matrix::RotationY(float Theta)
{
float CosT = cosf(Theta);
float SinT = sinf(Theta);
Identity();
(*this)[0][0] = CosT;
(*this)[0][2] = SinT;
(*this)[2][0] = -SinT;
(*this)[2][2] = CosT;
}
void Matrix::RotationZ(float Theta)
{
float CosT = cosf(Theta);
float SinT = sinf(Theta);
Identity();
(*this)[0][0] = CosT;
(*this)[0][1] = SinT;
(*this)[1][0] = -SinT;
(*this)[1][1] = CosT;
}
void Matrix::Scaling(const Vec3 &ScaleFactors)
{
(*this)[0][0] = ScaleFactors.x;
(*this)[1][0] = 0.0f;
(*this)[2][0] = 0.0f;
(*this)[3][0] = 0.0f;
(*this)[0][1] = 0.0f;
(*this)[1][1] = ScaleFactors.y;
(*this)[2][1] = 0.0f;
(*this)[3][1] = 0.0f;
(*this)[0][2] = 0.0f;
(*this)[1][2] = 0.0f;
(*this)[2][2] = ScaleFactors.z;
(*this)[3][2] = 0.0f;
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = 0.0f;
(*this)[3][3] = 1.0f;
}
void Matrix::Translation(const Vec3 &Pos)
{
(*this)[0][0] = 1.0f;
(*this)[1][0] = 0.0f;
(*this)[2][0] = 0.0f;
(*this)[3][0] = Pos.x;
(*this)[0][1] = 0.0f;
(*this)[1][1] = 1.0f;
(*this)[2][1] = 0.0f;
(*this)[3][1] = Pos.y;
(*this)[0][2] = 0.0f;
(*this)[1][2] = 0.0f;
(*this)[2][2] = 1.0f;
(*this)[3][2] = Pos.z;
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = 0.0f;
(*this)[3][3] = 1.0f;
}
void Matrix::Face(const Vec3 &_v1, const Vec3 &_v2)
{
//Rotate about the cross product of the two vectors by the angle between the two vectors
Vec3 Axis, v1 = _v1, v2 = _v2;
float Angle;
v1.Normalize();
v2.Normalize();
Vec3Cross(Axis, v1, v2);
float DotProduct = Vec3Dot(v1,v2);
if(DotProduct < -1.0f) DotProduct = -1.0f;
if(DotProduct > 1.0f) DotProduct = 1.0f;
Angle = acosf(DotProduct);
if(Angle == 0.0f || Axis.Length() == 0.0f)
Identity();
else
Rotation(Axis,Angle);
}
void Matrix::Viewport(float Width, float Height)
{
(*this)[0][0] = Width;
(*this)[1][0] = 0.0f;
(*this)[2][0] = 0.0f;
(*this)[3][0] = 0.0f;
(*this)[0][1] = 0.0f;
(*this)[1][1] = Height;
(*this)[2][1] = 0.0f;
(*this)[3][1] = 0.0f;
(*this)[0][2] = 0.0f;
(*this)[1][2] = 0.0f;
(*this)[2][2] = 1.0f;
(*this)[3][2] = 0.0f;
(*this)[0][3] = 0.0f;
(*this)[1][3] = 0.0f;
(*this)[2][3] = 0.0f;
(*this)[3][3] = 1.0f;
}
#ifdef USE_DIRECTX
Matrix::operator D3DXMATRIX()
{
D3DXMATRIX M;
int i,i2;
for(i=0;i<4;i++)
for(i2=0;i2<4;i2++)
{
M(i,i2) = (*this)[i][i2];
}
return M;
}
#endif
Vec4 operator * (const Vec4 &Right, const Matrix &Left)
{
return Vec4(Right.x * Left[0][0] + Right.y * Left[1][0] + Right.z * Left[2][0] + Right.w * Left[3][0],
Right.x * Left[0][1] + Right.y * Left[1][1] + Right.z * Left[2][1] + Right.w * Left[3][1],
Right.x * Left[0][2] + Right.y * Left[1][2] + Right.z * Left[2][2] + Right.w * Left[3][2],
Right.x * Left[0][3] + Right.y * Left[1][3] + Right.z * Left[2][3] + Right.w * Left[3][3]);
}
void Vec3TransformCoord(Vec3 &Target, const Vec3 &Left, const Matrix &Right)
{
Vec4 ExtendedLeft(Left.x, Left.y, Left.z, 1.0f);
ExtendedLeft = ExtendedLeft * Right;
Target.x = ExtendedLeft.x;
Target.y = ExtendedLeft.y;
Target.z = ExtendedLeft.z;
}
void Vec3TransformProjectCoord(Vec3 &Target, const Vec3 &Left, const Matrix &Right)
{
Vec4 ExtendedLeft(Left.x, Left.y, Left.z, 1.0f);
ExtendedLeft = ExtendedLeft * Right;
if(ExtendedLeft.w == 0.0f)
{
Target.x = ExtendedLeft.x;
Target.y = ExtendedLeft.y;
Target.z = ExtendedLeft.z;
} else {
Target.x = ExtendedLeft.x / ExtendedLeft.w;
Target.y = ExtendedLeft.y / ExtendedLeft.w;
Target.z = ExtendedLeft.z / ExtendedLeft.w;
if(ExtendedLeft.w < 0.0f) Target = -Target;
}
}
void Vec3TransformNormal(Vec3 &Target, const Vec3 &Left, const Matrix &Right)
{
Vec4 ExtendedLeft(Left.x, Left.y, Left.z, 0.0f);
ExtendedLeft = ExtendedLeft * Right;
Target.x = ExtendedLeft.x;
Target.y = ExtendedLeft.y;
Target.z = ExtendedLeft.z;
}
Top