254 lines
10 KiB
C++
254 lines
10 KiB
C++
|
|
#include "Matrix.h"
|
|
#include <math.h> /* fabs() */
|
|
#include <iostream>
|
|
#include <iomanip> /* setprecision() */
|
|
|
|
#define FP_EQ(x,y) (fabs((x)-(y)) < 0.00001)
|
|
|
|
Matrix::Matrix()
|
|
{
|
|
m_inverse_calculated = false;
|
|
}
|
|
|
|
Matrix Matrix::identity()
|
|
{
|
|
Matrix res;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
res[i][j] = i == j ? 1.0 : 0.0;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
double Matrix::determinant()
|
|
{
|
|
return m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][3]
|
|
+ m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][1]
|
|
+ m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][2]
|
|
+ m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][2]
|
|
|
|
+ m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][3]
|
|
+ m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][0]
|
|
+ m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][3]
|
|
+ m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][0]
|
|
|
|
+ m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][1]
|
|
+ m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][1]
|
|
+ m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][2]
|
|
+ m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][0]
|
|
|
|
- m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][2]
|
|
- m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][3]
|
|
- m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][1]
|
|
- m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][3]
|
|
|
|
- m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][0]
|
|
- m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][2]
|
|
- m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][1]
|
|
- m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][3]
|
|
|
|
- m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][0]
|
|
- m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][2]
|
|
- m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][0]
|
|
- m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][1];
|
|
}
|
|
|
|
void Matrix::calculateInverse()
|
|
{
|
|
if (m_inverse_calculated)
|
|
return;
|
|
m_inverse_calculated = true;
|
|
m_inverse_valid = false;
|
|
double det = determinant();
|
|
if (det == 0.0)
|
|
return;
|
|
m_inverse[0][0] = m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][3]
|
|
+ m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][1]
|
|
+ m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][2]
|
|
- m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][2]
|
|
- m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][3]
|
|
- m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][1];
|
|
m_inverse[0][1] = m_matrix[0][1]*m_matrix[2][3]*m_matrix[3][2]
|
|
+ m_matrix[0][2]*m_matrix[2][1]*m_matrix[3][3]
|
|
+ m_matrix[0][3]*m_matrix[2][2]*m_matrix[3][1]
|
|
- m_matrix[0][1]*m_matrix[2][2]*m_matrix[3][3]
|
|
- m_matrix[0][2]*m_matrix[2][3]*m_matrix[3][1]
|
|
- m_matrix[0][3]*m_matrix[2][1]*m_matrix[3][2];
|
|
m_inverse[0][2] = m_matrix[0][1]*m_matrix[1][2]*m_matrix[3][3]
|
|
+ m_matrix[0][2]*m_matrix[1][3]*m_matrix[3][1]
|
|
+ m_matrix[0][3]*m_matrix[1][1]*m_matrix[3][2]
|
|
- m_matrix[0][1]*m_matrix[1][3]*m_matrix[3][2]
|
|
- m_matrix[0][2]*m_matrix[1][1]*m_matrix[3][3]
|
|
- m_matrix[0][3]*m_matrix[1][2]*m_matrix[3][1];
|
|
m_inverse[0][3] = m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][2]
|
|
+ m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][3]
|
|
+ m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][1]
|
|
- m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][3]
|
|
- m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][1]
|
|
- m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][2];
|
|
m_inverse[1][0] = m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][2]
|
|
+ m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][3]
|
|
+ m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][0]
|
|
- m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][3]
|
|
- m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][0]
|
|
- m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][2];
|
|
m_inverse[1][1] = m_matrix[0][0]*m_matrix[2][2]*m_matrix[3][3]
|
|
+ m_matrix[0][2]*m_matrix[2][3]*m_matrix[3][0]
|
|
+ m_matrix[0][3]*m_matrix[2][0]*m_matrix[3][2]
|
|
- m_matrix[0][0]*m_matrix[2][3]*m_matrix[3][2]
|
|
- m_matrix[0][2]*m_matrix[2][0]*m_matrix[3][3]
|
|
- m_matrix[0][3]*m_matrix[2][2]*m_matrix[3][0];
|
|
m_inverse[1][2] = m_matrix[0][0]*m_matrix[1][3]*m_matrix[3][2]
|
|
+ m_matrix[0][2]*m_matrix[1][0]*m_matrix[3][3]
|
|
+ m_matrix[0][3]*m_matrix[1][2]*m_matrix[3][0]
|
|
- m_matrix[0][0]*m_matrix[1][2]*m_matrix[3][3]
|
|
- m_matrix[0][2]*m_matrix[1][3]*m_matrix[3][0]
|
|
- m_matrix[0][3]*m_matrix[1][0]*m_matrix[3][2];
|
|
m_inverse[1][3] = m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][3]
|
|
+ m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][0]
|
|
+ m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][2]
|
|
- m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][2]
|
|
- m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][3]
|
|
- m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][0];
|
|
m_inverse[2][0] = m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][3]
|
|
+ m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][0]
|
|
+ m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][1]
|
|
- m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][1]
|
|
- m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][3]
|
|
- m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][0];
|
|
m_inverse[2][1] = m_matrix[0][0]*m_matrix[2][3]*m_matrix[3][1]
|
|
+ m_matrix[0][1]*m_matrix[2][0]*m_matrix[3][3]
|
|
+ m_matrix[0][3]*m_matrix[2][1]*m_matrix[3][0]
|
|
- m_matrix[0][0]*m_matrix[2][1]*m_matrix[3][3]
|
|
- m_matrix[0][1]*m_matrix[2][3]*m_matrix[3][0]
|
|
- m_matrix[0][3]*m_matrix[2][0]*m_matrix[3][1];
|
|
m_inverse[2][2] = m_matrix[0][0]*m_matrix[1][1]*m_matrix[3][3]
|
|
+ m_matrix[0][1]*m_matrix[1][3]*m_matrix[3][0]
|
|
+ m_matrix[0][3]*m_matrix[1][0]*m_matrix[3][1]
|
|
- m_matrix[0][0]*m_matrix[1][3]*m_matrix[3][1]
|
|
- m_matrix[0][1]*m_matrix[1][0]*m_matrix[3][3]
|
|
- m_matrix[0][3]*m_matrix[1][1]*m_matrix[3][0];
|
|
m_inverse[2][3] = m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][1]
|
|
+ m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][3]
|
|
+ m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][0]
|
|
- m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][3]
|
|
- m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][0]
|
|
- m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][1];
|
|
m_inverse[3][0] = m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][1]
|
|
+ m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][2]
|
|
+ m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][0]
|
|
- m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][2]
|
|
- m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][0]
|
|
- m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][1];
|
|
m_inverse[3][1] = m_matrix[0][0]*m_matrix[2][1]*m_matrix[3][2]
|
|
+ m_matrix[0][1]*m_matrix[2][2]*m_matrix[3][0]
|
|
+ m_matrix[0][2]*m_matrix[2][0]*m_matrix[3][1]
|
|
- m_matrix[0][0]*m_matrix[2][2]*m_matrix[3][1]
|
|
- m_matrix[0][1]*m_matrix[2][0]*m_matrix[3][2]
|
|
- m_matrix[0][2]*m_matrix[2][1]*m_matrix[3][0];
|
|
m_inverse[3][2] = m_matrix[0][0]*m_matrix[1][2]*m_matrix[3][1]
|
|
+ m_matrix[0][1]*m_matrix[1][0]*m_matrix[3][2]
|
|
+ m_matrix[0][2]*m_matrix[1][1]*m_matrix[3][0]
|
|
- m_matrix[0][0]*m_matrix[1][1]*m_matrix[3][2]
|
|
- m_matrix[0][1]*m_matrix[1][2]*m_matrix[3][0]
|
|
- m_matrix[0][2]*m_matrix[1][0]*m_matrix[3][1];
|
|
m_inverse[3][3] = m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][2]
|
|
+ m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][0]
|
|
+ m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][1]
|
|
- m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][1]
|
|
- m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][2]
|
|
- m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][0];
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
m_inverse[i][j] /= det;
|
|
}
|
|
}
|
|
m_inverse_valid = true;
|
|
}
|
|
|
|
Matrix Matrix::getInverse()
|
|
{
|
|
calculateInverse();
|
|
Matrix m;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
m.m_matrix[i][j] = m_inverse[i][j];
|
|
m.m_inverse[i][j] = m_matrix[i][j];
|
|
}
|
|
}
|
|
m.m_inverse_calculated = true;
|
|
m.m_inverse_valid = m_inverse_valid;
|
|
return m;
|
|
}
|
|
|
|
Matrix operator*(const Matrix & m1, const Matrix & m2)
|
|
{
|
|
Matrix res;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
res[i][j] = m1[i][0] * m2[0][j]
|
|
+ m1[i][1] * m2[1][j]
|
|
+ m1[i][2] * m2[2][j]
|
|
+ m1[i][3] * m2[3][j];
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
Vector operator*(const Matrix & m, const Vector & v)
|
|
{
|
|
Vector res;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
res[i] = m[i][0] * v[0]
|
|
+ m[i][1] * v[1]
|
|
+ m[i][2] * v[2]
|
|
+ m[i][3]; /* v[3] is implicitly 1.0 */
|
|
}
|
|
return res;
|
|
}
|
|
|
|
bool operator==(const Matrix & m1, const Matrix & m2)
|
|
{
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
if (! FP_EQ(m1[i][j], m2[i][j]))
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
std::ostream & operator<<(std::ostream & out, const Matrix & m)
|
|
{
|
|
out << std::setprecision(3);
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
out << (i == 0 ? "[ " : " ");
|
|
out << "[ ";
|
|
for (int j = 0; j < 4; j++)
|
|
{
|
|
out << m[i][j];
|
|
if (j < 3)
|
|
out << ", ";
|
|
}
|
|
out << "]";
|
|
if (i == 3)
|
|
out << " ]";
|
|
out << std::endl;
|
|
}
|
|
return out;
|
|
}
|