diff --git a/src/algebra/TinyMatrix.hpp b/src/algebra/TinyMatrix.hpp index c7447be5faff25326816a4e9f5417887ac72b9b6..05cbd6813265103e1b8cb46f02d30eac7a5f6fbb 100644 --- a/src/algebra/TinyMatrix.hpp +++ b/src/algebra/TinyMatrix.hpp @@ -255,10 +255,43 @@ template <size_t N, typename T> KOKKOS_INLINE_FUNCTION T det(const TinyMatrix<N,T>& A) { - TinyMatrix<N,T> M=A; -#warning must code a Gauss pivoting approach and compare perfs with Camers for 3x3 matrices static_assert(std::is_arithmetic<T>::value, "determinent is not defined for non arithmetic types"); - return 0; + TinyMatrix<N,T> M = A; + + TinyVector<N, int> index; + for (int i=0; i<N; ++i) index[i]=i; + + T determinent = 1; + for (int i=0; i<N; ++i) { + for (int j=i; j<N; ++j) { + int l = j; + const int J = index[j]; + for (int k=j+1; k<N; ++k) { + if (std::abs(M(index[k],i)) > std::abs(M(J,i))) { + l=k; + } + } + if (l != j) { + std::swap(index[l], index[j]); + determinent *= -1; + } + } + const int I = index[i]; + if (M(I,i)==0) return 0; + double inv_Mii = 1./M(I,i); + for (int k=i+1; k<N; ++k) { + const int K = index[k]; + const T factor = M(K,i)*inv_Mii; + for (int l=i+1; l<N; ++l) { + M(K,l) -= factor*M(I,l); + } + } + } + + for (int i=0; i<N; ++i) { + determinent *= M(index[i],i); + } + return determinent; } template <typename T> @@ -289,9 +322,9 @@ T det(const TinyMatrix<3,T>& A) TinyMatrix<3,T> M=A; return - A(0,0)*(A(1,1)*A(2,2)-A(1,2)*A(2,1)) - -A(1,0)*(A(0,1)*A(2,2)-A(0,2)*A(2,1)) - +A(2,0)*(A(1,2)*A(0,1)-A(1,1)*A(0,2)); + A(0,0)*(A(1,1)*A(2,2)-A(2,1)*A(1,2)) + -A(1,0)*(A(0,1)*A(2,2)-A(2,1)*A(0,2)) + +A(2,0)*(A(0,1)*A(1,2)-A(1,1)*A(0,2)); } #endif // TINYMATRIX_HPP