From a73fb9307ce495e7f0678fd498451bbbe6321164 Mon Sep 17 00:00:00 2001 From: Danny Robson Date: Wed, 4 Nov 2015 23:23:46 +1100 Subject: [PATCH] matrix: extract size dependant operations --- Makefile.am | 3 + matrix.cpp | 332 ++++++++++++++++-------------------------------- matrix.hpp | 22 +++- matrix.ipp | 86 +++++++++++++ matrix2.cpp | 64 ++++++++++ matrix3.cpp | 75 +++++++++++ matrix4.cpp | 89 +++++++++++++ test/matrix.cpp | 42 +++++- 8 files changed, 487 insertions(+), 226 deletions(-) create mode 100644 matrix2.cpp create mode 100644 matrix3.cpp create mode 100644 matrix4.cpp diff --git a/Makefile.am b/Makefile.am index 2aac2dcf..de6fd67d 100644 --- a/Makefile.am +++ b/Makefile.am @@ -153,6 +153,9 @@ UTIL_FILES = \ maths.hpp \ maths.ipp \ matrix.cpp \ + matrix2.cpp \ + matrix3.cpp \ + matrix4.cpp \ matrix.hpp \ matrix.ipp \ memory.cpp \ diff --git a/matrix.cpp b/matrix.cpp index 45af099f..5d9b1e62 100644 --- a/matrix.cpp +++ b/matrix.cpp @@ -25,15 +25,17 @@ using namespace util; -//----------------------------------------------------------------------------- +/////////////////////////////////////////////////////////////////////////////// template matrix matrix::transposed (void) const { matrix m; + for (size_t i = 0; i < S; ++i) for (size_t j = 0; j < S; ++j) m.values[i][j] = values[j][i]; + return m; } @@ -51,185 +53,87 @@ matrix::transpose (void) } -//----------------------------------------------------------------------------- -template -matrix -matrix::inverse (void) const { - static_assert (S == 4, "assuming 4x4 matrices"); - - // GLM's implementation of 4x4 matrix inversion. Should allow use of - // vector instructions. - const auto &m = values; - - T Coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3]; - T Coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3]; - T Coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3]; - - T Coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3]; - T Coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3]; - T Coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3]; - - T Coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2]; - T Coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2]; - T Coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2]; - - T Coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3]; - T Coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3]; - T Coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3]; - - T Coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2]; - T Coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2]; - T Coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2]; - - T Coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1]; - T Coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1]; - T Coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1]; - - - vector<4,T> Fac0(Coef00, Coef00, Coef02, Coef03); - vector<4,T> Fac1(Coef04, Coef04, Coef06, Coef07); - vector<4,T> Fac2(Coef08, Coef08, Coef10, Coef11); - vector<4,T> Fac3(Coef12, Coef12, Coef14, Coef15); - vector<4,T> Fac4(Coef16, Coef16, Coef18, Coef19); - vector<4,T> Fac5(Coef20, Coef20, Coef22, Coef23); - - vector<4,T> Vec0(m[1][0], m[0][0], m[0][0], m[0][0]); - vector<4,T> Vec1(m[1][1], m[0][1], m[0][1], m[0][1]); - vector<4,T> Vec2(m[1][2], m[0][2], m[0][2], m[0][2]); - vector<4,T> Vec3(m[1][3], m[0][3], m[0][3], m[0][3]); - - vector<4,T> Inv0(Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2); - vector<4,T> Inv1(Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4); - vector<4,T> Inv2(Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5); - vector<4,T> Inv3(Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5); - - vector<4,T> SignA(+1, -1, +1, -1); - vector<4,T> SignB(-1, +1, -1, +1); - //matrix Inverse(Inv0 * SignA, Inv1 * SignB, Inv2 * SignA, Inv3 * SignB); - matrix<4,T> Inverse = { { { Inv0.x * SignA.x, Inv0.y * SignA.y, Inv0.z * SignA.z, Inv0.w * SignA.w }, - { Inv1.x * SignB.x, Inv1.y * SignB.y, Inv1.z * SignB.z, Inv1.w * SignB.w }, - { Inv2.x * SignA.x, Inv2.y * SignA.y, Inv2.z * SignA.z, Inv2.w * SignA.w }, - { Inv3.x * SignB.x, Inv3.y * SignB.y, Inv3.z * SignB.z, Inv3.w * SignB.w } } }; - - vector<4,T> Row0(Inverse.values[0][0], Inverse.values[1][0], Inverse.values[2][0], Inverse.values[3][0]); - - vector<4,T> Dot0( - m[0][0] * Row0.x, - m[0][1] * Row0.y, - m[0][2] * Row0.z, - m[0][3] * Row0.w - ); - T Dot1 = (Dot0.x + Dot0.y) + (Dot0.z + Dot0.w); - - T OneOverDeterminant = static_cast(1) / Dot1; - - return Inverse * OneOverDeterminant; - -} - - //----------------------------------------------------------------------------- template matrix& -matrix::invert (void) { +matrix::invert (void) +{ return *this = inverse (); } //----------------------------------------------------------------------------- -template -matrix -matrix::inverse_affine (void) const { - return matrix(*this).invert_affine (); -} - - -//----------------------------------------------------------------------------- -template -matrix& -matrix::invert_affine (void) { - CHECK (is_affine ()); - - // inv ([ M b ] == [ inv(M) -inv(M).b ] - // [ 0 1 ]) [ 0 1 ] - - // Invert the 3x3 M - T A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]); - T B = (values[1][2] * values[2][0] - values[1][0] * values[2][2]); - T C = (values[1][0] * values[2][1] - values[1][1] * values[2][0]); - - T D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]); - T E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]); - T F = (values[2][0] * values[0][1] - values[0][0] * values[2][1]); - - T G = (values[0][1] * values[1][2] - values[0][2] * values[1][1]); - T H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]); - T K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]); - - T d = values[0][0] * A + values[0][1] * B + values[0][2] * C; - CHECK_NEQ (d, 0.0); - - values[0][0] = A / d; - values[0][1] = D / d; - values[0][2] = G / d; - values[1][0] = B / d; - values[1][1] = E / d; - values[1][2] = H / d; - values[2][0] = C / d; - values[2][1] = F / d; - values[2][2] = K / d; - - // Multiply the b - T b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3]; - T b1 = - values[1][0] * values[0][3] - values[1][1] * values[1][3] - values[1][2] * values[2][3]; - T b2 = - values[2][0] * values[0][3] - values[2][1] * values[1][3] - values[2][2] * values[2][3]; - - values[0][3] = b0; - values[1][3] = b1; - values[2][3] = b2; - - return *this; -} +//template +//matrix& +//matrix::invert_affine (void) +//{ +// CHECK (is_affine ()); +// +// // inv ([ M b ] == [ inv(M) -inv(M).b ] +// // [ 0 1 ]) [ 0 1 ] +// +// // Invert the 3x3 M +// T A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]); +// T B = (values[1][2] * values[2][0] - values[1][0] * values[2][2]); +// T C = (values[1][0] * values[2][1] - values[1][1] * values[2][0]); +// +// T D = (values[0][2] * values[2][1] - values[0][1] * values[2][2]); +// T E = (values[0][0] * values[2][2] - values[0][2] * values[2][0]); +// T F = (values[2][0] * values[0][1] - values[0][0] * values[2][1]); +// +// T G = (values[0][1] * values[1][2] - values[0][2] * values[1][1]); +// T H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]); +// T K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]); +// +// T d = values[0][0] * A + values[0][1] * B + values[0][2] * C; +// CHECK_NEQ (d, 0.0); +// +// values[0][0] = A / d; +// values[0][1] = D / d; +// values[0][2] = G / d; +// values[1][0] = B / d; +// values[1][1] = E / d; +// values[1][2] = H / d; +// values[2][0] = C / d; +// values[2][1] = F / d; +// values[2][2] = K / d; +// +// // Multiply the b +// T b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3]; +// T b1 = - values[1][0] * values[0][3] - values[1][1] * values[1][3] - values[1][2] * values[2][3]; +// T b2 = - values[2][0] * values[0][3] - values[2][1] * values[1][3] - values[2][2] * values[2][3]; +// +// values[0][3] = b0; +// values[1][3] = b1; +// values[2][3] = b2; +// +// return *this; +//} //----------------------------------------------------------------------------- template T -matrix::det (void) const { - return values[0][3] * values[1][2] * values[2][1] * values[3][0] - - values[0][2] * values[1][3] * values[2][1] * values[3][0] - - values[0][3] * values[1][1] * values[2][2] * values[3][0] + - values[0][1] * values[1][3] * values[2][2] * values[3][0] + - values[0][2] * values[1][1] * values[2][3] * values[3][0] - - values[0][1] * values[1][2] * values[2][3] * values[3][0] - +util::matrix::determinant (void) const +{ + return util::determinant (*this); +} - values[0][3] * values[1][2] * values[2][0] * values[3][1] + - values[0][2] * values[1][3] * values[2][0] * values[3][1] + - values[0][3] * values[1][0] * values[2][2] * values[3][1] - - values[0][0] * values[1][3] * values[2][2] * values[3][1] - - values[0][2] * values[1][0] * values[2][3] * values[3][1] + - values[0][0] * values[1][2] * values[2][3] * values[3][1] + - values[0][3] * values[1][1] * values[2][0] * values[3][2] - - values[0][1] * values[1][3] * values[2][0] * values[3][2] - - values[0][3] * values[1][0] * values[2][1] * values[3][2] + - values[0][0] * values[1][3] * values[2][1] * values[3][2] + - values[0][1] * values[1][0] * values[2][3] * values[3][2] - - values[0][0] * values[1][1] * values[2][3] * values[3][2] - - - values[0][2] * values[1][1] * values[2][0] * values[3][3] + - values[0][1] * values[1][2] * values[2][0] * values[3][3] + - values[0][2] * values[1][0] * values[2][1] * values[3][3] - - values[0][0] * values[1][2] * values[2][1] * values[3][3] - - values[0][1] * values[1][0] * values[2][2] * values[3][3] + - values[0][0] * values[1][1] * values[2][2] * values[3][3]; +//----------------------------------------------------------------------------- +template +util::matrix +util::matrix::inverse (void) const +{ + return util::inverse (*this); } //----------------------------------------------------------------------------- template matrix -matrix::operator* (const matrix &rhs) const { +matrix::operator* (const matrix &rhs) const +{ matrix m; for (unsigned row = 0; row < S; ++row) { @@ -248,41 +152,23 @@ matrix::operator* (const matrix &rhs) const { //----------------------------------------------------------------------------- template matrix& -matrix::operator*=(const matrix &rhs) { +matrix::operator*=(const matrix &rhs) +{ return *this = *this * rhs; } /////////////////////////////////////////////////////////////////////////////// -//template -//vector<3,T> -//matrix::operator* (vector<3,T> v) const -//{ -// return ( -// *this * v.template homog () -// ).template redim<3> (); -//} -// -// -////----------------------------------------------------------------------------- -//template -//point<3,T> -//matrix::operator* (point<3,T> p) const -//{ -// return (*this * p.template homog ()).template redim<3> (); -//} - - -//----------------------------------------------------------------------------- template vector -matrix::operator* (const vector &rhs) const { - return vector { - values[0][0] * rhs.x + values[0][1] * rhs.y + values[0][2] * rhs.z + values[0][3] * rhs.w, - values[1][0] * rhs.x + values[1][1] * rhs.y + values[1][2] * rhs.z + values[1][3] * rhs.w, - values[2][0] * rhs.x + values[2][1] * rhs.y + values[2][2] * rhs.z + values[2][3] * rhs.w, - values[3][0] * rhs.x + values[3][1] * rhs.y + values[3][2] * rhs.z + values[3][3] * rhs.w - }; +matrix::operator* (const vector &rhs) const +{ + vector out; + + for (size_t i = 0; i < S; ++i) + out[i] = dot (rhs, values[i]); + + return out; } @@ -291,12 +177,12 @@ template point matrix::operator* (const point &rhs) const { - return point { - values[0][0] * rhs.x + values[0][1] * rhs.y + values[0][2] * rhs.z + values[0][3] * rhs.w, - values[1][0] * rhs.x + values[1][1] * rhs.y + values[1][2] * rhs.z + values[1][3] * rhs.w, - values[2][0] * rhs.x + values[2][1] * rhs.y + values[2][2] * rhs.z + values[2][3] * rhs.w, - values[3][0] * rhs.x + values[3][1] * rhs.y + values[3][2] * rhs.z + values[3][3] * rhs.w - }; + point out; + + for (size_t i = 0; i < S; ++i) + out[i] = dot (rhs, values[i]); + + return out; } @@ -318,7 +204,8 @@ matrix::operator* (T f) const //----------------------------------------------------------------------------- template matrix& -matrix::operator*= (T f){ +matrix::operator*= (T f) +{ for (size_t i = 0; i < S; ++i) for (size_t j = 0; j < S; ++j) values[i][j] *= f; @@ -329,22 +216,24 @@ matrix::operator*= (T f){ //----------------------------------------------------------------------------- template -matrix -matrix::operator/ (T s) const { - matrix m; +util::matrix +util::matrix::operator/ (T t) const +{ + auto out = *this; - for (size_t r = 0; r < m.rows; ++r) - for (size_t c = 0; c < m.cols; ++c) - m.values[r][c] = values[r][c] / s; + for (auto &i: out.values) + for (auto &j: i) + j /= t; - return m; + return out; } //----------------------------------------------------------------------------- template matrix& -matrix::operator/= (T s) { +matrix::operator/= (T s) +{ for (size_t r = 0; r < rows; ++r) for (size_t c = 0; c < cols; ++c) values[r][c] /= s; @@ -356,7 +245,8 @@ matrix::operator/= (T s) { //----------------------------------------------------------------------------- template bool -matrix::operator== (const matrix &rhs) const { +matrix::operator== (const matrix &rhs) const +{ for (size_t r = 0; r < rows; ++r) for (size_t c = 0; c < cols; ++c) if (!almost_equal (rhs.values[r][c], values[r][c])) @@ -368,11 +258,13 @@ matrix::operator== (const matrix &rhs) const { //----------------------------------------------------------------------------- template bool -matrix::is_affine (void) const { - return exactly_equal (values[3][0], T {0}) && - exactly_equal (values[3][1], T {0}) && - exactly_equal (values[3][2], T {0}) && - exactly_equal (values[3][3], T {1}); +matrix::is_affine (void) const +{ + for (size_t i = 0; i < S - 1; ++i) + if (!exactly_zero (values[S-1][i])) + return false; + + return exactly_equal (values[S-1][S-1], T{1}); } @@ -538,31 +430,28 @@ matrix::rotate (T angle, util::vector<3,T> about) //----------------------------------------------------------------------------- template const matrix -matrix::IDENTITY = { { { 1, 0, 0, 0 }, - { 0, 1, 0, 0 }, - { 0, 0, 1, 0 }, - { 0, 0, 0, 1 } } }; - - -template -const matrix -matrix::ZEROES = { { { 0, 0, 0, 0 }, - { 0, 0, 0, 0 }, - { 0, 0, 0, 0 }, - { 0, 0, 0, 0 } } }; +matrix::ZEROES { 0 }; //----------------------------------------------------------------------------- namespace util { + template struct matrix<2,float>; + template struct matrix<2,double>; + + template struct matrix<3,float>; + template struct matrix<3,double>; + template struct matrix<4,float>; template struct matrix<4,double>; } + //----------------------------------------------------------------------------- namespace util { template std::ostream& - operator<< (std::ostream &os, const matrix &m) { + operator<< (std::ostream &os, const matrix &m) + { os << "{ {" << m.values[0][0] << ", " << m.values[0][1] << ", " << m.values[0][2] << ", " @@ -584,5 +473,6 @@ namespace util { } } + template std::ostream& util::operator<< (std::ostream&, const matrix<4,float>&); template std::ostream& util::operator<< (std::ostream&, const matrix<4,double>&); diff --git a/matrix.hpp b/matrix.hpp index 19a9d358..81489f27 100644 --- a/matrix.hpp +++ b/matrix.hpp @@ -29,9 +29,14 @@ namespace util { static const size_t rows = S; static const size_t cols = S; + T* operator[] (size_t); + const T* operator[] (size_t) const; + matrix& transpose (void); matrix transposed (void) const; + T determinant (void) const; + matrix inverse (void) const; matrix& invert (void); matrix inverse_affine (void) const; @@ -42,9 +47,6 @@ namespace util { matrix operator* (const matrix&) const; matrix& operator*=(const matrix&); - //vector<3,T> operator* (vector<3,T>) const; - //point<3,T> operator* (point<3,T>) const; - vector operator* (const vector&) const; point operator* (const point &) const; @@ -78,12 +80,26 @@ namespace util { static const matrix ZEROES; }; + + template + T determinant (const matrix&); + + template + matrix + inverse (const matrix&); + template using matrix3 = matrix<3,T>; template using matrix4 = matrix<4,T>; template using matrixf = matrix; template using matrixd = matrix; + typedef matrix<2,float> matrix2f; + typedef matrix<2,double> matrix2d; + + typedef matrix<3,float> matrix3f; + typedef matrix<3,double> matrix3d; + typedef matrix<4,float> matrix4f; typedef matrix<4,double> matrix4d; diff --git a/matrix.ipp b/matrix.ipp index 40c31c7b..dd949aaa 100644 --- a/matrix.ipp +++ b/matrix.ipp @@ -21,6 +21,46 @@ #define __UTIL_MATRIX_IPP + +/////////////////////////////////////////////////////////////////////////////// +template +T* +util::matrix::operator[] (size_t idx) +{ + return this->values[idx]; +} + + +//----------------------------------------------------------------------------- +template +const T* +util::matrix::operator[] (size_t idx) const +{ + return this->values[idx]; +} + +/////////////////////////////////////////////////////////////////////////////// +//template +//vector<3,T> +//matrix::operator* (vector<3,T> v) const +//{ +// return ( +// *this * v.template homog () +// ).template redim<3> (); +//} +// +// +////----------------------------------------------------------------------------- +//template +//point<3,T> +//matrix::operator* (point<3,T> p) const +//{ +// return (*this * p.template homog ()).template redim<3> (); +//} +// + + +/////////////////////////////////////////////////////////////////////////////// template template util::matrix @@ -34,3 +74,49 @@ util::matrix::cast (void) const return out; } + + +///////////////////////////////////////////////////////////////////////////////// +//template +//T +//util::matrix::determinant (void) const +//{ +// return util::determinant (*this); +//} +// +// +////----------------------------------------------------------------------------- +//template +//util::matrix +//util::matrix::inverse (void) const +//{ +// return util::inverse (*this); +//} + + +/////////////////////////////////////////////////////////////////////////////// +//template +//util::matrix +//util::matrix::operator/ (T t) const +//{ +// auto out = *this; +// +// for (auto &i: out.values) +// for (auto &j: i) +// j /= t; +// +// return out; +//} +// +// +///////////////////////////////////////////////////////////////////////////////// +//template +//bool +//util::matrix::operator== (const matrix &m) const +//{ +// for (size_t i = 0; i < S; ++i) +// for (size_t j = 0; j < S; ++j) +// if (!exactly_equal (values[i][j], m[i][j])) +// return false; +// return true; +//} diff --git a/matrix2.cpp b/matrix2.cpp new file mode 100644 index 00000000..74386a1f --- /dev/null +++ b/matrix2.cpp @@ -0,0 +1,64 @@ +/* + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Copyright 2015 Danny Robson + */ + +#include "matrix.hpp" + +using util::matrix; + + +/////////////////////////////////////////////////////////////////////////////// +template +T +util::determinant (const matrix &m) +{ + static_assert (S == 2, "partial specialization for 2 dimensions"); + + return m[0][0] * m[1][1] - m[0][1] * m[1][0]; +} + +template float util::determinant (const matrix<2,float>&); +template double util::determinant (const matrix<2,double>&); + + +//----------------------------------------------------------------------------- +template +matrix +util::inverse (const matrix &m) +{ + static_assert (S == 2, "partial specialization for 2 dimensions"); + + return matrix { + m[1][1], -m[0][1], + -m[1][0], m[0][0] + } / determinant (m); +} + +template util::matrix<2,float> util::inverse (const matrix<2,float>&); +template util::matrix<2,double> util::inverse (const matrix<2,double>&); + + +/////////////////////////////////////////////////////////////////////////////// +template +const matrix +matrix::IDENTITY = { { + { 1, 0, }, + { 0, 1 } +} }; + + +/////////////////////////////////////////////////////////////////////////////// +template struct util::matrix<2,float>; +template struct util::matrix<2,double>; diff --git a/matrix3.cpp b/matrix3.cpp new file mode 100644 index 00000000..3b8ee1e0 --- /dev/null +++ b/matrix3.cpp @@ -0,0 +1,75 @@ +/* + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Copyright 2015 Danny Robson + */ + +#include "./matrix.hpp" + +using util::matrix; + + +/////////////////////////////////////////////////////////////////////////////// +template +T +util::determinant (const matrix& m) +{ + static_assert (S == 3, "hard coded 3x3 specialisation"); + + return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) - + m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); +} + +template float util::determinant (const matrix<3,float>&); +template double util::determinant (const matrix<3,double>&); + + +//----------------------------------------------------------------------------- +template +matrix +util::inverse (const matrix &m) +{ + static_assert (S == 3, "hard coded 3x3 specialisation"); + + return matrix { + m[1][1] * m[2][2] - m[2][1] * m[1][2], + m[0][2] * m[2][1] - m[0][1] * m[2][2], + m[0][1] * m[1][2] - m[0][2] * m[1][1], + + m[1][2] * m[2][0] - m[1][0] * m[2][2], + m[0][0] * m[2][2] - m[0][2] * m[2][0], + m[1][0] * m[0][2] - m[0][0] * m[1][2], + + m[1][0] * m[2][1] - m[2][0] * m[1][1], + m[2][0] * m[0][1] - m[0][0] * m[2][1], + m[0][0] * m[1][1] - m[1][0] * m[0][1], + } / determinant (m); +} + +template util::matrix<3,float> util::inverse (const matrix<3,float>&); +template util::matrix<3,double> util::inverse (const matrix<3,double>&); + + +/////////////////////////////////////////////////////////////////////////////// +template +const matrix +matrix::IDENTITY = { { + { 1, 0, 0, }, + { 0, 1, 0, }, + { 0, 0, 1 } +} }; + +/////////////////////////////////////////////////////////////////////////////// +template struct util::matrix<3,float>; +template struct util::matrix<3,double>; diff --git a/matrix4.cpp b/matrix4.cpp new file mode 100644 index 00000000..d405b369 --- /dev/null +++ b/matrix4.cpp @@ -0,0 +1,89 @@ +/* + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Copyright 2015 Danny Robson + */ + +#include "matrix.hpp" + +using util::matrix; + + +//----------------------------------------------------------------------------- +template +T +util::determinant (const matrix &m) +{ + static_assert (S == 4, "hard coded 4x4 specialisation"); + + return m[0][3] * m[1][2] * m[2][1] * m[3][0] - m[0][2] * m[1][3] * m[2][1] * m[3][0] - + m[0][3] * m[1][1] * m[2][2] * m[3][0] + m[0][1] * m[1][3] * m[2][2] * m[3][0] + + m[0][2] * m[1][1] * m[2][3] * m[3][0] - m[0][1] * m[1][2] * m[2][3] * m[3][0] - + m[0][3] * m[1][2] * m[2][0] * m[3][1] + m[0][2] * m[1][3] * m[2][0] * m[3][1] + + m[0][3] * m[1][0] * m[2][2] * m[3][1] - m[0][0] * m[1][3] * m[2][2] * m[3][1] - + m[0][2] * m[1][0] * m[2][3] * m[3][1] + m[0][0] * m[1][2] * m[2][3] * m[3][1] + + m[0][3] * m[1][1] * m[2][0] * m[3][2] - m[0][1] * m[1][3] * m[2][0] * m[3][2] - + m[0][3] * m[1][0] * m[2][1] * m[3][2] + m[0][0] * m[1][3] * m[2][1] * m[3][2] + + m[0][1] * m[1][0] * m[2][3] * m[3][2] - m[0][0] * m[1][1] * m[2][3] * m[3][2] - + m[0][2] * m[1][1] * m[2][0] * m[3][3] + m[0][1] * m[1][2] * m[2][0] * m[3][3] + + m[0][2] * m[1][0] * m[2][1] * m[3][3] - m[0][0] * m[1][2] * m[2][1] * m[3][3] - + m[0][1] * m[1][0] * m[2][2] * m[3][3] + m[0][0] * m[1][1] * m[2][2] * m[3][3]; +} + +template float util::determinant (const matrix<4,float>&); +template double util::determinant (const matrix<4,double>&); + + +//----------------------------------------------------------------------------- +template +matrix +util::inverse (const matrix &m) +{ + static_assert (S == 4, "hard coded 4x4 specialisation"); + + return matrix { + m[1][2]*m[2][3]*m[3][1] - m[1][3]*m[2][2]*m[3][1] + m[1][3]*m[2][1]*m[3][2] - m[1][1]*m[2][3]*m[3][2] - m[1][2]*m[2][1]*m[3][3] + m[1][1]*m[2][2]*m[3][3], + m[0][3]*m[2][2]*m[3][1] - m[0][2]*m[2][3]*m[3][1] - m[0][3]*m[2][1]*m[3][2] + m[0][1]*m[2][3]*m[3][2] + m[0][2]*m[2][1]*m[3][3] - m[0][1]*m[2][2]*m[3][3], + m[0][2]*m[1][3]*m[3][1] - m[0][3]*m[1][2]*m[3][1] + m[0][3]*m[1][1]*m[3][2] - m[0][1]*m[1][3]*m[3][2] - m[0][2]*m[1][1]*m[3][3] + m[0][1]*m[1][2]*m[3][3], + m[0][3]*m[1][2]*m[2][1] - m[0][2]*m[1][3]*m[2][1] - m[0][3]*m[1][1]*m[2][2] + m[0][1]*m[1][3]*m[2][2] + m[0][2]*m[1][1]*m[2][3] - m[0][1]*m[1][2]*m[2][3], + m[1][3]*m[2][2]*m[3][0] - m[1][2]*m[2][3]*m[3][0] - m[1][3]*m[2][0]*m[3][2] + m[1][0]*m[2][3]*m[3][2] + m[1][2]*m[2][0]*m[3][3] - m[1][0]*m[2][2]*m[3][3], + m[0][2]*m[2][3]*m[3][0] - m[0][3]*m[2][2]*m[3][0] + m[0][3]*m[2][0]*m[3][2] - m[0][0]*m[2][3]*m[3][2] - m[0][2]*m[2][0]*m[3][3] + m[0][0]*m[2][2]*m[3][3], + m[0][3]*m[1][2]*m[3][0] - m[0][2]*m[1][3]*m[3][0] - m[0][3]*m[1][0]*m[3][2] + m[0][0]*m[1][3]*m[3][2] + m[0][2]*m[1][0]*m[3][3] - m[0][0]*m[1][2]*m[3][3], + m[0][2]*m[1][3]*m[2][0] - m[0][3]*m[1][2]*m[2][0] + m[0][3]*m[1][0]*m[2][2] - m[0][0]*m[1][3]*m[2][2] - m[0][2]*m[1][0]*m[2][3] + m[0][0]*m[1][2]*m[2][3], + m[1][1]*m[2][3]*m[3][0] - m[1][3]*m[2][1]*m[3][0] + m[1][3]*m[2][0]*m[3][1] - m[1][0]*m[2][3]*m[3][1] - m[1][1]*m[2][0]*m[3][3] + m[1][0]*m[2][1]*m[3][3], + m[0][3]*m[2][1]*m[3][0] - m[0][1]*m[2][3]*m[3][0] - m[0][3]*m[2][0]*m[3][1] + m[0][0]*m[2][3]*m[3][1] + m[0][1]*m[2][0]*m[3][3] - m[0][0]*m[2][1]*m[3][3], + m[0][1]*m[1][3]*m[3][0] - m[0][3]*m[1][1]*m[3][0] + m[0][3]*m[1][0]*m[3][1] - m[0][0]*m[1][3]*m[3][1] - m[0][1]*m[1][0]*m[3][3] + m[0][0]*m[1][1]*m[3][3], + m[0][3]*m[1][1]*m[2][0] - m[0][1]*m[1][3]*m[2][0] - m[0][3]*m[1][0]*m[2][1] + m[0][0]*m[1][3]*m[2][1] + m[0][1]*m[1][0]*m[2][3] - m[0][0]*m[1][1]*m[2][3], + m[1][2]*m[2][1]*m[3][0] - m[1][1]*m[2][2]*m[3][0] - m[1][2]*m[2][0]*m[3][1] + m[1][0]*m[2][2]*m[3][1] + m[1][1]*m[2][0]*m[3][2] - m[1][0]*m[2][1]*m[3][2], + m[0][1]*m[2][2]*m[3][0] - m[0][2]*m[2][1]*m[3][0] + m[0][2]*m[2][0]*m[3][1] - m[0][0]*m[2][2]*m[3][1] - m[0][1]*m[2][0]*m[3][2] + m[0][0]*m[2][1]*m[3][2], + m[0][2]*m[1][1]*m[3][0] - m[0][1]*m[1][2]*m[3][0] - m[0][2]*m[1][0]*m[3][1] + m[0][0]*m[1][2]*m[3][1] + m[0][1]*m[1][0]*m[3][2] - m[0][0]*m[1][1]*m[3][2], + m[0][1]*m[1][2]*m[2][0] - m[0][2]*m[1][1]*m[2][0] + m[0][2]*m[1][0]*m[2][1] - m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2] + m[0][0]*m[1][1]*m[2][2], + } / determinant (m); +} + +template util::matrix<4,float> util::inverse (const matrix<4,float>&); +template util::matrix<4,double> util::inverse (const matrix<4,double>&); + + +/////////////////////////////////////////////////////////////////////////////// +template +const matrix +matrix::IDENTITY = { { { 1, 0, 0, 0 }, + { 0, 1, 0, 0 }, + { 0, 0, 1, 0 }, + { 0, 0, 0, 1 } } }; + + +/////////////////////////////////////////////////////////////////////////////// +template struct util::matrix<4,float>; +template struct util::matrix<4,double>; diff --git a/test/matrix.cpp b/test/matrix.cpp index b85a75ea..17f2ef4e 100644 --- a/test/matrix.cpp +++ b/test/matrix.cpp @@ -86,8 +86,44 @@ main (void) tap.expect (success, "identity inversion"); } + // Simple 2x2 inversion test + { + util::matrix2f m { { + { 1, 2 }, + { 3, 4 } + } }; + + tap.expect_eq (-2, m.determinant (), "2x2 determinant"); + + util::matrix2f r { { + { -4, 2 }, + { 3, -1 } + } }; + + tap.expect_eq (r / 2.f, m.inverse (), "2x2 inversion"); + } + + // Simple 3x3 inversion test + { + util::matrix3f m { { + { 3, 1, 2 }, + { 2, 3, 1 }, + { 4, 0, 2 } + } }; + + tap.expect_eq (-6, m.determinant (), "3x3 determinant"); + + util::matrix3f r { { + { -6, 2, 5 }, + { 0, 2, -1 }, + { 12, -4, -7 } + } }; + + tap.expect_eq (m.inverse (), r / 6.f, "3x3 inversion"); + } + + // Simple 4x4 inversion test { - // Simple inversion test util::matrix4f m { { { 4, 1, 2, 3 }, { 2, 3, 4, 1 }, @@ -95,6 +131,8 @@ main (void) { 1, 2, 3, 4 } } }; + tap.expect_eq (-160.f, m.determinant (), "4x4 determinant"); + util::matrix4f r { { { 11, 1, 1, -9 }, { -9, 1, 11, 1 }, @@ -102,7 +140,7 @@ main (void) { 1, -9, 1, 11 } } }; - tap.expect_eq (m.inverse (), r / 40.f, "simple inversion"); + tap.expect_eq (m.inverse (), r / 40.f, "4x4 inversion"); } return tap.status ();