diff --git a/src/algebra/TinyMatrix.hpp b/src/algebra/TinyMatrix.hpp index 2093edd338aff76d9e55893d133964591b619a70..96ba7680fb125726f5c8b015f5adfe117a9ae807 100644 --- a/src/algebra/TinyMatrix.hpp +++ b/src/algebra/TinyMatrix.hpp @@ -96,7 +96,6 @@ public: return std::move(Ax); } - KOKKOS_INLINE_FUNCTION friend std::ostream& operator<<(std::ostream& os, const TinyMatrix& A) { @@ -367,6 +366,34 @@ T det(const TinyMatrix<3,T>& A) +A(2,0)*(A(0,1)*A(1,2)-A(1,1)*A(0,2)); } +template <size_t N, typename T> +KOKKOS_INLINE_FUNCTION +TinyMatrix<N-1,T> getMinor(const TinyMatrix<N,T>& A, + const size_t& I, + const size_t& J) +{ + static_assert(N>=2, "minor calculation requires at least 2x2 matrices"); + Assert((I<N) and (J<N)); + TinyMatrix<N-1, T> M; + for (size_t i=0; i<I; ++i) { + for (size_t j=0; j<J; ++j) { + M(i,j)=A(i,j); + } + for (size_t j=J+1; j<N; ++j) { + M(i,j-1)=A(i,j); + } + } + for (size_t i=I+1; i<N; ++i) { + for (size_t j=0; j<J; ++j) { + M(i-1,j)=A(i,j); + } + for (size_t j=J+1; j<N; ++j) { + M(i-1,j-1)=A(i,j); + } + } + return std::move(M); +} + template <size_t N, typename T> KOKKOS_INLINE_FUNCTION TinyMatrix<N,T> inverse(const TinyMatrix<N,T>& A); @@ -404,10 +431,14 @@ TinyMatrix<3,T> inverse(const TinyMatrix<3,T>& A) static_assert(std::is_arithmetic<T>::value, "inverse is not defined for non arithmetic types"); static_assert(std::is_floating_point<T>::value, "inverse is defined for floating point types only"); - std::cerr << "inverse not implemented in 3D\n"; - std::exit(1); + const T determinent = det(A); + const T inv_determinent = 1./determinent; - return std::move(TinyMatrix<3,T>(identity)); + TinyMatrix<3,T> cofactor_T(det(getMinor(A,0,0)), det(getMinor(A,1,0)), det(getMinor(A,2,0)), + det(getMinor(A,0,1)), det(getMinor(A,1,1)), det(getMinor(A,2,1)), + det(getMinor(A,0,2)), det(getMinor(A,1,2)), det(getMinor(A,2,2))); + + return std::move(cofactor_T *= inv_determinent); } #endif // TINYMATRIX_HPP