├── .clang-format
├── .github
    └── workflows
    │   └── run_tests.yml
├── .gitignore
├── BasicLinearAlgebra.h
├── ElementStorage.h
├── LICENSE
├── README.md
├── examples
    ├── CustomMatrix
    │   └── CustomMatrix.ino
    ├── HowToUse
    │   └── HowToUse.ino
    ├── References
    │   └── References.ino
    ├── SolveLinearEquations
    │   └── SolveLinearEquations.ino
    └── Tensor
    │   └── Tensor.ino
├── impl
    ├── BasicLinearAlgebra.h
    ├── NotSoBasicLinearAlgebra.h
    └── Types.h
├── keywords.txt
├── library.properties
└── test
    ├── Arduino.h
    ├── CMakeLists.txt
    ├── Printable.h
    ├── test_arithmetic.cpp
    ├── test_examples.cpp
    └── test_linear_algebra.cpp


/.clang-format:
--------------------------------------------------------------------------------
1 | BasedOnStyle: Google
2 | ColumnLimit: 120
3 | IndentWidth: 4
4 | BreakBeforeBraces: Allman


--------------------------------------------------------------------------------
/.github/workflows/run_tests.yml:
--------------------------------------------------------------------------------
 1 | name: run_tests
 2 | 
 3 | on:
 4 |   push:
 5 |     branches: [ master ]
 6 |   pull_request:
 7 |     branches: [ master ]
 8 | 
 9 | env:
10 |   BUILD_TYPE: Release
11 | 
12 | jobs:
13 |   build:
14 |     runs-on: ubuntu-latest
15 | 
16 |     steps:
17 |     - uses: actions/checkout@v2
18 | 
19 |     - name: Configure CMake
20 |       run: cd test && cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
21 | 
22 |     - name: Build
23 |       run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
24 | 
25 |     - name: Test
26 |       working-directory: ${{github.workspace}}/build
27 |       run: ctest --rerun-failed --output-on-failure -C ${{env.BUILD_TYPE}}
28 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | **/build/**
2 | **/.vscode/**


--------------------------------------------------------------------------------
/BasicLinearAlgebra.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <math.h>
  4 | #include <stdlib.h>
  5 | #include <string.h>
  6 | 
  7 | #include "Arduino.h"
  8 | #include "Printable.h"
  9 | 
 10 | #include "ElementStorage.h"
 11 | 
 12 | namespace BLA
 13 | {
 14 | 
 15 | template <typename DerivedType, int rows, int cols, typename d_type>
 16 | struct MatrixBase : public Printable
 17 | {
 18 |    public:
 19 |     constexpr static int Rows = rows;
 20 |     constexpr static int Cols = cols;
 21 |     using DType = d_type;
 22 | 
 23 |     DType &operator()(int i, int j = 0) { return static_cast<DerivedType *>(this)->operator()(i, j); }
 24 | 
 25 |     DType operator()(int i, int j = 0) const { return static_cast<const DerivedType *>(this)->operator()(i, j); }
 26 | 
 27 |     MatrixBase() = default;
 28 | 
 29 |     template <typename MatType>
 30 |     MatrixBase(const MatrixBase<MatType, Rows, Cols, DType> &mat)
 31 |     {
 32 |         for (int i = 0; i < rows; ++i)
 33 |         {
 34 |             for (int j = 0; j < cols; ++j)
 35 |             {
 36 |                 static_cast<DerivedType &>(*this)(i, j) = mat(i, j);
 37 |             }
 38 |         }
 39 |     }
 40 | 
 41 |     MatrixBase &operator=(const MatrixBase &mat)
 42 |     {
 43 |         for (int i = 0; i < rows; ++i)
 44 |         {
 45 |             for (int j = 0; j < cols; ++j)
 46 |             {
 47 |                 static_cast<DerivedType &>(*this)(i, j) = mat(i, j);
 48 |             }
 49 |         }
 50 | 
 51 |         return static_cast<DerivedType &>(*this);
 52 |     }
 53 | 
 54 |     template <typename MatType>
 55 |     MatrixBase &operator=(const MatrixBase<MatType, Rows, Cols, DType> &mat)
 56 |     {
 57 |         for (int i = 0; i < rows; ++i)
 58 |         {
 59 |             for (int j = 0; j < cols; ++j)
 60 |             {
 61 |                 static_cast<DerivedType &>(*this)(i, j) = mat(i, j);
 62 |             }
 63 |         }
 64 | 
 65 |         return static_cast<DerivedType &>(*this);
 66 |     }
 67 | 
 68 |     DerivedType &operator=(DType elem)
 69 |     {
 70 |         for (int i = 0; i < rows; ++i)
 71 |         {
 72 |             for (int j = 0; j < cols; ++j)
 73 |             {
 74 |                 static_cast<DerivedType &>(*this)(i, j) = elem;
 75 |             }
 76 |         }
 77 | 
 78 |         return static_cast<DerivedType &>(*this);
 79 |     }
 80 | 
 81 |     void Fill(const DType &val) { *this = val; }
 82 | 
 83 |     template <typename DestType>
 84 |     Matrix<Rows, Cols, DestType> Cast()
 85 |     {
 86 |         Matrix<Rows, Cols, DestType> ret;
 87 | 
 88 |         for (int i = 0; i < rows; ++i)
 89 |         {
 90 |             for (int j = 0; j < cols; ++j)
 91 |             {
 92 |                 ret(i, j) = (DestType)(*this)(i, j);
 93 |             }
 94 |         }
 95 | 
 96 |         return ret;
 97 |     }
 98 | 
 99 |     template <int SubRows, int SubCols>
100 |     RefMatrix<DerivedType, SubRows, SubCols> Submatrix(int row_start, int col_start)
101 |     {
102 |         return RefMatrix<DerivedType, SubRows, SubCols>(static_cast<DerivedType &>(*this), row_start, col_start);
103 |     }
104 | 
105 |     template <int SubRows, int SubCols>
106 |     RefMatrix<const DerivedType, SubRows, SubCols> Submatrix(int row_start, int col_start) const
107 |     {
108 |         return RefMatrix<const DerivedType, SubRows, SubCols>(static_cast<const DerivedType &>(*this), row_start,
109 |                                                               col_start);
110 |     }
111 | 
112 |     RefMatrix<DerivedType, 1, Cols> Row(int row_start)
113 |     {
114 |         return RefMatrix<DerivedType, 1, Cols>(static_cast<DerivedType &>(*this), row_start, 0);
115 |     }
116 | 
117 |     RefMatrix<const DerivedType, 1, Cols> Row(int row_start) const
118 |     {
119 |         return RefMatrix<const DerivedType, 1, Cols>(static_cast<const DerivedType &>(*this), row_start, 0);
120 |     }
121 | 
122 |     RefMatrix<DerivedType, Rows, 1> Column(int col_start)
123 |     {
124 |         return RefMatrix<DerivedType, Rows, 1>(static_cast<DerivedType &>(*this), 0, col_start);
125 |     }
126 | 
127 |     RefMatrix<const DerivedType, Rows, 1> Column(int col_start) const
128 |     {
129 |         return RefMatrix<const DerivedType, Rows, 1>(static_cast<const DerivedType &>(*this), 0, col_start);
130 |     }
131 | 
132 |     MatrixTranspose<DerivedType> operator~() { return MatrixTranspose<DerivedType>(static_cast<DerivedType &>(*this)); }
133 | 
134 |     MatrixTranspose<const DerivedType> operator~() const
135 |     {
136 |         return MatrixTranspose<const DerivedType>(static_cast<const DerivedType &>(*this));
137 |     }
138 | 
139 |     Matrix<Rows, Cols, DType> operator-() const
140 |     {
141 |         Matrix<Rows, Cols, DType> ret;
142 | 
143 |         for (int i = 0; i < rows; ++i)
144 |         {
145 |             for (int j = 0; j < cols; ++j)
146 |             {
147 |                 ret(i, j) = -(*this)(i, j);
148 |             }
149 |         }
150 | 
151 |         return ret;
152 |     }
153 | 
154 |     size_t printTo(Print& p) const final
155 |     {
156 |         size_t n;
157 |         n = p.print('[');
158 | 
159 |         for (int i = 0; i < Rows; i++)
160 |         {
161 |             n += p.print('[');
162 | 
163 |             for (int j = 0; j < Cols; j++)
164 |             {
165 |                 n += p.print(static_cast<const DerivedType *>(this)->operator()(i, j));
166 |                 n += p.print((j == Cols - 1) ? ']' : ',');
167 |             }
168 | 
169 |             n += p.print((i == Rows - 1) ? ']' : ',');
170 |         }
171 |         return n;
172 |     }
173 | };
174 | 
175 | template <typename DerivedType>
176 | using DownCast = MatrixBase<DerivedType, DerivedType::Rows, DerivedType::Cols, typename DerivedType::DType>;
177 | 
178 | }  // namespace BLA
179 | 
180 | #include "impl/Types.h"
181 | #include "impl/BasicLinearAlgebra.h"
182 | #include "impl/NotSoBasicLinearAlgebra.h"
183 | 


--------------------------------------------------------------------------------
/ElementStorage.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | namespace BLA
  4 | {
  5 | 
  6 | template <typename DerivedType, int rows, int cols, typename DType>
  7 | struct MatrixBase;
  8 | 
  9 | template <int Rows, int Cols = 1, typename DType = float>
 10 | class Matrix : public MatrixBase<Matrix<Rows, Cols, DType>, Rows, Cols, DType>
 11 | {
 12 |    public:
 13 |     DType storage[Rows * Cols];
 14 | 
 15 |     DType &operator()(int i, int j = 0) { return storage[i * Cols + j]; }
 16 |     DType operator()(int i, int j = 0) const { return storage[i * Cols + j]; }
 17 | 
 18 |     Matrix() = default;
 19 | 
 20 |     template <typename... TAIL>
 21 |     Matrix(DType head, TAIL... args)
 22 |     {
 23 |         FillRowMajor(0, head, args...);
 24 |     }
 25 | 
 26 |     template <typename... TAIL>
 27 |     void FillRowMajor(int start_idx, DType head, TAIL... tail)
 28 |     {
 29 |         static_assert(Rows * Cols > sizeof...(TAIL), "Too many arguments passed to FillRowMajor");
 30 | 
 31 |         (*this)(start_idx / Cols, start_idx % Cols) = head;
 32 | 
 33 |         FillRowMajor(++start_idx, tail...);
 34 |     }
 35 | 
 36 |     void FillRowMajor(int start_idx)
 37 |     {
 38 |         for (int i = start_idx; i < Rows * Cols; ++i)
 39 |         {
 40 |             (*this)(i / Cols, i % Cols) = 0.0;
 41 |         }
 42 |     }
 43 | 
 44 |     template <typename DerivedType>
 45 |     Matrix(const MatrixBase<DerivedType, Rows, Cols, DType> &mat)
 46 |     {
 47 |         static_cast<MatrixBase<Matrix<Rows, Cols, DType>, Rows, Cols, DType> &>(*this) = mat;
 48 |     }
 49 | 
 50 |     Matrix &operator=(const Matrix &mat)
 51 |     {
 52 |         static_cast<MatrixBase<Matrix<Rows, Cols, DType>, Rows, Cols, DType> &>(*this) = mat;
 53 |         return *this;
 54 |     }
 55 | };
 56 | 
 57 | template <int Rows, int Cols = 1, typename DType = float>
 58 | class Zeros : public MatrixBase<Zeros<Rows, Cols, DType>, Rows, Cols, DType>
 59 | {
 60 |    public:
 61 |     DType operator()(int i, int j = 0) const { return 0.0; }
 62 | 
 63 |     Zeros() = default;
 64 | };
 65 | 
 66 | template <int Rows, int Cols = 1, typename DType = float>
 67 | class Ones : public MatrixBase<Ones<Rows, Cols, DType>, Rows, Cols, DType>
 68 | {
 69 |    public:
 70 |     DType operator()(int i, int j = 0) const { return 1.0; }
 71 | 
 72 |     Ones() = default;
 73 | };
 74 | 
 75 | template <int Rows, int Cols = 1, typename DType = float>
 76 | class Eye : public MatrixBase<Eye<Rows, Cols, DType>, Rows, Cols, DType>
 77 | {
 78 |    public:
 79 |     DType operator()(int i, int j = 0) const { return i == j; }
 80 | 
 81 |     Eye() = default;
 82 | };
 83 | 
 84 | template <typename RefType, int Rows, int Cols>
 85 | class RefMatrix : public MatrixBase<RefMatrix<RefType, Rows, Cols>, Rows, Cols, typename RefType::DType>
 86 | {
 87 |     RefType &parent_;
 88 |     const int row_offset_;
 89 |     const int col_offset_;
 90 | 
 91 |    public:
 92 |     explicit RefMatrix(RefType &parent, int row_offset = 0, int col_offset = 0)
 93 |         : parent_(parent), row_offset_(row_offset), col_offset_(col_offset)
 94 |     {
 95 |     }
 96 | 
 97 |     typename RefType::DType &operator()(int i, int j) { return parent_(i + row_offset_, j + col_offset_); }
 98 |     typename RefType::DType operator()(int i, int j) const { return parent_(i + row_offset_, j + col_offset_); }
 99 | 
100 |     template <typename MatType>
101 |     RefMatrix &operator=(const MatType &mat)
102 |     {
103 |         static_cast<MatrixBase<RefMatrix<RefType, Rows, Cols>, Rows, Cols, typename RefType::DType> &>(*this) = mat;
104 |         return *this;
105 |     }
106 | };
107 | 
108 | template <typename RefType>
109 | class MatrixTranspose
110 |     : public MatrixBase<MatrixTranspose<RefType>, RefType::Cols, RefType::Rows, typename RefType::DType>
111 | {
112 |     RefType &parent_;
113 | 
114 |    public:
115 |     explicit MatrixTranspose(RefType &parent) : parent_(parent) {}
116 | 
117 |     typename RefType::DType &operator()(int i, int j) { return parent_(j, i); }
118 |     typename RefType::DType operator()(int i, int j) const { return parent_(j, i); }
119 | 
120 |     template <typename MatType>
121 |     MatrixTranspose &operator=(const MatType &mat)
122 |     {
123 |         return static_cast<
124 |                    MatrixBase<MatrixTranspose<RefType>, RefType::Cols, RefType::Rows, typename RefType::DType> &>(
125 |                    *this) = mat;
126 |     }
127 | };
128 | 
129 | template <typename LeftType, typename RightType>
130 | struct HorizontalConcat : public MatrixBase<HorizontalConcat<LeftType, RightType>, LeftType::Rows,
131 |                                             LeftType::Cols + RightType::Cols, typename LeftType::DType>
132 | {
133 |     const LeftType &left;
134 |     const RightType &right;
135 | 
136 |     HorizontalConcat(const LeftType &l, const RightType &r) : left(l), right(r) {}
137 | 
138 |     typename LeftType::DType operator()(int row, int col) const
139 |     {
140 |         return col < LeftType::Cols ? left(row, col) : right(row, col - LeftType::Cols);
141 |     }
142 | };
143 | 
144 | template <typename TopType, typename BottomType>
145 | struct VerticalConcat : public MatrixBase<VerticalConcat<TopType, BottomType>, TopType::Rows + BottomType::Rows,
146 |                                           TopType::Cols, typename TopType::DType>
147 | {
148 |     const TopType &top;
149 |     const BottomType &bottom;
150 | 
151 |     VerticalConcat(const TopType &t, const BottomType &b) : top(t), bottom(b) {}
152 | 
153 |     typename TopType::DType operator()(int row, int col) const
154 |     {
155 |         return row < TopType::Rows ? top(row, col) : bottom(row - TopType::Rows, col);
156 |     }
157 | };
158 | 
159 | template <int Rows, int Cols, typename DType, int TableSize>
160 | struct SparseMatrix : public MatrixBase<SparseMatrix<Rows, Cols, DType, TableSize>, Rows, Cols, DType>
161 | {
162 |     DType end;
163 | 
164 |     static constexpr int Size = TableSize;
165 | 
166 |     struct Element
167 |     {
168 |         int row, col;
169 |         DType val;
170 | 
171 |         Element() { row = col = -1; }
172 | 
173 |     } table[TableSize];
174 | 
175 |     DType &operator()(int row, int col)
176 |     {
177 |         int hash = (row * Cols + col) % TableSize;
178 | 
179 |         for (int i = 0; i < TableSize; i++)
180 |         {
181 |             Element &item = table[(hash + i) % TableSize];
182 | 
183 |             if (item.row == -1 || item.val == 0)
184 |             {
185 |                 item.row = row;
186 |                 item.col = col;
187 |                 item.val = 0;
188 |             }
189 | 
190 |             if (item.row == row && item.col == col)
191 |             {
192 |                 return item.val;
193 |             }
194 |         }
195 | 
196 |         return end;
197 |     }
198 | 
199 |     DType operator()(int row, int col) const
200 |     {
201 |         int hash = (row * Cols + col) % TableSize;
202 | 
203 |         for (int i = 0; i < TableSize; i++)
204 |         {
205 |             const Element &item = table[(hash + i) % TableSize];
206 | 
207 |             if (item.row == row && item.col == col)
208 |             {
209 |                 return item.val;
210 |             }
211 |         }
212 | 
213 |         return 0;
214 |     }
215 | };
216 | 
217 | template <int Dim, class DType>
218 | struct PermutationMatrix : public MatrixBase<PermutationMatrix<Dim, DType>, Dim, Dim, DType>
219 | {
220 |     int idx[Dim];
221 | 
222 |     DType operator()(int row, int col) const { return idx[col] == row; }
223 | };
224 | 
225 | template <class ParentType>
226 | struct LowerUnitriangularMatrix : public MatrixBase<LowerUnitriangularMatrix<ParentType>, ParentType::Rows,
227 |                                                     ParentType::Cols, typename ParentType::DType>
228 | {
229 |     const ParentType &parent;
230 | 
231 |     LowerUnitriangularMatrix(const ParentType &obj) : parent(obj) {}
232 | 
233 |     typename ParentType::DType operator()(int row, int col) const
234 |     {
235 |         if (row > col)
236 |         {
237 |             return parent(row, col);
238 |         }
239 |         else if (row == col)
240 |         {
241 |             return 1;
242 |         }
243 |         else
244 |         {
245 |             return 0;
246 |         }
247 |     }
248 | };
249 | 
250 | template <class ParentType>
251 | struct LowerTriangularMatrix : public MatrixBase<LowerTriangularMatrix<ParentType>, ParentType::Rows, ParentType::Cols,
252 |                                                  typename ParentType::DType>
253 | {
254 |     const ParentType &parent;
255 | 
256 |     LowerTriangularMatrix(const ParentType &obj) : parent(obj) {}
257 | 
258 |     typename ParentType::DType operator()(int row, int col) const
259 |     {
260 |         if (row >= col)
261 |         {
262 |             return parent(row, col);
263 |         }
264 |         else
265 |         {
266 |             return 0;
267 |         }
268 |     }
269 | };
270 | 
271 | template <class ParentType>
272 | struct UpperTriangularMatrix : public MatrixBase<UpperTriangularMatrix<ParentType>, ParentType::Rows, ParentType::Cols,
273 |                                                  typename ParentType::DType>
274 | {
275 |     const ParentType &parent;
276 | 
277 |     UpperTriangularMatrix(const ParentType &obj) : parent(obj) {}
278 | 
279 |     typename ParentType::DType operator()(int row, int col) const
280 |     {
281 |         if (row <= col)
282 |         {
283 |             return parent(row, col);
284 |         }
285 |         else
286 |         {
287 |             return 0;
288 |         }
289 |     }
290 | };
291 | 
292 | }  // namespace BLA
293 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 tomstewart89
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Basic Linear Algebra
 2 | 
 3 | Is a library for representing matrices and doing matrix math on arduino. It provides a Matrix class which can be used to declare 2D matrices of arbitrary height, width, type and even storage policy (see below). The matrix overrides the +, +=, -, -=, *, *= and = operators so they can be used naturally in algebraic expressions. It also supports a few more advanced operations including matrix inversion. It doesn't use malloc so it's not prone to leaks or runtime errors and it even checks whether the dimensions of two matrices used in an expression are compatible at compile time (so it'll let you know when you try to add together two matrices with different dimensions for example).
 4 | 
 5 | ## How it Works
 6 | 
 7 | ### The Basics
 8 | This library defines a class Matrix which you can use to represent two dimensional matrices of arbitrary dimensions. 
 9 | 
10 | For example, you can declare a 3x2 matrix like so:
11 | ```
12 | Matrix<3,2> A;
13 | ```
14 | Or a 6x3 Matrix like so:
15 | ```
16 | Matrix<6,3> B;
17 | ```
18 | You can do lots of different things with matrices including basic algebra:
19 | ```
20 | Matrix<6,2> C = B * A;
21 | ```
22 | As well as some more advanced operations like Transposition, Concatenation and Inversion. For more on the basic useage of matrices have a look at the [HowToUse](https://github.com/tomstewart89/BasicLinearAlgebra/blob/master/examples/HowToUse/HowToUse.ino) example.
23 | 
24 | ### Changing the Element Type
25 | 
26 | By default, matrices are full of floats. If that doesn't suit your application, then you can specify what kind of datatype you'd like your matrix to be made of by passing it in to the class's parameters just after the dimensions like so:
27 | ```
28 | Matrix<3,2,int> D;
29 | ```
30 | Not only that, the datatype needn't be just a POD (float, bool, int etc) but it can be any class or struct you define too. So if you'd like to do matrix math with your own custom class then you can, so long as it implements the operators for addition, subtraction etc. For example, if you wanted, you could write a class to hold an imaginary number and then you could make a matrix out of it like so:
31 | ```
32 | Matrix<2,2,ImaginaryNumber> Im;
33 | ```
34 | This turned out to have intersting implications when you specify the element type as another matrix. For more on that, have a look at the [Tensor](https://github.com/tomstewart89/BasicLinearAlgebra/blob/master/examples/Tensor/Tensor.ino) example.
35 | 
36 | ### Changing the Way Elements are Retrieved
37 | 
38 | By default, matrices store their elements in an C-style array which is kept inside the object. Every time you access one of the elements directly using the () operator or indirectly when you do some algebra, the matrix returns the appropriate element from that array.
39 | 
40 | At times, it's handy to be able to instruct the matrix to do something else when asked to retrieve an element at a particular index. For example if you know that your matrix is very sparse (only a few elements are non-zero) then you can save yourself some memory and just store the non-zero elements and return zero when asked for anything else.
41 | 
42 | If you like, you can override the way in which the elements for the matrix are stored. For example, we can define a 2000x3000 matrix with a sparse storage policy like so:
43 | ```
44 | SparseMatrix<2000, 3000, 100, float> > sparseMatrix;
45 | ```
46 | In this case the ```SparseMatrix``` is a `Matrix` which provides storage for 100 elements which are assumed to be embedded in a matrix having 2000 rows and 3000 columns. You can find the implementation for this class in the ElementStorage.h file, but long story short, it's a hashmap.
47 | 
48 | You can implement the custom matrices in whatever way you like so long as it returns some element when passed a row/column index. For more on how to implement such a matrix, have a look at the [CustomMatrix](https://github.com/tomstewart89/BasicLinearAlgebra/blob/master/examples/CustomMatrix/CustomMatrix.ino) example.
49 | 
50 | ### Reference Matrices
51 | 
52 | One particularly useful part of being able to override the way matrices access their elements is that it lets us define reference matrices. Reference matrices don't actually own any memory themselves, instead they return the elements of another matrix when we access them. To create a reference matrix you can use the `Submatrix` method of the matrix class like so:
53 | ```
54 | auto ref = A.Submatrix<2,2>(1,0);
55 | ```
56 | Here, `ref` is a 2x2 matrix which returns the elements in the lower 2 rows of the 3x2 matrix A defined above.
57 | 
58 | In general, reference matrices are useful for isolating a subsection of a larger matrix. That lets us use just that section in matrix operations with other matrices of compatible dimensions, or to collectively assign a value to a particular section of a matrix. For more information on reference matrices check out the [References](https://github.com/tomstewart89/BasicLinearAlgebra/blob/master/examples/References/References.ino) example.
59 | 


--------------------------------------------------------------------------------
/examples/CustomMatrix/CustomMatrix.ino:
--------------------------------------------------------------------------------
 1 | #include <BasicLinearAlgebra.h>
 2 | 
 3 | /*
 4 |  * Many matrices have special properties which mean that they can be represented with less memory or that computations
 5 |  * that they are involved can be carried out much more efficiently. Sparse matrices for example might have enormous
 6 |  * dimensions but only a few non-zero elements. So in these kinds of matrices it's a waste to reserve a huge amount of
 7 |  * memory for a whole heap of zeros. Similarly, it's a waste to cycle through each of those zeros every time we want to
 8 |  * do a computation. Instead it'd be better to just store the non-zero elements and skip past the zeros when doing a
 9 |  * computation.
10 |  *
11 |  * For that reason, I've written the matrix class such that we can customise the way a given matrix type retrieves its
12 |  * elements. In this example we'll look at a diagonal matrix - a matrix whose elements are zero except for those along
13 |  * it's diagonal (row == column).
14 |  */
15 | 
16 | // All the functions in BasicLinearAlgebra are wrapped up inside the namespace BLA, so specify that we're using it like
17 | // so:
18 | using namespace BLA;
19 | 
20 | // To declare a custom matrix our class needs to inherit from MatrixBase. MatrixBase takes a few template parameters the
21 | // first of which is the custom matrix class itself. That's a bit confusing but just follow the template below and it'll
22 | // all work out!
23 | template <int Dim, typename DType = float>
24 | struct DiagonalMatrix : public MatrixBase<DiagonalMatrix<Dim>, Dim, Dim, DType>
25 | {
26 |     Matrix<Dim, 1, DType> diagonal;
27 | 
28 |     // For const matrices (ones whose elements can't be modified) you just need to implement this function:
29 |     DType operator()(int row, int col) const
30 |     {
31 |         // If it's on the diagonal and it's not larger than the matrix dimensions then return the element
32 |         if (row == col)
33 |             return diagonal(row);
34 |         else
35 |             // Otherwise return zero
36 |             return 0.0f;
37 |     }
38 | 
39 |     // If you want to declare a matrix whose elements can be modified then you'll need to define this function:
40 |     // DType& operator()(int row, int col)
41 | };
42 | 
43 | void setup()
44 | {
45 |     Serial.begin(115200);
46 | 
47 |     // If you've been through the HowToUse example you'll know that you can allocate a Matrix and explicitly specify
48 |     // it's type like so:
49 |     BLA::Matrix<4, 4> mat;
50 | 
51 |     // And as before it's a good idea to fill the matrix before we use it
52 |     mat.Fill(1);
53 | 
54 |     // Now let's declare a diagonal matrix. To do that we pass the Diagonal class from above along with whatever
55 |     // template parameters as a template parameter to Matrix, like so:
56 |     DiagonalMatrix<4> diag;
57 | 
58 |     // If we fill diag we'll get a matrix with all 1's along the diagonal, the identity matrix.
59 |     diag.diagonal.Fill(1);
60 | 
61 |     // So multiplying it with mat will do nothing:
62 |     Serial.print("still ones: ");
63 |     Serial.println(diag * mat);
64 | 
65 |     // Diagonal matrices have the handy property of scaling either the rows (premultiplication) or columns
66 |     // (postmultiplication) of a matrix
67 | 
68 |     // So if we modify the diagonal
69 |     for (int i = 0; i < diag.Rows; i++) diag.diagonal(i) = i + 1;
70 | 
71 |     // And multiply again, we'll see that the rows have been scaled
72 |     Serial.print("scaled rows: ");
73 |     Serial.print(diag * mat);
74 | 
75 |     // Point being, if you define a class which serves up something when called upon by the () operator, you can embed
76 |     // it in a matrix and define any kind of behaviour you like. Hopefully that'll let this library support lots more
77 |     // applications while catering to the arduino's limited amount of memory.
78 | }
79 | 
80 | void loop() {}
81 | 


