diff --git a/BasicLinearAlgebra.h b/BasicLinearAlgebra.h index ea35148..be3c413 100644 --- a/BasicLinearAlgebra.h +++ b/BasicLinearAlgebra.h @@ -177,5 +177,6 @@ using DownCast = MatrixBase -inline void bla_swap(T &a, T &b) +template +void Swap(MatrixBase &A, + MatrixBase &B) { - T tmp = a; - a = b; - b = tmp; + Dtype tmp; + for (int i = 0; i < ParentType::Rows; i++) + { + for (int j = 0; j < ParentType::Cols; j++) + { + tmp = A(i, j); + A(i, j) = B(i, j); + B(i, j) = tmp; + } + } } template @@ -135,14 +143,19 @@ LUDecomposition LUDecompose(MatrixBase Inverse( return out; } -template -typename ParentType::DType Determinant(const MatrixBase &A) +// LU-Decomposition only works for floating point numbers. Use Bareiss algorithm for (signed) integer types. +template +typename Types::enable_if::value, Dtype>::type +DeterminantLUDecomposition(const MatrixBase &A) { - Matrix A_copy = A; + Matrix A_copy = A; auto decomp = LUDecompose(A_copy); - typename ParentType::DType det = decomp.parity; + Dtype det = decomp.parity; for (int i = 0; i < Dim; ++i) { @@ -337,6 +352,51 @@ typename ParentType::DType Determinant(const MatrixBase +typename Types::enable_if::value, Dtype>::type +DeterminantBareissAlgorithm(const MatrixBase &A) +{ + Matrix A_copy = A; + + int sign = 1; + Dtype prev = 1; + + for (int i = 0; i < Dim; i++) + { + if (A_copy(i, i) == 0) + { + int j = i + 1; + for (; j < Dim; j++) + { + if (A_copy(j, i) != 0) break; + } + if (j == Dim) return 0; + auto row_i = A_copy.Row(i); + auto row_j = A_copy.Row(j); + Swap(row_i, row_j); + sign = - sign; + } + for (int j = i + 1; j < Dim; j++) + { + for (int k = i + 1; k < Dim; k++) + { + A_copy(j, k) = (A_copy(j, k) * A_copy(i, i) - A_copy(j, i) * A_copy(i, k)) / prev; + } + } + prev = A_copy(i, i); + } + return sign * A_copy(Dim - 1, Dim - 1); +} + +template +typename Types::enable_if::value, Dtype>::type +Determinant(const MatrixBase &A) { return DeterminantLUDecomposition(A); } + +template +typename Types::enable_if::value, Dtype>::type +Determinant(const MatrixBase &A) { return DeterminantBareissAlgorithm(A); } + template typename DerivedType::DType Norm(const DownCast &A) { diff --git a/impl/Types.h b/impl/Types.h new file mode 100644 index 0000000..6e2bdc7 --- /dev/null +++ b/impl/Types.h @@ -0,0 +1,46 @@ +#pragma once + +namespace BLA +{ +// This namespace exists because the header "typetraits" is not implemented in every Arduino environment. +namespace Types +{ +template struct is_same { static constexpr bool value = false; }; +template struct is_same { static constexpr bool value = true; }; + +template struct remove_const { typedef T type; }; +template struct remove_const { typedef T type; }; + +template +struct is_floating_point +{ + static constexpr bool value = + is_same::type>::value || + is_same::type>::value || + is_same::type>::value; +}; + +template +struct is_signed_integer +{ + static constexpr bool value = + is_same::type>::value || + is_same::type>::value || + is_same::type>::value || + is_same::type>::value || + is_same::type>::value; +}; + +template +struct is_signed +{ + static constexpr bool value = + is_floating_point::value || + is_signed_integer::value; +}; + +template struct enable_if {}; +template struct enable_if { typedef T type; }; +} +} // namespace BLA + diff --git a/test/test_linear_algebra.cpp b/test/test_linear_algebra.cpp index 3589a9e..f45d9ed 100644 --- a/test/test_linear_algebra.cpp +++ b/test/test_linear_algebra.cpp @@ -184,6 +184,15 @@ TEST(Arithmetic, Determinant) BLA::Matrix<3, 3> singular = {1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0}; EXPECT_FLOAT_EQ(Determinant(singular), 0.0); + + BLA::Matrix<4, 4, int16_t> C = {8, 5, 5, 8, 3, 1, 3, 2, 1, 1, 3, 0, 3, 3, 5, 9}; + int16_t det_C_expected = -140; + + EXPECT_EQ(Determinant(C), det_C_expected); + + BLA::Matrix<3, 3, int> singular_int = {1, 0, 0, 1, 0, 0, 1, 0, 0}; + + EXPECT_EQ(Determinant(singular_int), 0); } template