diff --git a/source/simple/geom/algorithm.h b/source/simple/geom/algorithm.h index 431d92c84e0bf41737274ab89edd6ddd3ef6843e..a7a3d5a7c22a7fa576d1d0715a7066cb803865ca 100644 --- a/source/simple/geom/algorithm.h +++ b/source/simple/geom/algorithm.h @@ -22,6 +22,19 @@ namespace simple::geom const vector<Coordinate, Dimensions, Order>& step, Function&& callback ); + + template <typename Coordinate, + size_t Rows, size_t Columns, + typename RowOrder, typename ColumnOrder + > + constexpr auto gauss_jordan_elimination + ( + vector< + vector<Coordinate, Columns, ColumnOrder>, + Rows, RowOrder + > + ); + } // namespace simple::geom #endif /* end of include guard */ diff --git a/source/simple/geom/algorithm.hpp b/source/simple/geom/algorithm.hpp index 06898e1b83bdef758fc34dc9939021ede51b4d6c..aedad9cac1bb5cb0bd9f6974f1eb7166ea558348 100644 --- a/source/simple/geom/algorithm.hpp +++ b/source/simple/geom/algorithm.hpp @@ -4,6 +4,8 @@ #include <functional> +#include "simple/support/algorithm.hpp" + namespace simple::geom { @@ -50,6 +52,36 @@ namespace simple::geom std::forward<Function>(callback)); } + template <typename Coordinate, + size_t Rows, size_t Columns, + typename RowOrder, typename ColumnOrder + > + constexpr auto gauss_jordan_elimination + ( + vector< + vector<Coordinate, Columns, ColumnOrder>, + Rows, RowOrder + > m + ) + { + constexpr size_t D = std::min(Rows, Columns); + for( size_t i = 0; i < D; ++i) + { + auto pivot = std::max_element(m.begin()+i, m.end(), [&i](auto a, auto b) + { + using std::abs; + return abs(a[i]) < abs(b[i]); + }); + support::swap(*pivot, m[i]); + m[i] /= +m[i][i]; // ouch, easy to shoot foot here + for(size_t j = i+1; j < D; ++j) + m[j] -= m[i] * m[j][i]; + for(size_t j = i; j --> 0;) + m[j] -= m[i] * m[j][i]; + } + return m; + } + } // namespace simple::geom #endif /* end of include guard */ diff --git a/unit_tests/algorithm.cpp b/unit_tests/algorithm.cpp index 6cf5c38dbcc49870b29660a0eaf2992df0d4d5fb..eb9592edf604197f0181bbc7ebd3f0f43be6be57 100644 --- a/unit_tests/algorithm.cpp +++ b/unit_tests/algorithm.cpp @@ -1,8 +1,10 @@ -#include "simple/geom/algorithm.hpp" #include <cmath> #include <cassert> #include <vector> +#include "simple/support/math.hpp" +#include "simple/geom/algorithm.hpp" + using namespace simple; using geom::vector; using float4 = vector<float, 4>; @@ -131,8 +133,134 @@ void MultidimentionalIteration() assert(data == test_data); } +struct rational +{ + long long num = 0; + long long den = 1; + + constexpr + bool operator < (const rational& other) const + { return num * other.den < other.num * den; } + + constexpr + bool operator == (const rational& other) const + { return num * other.den == other.num * den; } + + constexpr + rational operator+() { return *this; } const + constexpr + + rational operator*(const rational& other) const + { return {num*other.num, den*other.den}; } + constexpr + + void normalize() + { + auto gcd = std::gcd(num, den); + num /= gcd; + den /= gcd; + } + + constexpr + rational& operator/=(const rational& other) + { + den *= other.num; + num *= other.den; + normalize(); + return *this; + } + + constexpr + rational& operator-=(const rational& other) + { + num *= other.den; + num -= other.num * den; + den *= other.den; + normalize(); + return *this; + } + + constexpr + rational& operator+=(const rational& other) + { + num *= other.den; + num += other.num * den; + den *= other.den; + normalize(); + return *this; + } + + constexpr + rational operator+(const rational& other) const + { + return rational(*this)+=other;; + } +}; + +constexpr rational abs(rational r) +{ + return {support::abs(r.num) , support::abs(r.den)}; +} + +void MatrixElimination() +{ + using r = rational; + + auto inverse = gauss_jordan_elimination(vector( + vector(r{2},r{0},r{3}, r{1},r{0},r{0}), + vector(r{4},r{5},r{6}, r{0},r{1},r{0}), + vector(r{7},r{8},r{9}, r{0},r{0},r{1}) + )).mutant_clone(&vector<r,6>::last<3>); + assert ( vector(r{2},r{0},r{3})(inverse) == + vector(r{17,5}, r{-4,5}, r{-8,5})); + + assert( gauss_jordan_elimination( + vector( + vector(r{2},r{1},r{0}, r{1},r{0},r{0}), + vector(r{0},r{2},r{0}, r{0},r{1},r{0}), + vector(r{2},r{0},r{1}, r{0},r{0},r{1}) + )) + == + vector( + vector(r{1},r{0},r{0}, r{1,2},r{-1,4},r{0}), + vector(r{0},r{1},r{0}, r{0}, r{1,2}, r{0}), + vector(r{0},r{0},r{1}, r{-1}, r{1,2}, r{1}) + ) + ); + + assert( gauss_jordan_elimination( + vector( + vector(r{1},r{1},r{0}, r{1},r{0},r{0}), + vector(r{0},r{1},r{0}, r{0},r{1},r{0}), + vector(r{2},r{1},r{1}, r{0},r{0},r{1}) + )) + == + vector( + vector(r{1},r{0},r{0}, r{1}, r{-1},r{0}), + vector(r{0},r{1},r{0}, r{0}, r{1}, r{0}), + vector(r{0},r{0},r{1}, r{-2},r{1}, r{1}) + ) + ); + + assert( gauss_jordan_elimination( + vector( + vector(r{2},r{1},r{0}, r{1},r{0},r{0}), + vector(r{2},r{0},r{0}, r{0},r{1},r{0}), + vector(r{2},r{0},r{1}, r{0},r{0},r{1}) + )) + == + vector( + vector(r{1},r{0},r{0}, r{0},r{1,2},r{0}), + vector(r{0},r{1},r{0}, r{1},r{-1}, r{0}), + vector(r{0},r{0},r{1}, r{0},r{-1}, r{1}) + ) + ); + +} + int main() { Basic(); MultidimentionalIteration(); + MatrixElimination(); }