--------------------------------------------------------------------------------
/examples/HowToUse/HowToUse.ino:
--------------------------------------------------------------------------------
  1 | #include <BasicLinearAlgebra.h>
  2 | 
  3 | /*
  4 |  * This example sketch should show you everything you need to know in order to work with the Matrix library. It's a
  5 |  * faily long explantion but try to read through it carefully. Remember that everything is built around templates so the
  6 |  * compatibility of matrices used in all operations can be checked at compile time. For that reason though, if you try
  7 |  * to multiply matrices of the wrong dimensions etc you'll be met with some very cryptic compile errors so be careful!
  8 |  */
  9 | 
 10 | // All the functions in BasicLinearAlgebra are wrapped up inside the namespace BLA, so specify that we're using it like
 11 | // so:
 12 | using namespace BLA;
 13 | 
 14 | void setup()
 15 | {
 16 |     Serial.begin(115200);
 17 | 
 18 |     // Let's being by declaring a few matrices. Matrix is a template class so you'll have to specfify the dimensions as
 19 |     // <row,column> after Matrix like so:
 20 |     BLA::Matrix<3, 3> A;
 21 | 
 22 |     // NOTE: Turns out that one of the libraries included when using an Arduino DUE also declares a class called Matrix,
 23 |     // so to resolve the ambiguity you'll need to explicitly identify this library's Matrix class when declaring them by
 24 |     // prepending the declaration with BLA:: If you're using other boards then just writing using namespace BLA; will
 25 |     // suffice.
 26 | 
 27 |     // The cols parameters has a default of 1 so to declare a vector of length 3 you can simply write:
 28 |     BLA::Matrix<3> v;
 29 | 
 30 |     // Just like any other variable, Matrices should be initialised to some value before you use them. To set every
 31 |     // element of the Matrix to a certain value you can use the Fill function like so:
 32 |     v.Fill(0);
 33 | 
 34 |     // The matrix elements can be accessed individually using the brackets operator like so:
 35 |     v(2, 0) = 5.3;
 36 |     v(1) = 43.67;  // you can also just write v(2) = 5.3; since v has only one column
 37 | 
 38 |     // Or you can set the entire matrix like so:
 39 |     A = {3.25, 5.67, 8.67, 4.55, 7.23, 9.00, 2.35, 5.73, 10.56};
 40 | 
 41 |     // You can also set the entire array on construction like so:
 42 |     BLA::Matrix<3, 3> B = {6.54, 3.66, 2.95, 3.22, 7.54, 5.12, 8.98, 9.99, 1.56};
 43 | 
 44 |     // Ask the matrix how many rows and columns it has:
 45 |     v.Cols;
 46 |     v.Rows;
 47 | 
 48 |     // Now you can do some matrix math! The Matrix class supports addition and subtraction between matrices of the same
 49 |     // size:
 50 |     BLA::Matrix<3, 3> C = A + B;
 51 | 
 52 |     // You can't do addition between matrices of a different size. Since the class is built around templates, the
 53 |     // compiler will tell you if you've made a mistake. Try uncommenting the next line to see what I mean A + v;
 54 | 
 55 |     // You can use the Matrix class's unary operators, like so:
 56 |     C -= B;
 57 | 
 58 |     // Or like so:
 59 |     B += A;
 60 | 
 61 |     // As well as addition and subtraction, we can also do matrix multiplication. Note that again, the matrices,
 62 |     // including the one in which the result will stored must have the appropriate dimensions. The compiler will let you
 63 |     // know if they aren't
 64 |     BLA::Matrix<3, 1> D = A * v;
 65 | 
 66 |     // As well as algebra, Matrix supports a few other matrix related operations including transposition:
 67 |     BLA::Matrix<1, 3> D_T = ~D;
 68 | 
 69 |     // And concatenation, both horizontally...
 70 |     BLA::Matrix<3, 6> AleftOfB = A || B;
 71 | 
 72 |     // And vertically
 73 |     BLA::Matrix<6, 3> AonTopOfB = A && B;
 74 | 
 75 |     // An inverse of a matrix can also be calculated for square matrices via the Invert function. This will invert
 76 |     // the input matrix in-place.
 77 |     BLA::Matrix<3, 3> C_inv = C;
 78 |     bool is_nonsingular = Invert(C_inv);
 79 | 
 80 |     // If the matrix is singular, the inversion won't work. In those cases Invert will return false.
 81 | 
 82 |     // For convenience, there's also Inverse, which will return the inverse, leaving the input matrix unmodified.
 83 |     BLA::Matrix<3, 3> also_C_inv = Inverse(C);
 84 | 
 85 |     // If you want to print out the value of any element in the array you can do that with the Serial object:
 86 |     Serial.print("v(1): ");
 87 |     Serial.println(v(1));
 88 | 
 89 |     // Alternatively, you can write the whole matrix to Serial, which works like so:
 90 |     Serial.print("B: ");
 91 |     Serial.println(B);
 92 | 
 93 |     // You can even write some quite complex compound statements very succinctly. For example:
 94 |     Serial.print("identity matrix: ");
 95 |     Serial.print(AleftOfB * AonTopOfB - (A * A + B * B) + C * C_inv);
 96 | 
 97 |     // Or as a more real life example, here's how you might calculate an updated state estimate for a third order state
 98 |     // space model:
 99 |     BLA::Matrix<3> state;
100 |     BLA::Matrix<2> input;
101 |     BLA::Matrix<3, 2> input_matrix;
102 |     BLA::Matrix<3, 3> state_transition_matrix;
103 |     float dt;
104 |     state += (state_transition_matrix * state + input_matrix * input) * dt;
105 | 
106 |     // Enjoy!
107 | }
108 | 
109 | void loop() {}
110 | 


