From 55b73617171c9390940f946007f29de1bc282f13 Mon Sep 17 00:00:00 2001
From: Hartmut Seichter <hartmut@technotecture.com>
Date: Fri, 18 Jan 2019 09:34:48 +0100
Subject: [PATCH] basic function work and are validated against octave output
---
src/core/include/pw/core/matrixbase.hpp | 102 ++++++++++++++++++------
src/core/tests/pwcore_test_matrix.cpp | 45 ++++++++++-
2 files changed, 123 insertions(+), 24 deletions(-)
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 <utility>
#include <initializer_list>
-namespace pw {
+#include <iostream>
-// http://coliru.stacked-crooked.com/a/34db679ee8c13a80
+namespace pw {
template <typename T, typename Derived>
struct matrixbase_ {
@@ -97,6 +97,14 @@ struct matrix_ : matrixbase_<T, matrix_<T, R, C>> {
return *this;
}
+ template <typename... Arguments>
+ 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_<T, matrix_<T, R, C>> {
//! 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_<T, matrix_<T, R, C>> {
return *this;
}
+
+ template<std::size_t Rs,std::size_t Cs,bool RowMajorSlice = RowMajor>
+ matrix_<T,Rs,Cs,RowMajorSlice> minor(std::size_t r0,std::size_t c0) const
+ {
+ matrix_<T,Rs,Cs,RowMajorSlice> 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<R-1,C-1>(0,c).determinant();
- }
+ for (size_t c = 0; c < C; c++)
+ det += ((c % 2 == 0) ? (*this)(0,c) : -(*this)(0,c))
+ * this->minor<R-1,C-1,RowMajor>(0,c).determinant();
return det;
}
@@ -162,21 +192,17 @@ struct matrix_ : matrixbase_<T, matrix_<T, R, C>> {
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-1,C-1>(r,j);
- resMat(r,j) = minor.determinant() * sgn;
- }
- }
-
- resMat = resMat.transposed();
- resMat *= T(1)/this->determinant();
- return resMat;
+ matrix_<T,C,R,RowMajor> inverse() const {
+ T invDet = T(1) / this->determinant();
+ matrix_<T,C,R,RowMajor> inv;
+ for (int j = 0; j < C; j++)
+ for (int i = 0; i < R; i++)
+ {
+ const T minorDet = this->minor<R-1,C-1,RowMajor>(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_<T, matrix_<T, R, C>> {
template <> inline
float matrix_<float,1,1>::determinant() const
{
- return (*this)[0];
+ return (*this)(0,0);
}
template <> inline
double matrix_<double,1,1>::determinant() const
{
- return (*this)[0];
+ return (*this)(0,0);
}
+template <typename T, std::size_t R, std::size_t Ca,std::size_t Cb>
+auto operator * (const matrix_<T, R, Ca>& A,
+ const matrix_<T, R, Cb>& B
+ )
+{
+ matrix_<T,R,Cb> 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 <typename T, std::size_t N,bool RowMajor = false>
struct vector_ : matrix_<T, N, 1, RowMajor>
{
typedef matrix_<T, N, 1, RowMajor> derived_type;
+ vector_() = default;
vector_(const derived_type& rhs) : derived_type(rhs) {}
+ template <typename... Arguments>
+ vector_(Arguments ...values)
+ : derived_type( { values...} )
+ {
+ static_assert(sizeof...(Arguments) == N,
+ "Incorrect number of arguments");
+ }
};
template <typename T, typename U, std::size_t N,bool RowMajor = false>
@@ -233,9 +284,14 @@ auto dot(const vector_<T, N, RowMajor>& a, const vector_<U, N, RowMajor>& b)
//
template <typename T> using matrix2x2_ = matrix_<T, 2, 2>;
+template <typename T> using matrix3x3_ = matrix_<T, 3, 3>;
+template <typename T> using matrix4x4_ = matrix_<T, 4, 4>;
+
using matrix2x2f = matrix_<float, 2, 2>;
template <typename T> using vector2_ = vector_<T, 2>;
+template <typename T> using vector3_ = vector_<T, 3>;
+template <typename T> using vector4_ = vector_<T, 4>;
using vector2f = vector2_<float>;
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 <pw/core/debug.hpp>
#include <iostream>
+#include <sstream>
+
+template <typename T_,typename O_,size_t R,size_t C> inline static
+std::basic_ostream<O_>& operator << (std::basic_ostream<O_>& os,
+ const pw::matrix_<T_,R,C>& 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);