diff --git a/src/core/include/pw/core/matrixbase.hpp b/src/core/include/pw/core/matrixbase.hpp index d5608c5..402b631 100644 --- a/src/core/include/pw/core/matrixbase.hpp +++ b/src/core/include/pw/core/matrixbase.hpp @@ -31,9 +31,9 @@ #include #include -namespace pw { +#include -// http://coliru.stacked-crooked.com/a/34db679ee8c13a80 +namespace pw { template struct matrixbase_ { @@ -97,6 +97,14 @@ struct matrix_ : matrixbase_> { return *this; } + template + matrix_(Arguments ...values) + : data {values... } + { + static_assert(sizeof...(Arguments) == R*C, + "Incorrect number of arguments"); + } + //! rows inline std::size_t rows() const { return R; } @@ -106,7 +114,7 @@ struct matrix_ : matrixbase_> { //! get cell count inline std::size_t coefficients() const { return R * C; } - inline size_t offset(int r,int c) const { + inline size_t offset(size_t r,size_t c) const { return (RowMajor) ? r * C + c : c * R + r; } @@ -146,11 +154,33 @@ struct matrix_ : matrixbase_> { return *this; } + + template + matrix_ minor(std::size_t r0,std::size_t c0) const + { + matrix_ m; + size_t r = 0; + for (size_t ri = 0; ri < R; ri++) { + size_t c = 0; + if (ri == r0) + continue; + for (size_t ci = 0; ci < C; ci++) + { + if (ci == c0) + continue; + m(r,c) = (*this)(ri,ci); + c++; + } + r++; + } + return m; + } + T determinant() const { T det(0); - for (size_t c = 0; c < C; c++) { - det += ((c % 2 == 0) ? (*this)(0,c) : -(*this)(0,c)) * this->slice(0,c).determinant(); - } + for (size_t c = 0; c < C; c++) + det += ((c % 2 == 0) ? (*this)(0,c) : -(*this)(0,c)) + * this->minor(0,c).determinant(); return det; } @@ -162,21 +192,17 @@ struct matrix_ : matrixbase_> { return res; } - matrix_ inverse() const - { - matrix_ resMat; - - for ( unsigned int r = 0; r < C; ++r) { - for ( unsigned int j = 0; j < R; ++j) { - short sgn = ((r+j)%2) ? -1 : 1; - auto minor = this->slice(r,j); - resMat(r,j) = minor.determinant() * sgn; - } - } - - resMat = resMat.transposed(); - resMat *= T(1)/this->determinant(); - return resMat; + matrix_ inverse() const { + T invDet = T(1) / this->determinant(); + matrix_ inv; + for (int j = 0; j < C; j++) + for (int i = 0; i < R; i++) + { + const T minorDet = this->minor(j,i).determinant(); + const T coFactor = ((i + j) % 2 == 1) ? -minorDet : minorDet; + inv(i, j) = invDet * coFactor; + } + return inv; } inline bool row_major() const { @@ -190,23 +216,48 @@ struct matrix_ : matrixbase_> { template <> inline float matrix_::determinant() const { - return (*this)[0]; + return (*this)(0,0); } template <> inline double matrix_::determinant() const { - return (*this)[0]; + return (*this)(0,0); } +template +auto operator * (const matrix_& A, + const matrix_& B + ) +{ + matrix_ result; result.zero(); // zero the output + for (size_t r = 0; r < R; r++) + for (size_t c = 0; c < Cb; c++) + for (size_t iI = 0; iI < R; iI++) + result(r,c) += A(r,iI) * B(iI,c); // inner product + return result; +} + +// +// +// + template struct vector_ : matrix_ { typedef matrix_ derived_type; + vector_() = default; vector_(const derived_type& rhs) : derived_type(rhs) {} + template + vector_(Arguments ...values) + : derived_type( { values...} ) + { + static_assert(sizeof...(Arguments) == N, + "Incorrect number of arguments"); + } }; template @@ -233,9 +284,14 @@ auto dot(const vector_& a, const vector_& b) // template using matrix2x2_ = matrix_; +template using matrix3x3_ = matrix_; +template using matrix4x4_ = matrix_; + using matrix2x2f = matrix_; template using vector2_ = vector_; +template using vector3_ = vector_; +template using vector4_ = vector_; using vector2f = vector2_; diff --git a/src/core/tests/pwcore_test_matrix.cpp b/src/core/tests/pwcore_test_matrix.cpp index 885db41..ae063fc 100644 --- a/src/core/tests/pwcore_test_matrix.cpp +++ b/src/core/tests/pwcore_test_matrix.cpp @@ -7,21 +7,64 @@ #include #include +#include + +template inline static +std::basic_ostream& operator << (std::basic_ostream& os, + const pw::matrix_& m + ) +{ + for (size_t r = 0; r < R;r++){ + for (size_t c = 0;c < C;c++) { + os << m(r,c) << " "; + } + os << std::endl; + } + return os; +} int main(int argc,char **argv) { - using namespace pw; matrix2x2f m22; m22.zero(); + m22(0,0) = 1; m22(0,1) = 2; m22(1,0) = 3; m22(1,1) = 4; + vector2f v2; + v2[0] = 1; v2[1] = 3; + + vector2f v3(1.f,2.f); + + auto m22_inv = m22.inverse(); + auto m22_id = m22_inv * m22; + + auto v2_t = m22_id * v2; + auto v3_t = m22_id * v3; + + + + auto v2_f = m22 * v2; + auto v2_b = m22_inv * v2_f; + debug::d() << "offset(0,1) col-major " << m22.offset(0,1); debug::d() << "det " << m22.determinant(); + std::cout << "m22 " << m22 << std::endl; + std::cout << "m22-1 " << m22_inv << std::endl; + std::cout << "m22-i " << m22_id << std::endl; + std::cout << "v22_t " << v2_t << std::endl; + std::cout << "v3_t " << v3_t << std::endl; + + std::cout << "v2_f " << v2_f << std::endl; + std::cout << "v2_b " << v2_b << std::endl; + + + + // vector2f v2 = m22.slice<2,1>(0,0); // m22.set_slice<2,1>(v2,0,0);