#include "Matrix.h"
#include "Vector.h"
#include "Point.h"
#include "Quaternion.h"
#include <math.h>
Matrix::Matrix()
{
}
Matrix::Matrix(const int i)
{
const float s = (float)i;
Matrix &A = *this;
A(1, 1) = s; A(1, 2) = 0; A(1, 3) = 0; A(1, 4) = 0;
A(2, 1) = 0; A(2, 2) = s; A(2, 3) = 0; A(2, 4) = 0;
A(3, 1) = 0; A(3, 2) = 0; A(3, 3) = s; A(3, 4) = 0;
A(4, 1) = 0; A(4, 2) = 0; A(4, 3) = 0; A(4, 4) = s;
}
Matrix::Matrix(const float a[16])
{
Matrix &A = *this;
A(1, 1) = a[0]; A(1, 2) = a[1]; A(1, 3) = a[2]; A(1, 4) = a[3];
A(2, 1) = a[4]; A(2, 2) = a[5]; A(2, 3) = a[6]; A(2, 4) = a[7];
A(3, 1) = a[8]; A(3, 2) = a[8]; A(3, 3) = a[10]; A(3, 4) = a[11];
A(4, 1) = a[12]; A(4, 2) = a[13]; A(4, 3) = a[14]; A(4, 4) = a[15];
}
Matrix::Matrix(const float a[4][4])
{
Matrix &A = *this;
A[0][0] = a[0][0]; A[0][1] = a[0][1]; A[0][2] = a[0][2]; A[0][3] = a[0][3];
A[1][0] = a[1][0]; A[1][1] = a[1][1]; A[1][2] = a[0][2]; A[1][3] = a[0][3];
A[2][0] = a[2][0]; A[2][1] = a[2][1]; A[2][2] = a[0][2]; A[2][3] = a[0][3];
A[3][0] = a[3][0]; A[3][1] = a[3][1]; A[3][2] = a[0][2]; A[3][3] = a[0][3];
}
Matrix::Matrix(float A_11, float A_12, float A_13,
float A_21, float A_22, float A_23,
float A_31, float A_32, float A_33)
{
Matrix &A = *this;
A(1, 1) = A_11; A(1, 2) = A_12; A(1, 3) = A_13; A(1, 4) = 0;
A(2, 1) = A_21; A(2, 2) = A_22; A(2, 3) = A_23; A(2, 4) = 0;
A(3, 1) = A_31; A(3, 2) = A_32; A(3, 3) = A_33; A(3, 4) = 0;
A(4, 1) = 0; A(4, 2) = 0; A(4, 3) = 0; A(4, 4) = 1;
}
Matrix::Matrix(float A_11, float A_12, float A_13, float A_14,
float A_21, float A_22, float A_23, float A_24,
float A_31, float A_32, float A_33, float A_34,
float A_41, float A_42, float A_43, float A_44)
{
Matrix &A = *this;
A(1, 1) = A_11; A(1, 2) = A_12; A(1, 3) = A_13; A(1, 4) = A_14;
A(2, 1) = A_21; A(2, 2) = A_22; A(2, 3) = A_23; A(2, 4) = A_24;
A(3, 1) = A_31; A(3, 2) = A_32; A(3, 3) = A_33; A(3, 4) = A_34;
A(4, 1) = A_41; A(4, 2) = A_42; A(4, 3) = A_43; A(4, 4) = A_44;
}
Matrix::Matrix(const Vector &v1, const Vector &v2, const Vector &v3)
{
Matrix &A = *this;
A(1, 1) = v1.x; A(1, 2) = v2.x; A(1, 3) = v3.x; A(1, 4) = 0;
A(2, 1) = v1.y; A(2, 2) = v2.y; A(2, 3) = v3.y; A(2, 4) = 0;
A(3, 1) = v1.z; A(3, 2) = v2.z; A(3, 3) = v3.z; A(3, 4) = 0;
A(4, 1) = 0; A(4, 2) = 0; A(4, 3) = 0; A(4, 4) = 1;
}
Matrix &Matrix::operator=(const Matrix &B)
{
Matrix &A = *this;
A(1, 1) = B(1, 1); A(1, 2) = B(1, 2); A(1, 3) = B(1, 3); A(1, 4) = B(1, 4);
A(2, 1) = B(2, 1); A(2, 2) = B(2, 2); A(2, 3) = B(2, 3); A(2, 4) = B(2, 4);
A(3, 1) = B(3, 1); A(3, 2) = B(3, 2); A(3, 3) = B(3, 3); A(3, 4) = B(3, 4);
A(4, 1) = B(4, 1); A(4, 2) = B(4, 2); A(4, 3) = B(4, 3); A(4, 4) = B(4, 4);
return A;
}
Matrix diag(float A_11, float A_22, float A_33, float A_44)
{
Matrix A;
A(1, 1) = A_11; A(1, 2) = 0; A(1, 3) = 0; A(1, 4) = 0;
A(2, 1) = 0; A(2, 2) = A_22; A(2, 3) = 0; A(2, 4) = 0;
A(3, 1) = 0; A(3, 2) = 0; A(3, 3) = A_33; A(3, 4) = 0;
A(4, 1) = 0; A(4, 2) = 0; A(4, 3) = 0; A(4, 4) = A_44;
return A;
}
Matrix::operator float*()
{
return &(*this)(1, 1);
}
Matrix Matrix::operator+() const
{
return *this;
}
Matrix Matrix::operator-() const
{
const Matrix &A = *this;
return Matrix(-A(1, 1), -A(1, 2), -A(1, 3), -A(1, 4),
-A(2, 1), -A(2, 2), -A(2, 3), -A(2, 4),
-A(3, 1), -A(3, 2), -A(3, 3), -A(3, 4),
-A(4, 1), -A(4, 2), -A(4, 3), -A(4, 4));
}
Matrix Matrix::operator!() const
{
const Matrix &A = *this;
Matrix I;
float M3344 = A(3, 3) * A(4, 4) - A(4, 3) * A(3, 4);
float M2344 = A(2, 3) * A(4, 4) - A(4, 3) * A(2, 4);
float M2334 = A(2, 3) * A(3, 4) - A(3, 3) * A(2, 4);
float M3244 = A(3, 2) * A(4, 4) - A(4, 2) * A(3, 4);
float M2244 = A(2, 2) * A(4, 4) - A(4, 2) * A(2, 4);
float M2234 = A(2, 2) * A(3, 4) - A(3, 2) * A(2, 4);
float M3243 = A(3, 2) * A(4, 3) - A(4, 2) * A(3, 3);
float M2243 = A(2, 2) * A(4, 3) - A(4, 2) * A(2, 3);
float M2233 = A(2, 2) * A(3, 3) - A(3, 2) * A(2, 3);
float M1344 = A(1, 3) * A(4, 4) - A(4, 3) * A(1, 4);
float M1334 = A(1, 3) * A(3, 4) - A(3, 3) * A(1, 4);
float M1244 = A(1, 2) * A(4, 4) - A(4, 2) * A(1, 4);
float M1234 = A(1, 2) * A(3, 4) - A(3, 2) * A(1, 4);
float M1243 = A(1, 2) * A(4, 3) - A(4, 2) * A(1, 3);
float M1233 = A(1, 2) * A(3, 3) - A(3, 2) * A(1, 3);
float M1324 = A(1, 3) * A(2, 4) - A(2, 3) * A(1, 4);
float M1224 = A(1, 2) * A(2, 4) - A(2, 2) * A(1, 4);
float M1223 = A(1, 2) * A(2, 3) - A(2, 2) * A(1, 3);
// Adjoint Matrix
I(1, 1) = A(2, 2) * M3344 - A(3, 2) * M2344 + A(4, 2) * M2334;
I(2, 1) = -A(2, 1) * M3344 + A(3, 1) * M2344 - A(4, 1) * M2334;
I(3, 1) = A(2, 1) * M3244 - A(3, 1) * M2244 + A(4, 1) * M2234;
I(4, 1) = -A(2, 1) * M3243 + A(3, 1) * M2243 - A(4, 1) * M2233;
I(1, 2) = -A(1, 2) * M3344 + A(3, 2) * M1344 - A(4, 2) * M1334;
I(2, 2) = A(1, 1) * M3344 - A(3, 1) * M1344 + A(4, 1) * M1334;
I(3, 2) = -A(1, 1) * M3244 + A(3, 1) * M1244 - A(4, 1) * M1234;
I(4, 2) = A(1, 1) * M3243 - A(3, 1) * M1243 + A(4, 1) * M1233;
I(1, 3) = A(1, 2) * M2344 - A(2, 2) * M1344 + A(4, 2) * M1324;
I(2, 3) = -A(1, 1) * M2344 + A(2, 1) * M1344 - A(4, 1) * M1324;
I(3, 3) = A(1, 1) * M2244 - A(2, 1) * M1244 + A(4, 1) * M1224;
I(4, 3) = -A(1, 1) * M2243 + A(2, 1) * M1243 - A(4, 1) * M1223;
I(1, 4) = -A(1, 2) * M2334 + A(2, 2) * M1334 - A(3, 2) * M1324;
I(2, 4) = A(1, 1) * M2334 - A(2, 1) * M1334 + A(3, 1) * M1324;
I(3, 4) = -A(1, 1) * M2234 + A(2, 1) * M1234 - A(3, 1) * M1224;
I(4, 4) = A(1, 1) * M2233 - A(2, 1) * M1233 + A(3, 1) * M1223;
// Divide by determinant
I /= A(1, 1) * I(1, 1) +
A(2, 1) * I(1, 2) +
A(3, 1) * I(1, 3) +
A(4, 1) * I(1, 4);
return I;
}
Matrix Matrix::operator~() const
{
const Matrix &A = *this;
return Matrix(A(1, 1), A(2, 1), A(3, 1), A(4, 1),
A(1, 2), A(2, 2), A(3, 2), A(4, 2),
A(1, 3), A(2, 3), A(3, 3), A(4, 3),
A(1, 4), A(2, 4), A(3, 4), A(4, 4));
}
Matrix &Matrix::operator+=(const Matrix &B)
{
Matrix &A = *this;
A(1, 1) += B(1, 1); A(1, 2) += B(1, 2); A(1, 3) += B(1, 3); A(1, 4) += B(1, 4);
A(2, 1) += B(2, 1); A(2, 2) += B(2, 2); A(2, 3) += B(2, 3); A(2, 4) += B(2, 4);
A(3, 1) += B(3, 1); A(3, 2) += B(3, 2); A(3, 3) += B(3, 3); A(3, 4) += B(3, 4);
A(4, 1) += B(4, 1); A(4, 2) += B(4, 2); A(4, 3) += B(4, 3); A(4, 4) += B(4, 4);
return A;
}
Matrix &Matrix::operator-=(const Matrix &B)
{
Matrix &A = *this;
A(1, 1) -= B(1, 1); A(1, 2) -= B(1, 2); A(1, 3) -= B(1, 3); A(1, 4) -= B(1, 4);
A(2, 1) -= B(2, 1); A(2, 2) -= B(2, 2); A(2, 3) -= B(2, 3); A(2, 4) -= B(2, 4);
A(3, 1) -= B(3, 1); A(3, 2) -= B(3, 2); A(3, 3) -= B(3, 3); A(3, 4) -= B(3, 4);
A(4, 1) -= B(4, 1); A(4, 2) -= B(4, 2); A(4, 3) -= B(4, 3); A(4, 4) -= B(4, 4);
return A;
}
Matrix &Matrix::operator*=(float s)
{
Matrix &A = *this;
A(1, 1) *= s; A(1, 2) *= s; A(1, 3) *= s; A(1, 4) *= s;
A(2, 1) *= s; A(2, 2) *= s; A(2, 3) *= s; A(2, 4) *= s;
A(3, 1) *= s; A(3, 2) *= s; A(3, 3) *= s; A(3, 4) *= s;
A(4, 1) *= s; A(4, 2) *= s; A(4, 3) *= s; A(4, 4) *= s;
return A;
}
Matrix &Matrix::operator*=(const Matrix &A)
{
return *this = *this * A;
}
Matrix &Matrix::operator/=(float s)
{
float r = 1.0f / s;
return *this *= r;
}
bool operator==(const Matrix &A, const Matrix &B)
{
if(A(1, 1) == B(1, 1) && A(1, 2) == B(1, 2) && A(1, 3) == B(1, 3) && A(1, 4) == B(1, 4) &&
A(2, 1) == B(2, 1) && A(2, 2) == B(2, 2) && A(2, 3) == B(2, 3) && A(2, 4) == B(2, 4) &&
A(3, 1) == B(3, 1) && A(3, 2) == B(3, 2) && A(3, 3) == B(3, 3) && A(3, 4) == B(3, 4) &&
A(4, 1) == B(4, 1) && A(4, 2) == B(4, 2) && A(4, 3) == B(4, 3) && A(4, 4) == B(4, 4))
return true;
else
return false;
}
bool operator!=(const Matrix &A, const Matrix &B)
{
if(A(1, 1) != B(1, 1) || A(1, 2) != B(1, 2) || A(1, 3) != B(1, 3) || A(1, 4) != B(1, 4) ||
A(2, 1) != B(2, 1) || A(2, 2) != B(2, 2) || A(2, 3) != B(2, 3) || A(2, 4) != B(2, 4) ||
A(3, 1) != B(3, 1) || A(3, 2) != B(3, 2) || A(3, 3) != B(3, 3) || A(3, 4) != B(3, 4) ||
A(4, 1) != B(4, 1) || A(4, 2) != B(4, 2) || A(4, 3) != B(4, 3) || A(4, 4) != B(4, 4))
return true;
else
return false;
}
Matrix operator+(const Matrix &A, const Matrix &B)
{
return Matrix(A(1, 1) + B(1, 1), A(1, 2) + B(1, 2), A(1, 3) + B(1, 3), A(1, 4) + B(1, 4),
A(2, 1) + B(2, 1), A(2, 2) + B(2, 2), A(2, 3) + B(2, 3), A(2, 4) + B(2, 4),
A(3, 1) + B(3, 1), A(3, 2) + B(3, 2), A(3, 3) + B(3, 3), A(3, 4) + B(3, 4),
A(4, 1) + B(4, 1), A(4, 2) + B(4, 2), A(4, 3) + B(4, 3), A(4, 4) + B(4, 4));
}
Matrix operator-(const Matrix &A, const Matrix &B)
{
return Matrix(A(1, 1) - B(1, 1), A(1, 2) - B(1, 2), A(1, 3) - B(1, 3), A(1, 4) - B(1, 4),
A(2, 1) - B(2, 1), A(2, 2) - B(2, 2), A(2, 3) - B(2, 3), A(2, 4) - B(2, 4),
A(3, 1) - B(3, 1), A(3, 2) - B(3, 2), A(3, 3) - B(3, 3), A(3, 4) - B(3, 4),
A(4, 1) - B(4, 1), A(4, 2) - B(4, 2), A(4, 3) - B(4, 3), A(4, 4) - B(4, 4));
}
Matrix operator*(float s, const Matrix &A)
{
return Matrix(s * A(1, 1), s * A(1, 2), s * A(1, 3), s * A(1, 4),
s * A(2, 1), s * A(2, 2), s * A(2, 3), s * A(2, 4),
s * A(3, 1), s * A(3, 2), s * A(3, 3), s * A(3, 4),
s * A(4, 1), s * A(4, 2), s * A(4, 3), s * A(4, 4));
}
Matrix operator*(const Matrix &A, float s)
{
return Matrix(A(1, 1) * s, A(1, 2) * s, A(1, 3) * s, A(1, 4) * s,
A(2, 1) * s, A(2, 2) * s, A(2, 3) * s, A(2, 4) * s,
A(3, 1) * s, A(3, 2) * s, A(3, 3) * s, A(3, 4) * s,
A(4, 1) * s, A(4, 2) * s, A(4, 3) * s, A(4, 4) * s);
}
Matrix operator*(const Matrix &A, const Matrix &B)
{
return Matrix(A(1, 1) * B(1, 1) + A(1, 2) * B(2, 1) + A(1, 3) * B(3, 1) + A(1, 4) * B(4, 1), A(1, 1) * B(1, 2) + A(1, 2) * B(2, 2) + A(1, 3) * B(3, 2) + A(1, 4) * B(4, 2), A(1, 1) * B(1, 3) + A(1, 2) * B(2, 3) + A(1, 3) * B(3, 3) + A(1, 4) * B(4, 3), A(1, 1) * B(1, 4) + A(1, 2) * B(2, 4) + A(1, 3) * B(3, 4) + A(1, 4) * B(4, 4),
A(2, 1) * B(1, 1) + A(2, 2) * B(2, 1) + A(2, 3) * B(3, 1) + A(2, 4) * B(4, 1), A(2, 1) * B(1, 2) + A(2, 2) * B(2, 2) + A(2, 3) * B(3, 2) + A(2, 4) * B(4, 2), A(2, 1) * B(1, 3) + A(2, 2) * B(2, 3) + A(2, 3) * B(3, 3) + A(2, 4) * B(4, 3), A(2, 1) * B(1, 4) + A(2, 2) * B(2, 4) + A(2, 3) * B(3, 4) + A(2, 4) * B(4, 4),
A(3, 1) * B(1, 1) + A(3, 2) * B(2, 1) + A(3, 3) * B(3, 1) + A(3, 4) * B(4, 1), A(3, 1) * B(1, 2) + A(3, 2) * B(2, 2) + A(3, 3) * B(3, 2) + A(3, 4) * B(4, 2), A(3, 1) * B(1, 3) + A(3, 2) * B(2, 3) + A(3, 3) * B(3, 3) + A(3, 4) * B(4, 3), A(3, 1) * B(1, 4) + A(3, 2) * B(2, 4) + A(3, 3) * B(3, 4) + A(3, 4) * B(4, 4),
A(4, 1) * B(1, 1) + A(4, 2) * B(2, 1) + A(4, 3) * B(3, 1) + A(4, 4) * B(4, 1), A(4, 1) * B(1, 2) + A(4, 2) * B(2, 2) + A(4, 3) * B(3, 2) + A(4, 4) * B(4, 2), A(4, 1) * B(1, 3) + A(4, 2) * B(2, 3) + A(4, 3) * B(3, 3) + A(4, 4) * B(4, 3), A(4, 1) * B(1, 4) + A(4, 2) * B(2, 4) + A(4, 3) * B(3, 4) + A(4, 4) * B(4, 4));
}
Matrix operator/(const Matrix &A, float s)
{
float r = 1.0f / s;
return A * s;
}
Vector Matrix::operator*(const Vector &v) const
{
const Matrix &A = *this;
return Vector(A(1, 1) * v.x + A(1, 2) * v.y + A(1, 3) * v.z,
A(2, 1) * v.x + A(2, 2) * v.y + A(2, 3) * v.z,
A(3, 1) * v.x + A(3, 2) * v.y + A(3, 3) * v.z);
}
Point Matrix::operator*(const Point &P) const
{
const Matrix &A = *this;
return Point(A(1, 1) * P.x + A(1, 2) * P.y + A(1, 3) * P.z + A(1, 4),
A(2, 1) * P.x + A(2, 2) * P.y + A(2, 3) * P.z + A(2, 4),
A(3, 1) * P.x + A(3, 2) * P.y + A(3, 3) * P.z + A(3, 4));
}
Quaternion Matrix::operator*(const Quaternion &Q) const
{
const Matrix &A = *this;
return Quaternion(A(1, 1) * Q.x + A(1, 2) * Q.y + A(1, 3) * Q.z + A(1, 4) * Q.w,
A(2, 1) * Q.x + A(2, 2) * Q.y + A(2, 3) * Q.z + A(2, 4) * Q.w,
A(3, 1) * Q.x + A(3, 2) * Q.y + A(3, 3) * Q.z + A(3, 4) * Q.w,
A(4, 1) * Q.x + A(4, 2) * Q.y + A(4, 3) * Q.z + A(4, 4) * Q.w);
}
Vector operator*(const Vector &v, const Matrix &A)
{
return Vector(v.x * A(1, 1) + v.y * A(2, 1) + v.z * A(3, 1) + A(4, 1),
v.x * A(1, 2) + v.y * A(2, 2) + v.z * A(3, 2) + A(4, 2),
v.x * A(1, 3) + v.y * A(2, 3) + v.z * A(3, 3) + A(4, 3));
}
Point operator*(const Point &P, const Matrix &A)
{
return Point(P.x * A(1, 1) + P.y * A(2, 1) + P.z * A(3, 1),
P.x * A(1, 2) + P.y * A(2, 2) + P.z * A(3, 2),
P.x * A(1, 3) + P.y * A(2, 3) + P.z * A(3, 3));
}
Quaternion operator*(const Quaternion &Q, const Matrix &A)
{
return Quaternion(Q.x * A(1, 1) + Q.y * A(2, 1) + Q.z + A(3, 1) + Q.w * A(4, 1),
Q.x * A(1, 2) + Q.y * A(2, 2) + Q.z + A(3, 2) + Q.w * A(4, 2),
Q.x * A(1, 3) + Q.y * A(2, 3) + Q.z + A(3, 3) + Q.w * A(4, 3),
Q.x * A(1, 4) + Q.y * A(2, 4) + Q.z + A(3, 4) + Q.w * A(4, 4));
}
Vector &operator*=(Vector &v, const Matrix &A)
{
return v = v * A;
}
Point &operator*=(Point &P, const Matrix &A)
{
return P = P * A;
}
Quaternion &operator*=(Quaternion &Q, const Matrix &A)
{
return Q = Q * A;
}
float det(const Matrix &A)
{
float M3344 = A(3, 3) * A(4, 4) - A(4, 3) * A(3, 4);
float M2344 = A(2, 3) * A(4, 4) - A(4, 3) * A(2, 4);
float M2334 = A(2, 3) * A(3, 4) - A(3, 3) * A(2, 4);
float M1344 = A(1, 3) * A(4, 4) - A(4, 3) * A(1, 4);
float M1334 = A(1, 3) * A(3, 4) - A(3, 3) * A(1, 4);
float M1324 = A(1, 3) * A(2, 4) - A(2, 3) * A(1, 4);
return A(1, 1) * (A(2, 2) * M3344 - A(3, 2) * M2344 + A(4, 2) * M2334) -
A(2, 1) * (A(1, 2) * M3344 - A(3, 2) * M1344 + A(4, 2) * M1334) +
A(3, 1) * (A(1, 2) * M2344 - A(2, 2) * M1344 + A(4, 2) * M1324) -
A(4, 1) * (A(1, 2) * M2334 - A(2, 2) * M1334 + A(3, 2) * M1324);
}
float det(float A_11)
{
return A_11;
}
float det(float A_11, float A_12,
float A_21, float A_22)
{
return A_11 * A_22 - A_12 * A_21;
}
float det(float A_11, float A_12, float A_13,
float A_21, float A_22, float A_23,
float A_31, float A_32, float A_33)
{
return A_11 * (A_22 * A_33 - A_32 * A_23) -
A_21 * (A_12 * A_33 - A_32 * A_13) +
A_31 * (A_12 * A_23 - A_22 * A_13);
}
float det(float A_11, float A_12, float A_13, float A_14,
float A_21, float A_22, float A_23, float A_24,
float A_31, float A_32, float A_33, float A_34,
float A_41, float A_42, float A_43, float A_44)
{
float M3344 = A_33 * A_44 - A_43 * A_34;
float M2344 = A_23 * A_44 - A_43 * A_24;
float M2334 = A_23 * A_34 - A_33 * A_24;
float M1344 = A_13 * A_44 - A_43 * A_14;
float M1334 = A_13 * A_34 - A_33 * A_14;
float M1324 = A_13 * A_24 - A_23 * A_14;
return A_11 * (A_22 * M3344 - A_32 * M2344 + A_42 * M2334) -
A_21 * (A_12 * M3344 - A_32 * M1344 + A_42 * M1334) +
A_31 * (A_12 * M2344 - A_22 * M1344 + A_42 * M1324) -
A_41 * (A_12 * M2334 - A_22 * M1334 + A_32 * M1324);
}
float det(const Vector &v1, const Vector &v2, const Vector &v3)
{
return v1 * (v2 % v3);
}
float tr(const Matrix &A)
{
return A(1, 1) + A(2, 2) + A(3, 3) + A(4, 4);
}
|