--------------------------------------------------------------------------------
/examples/References/References.ino:
--------------------------------------------------------------------------------
 1 | #include <BasicLinearAlgebra.h>
 2 | 
 3 | /*
 4 |  * This example sketch shows how to use a reference matrix to isolate a section of a larger matrix and use it in
 5 |  * arithmetic with smaller matrices of the appropriate dimensions.
 6 |  */
 7 | 
 8 | // All the functions in BasicLinearAlgebra are wrapped up inside the namespace BLA, so specify that we're using it like
 9 | // so:
10 | using namespace BLA;
11 | 
12 | void setup()
13 | {
14 |     Serial.begin(115200);
15 | 
16 |     // If you've been through the HowToUse example you'll know that you can allocate a Matrix like so:
17 |     BLA::Matrix<8, 8> bigMatrix;
18 | 
19 |     // And as before it's a good idea to fill the matrix before we use it
20 |     bigMatrix.Fill(0);
21 | 
22 |     // Now we'll make a refence matrix which refers to bigMatrix. The reference has no internal memory of it's own, it
23 |     // just goes and gets whatever it needs from bigMatrix when it takes part in an operation. It'll also stores the
24 |     // result of an operation there if needs be too. Reference matrices are handy in that we can use them to select just
25 |     // a submatrix of the original and use that in operations with smaller matrices.
26 | 
27 |     // To allocate a reference matrix you can use the RefMatrix type. RefMatrix is a template class just like Matrix but
28 |     // it has one extra template parameter that needs filling in, that is, the type of matrix that it refers to. In this
29 |     // example we'll just be referring to regular Matrices so that parameter should look like Array<N,M> where N and M
30 |     // are the number of rows and columns of the matrix being referred to (the parent) respectively.
31 | 
32 |     // When declaring a RefMatrix, it needs to be told which matrix it's referring to and what the offset is between
33 |     // it's (0,0) and that of it's parent's (0,0) element. To do that you can use the Submatrix method of the Matrix
34 |     // class. The Submatrix expects two sets of parameters. The template parameter (the one in the <>) indicates how
35 |     // many elements to select while the other indicates the offset between the parent matrix and the reference.
36 | 
37 |     // So for example, to create a 4x4 reference to bigMatrix starting at element (4,2) the declaration would be as
38 |     // follows:
39 |     RefMatrix<BLA::Matrix<8, 8>, 4, 4> bigMatrixRef(bigMatrix.Submatrix<4, 4>(4, 2));
40 | 
41 |     // If we set the (0,0) element of bigMatrixRef, we're effectively setting the (4,2) element of bigMatrix. So let's
42 |     // do that
43 |     bigMatrixRef(0, 0) = 45.67434;
44 | 
45 |     // And we can see that the original matrix has been set accordingly
46 |     Serial.print("bigMatrix(4,2): ");
47 |     Serial.println(bigMatrix(4, 2));
48 | 
49 |     // The submatrix function actually returns a RefMatrix so if you like you can just use it directly. For example you
50 |     // can set a section of bigMatrix using an array like so:
51 |     bigMatrix.Submatrix<4, 4>(4, 2) = BLA::Matrix<4, 4>{23.44, 43.23, 12.45, 6.23, 93.94, 27.23, 1.44,  101.23,
52 |                                                         1.23,  3.21,  4.56,  8.76, 12.34, 34.56, 76.54, 21.09};
53 | 
54 |     // For all intents and purposes you can treat reference matrices as just regular matrices.
55 |     RefMatrix<BLA::Matrix<8, 8>, 2, 4> anotherRef =
56 |         bigMatrix.Submatrix<2, 4>(2, 1);  // this creates a 2x4 reference matrix starting at element (2,1) of bigMatrix
57 | 
58 |     // You can fill them
59 |     anotherRef.Fill(1.1111);
60 | 
61 |     // Do arithmetic with them
62 |     BLA::Matrix<2, 4> res = anotherRef * bigMatrixRef;
63 | 
64 |     // Invert them
65 |     Invert(bigMatrixRef);
66 | 
67 |     // Print them
68 |     Serial.print("bigMatrixRef: ");
69 |     Serial.println(bigMatrixRef);
70 | 
71 |     // You can even make a reference to a reference matrix, do arithmetic with that and then print the result
72 |     Serial.print("result of convoluted operation: ");
73 |     Serial.println(anotherRef += bigMatrixRef.Submatrix<2, 4>(0, 0));
74 | 
75 |     // The only thing that you can't (shouldn't) really do is operate on two matrix references whose underlying memory
76 |     // overlap, particularly when doing matrix multiplication.
77 | 
78 |     // Lastly, let's look at what became of bigMatrix after all of this, you might be able to make out the values of
79 |     // bigMatrixRef and anotherRef in their respective areas of bigMatrix.
80 |     Serial.print("bigMatrix: ");
81 |     Serial.print(bigMatrix);
82 | }
83 | 
84 | void loop() {}
85 | 


--------------------------------------------------------------------------------
/examples/SolveLinearEquations/SolveLinearEquations.ino:
--------------------------------------------------------------------------------
 1 | #include <BasicLinearAlgebra.h>
 2 | 
 3 | using namespace BLA;
 4 | 
 5 | void setup()
 6 | {
 7 |     Serial.begin(115200);
 8 | 
 9 |     // One common thing that you might like to do with matrices is to solve systems
10 |     // of linear equations of the form: A * x = b.
11 | 
12 |     // Let's go ahead and declare a decent sized system of equations to work with:
13 |     Matrix<6, 6> A = {16, 78, 50, 84, 70, 63, 2,  32, 33, 61, 40, 17, 96, 98, 50, 80, 78, 27,
14 |                       86, 49, 57, 10, 42, 96, 44, 87, 60, 67, 16, 59, 53, 8,  64, 97, 41, 90};
15 | 
16 |     Matrix<6> b = {3, 99, 95, 72, 57, 43};
17 | 
18 |     // To solve for x you might be tempted to take the inverse of A then
19 |     // calculate x like so: x = A^-1 * b
20 | 
21 |     // As it turns out though, actually taking the inverse of A to solve these kinds of equations is quite inefficient.
22 |     // Textbooks often talk about x = A^-1 * b, but in practice we don't actually compute x this way.
23 | 
24 |     // Instead we use something called an LU decomposition (or if A has some special properties you can use some other
25 |     // type of decomposition but we won't get into that here). LU decomposition factors an A matrix into a permutation
26 |     // matrix, a lower and an upper triangular matrix:
27 |     auto A_decomp = A;  // LUDecompose will destroy A here so we'll pass in a copy so we can refer back to A later
28 |     auto decomp = LUDecompose(A_decomp);
29 | 
30 |     // You can take a look at these matrices if you like:
31 |     decomp.P;  // P essentially rearranges the rows of the matrix that results when we multiply L by U
32 |     decomp.L;  // Will have ones along its diagonal and zeros above it
33 |     decomp.U;  // Will have zeros below its diagonal
34 | 
35 |     // And if we multiply them all together we'll recover the original A matrix:
36 |     Serial.print("reconstructed A: ");
37 |     Serial.println(decomp.P * decomp.L * decomp.U);
38 | 
39 |     // Once we've done the decomposition we can solve for x very efficiently:
40 |     Matrix<6> x_lusolve = LUSolve(decomp, b);
41 | 
42 |     Serial.print("x (via LU decomposition): ");
43 |     Serial.println(x_lusolve);
44 | 
45 |     // We can also recompute x for a new b vector without having to repeat the decomposition:
46 |     Matrix<6> another_b = {23, 19, 86, 3, 23, 90};
47 |     x_lusolve = LUSolve(decomp, another_b);
48 | 
49 |     // If you really need the inverse of A you can still compute it and calculate x with it:
50 |     auto A_inv = A;
51 |     Invert(A_inv);
52 |     Matrix<6> x_Ainvb = A_inv * b;
53 | 
54 |     Serial.print("x (via inverse A): ");
55 |     Serial.print(x_Ainvb);
56 | 
57 |     // Fun fact though, we actually calculate A_inv by running LUDecompose then calling LUSolve for each column in A.
58 |     // This is actually no less efficient than other methods for calculating the inverse.
59 | }
60 | 
61 | void loop() {}
62 | 


--------------------------------------------------------------------------------
/examples/Tensor/Tensor.ino:
--------------------------------------------------------------------------------
 1 | #include <BasicLinearAlgebra.h>
 2 | 
 3 | /*
 4 |  * This example sketch shows how the template parameter can be nested to form multi-dimensional matrices. In general,
 5 |  * this library isn't designed to support matrices of dimensions higher than 2, but it turns out that by defining the
 6 |  * element type of a matrix as another matrix then something resembling multi-dimensional matrices are possible. I
 7 |  * thought was pretty neat and worth making into an example.
 8 |  */
 9 | 
10 | // All the functions in BasicLinearAlgebra are wrapped up inside the namespace BLA, so specify that we're using it like
11 | // so:
12 | using namespace BLA;
13 | 
14 | void setup()
15 | {
16 |     Serial.begin(115200);
17 | 
18 |     // In addition to the Matrix dimensions, the Matrix template has a third parameter which when not specified just
19 |     // defaults to the type: Array<rows,cols,float>. This third parameter specifies how the Matrix should store it's
20 |     // elements such that the way that the Matrix serves up it's elements ca be customised (see the CustomMatrix example
21 |     // for more). The Array type simply means that the Matrix stores it's elements in a big array of size rows x cols.
22 | 
23 |     // In any case, written in full, a Matrix declaration looks like this:
24 |     BLA::Matrix<3, 3, float> floatA;
25 | 
26 |     // The default underlying type of the Array class's array is float. If you want to use a different type, say int for
27 |     // example, then just pass it as a template parameter like so:
28 |     BLA::Matrix<3, 3, int> intA = {1, 2, 3, 4, 5, 6, 7, 8, 9};
29 | 
30 |     // From here you'll be able to do everything you'd be able to do with a float Matrix, but with int precision and
31 |     // memory useage.
32 | 
33 |     // You can actually pass any datatype you like to the template and it'll do it's best to make a Matrix out of it.
34 |     BLA::Matrix<3, 3, unsigned char> charA;
35 |     BLA::Matrix<3, 3, double> doubleA;  // etc
36 | 
37 |     // This includes parameters of type Matrix, meaning that you can declare matrices of more than two dimensions. For
38 |     // example:
39 |     BLA::Matrix<4, 4, BLA::Matrix<4>> cubeA, cubeB;  // a 4x4x4 Matrix (3rd order tensor)
40 | 
41 |     // And so on:
42 |     BLA::Matrix<2, 2, BLA::Matrix<2, 2>> hyperA;  // a 2x2x2x2 dimensional Matrix (4th order tensor)
43 |     BLA::Matrix<3, 3, BLA::Matrix<3, 3, BLA::Matrix<3, 3>>>
44 |         tensorA;  // a 3x3x3x3x3x3 dimensional Matrix (6th order tensor)
45 | 
46 |     // You can access the elements of an arbitrary rank tensor with the brackets operator like so:
47 |     cubeA(0, 1)(1) = cubeB(2, 3)(3) = 56.34;
48 |     hyperA(1, 0)(1, 1) = 56.34;
49 |     tensorA(2, 0)(1, 2)(1, 1) = 0.056;
50 | 
51 |     // Addition, subtraction and transposition all work as usual
52 |     cubeA + cubeB;
53 | 
54 |     // As does concatenation
55 |     BLA::Matrix<4, 8, BLA::Matrix<4>> cubeAleftOfcubeB = cubeA || cubeB;
56 | 
57 |     // You can also do multiplication on square tensors with an even rank
58 |     for (int i = 0; i < 2; i++)
59 |         for (int j = 0; j < 2; j++)
60 |             for (int k = 0; k < 2; k++)
61 |                 for (int l = 0; l < 2; l++) hyperA(i, j)(k, l) = i + j + k + l;
62 | 
63 |     BLA::Matrix<2, 2, BLA::Matrix<2, 2>> hyperB = (hyperA * hyperA);
64 | 
65 |     // Everything can be printed too
66 |     Serial.print("Hyper B: ");
67 |     Serial.print(hyperB);
68 | 
69 |     // Inversion doesn't work. If it did, it'd probably take quite a while for arduino to calculate anyway so maybe it's
70 |     // for the best
71 | }
72 | 
73 | void loop() {}
74 | 


