LCOV - code coverage report
Current view: top level - include/Kinematics - Matrix.ipp (source / functions) Hit Total Coverage
Test: HLR Lines: 191 191 100.0 %
Date: 2018-01-10 14:31:03 Functions: 130 134 97.0 %

          Line data    Source code
       1             : #include "Matrix.hpp"
       2             : 
       3             : #include <cmath>
       4             : #include <iomanip>
       5             : #include <sstream>
       6             : #include <stdexcept>
       7             : 
       8             : #include <iostream>
       9             : 
      10             : namespace HLR
      11             : {
      12             : namespace Kinematics
      13             : {
      14             : template<typename T, std::size_t m, std::size_t n>
      15             : constexpr Matrix<T, m, n> Matrix<T, m, n>::get_identity() noexcept
      16             : {
      17             :     auto identity = Matrix<T, m, m>();
      18             : 
      19             :     for (std::size_t i = 0; i < m; ++i)
      20             :     {
      21             :         identity[i][i] = 1;
      22             :     }
      23             : 
      24             :     return identity;
      25             : }
      26             : 
      27             : template<typename T, std::size_t m, std::size_t n>
      28           3 : constexpr Matrix<T, m, n>::Matrix(
      29           3 :     const std::initializer_list<std::initializer_list<T>> matrix_data)
      30             : {
      31           3 :     if (matrix_data.size() != m)
      32             :     {
      33           1 :         throw std::invalid_argument("Invalid amount of rows.");
      34             :     }
      35             : 
      36           3 :     for (const auto& col : matrix_data)
      37             :     {
      38           2 :         if (col.size() != n)
      39             :         {
      40           1 :             throw std::invalid_argument("Invalid amount of columns.");
      41             :         }
      42             :     }
      43             : 
      44           1 :     std::size_t pos_i = 0;
      45           1 :     std::size_t pos_j = 0;
      46             : 
      47           2 :     for (auto i = matrix_data.begin(); i != matrix_data.end(); ++i)
      48             :     {
      49           2 :         for (auto j = i->begin(); j != i->end(); ++j)
      50             :         {
      51           1 :             this->data[pos_i][pos_j] = *j;
      52           1 :             ++pos_j;
      53             :         }
      54           1 :         ++pos_i;
      55           1 :         pos_j = 0;
      56             :     }
      57           1 : }
      58             : 
      59             : template<typename T, std::size_t m, std::size_t n>
      60      877436 : constexpr std::size_t Matrix<T, m, n>::get_m() const noexcept
      61             : {
      62      877436 :     return m;
      63             : }
      64             : 
      65             : template<typename T, std::size_t m, std::size_t n>
      66           1 : constexpr std::size_t Matrix<T, m, n>::get_n() const noexcept
      67             : {
      68           1 :     return n;
      69             : }
      70             : 
      71             : template<typename T, std::size_t m, std::size_t n>
      72    53819894 : constexpr std::array<T, n>& Matrix<T, m, n>::operator[](
      73             :     const std::size_t pos) noexcept
      74             : {
      75    53819894 :     return this->data[pos];
      76             : }
      77             : 
      78             : template<typename T, std::size_t m, std::size_t n>
      79    24159279 : constexpr const std::array<T, n>& Matrix<T, m, n>::operator[](
      80             :     const std::size_t pos) const noexcept
      81             : {
      82    24159279 :     return this->data[pos];
      83             : }
      84             : 
      85             : template<typename T, std::size_t m, std::size_t n>
      86           2 : constexpr std::array<T, n>& Matrix<T, m, n>::at(const std::size_t pos)
      87             : {
      88           2 :     if (pos >= m)
      89             :     {
      90           1 :         throw std::out_of_range("Index out of range.");
      91             :     }
      92             : 
      93           1 :     return this->data.at(pos);
      94             : }
      95             : 
      96             : template<typename T, std::size_t m, std::size_t n>
      97           2 : constexpr const std::array<T, n>& Matrix<T, m, n>::at(
      98             :     const std::size_t pos) const
      99             : {
     100           2 :     if (pos >= m)
     101             :     {
     102           1 :         throw std::out_of_range("Index out of range.");
     103             :     }
     104             : 
     105           1 :     return this->data.at(pos);
     106             : }
     107             : 
     108             : template<typename T, std::size_t m, std::size_t n>
     109          43 : constexpr bool Matrix<T, m, n>::fuzzy_equal(const Matrix<T, m, n>& mat,
     110             :                                             T fuzz) const noexcept
     111             : {
     112         146 :     for (std::size_t i = 0; i < m; ++i)
     113             :     {
     114         436 :         for (std::size_t j = 0; j < n; ++j)
     115             :         {
     116         333 :             if (std::abs((*this)[i][j] - mat[i][j]) > fuzz)
     117             :             {
     118           4 :                 return false;
     119             :             }
     120             :         }
     121             :     }
     122             : 
     123          39 :     return true;
     124             : }
     125             : 
     126             : template<typename T, std::size_t m, std::size_t n>
     127          36 : constexpr bool Matrix<T, m, n>::operator==(const Matrix<T, m, n>& mat) noexcept
     128             : {
     129          36 :     return this->fuzzy_equal(mat, 0);
     130             : }
     131             : 
     132             : template<typename T, std::size_t m, std::size_t n>
     133           3 : constexpr bool Matrix<T, m, n>::operator!=(const Matrix<T, m, n>& mat) noexcept
     134             : {
     135           3 :     return !(*this == mat);
     136             : }
     137             : 
     138             : template<typename T, std::size_t m, std::size_t n>
     139           8 : constexpr void Matrix<T, m, n>::operator+=(T scalar) noexcept
     140             : {
     141          24 :     for (std::size_t i = 0; i < m; ++i)
     142             :     {
     143          64 :         for (std::size_t j = 0; j < n; ++j)
     144             :         {
     145          48 :             (*this)[i][j] += scalar;
     146             :         }
     147             :     }
     148           8 : }
     149             : 
     150             : template<typename T, std::size_t m, std::size_t n>
     151           4 : constexpr void Matrix<T, m, n>::operator-=(T scalar) noexcept
     152             : {
     153           4 :     (*this) += -1 * scalar;
     154           4 : }
     155             : 
     156             : template<typename T, std::size_t m, std::size_t n>
     157     1057668 : constexpr void Matrix<T, m, n>::operator*=(T scalar) noexcept
     158             : {
     159     4579208 :     for (std::size_t i = 0; i < m; ++i)
     160             :     {
     161     7043104 :         for (std::size_t j = 0; j < n; ++j)
     162             :         {
     163     3521564 :             (*this)[i][j] *= scalar;
     164             :         }
     165             :     }
     166     1057668 : }
     167             : 
     168             : template<typename T, std::size_t m, std::size_t n>
     169           4 : constexpr void Matrix<T, m, n>::operator/=(T scalar) noexcept
     170             : {
     171          12 :     for (std::size_t i = 0; i < m; ++i)
     172             :     {
     173          32 :         for (std::size_t j = 0; j < n; ++j)
     174             :         {
     175          24 :             (*this)[i][j] /= scalar;
     176             :         }
     177             :     }
     178           4 : }
     179             : 
     180             : template<typename T, std::size_t m, std::size_t n>
     181      877346 : constexpr void Matrix<T, m, n>::operator+=(const Matrix<T, m, n>& mat) noexcept
     182             : {
     183     3689673 :     for (std::size_t i = 0; i < m; ++i)
     184             :     {
     185     5624670 :         for (std::size_t j = 0; j < n; ++j)
     186             :         {
     187     2812343 :             (*this)[i][j] += mat[i][j];
     188             :         }
     189             :     }
     190      877346 : }
     191             : 
     192             : template<typename T, std::size_t m, std::size_t n>
     193      528829 : constexpr void Matrix<T, m, n>::operator-=(const Matrix<T, m, n>& mat) noexcept
     194             : {
     195      528829 :     (*this) += mat * static_cast<T>(-1);
     196      528829 : }
     197             : 
     198             : template<typename T, std::size_t m, std::size_t n>
     199      348516 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator+(
     200             :     const Matrix<T, m, n>& mat) const noexcept
     201             : {
     202      348516 :     Matrix<T, m, n> copy(*this);
     203      348516 :     copy += mat;
     204      348516 :     return copy;
     205             : }
     206             : 
     207             : template<typename T, std::size_t m, std::size_t n>
     208      528828 : constexpr Matrix<T, m, n> Matrix<T, m, n>::operator-(
     209             :     const Matrix<T, m, n>& mat) const noexcept
     210             : {
     211      528828 :     Matrix<T, m, n> copy(*this);
     212      528828 :     copy -= mat;
     213      528828 :     return copy;
     214             : }
     215             : 
     216             : template<typename T, std::size_t m, std::size_t n>
     217             : template<std::size_t p>
     218      528848 : constexpr Matrix<T, m, p> Matrix<T, m, n>::operator*(
     219             :     const Matrix<T, n, p>& mat) const noexcept
     220             : {
     221      528848 :     Matrix<T, m, p> product;
     222             : 
     223     1418286 :     for (std::size_t i = 0; i < m; ++i)
     224             :     {
     225     1778892 :         for (std::size_t j = 0; j < p; ++j)
     226             :         {
     227     3557832 :             for (std::size_t k = 0; k < n; ++k)
     228             :             {
     229     2668378 :                 product[i][j] += (*this)[i][k] * mat[k][j];
     230             :             }
     231             :         }
     232             :     }
     233             : 
     234      528848 :     return product;
     235             : }
     236             : 
     237             : template<typename T, std::size_t m, std::size_t n>
     238             : template<std::size_t p>
     239      180299 : constexpr Matrix<T, m, n + p> Matrix<T, m, n>::horizontal_augment(
     240             :     const Matrix<T, m, p>& mat) const noexcept
     241             : {
     242      180299 :     Matrix<T, m, n + p> augmented;
     243             : 
     244      721196 :     for (std::size_t i = 0; i < m; ++i)
     245             :     {
     246     2163586 :         for (std::size_t j = 0; j < n; ++j)
     247             :         {
     248     1622689 :             augmented[i][j] = (*this)[i][j];
     249             :         }
     250             : 
     251     2163590 :         for (std::size_t j = n; j < n + p; ++j)
     252             :         {
     253     1622693 :             augmented[i][j] = mat[i][j - n];
     254             :         }
     255             :     }
     256             : 
     257      180299 :     return augmented;
     258             : }
     259             : 
     260             : template<typename T, std::size_t m, std::size_t n>
     261             : template<std::size_t p>
     262           2 : constexpr Matrix<T, m + p, n> Matrix<T, m, n>::vertical_augment(
     263             :     const Matrix<T, p, n>& mat) const noexcept
     264             : {
     265           2 :     Matrix<T, m + p, n> augmented;
     266             : 
     267           5 :     for (std::size_t i = 0; i < n; ++i)
     268             :     {
     269          10 :         for (std::size_t j = 0; j < m; ++j)
     270             :         {
     271           7 :             augmented[j][i] = (*this)[j][i];
     272             :         }
     273             : 
     274          11 :         for (std::size_t j = m; j < m + p; ++j)
     275             :         {
     276           8 :             augmented[j][i] = mat[j - m][i];
     277             :         }
     278             :     }
     279             : 
     280           2 :     return augmented;
     281             : }
     282             : 
     283             : template<typename T, std::size_t m, std::size_t n>
     284             : template<std::size_t y, std::size_t x, std::size_t p, std::size_t q>
     285      528826 : constexpr Matrix<T, p, q> Matrix<T, m, n>::slice() const noexcept
     286             : {
     287      528826 :     Matrix<T, p, q> mat;
     288             : 
     289     2115304 :     for (std::size_t i = y; i < m && i - y < p; ++i)
     290             :     {
     291     4254739 :         for (std::size_t j = x; j < n && j - x < q; ++j)
     292             :         {
     293     2668261 :             mat[i - y][j - x] = (*this)[i][j];
     294             :         }
     295             :     }
     296             : 
     297      528826 :     return mat;
     298             : }
     299             : 
     300             : template<typename T, std::size_t m, std::size_t n>
     301      348556 : constexpr Matrix<T, n, m> Matrix<T, m, n>::transpose() const noexcept
     302             : {
     303      348556 :     Matrix<T, n, m> transposed;
     304             : 
     305     1394220 :     for (std::size_t i = 0; i < m; ++i)
     306             :     {
     307     2091340 :         for (std::size_t j = 0; j < n; ++j)
     308             :         {
     309     1045676 :             transposed[j][i] = (*this)[i][j];
     310             :         }
     311             :     }
     312             : 
     313      348556 :     return transposed;
     314             : }
     315             : 
     316             : template<typename T, std::size_t m, std::size_t n>
     317      180297 : constexpr std::optional<Matrix<T, m, n>> Matrix<T, m, n>::inverse() const
     318             :     noexcept
     319             : {
     320             :     static_assert(m == n,
     321             :                   "Inverse matrix is only defined for square matrices.");
     322             : 
     323      180297 :     auto identity = Matrix<T, m, n>::get_identity();
     324      180297 :     auto augmented = this->horizontal_augment(identity);
     325             : 
     326      721185 :     for (std::size_t i = 0; i < m; ++i)
     327             :     {
     328             :         /**
     329             :          *  Find the diagonal element closest to -1 or 1.
     330             :          *  And if it isn't in the current row swap it.
     331             :          *  This is done to make the inverse mroe numerically stable.
     332             :          *  Also check if that the row may not be zero since that would
     333             :          *  result in a divide by zero error.
     334             :          */
     335      540890 :         std::size_t min_row_index = i;
     336      540890 :         std::optional<T> closest_to_one;
     337             : 
     338     1622673 :         for (std::size_t j = min_row_index; j < m; ++j)
     339             :         {
     340     1081783 :             if (std::abs(augmented[j][i]) < 0.000000001)
     341             :             {
     342      230325 :                 continue;
     343             :             }
     344             : 
     345      851458 :             T abs_value = std::abs(1 - std::abs(augmented[j][i]));
     346             : 
     347     1702916 :             if (!closest_to_one.has_value()
     348      851458 :                 || abs_value < closest_to_one.value())
     349             :             {
     350      766756 :                 closest_to_one = abs_value;
     351      766756 :                 min_row_index = j;
     352             :             }
     353             :         }
     354             : 
     355      540890 :         if (!closest_to_one.has_value())
     356             :         {
     357           2 :             return std::nullopt;
     358             :         }
     359             : 
     360      540888 :         if (min_row_index != i)
     361             :         {
     362     1931271 :             for (std::size_t j = 0; j < 2 * m; ++j)
     363             :             {
     364     1655376 :                 T tmp = augmented[i][j];
     365     1655376 :                 augmented[i][j] = augmented[min_row_index][j];
     366     1655376 :                 augmented[min_row_index][j] = tmp;
     367             :             }
     368             :         }
     369             : 
     370             :         // Divide the current row by a scalar so that the current diagonal
     371             :         // element will be 1.
     372      540888 :         auto divisor = augmented[i][i];
     373             : 
     374     3786224 :         for (std::size_t j = 0; j < 2 * m; ++j)
     375             :         {
     376     3245336 :             augmented[i][j] /= divisor;
     377             :         }
     378             : 
     379             :         // Set all other rows to 0 by using the row that is 1.
     380     2163556 :         for (std::size_t j = 0; j < m; ++j)
     381             :         {
     382     1622668 :             if (i == j)
     383             :             {
     384      540888 :                 continue;
     385             :             }
     386             : 
     387     1081780 :             auto multiplier = augmented[j][i];
     388             : 
     389     7572484 :             for (std::size_t k = 0; k < 2 * m; ++k)
     390             :             {
     391     6490704 :                 augmented[j][k] -= multiplier * augmented[i][k];
     392             :             }
     393             :         }
     394             :     }
     395             : 
     396      180295 :     return augmented.template slice<0, m, m, m>();
     397             : }
     398             : 
     399             : template<typename T, std::size_t m, std::size_t n>
     400      348553 : constexpr T Matrix<T, m, n>::get_magnitude() const noexcept
     401             : {
     402             :     static_assert(
     403             :         m == 1 || n == 1,
     404             :         "Magnitude of a matrix is only defined for column and row matrices.");
     405             : 
     406      348553 :     T magnitude = 0;
     407             : 
     408             :     if constexpr (m == 1) // NOLINT
     409             :     {
     410           1 :         magnitude =
     411           2 :             static_cast<T>(std::sqrt(((*this) * this->transpose())[0][0]));
     412             :     }
     413             :     else
     414             :     {
     415      348552 :         magnitude =
     416      697104 :             static_cast<T>(std::sqrt((this->transpose() * (*this))[0][0]));
     417             :     }
     418             : 
     419      348553 :     return magnitude;
     420             : }
     421             : 
     422             : template<typename T, std::size_t m, std::size_t n>
     423           1 : std::string Matrix<T, m, n>::str() const
     424             : {
     425           2 :     std::stringstream ss;
     426           1 :     ss << std::setprecision(9) << std::setw(6) << std::fixed;
     427             : 
     428           3 :     for (std::size_t i = 0; i < m; ++i)
     429             :     {
     430           6 :         for (std::size_t j = 0; j < n; ++j)
     431             :         {
     432           4 :             ss << (*this)[i][j] << "\t";
     433             :         }
     434           2 :         ss << std::endl;
     435             :     }
     436             : 
     437           2 :     return ss.str();
     438             : }
     439             : 
     440             : template<typename T, std::size_t m, std::size_t n>
     441           2 : constexpr Matrix<T, m, n> operator+(const Matrix<T, m, n>& lhs, T rhs) noexcept
     442             : {
     443           2 :     Matrix<T, m, n> copy(lhs);
     444           2 :     copy += rhs;
     445           2 :     return copy;
     446             : }
     447             : 
     448             : template<typename T, std::size_t m, std::size_t n>
     449           1 : constexpr Matrix<T, m, n> operator+(T lhs, const Matrix<T, m, n>& rhs) noexcept
     450             : {
     451           1 :     Matrix<T, m, n> copy(rhs);
     452           1 :     copy += lhs;
     453           1 :     return copy;
     454             : }
     455             : 
     456             : template<typename T, std::size_t m, std::size_t n>
     457           2 : constexpr Matrix<T, m, n> operator-(const Matrix<T, m, n>& lhs, T rhs) noexcept
     458             : {
     459           2 :     Matrix<T, m, n> copy(lhs);
     460           2 :     copy -= rhs;
     461           2 :     return copy;
     462             : }
     463             : 
     464             : template<typename T, std::size_t m, std::size_t n>
     465           1 : constexpr Matrix<T, m, n> operator-(T lhs, const Matrix<T, m, n>& rhs) noexcept
     466             : {
     467           1 :     Matrix<T, m, n> copy(rhs);
     468           1 :     copy -= lhs;
     469           1 :     return copy;
     470             : }
     471             : 
     472             : template<typename T, std::size_t m, std::size_t n>
     473      709124 : constexpr Matrix<T, m, n> operator*(const Matrix<T, m, n>& lhs, T rhs) noexcept
     474             : {
     475      709124 :     Matrix<T, m, n> copy(lhs);
     476      709124 :     copy *= rhs;
     477      709124 :     return copy;
     478             : }
     479             : 
     480             : template<typename T, std::size_t m, std::size_t n>
     481      180294 : constexpr Matrix<T, m, n> operator*(T lhs, const Matrix<T, m, n>& rhs) noexcept
     482             : {
     483      180294 :     Matrix<T, m, n> copy(rhs);
     484      180294 :     copy *= lhs;
     485      180294 :     return copy;
     486             : }
     487             : 
     488             : template<typename T, std::size_t m, std::size_t n>
     489           2 : constexpr Matrix<T, m, n> operator/(const Matrix<T, m, n>& lhs, T rhs) noexcept
     490             : {
     491           2 :     Matrix<T, m, n> copy(lhs);
     492           2 :     copy /= rhs;
     493           2 :     return copy;
     494             : }
     495             : 
     496             : template<typename T, std::size_t m, std::size_t n>
     497           1 : constexpr Matrix<T, m, n> operator/(T lhs, const Matrix<T, m, n>& rhs) noexcept
     498             : {
     499           1 :     Matrix<T, m, n> copy(rhs);
     500           1 :     copy /= lhs;
     501           1 :     return copy;
     502             : }
     503             : 
     504             : template<typename T, std::size_t m, std::size_t n>
     505           1 : std::ostream& operator<<(std::ostream& os, const Matrix<T, m, n>& mat)
     506             : {
     507           1 :     return os << mat.str();
     508             : }
     509             : 
     510             : } // namespace Kinematics
     511             : } // namespace HLR

Generated by: LCOV version 1.12