josh/fart
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

slight format changes to util/Matrix

Browse Source 
git-svn-id: svn://anubis/fart/trunk@18 7f9b0f55-74a9-4bce-be96-3c2cd072584d
This commit is contained in:
Josh Holtrop 2009-01-20 14:22:22 +00:00
parent 6283577b42
commit 0c8597f31b
1 changed files with 123 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

244
util/Matrix.cc
View File

@ -24,39 +24,41 @@ Matrix Matrix::identity()
return res; return res;
} }
/* Formulas from http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html */
double Matrix::determinant() double Matrix::determinant()
{ {
return m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][3] 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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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][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]; - m_matrix[0][3] * m_matrix[1][2] * m_matrix[2][0] * m_matrix[3][1];
} }
/* Formulas from http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html */
void Matrix::calculateInverse() void Matrix::calculateInverse()
{ {
if (m_inverse_calculated) if (m_inverse_calculated)
@ -66,102 +68,102 @@ void Matrix::calculateInverse()
double det = determinant(); double det = determinant();
if (det == 0.0) if (det == 0.0)
return; return;
m_inverse[0][0] = m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][3] 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][2] * m_matrix[2][3] * m_matrix[3][1]
+ m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][2] + 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][1] * m_matrix[2][3] * m_matrix[3][2]
- m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][3] - 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_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_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][2] * m_matrix[2][1] * m_matrix[3][3]
+ m_matrix[0][3]*m_matrix[2][2]*m_matrix[3][1] + 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][1] * m_matrix[2][2] * m_matrix[3][3]
- m_matrix[0][2]*m_matrix[2][3]*m_matrix[3][1] - 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_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_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][2] * m_matrix[1][3] * m_matrix[3][1]
+ m_matrix[0][3]*m_matrix[1][1]*m_matrix[3][2] + 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][1] * m_matrix[1][3] * m_matrix[3][2]
- m_matrix[0][2]*m_matrix[1][1]*m_matrix[3][3] - 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_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_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][2] * m_matrix[1][1] * m_matrix[2][3]
+ m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][1] + 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][1] * m_matrix[1][2] * m_matrix[2][3]
- m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][1] - 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_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_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][2] * m_matrix[2][0] * m_matrix[3][3]
+ m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][0] + 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][0] * m_matrix[2][2] * m_matrix[3][3]
- m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][0] - 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_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_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][2] * m_matrix[2][3] * m_matrix[3][0]
+ m_matrix[0][3]*m_matrix[2][0]*m_matrix[3][2] + 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][0] * m_matrix[2][3] * m_matrix[3][2]
- m_matrix[0][2]*m_matrix[2][0]*m_matrix[3][3] - 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_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_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][2] * m_matrix[1][0] * m_matrix[3][3]
+ m_matrix[0][3]*m_matrix[1][2]*m_matrix[3][0] + 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][0] * m_matrix[1][2] * m_matrix[3][3]
- m_matrix[0][2]*m_matrix[1][3]*m_matrix[3][0] - 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_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_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][2] * m_matrix[1][3] * m_matrix[2][0]
+ m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][2] + 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][0] * m_matrix[1][3] * m_matrix[2][2]
- m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][3] - 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_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_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][1] * m_matrix[2][3] * m_matrix[3][0]
+ m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][1] + 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][0] * m_matrix[2][3] * m_matrix[3][1]
- m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][3] - 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_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_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][1] * m_matrix[2][0] * m_matrix[3][3]
+ m_matrix[0][3]*m_matrix[2][1]*m_matrix[3][0] + 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][0] * m_matrix[2][1] * m_matrix[3][3]
- m_matrix[0][1]*m_matrix[2][3]*m_matrix[3][0] - 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_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_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][1] * m_matrix[1][3] * m_matrix[3][0]
+ m_matrix[0][3]*m_matrix[1][0]*m_matrix[3][1] + 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][0] * m_matrix[1][3] * m_matrix[3][1]
- m_matrix[0][1]*m_matrix[1][0]*m_matrix[3][3] - 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_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_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][1] * m_matrix[1][0] * m_matrix[2][3]
+ m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][0] + 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][0] * m_matrix[1][1] * m_matrix[2][3]
- m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][0] - 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_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_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][1] * m_matrix[2][0] * m_matrix[3][2]
+ m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][0] + 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][0] * m_matrix[2][1] * m_matrix[3][2]
- m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][0] - 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_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_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][1] * m_matrix[2][2] * m_matrix[3][0]
+ m_matrix[0][2]*m_matrix[2][0]*m_matrix[3][1] + 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][0] * m_matrix[2][2] * m_matrix[3][1]
- m_matrix[0][1]*m_matrix[2][0]*m_matrix[3][2] - 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_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_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][1] * m_matrix[1][0] * m_matrix[3][2]
+ m_matrix[0][2]*m_matrix[1][1]*m_matrix[3][0] + 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][0] * m_matrix[1][1] * m_matrix[3][2]
- m_matrix[0][1]*m_matrix[1][2]*m_matrix[3][0] - 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_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_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][1] * m_matrix[1][2] * m_matrix[2][0]
+ m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][1] + 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][0] * m_matrix[1][2] * m_matrix[2][1]
- m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][2] - m_matrix[0][1] * m_matrix[1][0] * m_matrix[2][2]
- m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][0]; - m_matrix[0][2] * m_matrix[1][1] * m_matrix[2][0];
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
{ {
for (int j = 0; j < 4; j++) for (int j = 0; j < 4; j++)
@ -244,7 +246,7 @@ std::ostream & operator<<(std::ostream & out, const Matrix & m)
if (j < 3) if (j < 3)
out << ", "; out << ", ";
} }
out << "]"; out << " ]";
if (i == 3) if (i == 3)
out << " ]"; out << " ]";
out << std::endl; out << std::endl;