--------------------------------------------------------------------------------
/impl/BasicLinearAlgebra.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <math.h>
  4 | #include <stdlib.h>
  5 | #include <string.h>
  6 | 
  7 | namespace BLA
  8 | {
  9 | template <typename MatAType, typename MatBType, int MatARows, int MatACols, int MatBCols, typename DType>
 10 | Matrix<MatARows, MatBCols, DType> operator*(const MatrixBase<MatAType, MatARows, MatACols, DType> &matA,
 11 |                                             const MatrixBase<MatBType, MatACols, MatBCols, DType> &matB)
 12 | {
 13 |     Matrix<MatARows, MatBCols, DType> ret;
 14 | 
 15 |     for (int i = 0; i < MatARows; ++i)
 16 |     {
 17 |         for (int j = 0; j < MatBCols; ++j)
 18 |         {
 19 |             if (MatACols > 0)
 20 |             {
 21 |                 ret(i, j) = matA(i, 0) * matB(0, j);
 22 |             }
 23 | 
 24 |             for (int k = 1; k < MatACols; k++)
 25 |             {
 26 |                 ret(i, j) += matA(i, k) * matB(k, j);
 27 |             }
 28 |         }
 29 |     }
 30 |     return ret;
 31 | }
 32 | 
 33 | template <typename MatAType, typename MatBType, int MatARows, int MatACols, int MatBCols, typename DType>
 34 | MatrixBase<MatAType, MatARows, MatACols, DType> &operator*=(MatrixBase<MatAType, MatARows, MatACols, DType> &matA,
 35 |                                                             const MatrixBase<MatBType, MatACols, MatBCols, DType> &matB)
 36 | {
 37 |     matA = matA * matB;
 38 |     return matA;
 39 | }
 40 | 
 41 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
 42 | MatrixBase<MatAType, Rows, Cols, DType> &operator+=(MatrixBase<MatAType, Rows, Cols, DType> &matA,
 43 |                                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
 44 | {
 45 |     for (int i = 0; i < Rows; ++i)
 46 |     {
 47 |         for (int j = 0; j < Cols; ++j)
 48 |         {
 49 |             matA(i, j) += matB(i, j);
 50 |         }
 51 |     }
 52 | 
 53 |     return matA;
 54 | }
 55 | 
 56 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
 57 | MatrixBase<MatAType, Rows, Cols, DType> &operator-=(MatrixBase<MatAType, Rows, Cols, DType> &matA,
 58 |                                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
 59 | {
 60 |     for (int i = 0; i < Rows; ++i)
 61 |     {
 62 |         for (int j = 0; j < Cols; ++j)
 63 |         {
 64 |             matA(i, j) -= matB(i, j);
 65 |         }
 66 |     }
 67 | 
 68 |     return matA;
 69 | }
 70 | 
 71 | template <typename MatType, int Rows, int Cols, typename DType>
 72 | MatrixBase<MatType, Rows, Cols, DType> &operator*=(MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
 73 | {
 74 |     for (int i = 0; i < Rows; ++i)
 75 |     {
 76 |         for (int j = 0; j < Cols; ++j)
 77 |         {
 78 |             mat(i, j) *= k;
 79 |         }
 80 |     }
 81 |     return mat;
 82 | }
 83 | 
 84 | template <typename MatType, int Rows, int Cols, typename DType>
 85 | MatrixBase<MatType, Rows, Cols, DType> &operator/=(MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
 86 | {
 87 |     for (int i = 0; i < Rows; ++i)
 88 |     {
 89 |         for (int j = 0; j < Cols; ++j)
 90 |         {
 91 |             mat(i, j) /= k;
 92 |         }
 93 |     }
 94 |     return mat;
 95 | }
 96 | template <typename MatType, int Rows, int Cols, typename DType>
 97 | MatrixBase<MatType, Rows, Cols, DType> &operator+=(MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
 98 | {
 99 |     for (int i = 0; i < Rows; ++i)
100 |     {
101 |         for (int j = 0; j < Cols; ++j)
102 |         {
103 |             mat(i, j) += k;
104 |         }
105 |     }
106 |     return mat;
107 | }
108 | template <typename MatType, int Rows, int Cols, typename DType>
109 | MatrixBase<MatType, Rows, Cols, DType> &operator-=(MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
110 | {
111 |     for (int i = 0; i < Rows; ++i)
112 |     {
113 |         for (int j = 0; j < Cols; ++j)
114 |         {
115 |             mat(i, j) -= k;
116 |         }
117 |     }
118 |     return mat;
119 | }
120 | 
121 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
122 | Matrix<Rows, Cols, DType> operator+(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
123 |                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
124 | {
125 |     Matrix<Rows, Cols, DType> ret = matA;
126 |     ret += matB;
127 |     return ret;
128 | }
129 | 
130 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
131 | Matrix<Rows, Cols, DType> operator-(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
132 |                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
133 | {
134 |     Matrix<Rows, Cols, DType> ret = matA;
135 |     ret -= matB;
136 |     return ret;
137 | }
138 | 
139 | template <typename MatType, int Rows, int Cols, typename DType>
140 | Matrix<Rows, Cols, DType> operator+(const MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
141 | {
142 |     Matrix<Rows, Cols, DType> ret = mat;
143 |     ret += k;
144 |     return ret;
145 | }
146 | 
147 | template <typename MatType, int Rows, int Cols, typename DType>
148 | Matrix<Rows, Cols, DType> operator+(const DType k, const MatrixBase<MatType, Rows, Cols, DType> &mat)
149 | {
150 |     return mat + k;
151 | }
152 | 
153 | template <typename MatType, int Rows, int Cols, typename DType>
154 | Matrix<Rows, Cols, DType> operator-(const MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
155 | {
156 |     Matrix<Rows, Cols, DType> ret = mat;
157 |     ret -= k;
158 |     return ret;
159 | }
160 | 
161 | template <typename MatType, int Rows, int Cols, typename DType>
162 | Matrix<Rows, Cols, DType> operator-(const DType k, const MatrixBase<MatType, Rows, Cols, DType> &mat)
163 | {
164 |     return (-mat) + k;
165 | }
166 | 
167 | template <typename MatType, int Rows, int Cols, typename DType>
168 | Matrix<Rows, Cols, DType> operator*(const MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
169 | {
170 |     Matrix<Rows, Cols, DType> ret = mat;
171 |     ret *= k;
172 |     return ret;
173 | }
174 | 
175 | template <typename MatType, int Rows, int Cols, typename DType>
176 | Matrix<Rows, Cols, DType> operator*(const DType k, const MatrixBase<MatType, Rows, Cols, DType> &mat)
177 | {
178 |     return mat * k;
179 | }
180 | 
181 | template <typename MatType, int Rows, int Cols, typename DType>
182 | Matrix<Rows, Cols, DType> operator/(const MatrixBase<MatType, Rows, Cols, DType> &mat, const DType k)
183 | {
184 |     Matrix<Rows, Cols, DType> ret = mat;
185 |     ret /= k;
186 |     return ret;
187 | }
188 | 
189 | template <typename MatType, int Rows, int Cols, typename DType>
190 | Matrix<Rows, Cols, DType> operator/(const DType k, const MatrixBase<MatType, Rows, Cols, DType> &mat)
191 | {
192 |     Matrix<Rows, Cols, DType> ret;
193 |     for (int i = 0; i < Rows; ++i)
194 |     {
195 |         for (int j = 0; j < Cols; ++j)
196 |         {
197 |             ret(i, j) = k / mat(i, j);
198 |         }
199 |     }
200 |     return ret;
201 | }
202 | 
203 | template <typename DerivedType, typename OperandType, int Rows, typename DType>
204 | HorizontalConcat<DerivedType, OperandType> operator||(
205 |     const MatrixBase<DerivedType, Rows, DerivedType::Cols, DType> &left,
206 |     const MatrixBase<OperandType, Rows, OperandType::Cols, DType> &right)
207 | {
208 |     return HorizontalConcat<DerivedType, OperandType>(static_cast<const DerivedType &>(left),
209 |                                                       static_cast<const OperandType &>(right));
210 | }
211 | 
212 | template <typename DerivedType, typename OperandType, int Cols, typename DType>
213 | VerticalConcat<DerivedType, OperandType> operator&&(
214 |     const MatrixBase<DerivedType, DerivedType::Rows, Cols, DType> &top,
215 |     const MatrixBase<OperandType, OperandType::Rows, Cols, DType> &bottom)
216 | 
217 | {
218 |     return VerticalConcat<DerivedType, OperandType>(static_cast<const DerivedType &>(top),
219 |                                                     static_cast<const OperandType &>(bottom));
220 | }
221 | 
222 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
223 | Matrix<Rows, Cols, bool> operator==(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
224 |                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
225 | {
226 |     Matrix<Rows, Cols, bool> ret;
227 | 
228 |     for (int i = 0; i < Rows; ++i)
229 |     {
230 |         for (int j = 0; j < Cols; ++j)
231 |         {
232 |             ret(i, j) = matA(i, j) == matB(i, j);
233 |         }
234 |     }
235 |     return ret;
236 | }
237 | 
238 | template <typename MatAType, typename MatBType, int Rows, int Cols>
239 | Matrix<Rows, Cols, bool> operator&(const MatrixBase<MatAType, Rows, Cols, bool> &matA,
240 |                                    const MatrixBase<MatBType, Rows, Cols, bool> &matB)
241 | {
242 |     Matrix<Rows, Cols, bool> ret;
243 | 
244 |     for (int i = 0; i < Rows; ++i)
245 |     {
246 |         for (int j = 0; j < Cols; ++j)
247 |         {
248 |             ret(i, j) = matA(i, j) & matB(i, j);
249 |         }
250 |     }
251 |     return ret;
252 | }
253 | 
254 | template <typename MatAType, typename MatBType, int Rows, int Cols>
255 | Matrix<Rows, Cols, bool> operator|(const MatrixBase<MatAType, Rows, Cols, bool> &matA,
256 |                                    const MatrixBase<MatBType, Rows, Cols, bool> &matB)
257 | {
258 |     Matrix<Rows, Cols, bool> ret;
259 | 
260 |     for (int i = 0; i < Rows; ++i)
261 |     {
262 |         for (int j = 0; j < Cols; ++j)
263 |         {
264 |             ret(i, j) = matA(i, j) | matB(i, j);
265 |         }
266 |     }
267 |     return ret;
268 | }
269 | 
270 | template <typename DerivedType>
271 | Matrix<DerivedType::Rows, DerivedType::Cols, bool> operator!(
272 |     const MatrixBase<DerivedType, DerivedType::Rows, DerivedType::Cols, bool> &matA)
273 | {
274 |     Matrix<DerivedType::Rows, DerivedType::Cols, bool> ret;
275 | 
276 |     for (int i = 0; i < DerivedType::Rows; ++i)
277 |     {
278 |         for (int j = 0; j < DerivedType::Cols; ++j)
279 |         {
280 |             ret(i, j) = !matA(i, j);
281 |         }
282 |     }
283 |     return ret;
284 | }
285 | 
286 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
287 | Matrix<Rows, Cols, bool> operator>(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
288 |                                    const MatrixBase<MatBType, Rows, Cols, DType> &matB)
289 | {
290 |     Matrix<Rows, Cols, bool> ret;
291 | 
292 |     for (int i = 0; i < Rows; ++i)
293 |     {
294 |         for (int j = 0; j < Cols; ++j)
295 |         {
296 |             ret(i, j) = matA(i, j) > matB(i, j);
297 |         }
298 |     }
299 |     return ret;
300 | }
301 | 
302 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
303 | Matrix<Rows, Cols, bool> operator<(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
304 |                                    const MatrixBase<MatBType, Rows, Cols, DType> &matB)
305 | {
306 |     Matrix<Rows, Cols, bool> ret;
307 | 
308 |     for (int i = 0; i < Rows; ++i)
309 |     {
310 |         for (int j = 0; j < Cols; ++j)
311 |         {
312 |             ret(i, j) = matA(i, j) < matB(i, j);
313 |         }
314 |     }
315 |     return ret;
316 | }
317 | 
318 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
319 | Matrix<Rows, Cols, bool> operator<=(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
320 |                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
321 | {
322 |     Matrix<Rows, Cols, bool> ret;
323 | 
324 |     for (int i = 0; i < Rows; ++i)
325 |     {
326 |         for (int j = 0; j < Cols; ++j)
327 |         {
328 |             ret(i, j) = matA(i, j) <= matB(i, j);
329 |         }
330 |     }
331 |     return ret;
332 | }
333 | 
334 | template <typename MatAType, typename MatBType, int Rows, int Cols, typename DType>
335 | Matrix<Rows, Cols, bool> operator>=(const MatrixBase<MatAType, Rows, Cols, DType> &matA,
336 |                                     const MatrixBase<MatBType, Rows, Cols, DType> &matB)
337 | {
338 |     Matrix<Rows, Cols, bool> ret;
339 | 
340 |     for (int i = 0; i < Rows; ++i)
341 |     {
342 |         for (int j = 0; j < Cols; ++j)
343 |         {
344 |             ret(i, j) = matA(i, j) >= matB(i, j);
345 |         }
346 |     }
347 |     return ret;
348 | }
349 | 
350 | template <typename DerivedType>
351 | Matrix<DerivedType::Rows, DerivedType::Cols, bool> operator>(const DownCast<DerivedType> &mat,
352 |                                                              const typename DerivedType::DType k)
353 | {
354 |     Matrix<DerivedType::Rows, DerivedType::Cols, bool> ret;
355 | 
356 |     for (int i = 0; i < DerivedType::Rows; ++i)
357 |     {
358 |         for (int j = 0; j < DerivedType::Cols; ++j)
359 |         {
360 |             ret(i, j) = mat(i, j) > k;
361 |         }
362 |     }
363 |     return ret;
364 | }
365 | 
366 | template <typename DerivedType>
367 | Matrix<DerivedType::Rows, DerivedType::Cols, bool> operator>=(const DownCast<DerivedType> &mat,
368 |                                                               const typename DerivedType::DType k)
369 | {
370 |     Matrix<DerivedType::Rows, DerivedType::Cols, bool> ret;
371 | 
372 |     for (int i = 0; i < DerivedType::Rows; ++i)
373 |     {
374 |         for (int j = 0; j < DerivedType::Cols; ++j)
375 |         {
376 |             ret(i, j) = mat(i, j) >= k;
377 |         }
378 |     }
379 |     return ret;
380 | }
381 | 
382 | template <typename DerivedType>
383 | Matrix<DerivedType::Rows, DerivedType::Cols, bool> operator<(const DownCast<DerivedType> &mat,
384 |                                                              const typename DerivedType::DType k)
385 | {
386 |     Matrix<DerivedType::Rows, DerivedType::Cols, bool> ret;
387 | 
388 |     for (int i = 0; i < DerivedType::Rows; ++i)
389 |     {
390 |         for (int j = 0; j < DerivedType::Cols; ++j)
391 |         {
392 |             ret(i, j) = mat(i, j) < k;
393 |         }
394 |     }
395 |     return ret;
396 | }
397 | 
398 | template <typename DerivedType>
399 | Matrix<DerivedType::Rows, DerivedType::Cols, bool> operator<=(const DownCast<DerivedType> &mat,
400 |                                                               const typename DerivedType::DType k)
401 | {
402 |     Matrix<DerivedType::Rows, DerivedType::Cols, bool> ret;
403 | 
404 |     for (int i = 0; i < DerivedType::Rows; ++i)
405 |     {
406 |         for (int j = 0; j < DerivedType::Cols; ++j)
407 |         {
408 |             ret(i, j) = mat(i, j) <= k;
409 |         }
410 |     }
411 |     return ret;
412 | }
413 | 
414 | template <typename DerivedType>
415 | bool Any(const MatrixBase<DerivedType, DerivedType::Rows, DerivedType::Cols, bool> &matA)
416 | {
417 |     for (int i = 0; i < DerivedType::Rows; ++i)
418 |     {
419 |         for (int j = 0; j < DerivedType::Cols; ++j)
420 |         {
421 |             if (matA(i, j))
422 |             {
423 |                 return true;
424 |             }
425 |         }
426 |     }
427 | 
428 |     return false;
429 | }
430 | 
431 | template <typename DerivedType>
432 | bool All(const MatrixBase<DerivedType, DerivedType::Rows, DerivedType::Cols, bool> &matA)
433 | {
434 |     for (int i = 0; i < DerivedType::Rows; ++i)
435 |     {
436 |         for (int j = 0; j < DerivedType::Cols; ++j)
437 |         {
438 |             if (!matA(i, j))
439 |             {
440 |                 return false;
441 |             }
442 |         }
443 |     }
444 | 
445 |     return true;
446 | }
447 | 
448 | }  // namespace BLA
449 | 


--------------------------------------------------------------------------------
/impl/NotSoBasicLinearAlgebra.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | namespace BLA
  4 | {
  5 | template <typename ParentType, typename Dtype>
  6 | void Swap(MatrixBase<ParentType, ParentType::Rows, ParentType::Cols, Dtype> &A,
  7 |           MatrixBase<ParentType, ParentType::Rows, ParentType::Cols, Dtype> &B)
  8 | {
  9 |     Dtype tmp;
 10 |     for (int i = 0; i < ParentType::Rows; i++)
 11 |     {
 12 |         for (int j = 0; j < ParentType::Cols; j++)
 13 |         {
 14 |             tmp = A(i, j);
 15 |             A(i, j) = B(i, j);
 16 |             B(i, j) = tmp;
 17 |         }
 18 |     }
 19 | }
 20 | 
 21 | template <typename ParentTypeA, typename ParentTypeB, int Cols>
 22 | Matrix<3, Cols, typename ParentTypeA::DType> CrossProduct(
 23 |     const MatrixBase<ParentTypeA, 3, Cols, typename ParentTypeA::DType> &matA,
 24 |     const MatrixBase<ParentTypeB, 3, Cols, typename ParentTypeA::DType> &matB)
 25 | {
 26 |     Matrix<3, Cols, typename ParentTypeA::DType> ret;
 27 |     for (int i = 0; i < Cols; ++i)
 28 |     {
 29 |         ret(0,i) = matA(1,i) * matB(2,i) - matB(1,i) * matA(2,i);
 30 |         ret(1,i) = matA(2,i) * matB(0,i) - matB(2,i) * matA(0,i);
 31 |         ret(2,i) = matA(0,i) * matB(1,i) - matB(0,i) * matA(1,i);
 32 |     }
 33 |     return ret;
 34 | }
 35 | 
 36 | template <typename ParentTypeA, typename ParentTypeB, int Dim>
 37 | typename ParentTypeA::DType DotProduct(
 38 |     const MatrixBase<ParentTypeA, Dim, 1, typename ParentTypeA::DType> &vecA,
 39 |     const MatrixBase<ParentTypeB, Dim, 1, typename ParentTypeA::DType> &vecB)
 40 | {
 41 |     typename ParentTypeA::DType ret = 0;
 42 |     for (int i = 0; i < Dim; i++) {
 43 |         ret += vecA(i) * vecB(i);
 44 |     }
 45 |     return ret;
 46 | }
 47 | 
 48 | template <typename ParentType>
 49 | struct LUDecomposition
 50 | {
 51 |     bool singular;
 52 |     typename ParentType::DType parity;
 53 |     PermutationMatrix<ParentType::Rows, typename ParentType::DType> P;
 54 |     LowerUnitriangularMatrix<ParentType> L;
 55 |     UpperTriangularMatrix<ParentType> U;
 56 | 
 57 |     LUDecomposition(MatrixBase<ParentType, ParentType::Rows, ParentType::Cols, typename ParentType::DType> &A)
 58 |         : L(static_cast<ParentType &>(A)), U(static_cast<ParentType &>(A))
 59 |     {
 60 |         static_assert(ParentType::Rows == ParentType::Cols, "Input matrix must be square");
 61 |     }
 62 | };
 63 | 
 64 | template <typename ParentType>
 65 | struct CholeskyDecomposition
 66 | {
 67 |     bool positive_definite = true;
 68 |     LowerTriangularMatrix<ParentType> L;
 69 | 
 70 |     CholeskyDecomposition(MatrixBase<ParentType, ParentType::Rows, ParentType::Cols, typename ParentType::DType> &A)
 71 |         : L(static_cast<ParentType &>(A))
 72 |     {
 73 |     }
 74 | };
 75 | 
 76 | template <typename ParentType, int Dim>
 77 | LUDecomposition<ParentType> LUDecompose(MatrixBase<ParentType, Dim, Dim, typename ParentType::DType> &A)
 78 | {
 79 |     LUDecomposition<ParentType> decomp(A);
 80 |     auto &idx = decomp.P.idx;
 81 |     decomp.parity = 1.0;
 82 | 
 83 |     for (int i = 0; i < Dim; ++i)
 84 |     {
 85 |         idx[i] = i;
 86 |     }
 87 | 
 88 |     // row_scale stores the implicit scaling of each row
 89 |     typename ParentType::DType row_scale[Dim];
 90 | 
 91 |     for (int i = 0; i < Dim; ++i)
 92 |     {
 93 |         // Loop over rows to get the implicit scaling information.
 94 |         typename ParentType::DType largest_elem = 0.0;
 95 | 
 96 |         for (int j = 0; j < Dim; ++j)
 97 |         {
 98 |             typename ParentType::DType this_elem = fabs(A(i, j));
 99 |             largest_elem = max(this_elem, largest_elem);
100 |         }
101 | 
102 |         // No nonzero largest element.
103 |         if (largest_elem == 0.0)
104 |         {
105 |             decomp.singular = true;
106 |             return decomp;
107 |         }
108 | 
109 |         row_scale[i] = 1.0 / largest_elem;
110 |     }
111 | 
112 |     // This is the loop over columns of Crout’s method.
113 |     for (int j = 0; j < Dim; ++j)
114 |     {
115 |         // Calculate beta ij
116 |         for (int i = 0; i < j; ++i)
117 |         {
118 |             typename ParentType::DType sum = 0.0;
119 | 
120 |             for (int k = 0; k < i; ++k)
121 |             {
122 |                 sum += A(i, k) * A(k, j);
123 |             }
124 | 
125 |             A(i, j) -= sum;
126 |         }
127 | 
128 |         // Calcuate alpha ij (before division by the pivot)
129 |         for (int i = j; i < Dim; ++i)
130 |         {
131 |             typename ParentType::DType sum = 0.0;
132 | 
133 |             for (int k = 0; k < j; ++k)
134 |             {
135 |                 sum += A(i, k) * A(k, j);
136 |             }
137 | 
138 |             A(i, j) -= sum;
139 |         }
140 | 
141 |         // Search for largest pivot element
142 |         typename ParentType::DType largest_elem = 0.0;
143 |         int argmax = j;
144 | 
145 |         for (int i = j; i < Dim; i++)
146 |         {
147 |             typename ParentType::DType this_elem = row_scale[i] * fabs(A(i, j));
148 | 
149 |             if (this_elem >= largest_elem)
150 |             {
151 |                 largest_elem = this_elem;
152 |                 argmax = i;
153 |             }
154 |         }
155 | 
156 |         if (j != argmax)
157 |         {
158 |             auto row_argmax = A.Row(argmax);
159 |             auto row_j = A.Row(j);
160 |             Swap(row_argmax, row_j);
161 | 
162 |             decomp.parity = -decomp.parity;
163 | 
164 |             // swap indices
165 |             {
166 |                 auto tmp = idx[j];
167 |                 idx[j] = idx[argmax];
168 |                 idx[argmax] = tmp;
169 |             }
170 | 
171 |             row_scale[argmax] = row_scale[j];
172 |         }
173 | 
174 |         if (A(j, j) == 0.0)
175 |         {
176 |             decomp.singular = true;
177 |             return decomp;
178 |         }
179 | 
180 |         if (j != Dim)
181 |         {
182 |             // Now, finally, divide by the pivot element.
183 |             typename ParentType::DType pivot_inv = 1.0 / A(j, j);
184 | 
185 |             for (int i = j + 1; i < Dim; ++i)
186 |             {
187 |                 A(i, j) *= pivot_inv;
188 |             }
189 |         }
190 |     }
191 | 
192 |     decomp.singular = false;
193 |     return decomp;
194 | }
195 | 
196 | template <int Dim, class LUType, class BType>
197 | Matrix<Dim, 1, typename BType::DType> LUSolve(const LUDecomposition<LUType> &decomp,
198 |                                               const MatrixBase<BType, Dim, 1, typename BType::DType> &b)
199 | {
200 |     Matrix<Dim, 1, typename BType::DType> x, tmp;
201 | 
202 |     auto &idx = decomp.P.idx;
203 |     auto &LU = decomp.L.parent;
204 | 
205 |     // Forward substitution to solve L * y = b
206 |     for (int i = 0; i < Dim; ++i)
207 |     {
208 |         typename BType::DType sum = 0.0;
209 | 
210 |         for (int j = 0; j < i; ++j)
211 |         {
212 |             sum += LU(i, j) * tmp(idx[j]);
213 |         }
214 | 
215 |         tmp(idx[i]) = b(idx[i]) - sum;
216 |     }
217 | 
218 |     // Backward substitution to solve U * x = y
219 |     for (int i = Dim - 1; i >= 0; --i)
220 |     {
221 |         typename BType::DType sum = 0.0;
222 | 
223 |         for (int j = i + 1; j < Dim; ++j)
224 |         {
225 |             sum += LU(i, j) * tmp(idx[j]);
226 |         }
227 | 
228 |         tmp(idx[i]) = (tmp(idx[i]) - sum) / LU(i, i);
229 |     }
230 | 
231 |     // Undo the permutation
232 |     for (int i = 0; i < Dim; ++i)
233 |     {
234 |         x(i) = tmp(idx[i]);
235 |     }
236 | 
237 |     return x;
238 | }
239 | 
240 | template <typename ParentType, int Dim>
241 | CholeskyDecomposition<ParentType> CholeskyDecompose(MatrixBase<ParentType, Dim, Dim, typename ParentType::DType> &A)
242 | {
243 |     CholeskyDecomposition<ParentType> chol(A);
244 | 
245 |     for (int i = 0; i < Dim; ++i)
246 |     {
247 |         for (int j = i; j < Dim; ++j)
248 |         {
249 |             float sum = A(i, j);
250 | 
251 |             for (int k = i - 1; k >= 0; --k)
252 |             {
253 |                 sum -= A(i, k) * A(j, k);
254 |             }
255 | 
256 |             if (i == j)
257 |             {
258 |                 if (sum <= 0.0)
259 |                 {
260 |                     chol.positive_definite = false;
261 |                     return chol;
262 |                 }
263 |                 A(i, i) = sqrt(sum);
264 |             }
265 |             else
266 |             {
267 |                 A(j, i) = sum / A(i, i);
268 |             }
269 |         }
270 |     }
271 | 
272 |     return chol;
273 | }
274 | 
275 | template <int Dim, class LUType, class BType>
276 | Matrix<Dim, 1, typename BType::DType> CholeskySolve(const CholeskyDecomposition<LUType> &decomp,
277 |                                                     const MatrixBase<BType, Dim, 1, typename BType::DType> &b)
278 | {
279 |     Matrix<Dim, 1, typename BType::DType> x;
280 |     auto &A = decomp.L.parent;
281 | 
282 |     for (int i = 0; i < Dim; ++i)
283 |     {
284 |         float sum = b(i);
285 | 
286 |         for (int k = i - 1; k >= 0; --k)
287 |         {
288 |             sum -= A(i, k) * x(k);
289 |         }
290 | 
291 |         x(i) = sum / A(i, i);
292 |     }
293 | 
294 |     for (int i = Dim - 1; i >= 0; --i)
295 |     {
296 |         float sum = x(i);
297 | 
298 |         for (int k = i + 1; k < Dim; ++k)
299 |         {
300 |             sum -= A(k, i) * x(k);
301 |         }
302 | 
303 |         x(i) = sum / A(i, i);
304 |     }
305 | 
306 |     return x;
307 | }
308 | 
309 | template <int Dim, typename InType, typename OutType, typename DType>
310 | bool Invert(const MatrixBase<InType, Dim, Dim, DType> &A, MatrixBase<OutType, Dim, Dim, DType> &out)
311 | {
312 |     Matrix<Dim, Dim, DType> A_copy = A;
313 | 
314 |     auto decomp = LUDecompose(A_copy);
315 | 
316 |     if (decomp.singular)
317 |     {
318 |         return false;
319 |     }
320 | 
321 |     Matrix<Dim, 1, DType> b = Zeros<Dim, 1, DType>();
322 | 
323 |     for (int j = 0; j < Dim; ++j)
324 |     {
325 |         b(j) = 1.0;
326 |         out.Column(j) = LUSolve(decomp, b);
327 |         b(j) = 0.0;
328 |     }
329 | 
330 |     return true;
331 | }
332 | 
333 | template <int Dim, class ParentType>
334 | bool Invert(MatrixBase<ParentType, Dim, Dim, typename ParentType::DType> &A)
335 | {
336 |     return Invert(A, A);
337 | }
338 | 
339 | template <int Dim, class ParentType>
340 | Matrix<Dim, Dim, typename ParentType::DType> Inverse(
341 |     const MatrixBase<ParentType, Dim, Dim, typename ParentType::DType> &A)
342 | {
343 |     Matrix<Dim, Dim, typename ParentType::DType> out;
344 |     Invert(A, out);
345 |     return out;
346 | }
347 | 
348 | // LU-Decomposition only works for floating point numbers. Use Bareiss algorithm for (signed) integer types.
349 | template <typename ParentType, typename Dtype, int Dim>
350 | typename Types::enable_if<Types::is_floating_point<Dtype>::value, Dtype>::type
351 | DeterminantLUDecomposition(const MatrixBase<ParentType, Dim, Dim, Dtype> &A)
352 | {
353 |     Matrix<Dim, Dim, Dtype> A_copy = A;
354 | 
355 |     auto decomp = LUDecompose(A_copy);
356 | 
357 |     Dtype det = decomp.parity;
358 | 
359 |     for (int i = 0; i < Dim; ++i)
360 |     {
361 |         det *= decomp.U(i, i);
362 |     }
363 | 
364 |     return det;
365 | }
366 | 
367 | // Bareiss algorithm works for all (signed) types, but for floating-point numbers LU-Decomposition is faster.
368 | template <typename ParentType, typename Dtype, int Dim>
369 | typename Types::enable_if<Types::is_signed<Dtype>::value, Dtype>::type
370 | DeterminantBareissAlgorithm(const MatrixBase<ParentType, Dim, Dim, Dtype> &A)
371 | {
372 |     Matrix<Dim, Dim, Dtype> A_copy = A;
373 | 
374 |     int sign = 1;
375 |     Dtype prev = 1;
376 | 
377 |     for (int i = 0; i < Dim; i++)
378 |     {
379 |         if (A_copy(i, i) == 0)
380 |         {
381 |             int j = i + 1;
382 |             for (; j < Dim; j++)
383 |             {
384 |                 if (A_copy(j, i) != 0) break;
385 |             }
386 |             if (j == Dim) return 0;
387 |             auto row_i = A_copy.Row(i);
388 |             auto row_j = A_copy.Row(j);
389 |             Swap(row_i, row_j);
390 |             sign = - sign;
391 |         }
392 |         for (int j = i + 1; j < Dim; j++)
393 |         {
394 |             for (int k = i + 1; k < Dim; k++)
395 |             {
396 |                 A_copy(j, k) = (A_copy(j, k) * A_copy(i, i) - A_copy(j, i) * A_copy(i, k)) / prev;
397 |             }
398 |         }
399 |         prev = A_copy(i, i);
400 |     }
401 |     return sign * A_copy(Dim - 1, Dim - 1);
402 | }
403 | 
404 | template <typename ParentType, typename Dtype, int Dim>
405 | typename Types::enable_if<Types::is_floating_point<Dtype>::value, Dtype>::type
406 | Determinant(const MatrixBase<ParentType, Dim, Dim, Dtype> &A) { return DeterminantLUDecomposition(A); }
407 | 
408 | template <typename ParentType, typename Dtype, int Dim>
409 | typename Types::enable_if<Types::is_signed_integer<Dtype>::value, Dtype>::type
410 | Determinant(const MatrixBase<ParentType, Dim, Dim, Dtype> &A) { return DeterminantBareissAlgorithm(A); }
411 | 
412 | template <typename DerivedType>
413 | typename DerivedType::DType Norm(const DownCast<DerivedType> &A)
414 | {
415 |     typename DerivedType::DType sum_sq = 0.0;
416 | 
417 |     for (int i = 0; i < DerivedType::Rows; ++i)
418 |     {
419 |         for (int j = 0; j < DerivedType::Cols; ++j)
420 |         {
421 |             sum_sq += A(i, j) * A(i, j);
422 |         }
423 |     }
424 |     return sqrt(sum_sq);
425 | }
426 | 
427 | template <class DerivedType>
428 | typename DerivedType::DType Trace(const DownCast<DerivedType> &A)
429 | {
430 |     typename DerivedType::DType sum_diag = 0.0;
431 | 
432 |     for (int i = 0; i < DerivedType::Rows; ++i)
433 |     {
434 |         sum_diag += A(i, i);
435 |     }
436 |     return sum_diag;
437 | }
438 | 
439 | template <int Inputs, int Outputs, typename DType>
440 | struct MatrixFunctor
441 | {
442 |     virtual Matrix<Outputs, 1, DType> operator()(const Matrix<Inputs, 1, DType> &x) const = 0;
443 | };
444 | 
445 | template <int Inputs, int Outputs, typename InType>
446 | Matrix<Outputs, Inputs, typename InType::DType> Jacobian(
447 |     const MatrixFunctor<Inputs, Outputs, typename InType::DType> &f,
448 |     const MatrixBase<InType, Inputs, 1, typename InType::DType> &x, const typename InType::DType h = 1e-4)
449 | {
450 |     using DType = typename InType::DType;
451 | 
452 |     Matrix<Outputs, Inputs, DType> jacobian;
453 |     Matrix<Outputs> f_x = f(x);
454 |     Matrix<Inputs, 1, DType> h_vec = Zeros<Inputs, 1, DType>();
455 | 
456 |     for (int i = 0; i < Inputs; i++)
457 |     {
458 |         h_vec(i) = h;
459 |         Matrix<Outputs, 1, DType> f_xh = f(x + h_vec);
460 |         jacobian.Column(i) = (f_xh - f_x) / h;
461 |         h_vec(i) = 0;
462 |     }
463 | 
464 |     return jacobian;
465 | }
466 | 
467 | }  // namespace BLA
468 | 


--------------------------------------------------------------------------------
/impl/Types.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | namespace BLA
 4 | {
 5 | // This namespace exists because the header "typetraits" is not implemented in every Arduino environment.
 6 | namespace Types
 7 | {
 8 | template<class T, class U> struct is_same       { static constexpr bool value = false; };
 9 | template<class T>          struct is_same<T, T> { static constexpr bool value = true;  };
10 | 
11 | template<class T> struct remove_const { typedef T type; };
12 | template<class T> struct remove_const<const T> { typedef T type; };
13 | 
14 | template<class T>
15 | struct is_floating_point
16 | {
17 |     static constexpr bool value =
18 |         is_same<float,       typename remove_const<T>::type>::value ||
19 |         is_same<double,      typename remove_const<T>::type>::value ||
20 |         is_same<long double, typename remove_const<T>::type>::value;
21 | };
22 | 
23 | template<class T>
24 | struct is_signed_integer
25 | {
26 |     static constexpr bool value =
27 |         is_same<signed char,      typename remove_const<T>::type>::value ||
28 |         is_same<signed short,     typename remove_const<T>::type>::value ||
29 |         is_same<signed int,       typename remove_const<T>::type>::value ||
30 |         is_same<signed long,      typename remove_const<T>::type>::value ||
31 |         is_same<signed long long, typename remove_const<T>::type>::value;
32 | };
33 | 
34 | template<class T>
35 | struct is_signed
36 | {
37 |     static constexpr bool value =
38 |         is_floating_point<T>::value ||
39 |         is_signed_integer<T>::value;
40 | };
41 | 
42 | template<bool, typename T = void> struct enable_if {};
43 | template<typename T> struct enable_if<true, T> { typedef T type; };
44 | }
45 | } // namespace BLA
46 | 
47 | 


--------------------------------------------------------------------------------
/keywords.txt:
--------------------------------------------------------------------------------
 1 | #######################################
 2 | # Syntax Coloring Map For BasicLinearAlgebra
 3 | #######################################
 4 | 
 5 | #######################################
 6 | # Datatypes (KEYWORD1)
 7 | #######################################
 8 | 
 9 | Matrix	KEYWORD1
10 | 
11 | #######################################
12 | # Methods and Functions (KEYWORD2)
13 | #######################################
14 | 
15 | Submatrix	KEYWORD2
16 | Fill	KEYWORD2
17 | Invert	KEYWORD2
18 | LUDecompose	KEYWORD2
19 | LUSovle	KEYWORD2
20 | Rows	KEYWORD2
21 | Cols	KEYWORD2
22 | HorzCat	KEYWORD2
23 | VertCat	KEYWORD2
24 | 
25 | #######################################
26 | # Constants (LITERAL1)
27 | #######################################
28 | 


--------------------------------------------------------------------------------
/library.properties:
--------------------------------------------------------------------------------
 1 | name=BasicLinearAlgebra
 2 | version=5.1
 3 | author=Tom Stewart <tomstewart89@hotmail.com>
 4 | maintainer=Tom Stewart <tomstewart89@hotmail.com>
 5 | sentence=A library for representing matrices and doing matrix math on arduino 
 6 | paragraph=Supports most common matrix operations including LU decomposition and inversion without the need for dynamic memory allocation. It also does compile time checking of the dimensions and type of matrices used as operands.
 7 | category=Other
 8 | url=https://github.com/tomstewart89/BasicLinearAlgebra
 9 | architectures=*
10 | 


--------------------------------------------------------------------------------
/test/Arduino.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <algorithm>
 4 | #include <cstddef>
 5 | #include <iomanip>
 6 | #include <sstream>
 7 | 
 8 | #include "Printable.h"
 9 | 
10 | struct Print
11 | {
12 |     std::stringstream buf;
13 | 
14 |     template <typename T>
15 |     typename std::enable_if<!std::is_base_of<Printable, T>::value, size_t>::type
16 |     print(const T& obj)
17 |     {
18 |         buf << obj;
19 |         return 0;
20 |     }
21 | 
22 |     template <typename T>
23 |     typename std::enable_if<!std::is_base_of<Printable, T>::value, size_t>::type
24 |     println(const T& obj)
25 |     {
26 |         buf << obj << std::endl;
27 |         return 0;
28 |     }
29 | 
30 |     size_t print(const Printable& x)
31 |     {
32 |         x.printTo(*this);
33 |         return 0;
34 |     }
35 | 
36 |     size_t println(const Printable& x)
37 |     {
38 |         x.printTo(*this);
39 |         println("");
40 |         return 0;
41 |     }
42 | 
43 |     void begin(int)
44 |     {
45 |         buf << std::fixed << std::showpoint << std::setprecision(2);
46 |         buf.str("");
47 |     }
48 | 
49 |     Print& operator<<(std::ostream& (*pf)(std::ostream&))
50 |     {
51 |         buf << pf;
52 |         return *this;
53 |     }
54 | 
55 | } Serial;
56 | 
57 | using std::endl;
58 | using std::max;
59 | 


--------------------------------------------------------------------------------
/test/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.14)
 2 | project(bla_tests)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 11)
 5 | 
 6 | include(FetchContent)
 7 | FetchContent_Declare(
 8 |   googletest
 9 |   URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip
10 | )
11 | 
12 | FetchContent_MakeAvailable(googletest)
13 | 
14 | enable_testing()
15 | 
16 | include_directories("${PROJECT_SOURCE_DIR}" "${PROJECT_SOURCE_DIR}/..")
17 | 
18 | add_executable(test_arithmetic test_arithmetic.cpp)
19 | target_link_libraries(test_arithmetic gtest_main)
20 | 
21 | add_executable(test_linear_algebra test_linear_algebra.cpp)
22 | target_link_libraries(test_linear_algebra gtest_main)
23 | 
24 | add_executable(test_examples test_examples.cpp)
25 | target_link_libraries(test_examples gtest_main)
26 | 
27 | include(GoogleTest)
28 | 
29 | gtest_discover_tests(test_arithmetic)
30 | gtest_discover_tests(test_linear_algebra)
31 | gtest_discover_tests(test_examples)
32 | 


--------------------------------------------------------------------------------
/test/Printable.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <cstddef>
 4 | 
 5 | class Print;
 6 | 
 7 | class Printable
 8 | {
 9 |   public:
10 |     virtual size_t printTo(Print& p) const = 0;
11 | };
12 | 


--------------------------------------------------------------------------------
/test/test_arithmetic.cpp:
--------------------------------------------------------------------------------
  1 | #include <gtest/gtest.h>
  2 | 
  3 | #include "../BasicLinearAlgebra.h"
  4 | 
  5 | using namespace BLA;
  6 | 
  7 | TEST(Arithmetic, BraceInitialisation)
  8 | {
  9 |     Matrix<2, 2> B{0.0, 45.34, 32.98, 1456.1222};
 10 | 
 11 |     EXPECT_FLOAT_EQ(B(0, 0), 0);
 12 |     EXPECT_FLOAT_EQ(B(0, 1), 45.34);
 13 |     EXPECT_FLOAT_EQ(B(1, 0), 32.98);
 14 |     EXPECT_FLOAT_EQ(B(1, 1), 1456.1222);
 15 | 
 16 |     Matrix<2, 2> C{1.54, 5.98};
 17 | 
 18 |     EXPECT_FLOAT_EQ(C(0, 0), 1.54);
 19 |     EXPECT_FLOAT_EQ(C(0, 1), 5.98);
 20 |     EXPECT_FLOAT_EQ(C(1, 0), 0.0);
 21 |     EXPECT_FLOAT_EQ(C(1, 1), 0.0);
 22 | }
 23 | 
 24 | TEST(Arithmetic, Fill)
 25 | {
 26 |     Matrix<2, 2> A;
 27 |     A.Fill(0.0f);
 28 | 
 29 |     for (int i = 0; i < 2; ++i)
 30 |     {
 31 |         for (int j = 0; j < 2; ++j)
 32 |         {
 33 |             EXPECT_FLOAT_EQ(A(i, j), 0);
 34 |         }
 35 |     }
 36 | }
 37 | 
 38 | TEST(Arithmetic, OnesTest)
 39 | {
 40 |     Matrix<2, 2> A = Zeros<2, 2>();
 41 |     Matrix<2, 2> B = Ones<2, 2>();
 42 | 
 43 |     for (int i = 0; i < 2; ++i)
 44 |     {
 45 |         for (int j = 0; j < 2; ++j)
 46 |         {
 47 |             EXPECT_FLOAT_EQ(A(i, j), 0.0f);
 48 |             EXPECT_FLOAT_EQ(B(i, j), 1.0f);
 49 |         }
 50 |     }
 51 | }
 52 | 
 53 | TEST(Arithmetic, EyeTest)
 54 | {
 55 |     auto I = BLA::Eye<2, 2>();
 56 |     auto Z = BLA::Zeros<2, 2>();
 57 |     auto R = I + Z;
 58 | 
 59 |     for (int i = 0; i < 2; ++i)
 60 |     {
 61 |         for (int j = 0; j < 2; ++j)
 62 |         {
 63 |             EXPECT_FLOAT_EQ(I(i, j), i == j ? 1.0f : 0.0f);
 64 |             EXPECT_FLOAT_EQ(Z(i, j), 0.0f);
 65 |             EXPECT_FLOAT_EQ(R(i, j), i == j ? 1.0f : 0.0f);
 66 |         }
 67 |     }
 68 | }
 69 | 
 70 | TEST(Arithmetic, AdditionSubtraction)
 71 | {
 72 |     Matrix<3, 3> A = {3.25, 5.67, 8.67, 4.55, 7.23, 9.00, 2.35, 5.73, 10.56};
 73 | 
 74 |     Matrix<3, 3> B = {6.54, 3.66, 2.95, 3.22, 7.54, 5.12, 8.98, 9.99, 1.56};
 75 | 
 76 |     auto C = A + B;
 77 |     auto D = A - B;
 78 | 
 79 |     for (int i = 0; i < 3; ++i)
 80 |     {
 81 |         for (int j = 0; j < 3; ++j)
 82 |         {
 83 |             EXPECT_FLOAT_EQ(C(i, j), A(i, j) + B(i, j));
 84 |             EXPECT_FLOAT_EQ(D(i, j), A(i, j) - B(i, j));
 85 |         }
 86 |     }
 87 | 
 88 |     C -= B;
 89 |     D += B;
 90 | 
 91 |     for (int i = 0; i < 3; ++i)
 92 |     {
 93 |         for (int j = 0; j < 3; ++j)
 94 |         {
 95 |             EXPECT_FLOAT_EQ(C(i, j), D(i, j));
 96 |         }
 97 |     }
 98 | }
 99 | 
100 | TEST(Arithmetic, OtherDTypes)
101 | {
102 |     Matrix<3, 3, int> A = {3, 6, 5, 8, 34, 7, 3, 7, 9};
103 | 
104 |     Matrix<3, 3, bool> B = {true, false, true, true, false, false};
105 | 
106 |     auto C = A + 5;
107 |     auto D = B * false;
108 | 
109 |     for (int i = 0; i < 2; ++i)
110 |     {
111 |         for (int j = 0; j < 2; ++j)
112 |         {
113 |             EXPECT_EQ(C(i, j), A(i, j) + 5);
114 |             EXPECT_FALSE(D(i, j));
115 |         }
116 |     }
117 | }
118 | 
119 | TEST(Arithmetic, ElementwiseOperations)
120 | {
121 |     Matrix<3, 3> A = {3.25f, 5.67f, 8.67f, 4.55f, 7.23f, 9.00f, 2.35f, 5.73f, 10.56f};
122 | 
123 |     auto C = A + 2.5f;
124 |     auto D = A - 3.7f;
125 |     auto E = A * 1.2f;
126 |     auto F = A / 6.7f;
127 |     auto G = -A;
128 |     const auto H = 2.5f + A;
129 |     const auto I = 3.7f - A;
130 |     const auto J = 1.2f * A;
131 |     const auto K = 6.7f / A;
132 | 
133 |     for (int i = 0; i < 2; ++i)
134 |     {
135 |         for (int j = 0; j < 2; ++j)
136 |         {
137 |             EXPECT_FLOAT_EQ(C(i, j), A(i, j) + 2.5);
138 |             EXPECT_FLOAT_EQ(D(i, j), A(i, j) - 3.7);
139 |             EXPECT_FLOAT_EQ(E(i, j), A(i, j) * 1.2);
140 |             EXPECT_FLOAT_EQ(F(i, j), A(i, j) / 6.7);
141 |             EXPECT_FLOAT_EQ(G(i, j), -A(i, j));
142 |             EXPECT_FLOAT_EQ(H(i, j), 2.5 + A(i, j));
143 |             EXPECT_FLOAT_EQ(I(i, j), 3.7 - A(i, j));
144 |             EXPECT_FLOAT_EQ(J(i, j), 1.2 * A(i, j));
145 |             EXPECT_FLOAT_EQ(K(i, j), 6.7 / A(i, j));
146 |         }
147 |     }
148 | 
149 |     C -= 2.5f;
150 |     D += 3.7f;
151 |     E /= 1.2f;
152 |     F *= 6.7f;
153 | 
154 |     for (int i = 0; i < 2; ++i)
155 |     {
156 |         for (int j = 0; j < 2; ++j)
157 |         {
158 |             EXPECT_FLOAT_EQ(C(i, j), A(i, j));
159 |             EXPECT_FLOAT_EQ(D(i, j), A(i, j));
160 |             EXPECT_FLOAT_EQ(E(i, j), A(i, j));
161 |             EXPECT_FLOAT_EQ(F(i, j), A(i, j));
162 |         }
163 |     }
164 | }
165 | 
166 | TEST(Arithmetic, Multiplication)
167 | {
168 |     Matrix<3, 3> A = {3., 5., 8., 4., 7., 9., 2., 5.0, 10.};
169 | 
170 |     Matrix<3, 3> B = {6., 3., 2., 3., 7., 5., 8., 9., 1.};
171 | 
172 |     auto C = A * B;
173 | 
174 |     EXPECT_FLOAT_EQ(C(0, 0), 97.);
175 |     EXPECT_FLOAT_EQ(C(0, 1), 116.);
176 |     EXPECT_FLOAT_EQ(C(0, 2), 39.);
177 |     EXPECT_FLOAT_EQ(C(1, 0), 117.);
178 |     EXPECT_FLOAT_EQ(C(1, 1), 142.);
179 |     EXPECT_FLOAT_EQ(C(1, 2), 52.);
180 |     EXPECT_FLOAT_EQ(C(2, 0), 107);
181 |     EXPECT_FLOAT_EQ(C(2, 1), 131.);
182 |     EXPECT_FLOAT_EQ(C(2, 2), 39.);
183 | 
184 |     for (int i = 0; i < 3; ++i)
185 |     {
186 |         for (int j = 0; j < 3; ++j)
187 |         {
188 |             EXPECT_FLOAT_EQ(C(i, j), (A.Row(i) * B.Column(j))(0, 0));
189 |         }
190 |     }
191 | 
192 |     A *= B;
193 | 
194 |     for (int i = 0; i < 3; ++i)
195 |     {
196 |         for (int j = 0; j < 3; ++j)
197 |         {
198 |             EXPECT_FLOAT_EQ(A(i, j), C(i, j));
199 |         }
200 |     }
201 | }
202 | 
203 | TEST(Arithmetic, Concatenation)
204 | {
205 |     Matrix<3, 3> A = {3.25, 5.67, 8.67, 4.55, 7.23, 9.00, 2.35, 5.73, 10.56};
206 | 
207 |     Matrix<3, 3> B = {6.54, 3.66, 2.95, 3.22, 7.54, 5.12, 8.98, 9.99, 1.56};
208 | 
209 |     auto AleftOfB = A || B;
210 |     auto AonTopOfB = A && B;
211 | 
212 |     for (int i = 0; i < 3; ++i)
213 |     {
214 |         for (int j = 0; j < 3; ++j)
215 |         {
216 |             EXPECT_FLOAT_EQ(AleftOfB(i, j), A(i, j));
217 |             EXPECT_FLOAT_EQ(AleftOfB(i, j + 3), B(i, j));
218 |             EXPECT_FLOAT_EQ(AonTopOfB(i, j), A(i, j));
219 |             EXPECT_FLOAT_EQ(AonTopOfB(i + 3, j), B(i, j));
220 |         }
221 |     }
222 | }
223 | 
224 | TEST(Arithmetic, OuterProduct)
225 | {
226 |     Matrix<3> v = {1.0, 2.0, 3.0};
227 | 
228 |     Matrix<3, 3> A = v * ~v;
229 | 
230 |     for (int i = 0; i < A.Rows; ++i)
231 |     {
232 |         for (int j = 0; j < A.Cols; ++j)
233 |         {
234 |             EXPECT_FLOAT_EQ(A(i, j), v(i) * v(j));
235 |         }
236 |     }
237 | }
238 | 
239 | TEST(Arithmetic, Reference)
240 | {
241 |     const Matrix<3, 3> A = {3.25, 5.67, 8.67, 4.55, 7.23, 9.00, 2.35, 5.73, 10.56};
242 |     Matrix<3, 3> B = {2.25, 6.77, 9.67, 14.55, 0.23, 3.21, 5.67, 6.75, 11.56};
243 | 
244 |     const auto A_ref = A.Submatrix<2, 2>(0, 1);
245 |     auto B_ref = B.Submatrix<2, 2>(0, 1);
246 | 
247 |     B_ref(0, 0) = A(0, 0) * A_ref(0, 0);
248 | 
249 |     EXPECT_FLOAT_EQ(B(0, 1), A(0, 0) * A(0, 1));
250 | 
251 |     B.Submatrix<3, 3>(0, 0) = A;
252 | 
253 |     for (int i = 0; i < 3; ++i)
254 |     {
255 |         for (int j = 0; j < 3; ++j)
256 |         {
257 |             EXPECT_FLOAT_EQ(B(i, j), A(i, j));
258 |         }
259 |     }
260 | }
261 | 
262 | TEST(Arithmetic, Cast)
263 | {
264 |     Matrix<3, 3> A = {3.25, 5.67, 8.67, 4.55, 7.23, 9.00, 2.35, 5.73, 10.56};
265 | 
266 |     auto bool_A = A.Cast<bool>();
267 | 
268 |     EXPECT_TRUE(All(bool_A));
269 | 
270 |     Matrix<3, 3, double> A_double = A.Cast<double>();
271 | 
272 |     EXPECT_LT(Norm(A * A_double.Cast<float>() - A * A), 1e-5);
273 | }
274 | 
275 | TEST(Arithmetic, LogicalOperators)
276 | {
277 |     Matrix<3, 3> A = {3.25, 5.67, 8.67, 4.55, 7.23, 9.00, 2.35, 5.73, 10.56};
278 |     Matrix<3, 3> B = {3.25, 6.77, 9.67, 14.55, 0.23, 3.21, 5.67, 6.75, 11.56};
279 | 
280 |     auto A_less_than_three = A < 3.0;
281 |     auto A_greater_than_or_equal_to_three_and_a_bit = A <= 3.25;
282 |     auto A_greater_than_one_hundred = A > 100.0;
283 |     auto A_greater_than_or_equal_to_ten_point_fiveish = A >= 10.56;
284 |     auto A_less_than_B = A < B;
285 |     auto A_less_than_or_equal_to_B = A <= B;
286 |     auto A_greater_than_B = A > B;
287 |     auto A_greater_than_or_equal_to_B = A >= B;
288 |     auto A_equals_B = A == B;
289 | 
290 |     for (int i = 0; i < 3; ++i)
291 |     {
292 |         for (int j = 0; j < 3; ++j)
293 |         {
294 |             EXPECT_TRUE(A_less_than_three(i, j) == A(i, j) < 3.0);
295 |             EXPECT_TRUE(A_greater_than_or_equal_to_three_and_a_bit(i, j) == A(i, j) <= 3.25);
296 |             EXPECT_TRUE(A_greater_than_one_hundred(i, j) == A(i, j) > 100.0);
297 |             EXPECT_TRUE(A_greater_than_or_equal_to_ten_point_fiveish(i, j) == A(i, j) >= 10.56);
298 |             EXPECT_TRUE(A_less_than_B(i, j) == A(i, j) < B(i, j));
299 |             EXPECT_TRUE(A_less_than_or_equal_to_B(i, j) == A(i, j) <= B(i, j));
300 |             EXPECT_TRUE(A_greater_than_B(i, j) == A(i, j) > B(i, j));
301 |             EXPECT_TRUE(A_greater_than_or_equal_to_B(i, j) == A(i, j) >= B(i, j));
302 |         }
303 |     }
304 | 
305 |     EXPECT_TRUE(!All(A_greater_than_one_hundred));
306 |     EXPECT_FALSE(Any(A_greater_than_one_hundred));
307 |     EXPECT_TRUE(Any(A_less_than_three));
308 |     EXPECT_FALSE(All(A_less_than_three));
309 | }
310 | 
311 | int main(int argc, char **argv)
312 | {
313 |     ::testing::InitGoogleTest(&argc, argv);
314 |     return RUN_ALL_TESTS();
315 | }
316 | 


--------------------------------------------------------------------------------
/test/test_examples.cpp:
--------------------------------------------------------------------------------
  1 | #include <BasicLinearAlgebra.h>
  2 | #include <gtest/gtest.h>
  3 | 
  4 | using namespace BLA;
  5 | 
  6 | namespace References
  7 | {
  8 | #include "../examples/References/References.ino"
  9 | }
 10 | 
 11 | TEST(Examples, References)
 12 | {
 13 |     References::setup();
 14 | 
 15 |     EXPECT_STREQ(Serial.buf.str().c_str(),
 16 |                  "bigMatrix(4,2): 45.67\n"
 17 |                  "bigMatrixRef: "
 18 |                  "[[-0.00,0.01,-0.17,0.01],[0.03,-0.01,0.08,-0.01],[-0.01,0.00,-0.05,0.02],[-0.00,0.00,0.13,-0.01]]\n"
 19 |                  "result of convoluted operation: [[1.11,1.12,0.95,1.12],[1.14,1.10,1.20,1.10]]\n"
 20 |                  "bigMatrix: "
 21 |                  "[[0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00],"
 22 |                  "[0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00],"
 23 |                  "[0.00,1.11,1.12,0.95,1.12,0.00,0.00,0.00],"
 24 |                  "[0.00,1.14,1.10,1.20,1.10,0.00,0.00,0.00],"
 25 |                  "[0.00,0.00,-0.00,0.01,-0.17,0.01,0.00,0.00],"
 26 |                  "[0.00,0.00,0.03,-0.01,0.08,-0.01,0.00,0.00],"
 27 |                  "[0.00,0.00,-0.01,0.00,-0.05,0.02,0.00,0.00],"
 28 |                  "[0.00,0.00,-0.00,0.00,0.13,-0.01,0.00,0.00]]");
 29 | }
 30 | 
 31 | namespace SolveLinearEquations
 32 | {
 33 | #include "../examples/SolveLinearEquations/SolveLinearEquations.ino"
 34 | }
 35 | 
 36 | TEST(Examples, SolveLinearEquations)
 37 | {
 38 |     SolveLinearEquations::setup();
 39 | 
 40 |     EXPECT_STREQ(Serial.buf.str().c_str(),
 41 |                  "reconstructed A: "
 42 |                  "[[16.00,78.00,50.00,84.00,70.00,63.00],"
 43 |                  "[2.00,32.00,33.00,61.00,40.00,17.00],"
 44 |                  "[96.00,98.00,50.00,80.00,78.00,27.00],"
 45 |                  "[86.00,49.00,57.00,10.00,42.00,96.00],"
 46 |                  "[44.00,87.00,60.00,67.00,16.00,59.00],"
 47 |                  "[53.00,8.00,64.00,97.00,41.00,90.00]]\n"
 48 | 
 49 |                  "x (via LU decomposition): [[-0.34],[-1.24],[8.40],[-1.96],[1.05],[-3.55]]\n"
 50 |                  "x (via inverse A): [[-0.34],[-1.24],[8.40],[-1.96],[1.05],[-3.55]]");
 51 | }
 52 | 
 53 | namespace Tensor
 54 | {
 55 | #include "../examples/Tensor/Tensor.ino"
 56 | }
 57 | 
 58 | TEST(Examples, Tensor)
 59 | {
 60 |     Tensor::setup();
 61 | 
 62 |     EXPECT_STREQ(Serial.buf.str().c_str(),
 63 |                  "Hyper B: "
 64 |                  "[[[[6.00,10.00],[10.00,18.00]],[[10.00,14.00],[18.00,26.00]]],[[[10.00,18.00],[14.00,26.00]],[[18.00,"
 65 |                  "26.00],[26.00,38.00]]]]");
 66 | }
 67 | 
 68 | namespace CustomMatrix
 69 | {
 70 | #include "../examples/CustomMatrix/CustomMatrix.ino"
 71 | }
 72 | 
 73 | TEST(Examples, CustomMatrix)
 74 | {
 75 |     CustomMatrix::setup();
 76 | 
 77 |     EXPECT_STREQ(
 78 |         Serial.buf.str().c_str(),
 79 |         "still ones: [[1.00,1.00,1.00,1.00],[1.00,1.00,1.00,1.00],[1.00,1.00,1.00,1.00],[1.00,1.00,1.00,1.00]]\n"
 80 |         "scaled rows: [[1.00,1.00,1.00,1.00],[2.00,2.00,2.00,2.00],[3.00,3.00,3.00,3.00],[4.00,4.00,4.00,4.00]]");
 81 | }
 82 | 
 83 | namespace HowToUse
 84 | {
 85 | #include "../examples/HowToUse/HowToUse.ino"
 86 | }
 87 | 
 88 | TEST(Examples, HowToUse)
 89 | {
 90 |     HowToUse::setup();
 91 | 
 92 |     EXPECT_STREQ(Serial.buf.str().c_str(),
 93 |                  "v(1): 43.67\n"
 94 |                  "B: [[9.79,9.33,11.62],[7.77,14.77,14.12],[11.33,15.72,12.12]]\n"
 95 |                  "identity matrix: [[1.00,-0.00,-0.00],[0.00,1.00,-0.00],[0.00,0.00,1.00]]");
 96 | }
 97 | 
 98 | int main(int argc, char **argv)
 99 | {
100 |     ::testing::InitGoogleTest(&argc, argv);
101 |     return RUN_ALL_TESTS();
102 | }
103 | 


--------------------------------------------------------------------------------
/test/test_linear_algebra.cpp:
--------------------------------------------------------------------------------
  1 | #include <gtest/gtest.h>
  2 | 
  3 | #include "../BasicLinearAlgebra.h"
  4 | 
  5 | using namespace BLA;
  6 | 
  7 | TEST(LinearAlgebra, CrossProduct)
  8 | {
  9 |     // Axis 0
 10 |     const Matrix<3> A = {1, 2,  3};
 11 |     const Matrix<3> B = {5, 7, 11};
 12 |     const Matrix<3> AB_expected = {1, 4, -3};
 13 |     const auto AB_result = CrossProduct(A, B);
 14 | 
 15 |     for (int i = 0; i < AB_result.Rows; i++)
 16 |     {
 17 |         for (int j = 0; j < AB_result.Cols; j++)
 18 |         {
 19 |             EXPECT_FLOAT_EQ(AB_result(i, j), AB_expected(i, j));
 20 |         }
 21 |     }
 22 |     // The cross product will always be computed on the second axis.
 23 |     // This can be changed by transposing the matricies.
 24 |     const Matrix<3, 3> E = { 1,  2,  3,  5,  7, 11, 13, 17, 19};
 25 |     const Matrix<3, 3> F = {23, 29, 31, 37, 41, 43, 47, 53, 59};
 26 |     const Matrix<3, 3> EF_expected = {-25, 38, -17, -150, 192, -54, -4, 126, -110};
 27 |     const auto EF_result = CrossProduct(~E, ~F);
 28 | 
 29 |     for (int i = 0; i < EF_expected.Rows; i++)
 30 |     {
 31 |         for (int j = 0; j < EF_expected.Cols; j++)
 32 |         {
 33 |             EXPECT_FLOAT_EQ(EF_result(i, j), EF_expected(j, i));
 34 |         }
 35 |     }
 36 | }
 37 | 
 38 | TEST(LinearAlgebra, DotProduct)
 39 | {
 40 |     // Test with Dimension 3, int
 41 |     Matrix<3, 1, int> A = {2, 3, 4};
 42 |     Matrix<3, 1, int> B = {7, 8, 9};
 43 |     EXPECT_FLOAT_EQ(DotProduct(A, B), 74);
 44 | 
 45 |     // Test with Dimension 4, float
 46 |     Matrix<4> C = {2.5f, -3.4f, 4.3f, 5.2f};
 47 |     Matrix<4> D = {6.7f, 7.6f, -8.5f, 9.4f};
 48 |     EXPECT_NEAR(DotProduct(C, D), 3.23999f, 1e-5);
 49 | }
 50 | 
 51 | TEST(LinearAlgebra, LUDecomposition)
 52 | {
 53 |     Matrix<7, 7> A = {16, 78, 50, 84, 70, 63, 2, 32, 33, 61, 40, 17, 96, 98, 50, 80, 78, 27, 86, 49, 57, 10, 42, 96, 44,
 54 |                       87, 60, 67, 16, 59, 53, 8, 64, 97, 41, 90, 56, 22, 48, 32, 12, 4,  45, 78, 43, 11, 7,  8,  12};
 55 | 
 56 |     auto A_orig = A;
 57 | 
 58 |     auto decomp = LUDecompose(A);
 59 | 
 60 |     EXPECT_FALSE(decomp.singular);
 61 | 
 62 |     auto A_reconstructed = decomp.P * decomp.L * decomp.U;
 63 | 
 64 |     for (int i = 0; i < A.Rows; ++i)
 65 |     {
 66 |         for (int j = 0; j < A.Cols; ++j)
 67 |         {
 68 |             EXPECT_FLOAT_EQ(A_reconstructed(i, j), A_orig(i, j));
 69 |         }
 70 |     }
 71 | }
 72 | 
 73 | TEST(LinearAlgebra, LUSolution)
 74 | {
 75 |     Matrix<3, 3> A{2, 5, 8, 0, 8, 6, 6, 7, 5};
 76 |     Matrix<3, 1> b{10, 11, 12};
 77 |     Matrix<3, 1> x_expected = {0.41826923, 0.97115385, 0.53846154};
 78 | 
 79 |     auto decomp = LUDecompose(A);
 80 | 
 81 |     auto x = LUSolve(decomp, b);
 82 | 
 83 |     for (int i = 0; i < x_expected.Rows; ++i)
 84 |     {
 85 |         EXPECT_FLOAT_EQ(x_expected(i), x(i));
 86 |     }
 87 | }
 88 | 
 89 | TEST(LinearAlgebra, CholeskyDecomposition)
 90 | {
 91 |     // clang-format off
 92 | 
 93 |     // We could fill in this lower triangle but since A is required to be symmetric it can be (and is) inferred
 94 |     // from the upper triangle
 95 |     Matrix<4, 4> A = {0.60171582, -0.20854924,  0.52925771,  0.24206045,
 96 |                       0.0,         0.33012847, -0.28941531, -0.33854164,
 97 |                       0.0,         0.0,         3.54506632,  1.56758518,
 98 |                       0.0,         0.0,         0.0,         1.75291733};
 99 |     // clang-format on
100 | 
101 |     auto A_orig = A;
102 | 
103 |     auto chol = CholeskyDecompose(A);
104 | 
105 |     EXPECT_TRUE(chol.positive_definite);
106 | 
107 |     auto A_reconstructed = chol.L * ~chol.L;
108 | 
109 |     // Compare the recontruction to the upper triangle of A (the lower triangle will be overwritten by decompose)
110 |     for (int i = 0; i < 4; ++i)
111 |     {
112 |         for (int j = 0; j < 4; ++j)
113 |         {
114 |             if (i <= j)
115 |             {
116 |                 EXPECT_FLOAT_EQ(A_reconstructed(i, j), A_orig(i, j));
117 |             }
118 |         }
119 |     }
120 | }
121 | 
122 | TEST(LinearAlgebra, CholeskySolution)
123 | {
124 |     Matrix<5, 5> A = {0.78183123,  0.08385324,  0.37172332,  -0.72518705, -1.11317593, 0.08385324, 0.56011595,
125 |                       0.19965695,  -0.17488402, -0.12703805, 0.37172332,  0.19965695,  0.52769031, -0.19284881,
126 |                       -0.45321194, -0.72518705, -0.17488402, -0.19284881, 2.19127456,  2.13045896, -1.11317593,
127 |                       -0.12703805, -0.45321194, 2.13045896,  3.50184434};
128 | 
129 |     Matrix<5> b = {1.0, 2.0, 3.0, 4.0, 5.0};
130 |     Matrix<5> x_expected = {3.15866835, 2.12529984, 5.23818026, 0.98626514, 2.58690994};
131 | 
132 |     Matrix<5, 5> A_copy = A;
133 | 
134 |     auto chol = CholeskyDecompose(A);
135 | 
136 |     auto x = CholeskySolve(chol, b);
137 | 
138 |     for (int i = 0; i < 5; ++i)
139 |     {
140 |         EXPECT_NEAR(x_expected(i), x(i), 1e-5);
141 |     }
142 | }
143 | 
144 | TEST(LinearAlgebra, Inversion)
145 | {
146 |     BLA::Matrix<3, 3> A = {9.79, 9.33, 11.62, 7.77, 14.77, 14.12, 11.33, 15.72, 12.12};
147 | 
148 |     auto A_inv = A;
149 |     Invert(A_inv);
150 | 
151 |     auto I = A_inv * A;
152 | 
153 |     for (int i = 0; i < A.Rows; ++i)
154 |     {
155 |         for (int j = 0; j < A.Cols; ++j)
156 |         {
157 |             if (i == j)
158 |             {
159 |                 EXPECT_NEAR(I(i, j), 1.0, 1e-5);
160 |             }
161 |             else
162 |             {
163 |                 EXPECT_NEAR(I(i, j), 0.0, 1e-5);
164 |             }
165 |         }
166 |     }
167 | 
168 |     BLA::Matrix<3, 3> singular = {1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
169 | 
170 |     EXPECT_FALSE(Invert(singular));
171 | }
172 | 
173 | TEST(LinearAlgebra, DoublePrecisionInverse)
174 | {
175 |     Matrix<6, 6, double> A = {1. / 48.,  0,          0,         0, 0, 0, 0, 1. / 48.,  0,        0,       0, 0, 0,
176 |                               -1. / 48., 1. / 48.,   0,         0, 0, 0, 0, 0,         1. / 24., 0,       0, 0, 0,
177 |                               0,         -1. / 28.8, 1. / 28.8, 0, 0, 0, 0, -1. / 12., 1. / 24., 1. / 24.};
178 | 
179 |     auto A_inv = Inverse(A * 1.8);
180 | 
181 |     EXPECT_DOUBLE_EQ(A_inv(0, 0), 80.0 / 3.0);
182 |     EXPECT_DOUBLE_EQ(A_inv(5, 5), 40.0 / 3.0);
183 | }
184 | 
185 | TEST(Arithmetic, Determinant)
186 | {
187 |     Matrix<6, 6> B = {0.05508292, 0.82393504, 0.34938018, 0.63818054, 0.18291131, 0.1986636,  0.56799604, 0.81077491,
188 |                       0.71472733, 0.68527613, 0.72759853, 0.25983183, 0.99035713, 0.76096889, 0.26130098, 0.16855372,
189 |                       0.0253581,  0.47907605, 0.58735833, 0.0913456,  0.03221577, 0.5210331,  0.61583369, 0.33233299,
190 |                       0.20578816, 0.356537,   0.70661899, 0.6569476,  0.90074756, 0.59771572, 0.20054716, 0.41290408,
191 |                       0.70679818, 0.321249,   0.81886099, 0.77819212};
192 | 
193 |     float det_numpy = -0.03919640039505248;
194 | 
195 |     EXPECT_FLOAT_EQ(Determinant(B), det_numpy);
196 | 
197 |     BLA::Matrix<3, 3> singular = {1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
198 | 
199 |     EXPECT_FLOAT_EQ(Determinant(singular), 0.0);
200 | 
201 |     BLA::Matrix<4, 4, int16_t> C = {8, 5, 5, 8, 3, 1, 3, 2, 1, 1, 3, 0, 3, 3, 5, 9};
202 |     int16_t det_C_expected = -140;
203 | 
204 |     EXPECT_EQ(Determinant(C), det_C_expected);
205 | 
206 |     BLA::Matrix<3, 3, int> singular_int = {1, 0, 0, 1, 0, 0, 1, 0, 0};
207 | 
208 |     EXPECT_EQ(Determinant(singular_int), 0);
209 | }
210 | 
211 | template <typename SparseMatA, typename SparseMatB, int OutTableSize = 100>
212 | SparseMatrix<SparseMatA::Rows, SparseMatB::Cols, typename SparseMatA::DType, OutTableSize> sparse_mul(
213 |     const SparseMatA &A, const SparseMatB &B)
214 | {
215 |     static_assert(A.Cols == B.Rows, "Incompatible dimensions for sparse matrix multiplication");
216 | 
217 |     SparseMatrix<A.Rows, B.Cols, typename SparseMatA::DType, OutTableSize> out;
218 | 
219 |     for (int i = 0; i < SparseMatA::Size; ++i)
220 |     {
221 |         for (int j = 0; j < SparseMatB::Size; ++j)
222 |         {
223 |             const auto &elem_a = A.table[i];
224 |             const auto &elem_b = B.table[j];
225 | 
226 |             if (elem_a.row >= 0 && elem_b.row >= 0)
227 |             {
228 |                 out(elem_a.row, elem_b.col) += elem_a.val * elem_b.val;
229 |             }
230 |         }
231 |     }
232 | 
233 |     return out;
234 | }
235 | 
236 | TEST(Examples, SparseMatrix)
237 | {
238 |     SparseMatrix<1, 3000, float, 100> A;
239 |     SparseMatrix<3000, 1, float, 100> B;
240 | 
241 |     A(0, 1000) = 5.0;
242 |     B(1000, 0) = 5.0;
243 | 
244 |     auto C = sparse_mul(A, B);
245 | 
246 |     EXPECT_EQ(C(0, 0), 25.0);
247 | }
248 | 
249 | class JacobianTestFunctor : public MatrixFunctor<2, 3, float>
250 | {
251 |     Matrix<3, 1, float> operator()(const Matrix<2, 1, float> &x) const override
252 |     {
253 |         Matrix<3> p1 = {cos(x(0)), sin(x(0)), x(0)};
254 |         Matrix<3> p2 = {cos((x(0) + x(1))), sin((x(0) + x(1))), x(1)};
255 | 
256 |         return p1 + p2;
257 |     }
258 | };
259 | 
260 | TEST(Examples, NumericJacobian)
261 | {
262 |     // JacobianTestFunctor(x) = [cos(x1) + cos(x1 + x2), sin(x1) + sin(x1 + x2), x1 + x2]
263 |     // jacobian =
264 |     //       [[-sin(x1) - sin(x1 + x2), -sin(x1 + x2)]
265 |     //        [cos(x1) + cos(x1 + x2) ,  cos(x1 + x2)]
266 |     //        [      1                ,          1   ]]
267 | 
268 |     Matrix<2> x = {0.0f, 0.0f};
269 |     JacobianTestFunctor functor;
270 | 
271 |     Matrix<3, 2> jacobian = Jacobian<2, 3>(JacobianTestFunctor(), x);
272 | 
273 |     EXPECT_NEAR(jacobian(0, 0), 0, 1e-4);
274 |     EXPECT_NEAR(jacobian(0, 1), 0, 1e-4);
275 | 
276 |     EXPECT_NEAR(jacobian(1, 0), 2, 1e-4);
277 |     EXPECT_NEAR(jacobian(1, 1), 1, 1e-4);
278 | 
279 |     EXPECT_NEAR(jacobian(2, 0), 1, 1e-4);
280 |     EXPECT_NEAR(jacobian(2, 1), 1, 1e-4);
281 | }
282 | 
283 | int main(int argc, char **argv)
284 | {
285 |     ::testing::InitGoogleTest(&argc, argv);
286 |     return RUN_ALL_TESTS();
287 | }
288 | 


--------------------------------------------------------------------------------