From 0c8597f31b2109ccaa50e75b746da4b553751b9c Mon Sep 17 00:00:00 2001 From: Josh Holtrop Date: Tue, 20 Jan 2009 14:22:22 +0000 Subject: [PATCH] slight format changes to util/Matrix git-svn-id: svn://anubis/fart/trunk@18 7f9b0f55-74a9-4bce-be96-3c2cd072584d --- util/Matrix.cc | 244 +++++++++++++++++++++++++------------------------ 1 file changed, 123 insertions(+), 121 deletions(-) diff --git a/util/Matrix.cc b/util/Matrix.cc index 1303870..3cdc5e1 100644 --- a/util/Matrix.cc +++ b/util/Matrix.cc @@ -24,39 +24,41 @@ Matrix Matrix::identity() return res; } +/* Formulas from http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html */ 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] + 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][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][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][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][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]; + - 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]; } +/* Formulas from http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html */ void Matrix::calculateInverse() { if (m_inverse_calculated) @@ -66,102 +68,102 @@ void Matrix::calculateInverse() 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]; + 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++) @@ -244,7 +246,7 @@ std::ostream & operator<<(std::ostream & out, const Matrix & m) if (j < 3) out << ", "; } - out << "]"; + out << " ]"; if (i == 3) out << " ]"; out << std::endl;