fmatvec 0.0.0

linear_algebra.h
1 /* Copyright (C) 2003-2005 Martin Förg, Rober Huber
2
3 * This library is free software; you can redistribute it and/or
4 * modify it under the terms of the GNU Lesser General Public
5 * License as published by the Free Software Foundation; either
6 * version 2.1 of the License, or (at your option) any later version.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public
14 * License along with this library; if not, write to the Free Software
15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16 *
17 * Contact:
18 * martin.o.foerg@googlemail.com
19 *
20 */
21
22 #ifndef linear_algebra_h
23 #define linear_algebra_h
24 #if defined(_WIN32) && !defined(NOMINMAX)
25 #define NOMINMAX
26 #endif
27
28 #include "square_matrix.h"
29 #include "vector.h"
30 #include "row_vector.h"
31 #include "fixed_square_matrix.h"
32 #include "fixed_vector.h"
33 #include "fixed_row_vector.h"
34 #include "fixed_symmetric_matrix.h"
35 #include "var_square_matrix.h"
36 #include "var_symmetric_matrix.h"
37 #include "var_vector.h"
38 #include "var_row_vector.h"
39 #include "var_fixed_general_matrix.h"
40 #include "fixed_var_general_matrix.h"
41 #include "diagonal_matrix.h"
42 #include "sparse_matrix.h"
43 #include "symmetric_sparse_matrix.h"
44 #include <cmath>
45 #ifndef NDEBUG
46 #include <iostream>
47 #include <boost/stacktrace.hpp>
48 #endif
49
50 namespace fmatvec {
51
52 // template declaration for complex type defined in linear_algebra_complex.h
53 template<class T> std::complex<T> operator+(const std::complex<T> &x, int y);
54 template<class T> std::complex<T> operator-(const std::complex<T> &x, int y);
55 template<class T> std::complex<T> operator*(const std::complex<T> &x, int y);
56 template<class T> std::complex<T> operator/(const std::complex<T> &x, int y);
57 template<class T> std::complex<T> operator+(int x, const std::complex<T> &y);
58 template<class T> std::complex<T> operator-(int x, const std::complex<T> &y);
59 template<class T> std::complex<T> operator*(int x, const std::complex<T> &y);
60 template<class T> std::complex<T> operator/(int x, const std::complex<T> &y);
61
63
64 // Vector-Vector
65 template <class Row1, class Row2, class Row3, class AT1, class AT2, class AT3>
66 inline void add(const Vector<Row1, AT1> &a1, const Vector<Row2, AT2> &a2, Vector<Row3, AT3> &a3) {
67 FMATVEC_ASSERT(a1.size() == a2.size(), AT3);
68 for (int i = 0; i < a1.size(); i++)
69 a3.e(i) = a1.e(i) + a2.e(i);
70 }
71
72 template <class Row1, class Row2, class AT>
73 inline void add(Vector<Row1, AT> &a1, const Vector<Row2, AT> &a2) {
74 FMATVEC_ASSERT(a1.size() == a2.size(), AT);
75 for (int i = 0; i < a1.size(); i++)
76 a1.e(i) += a2.e(i);
77 }
78
79 template <class Row1, class Row2, class Row3, class AT1, class AT2>
80 inline void sub(const Vector<Row1, AT1> &a1, const Vector<Row2, AT2> &a2, Vector<Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &a3) {
81 FMATVEC_ASSERT(a1.size() == a2.size(), AT2);
82 for (int i = 0; i < a1.size(); i++)
83 a3.e(i) = a1.e(i) - a2.e(i);
84 }
85
86 template <class Row1, class Row2, class AT1, class AT2>
87 inline void sub(Vector<Row1, AT1> &a1, const Vector<Row2, AT2> &a2) {
88 FMATVEC_ASSERT(a1.size() == a2.size(), AT2);
89 for (int i = 0; i < a1.size(); i++)
90 a1.e(i) -= a2.e(i);
91 }
92
93 // Addition
94 template <class AT1, class AT2, class Row1, class Row2>
95 inline Vector<Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Vector<Row1, AT1> &a, const Vector<Row2, AT2> &b) {
96 Vector<Var, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
97 add(a, b, c);
98 return c;
99 }
100 template <class AT1, class AT2, class Row>
101 inline Vector<Row, typename OperatorResult<AT1, AT2>::Type> operator+(const Vector<Row, AT1> &a, const Vector<Row, AT2> &b) {
102 Vector<Row, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
103 add(a, b, c);
104 return c;
105 }
106 template <class AT1, class AT2, int M, class Row>
107 inline Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator+(const Vector<Fixed<M>, AT1> &a, const Vector<Row, AT2> &b) {
108 Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
109 add(a, b, c);
110 return c;
111 }
112 template <class AT1, class AT2, int M>
113 inline Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator+(const Vector<Fixed<M>, AT1> &a, const Vector<Fixed<M>, AT2> &b) {
114 Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
115 add(a, b, c);
116 return c;
117 }
118 // move
119 template <class Row1, class Row2>
120 inline Vector<Row2, double> operator+(Vector<Row1, double> &&a, Vector<Row2, double> &&b) {
121 add(a, b);
122 return std::move(a);
123 }
124 template <class Row1, class Row2>
125 inline Vector<Row1, double> operator+(Vector<Row1, double> &&a, const Vector<Row2, double> &b) {
126 add(a, b);
127 return std::move(a);
128 }
129 template <class Row1, class Row2>
130 inline Vector<Row2, double> operator+(const Vector<Row1, double> &a, Vector<Row2, double> &&b) {
131 add(b, a);
132 return std::move(b);
133 }
134 template <class AT, class Row1, class Row2>
135 inline Vector<Row1, AT> operator+=(Vector<Row1, AT> &a, const Vector<Row2, AT> &b) {
136 add(a, b);
137 return a;
138 }
139
140 // Subtraction
141 template <class AT1, class AT2, class Row1, class Row2>
142 inline Vector<Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Vector<Row1, AT1> &a, const Vector<Row2, AT2> &b) {
143 Vector<Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
144 sub(a, b, c);
145 return c;
146 }
147 template <class AT1, class AT2, class Row>
148 inline Vector<Row, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Vector<Row, AT1> &a, const Vector<Row, AT2> &b) {
149 Vector<Row, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
150 sub(a, b, c);
151 return c;
152 }
153 template <class AT1, class AT2, int M, class Row2>
154 inline Vector<Fixed<M>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Vector<Fixed<M>, AT1> &a, const Vector<Row2, AT2> &b) {
155 Vector<Fixed<M>, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
156 sub(a, b, c);
157 return c;
158 }
159 template <class AT1, class AT2, int M>
160 inline Vector<Fixed<M>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Vector<Fixed<M>, AT1> &a, const Vector<Fixed<M>, AT2> &b) {
161 Vector<Fixed<M>, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
162 sub(a, b, c);
163 return c;
164 }
165 // move
166 template <class Row1, class Row2>
167 inline Vector<Row2, double> operator-(Vector<Row1, double> &&a, Vector<Row2, double> &&b) {
168 sub(a, b);
169 return std::move(a);
170 }
171 template <class Row1, class Row2>
172 inline Vector<Row1, double> operator-(Vector<Row1, double> &&a, const Vector<Row2, double> &b) {
173 sub(a, b);
174 return std::move(a);
175 }
176 template <class Row1, class Row2>
177 inline Vector<Row2, double> operator-(const Vector<Row1, double> &a, Vector<Row2, double> &&b) {
178 sub(a, b, b);
179 return std::move(b);
180 }
181 template <class AT, class Row1, class Row2>
182 inline Vector<Row1, AT> operator-=(Vector<Row1, AT> &a, const Vector<Row2, AT> &b) {
183 sub(a, b);
184 return a;
185 }
186
187 // RowVector-Vector
188 template <class Col1, class Col2, class Col3, class AT>
189 inline void add(const RowVector<Col1, AT> &a1, const RowVector<Col2, AT> &a2, RowVector<Col3, AT> &a3) {
190 FMATVEC_ASSERT(a1.size() == a2.size(), AT);
191 for (int i = 0; i < a1.size(); i++)
192 a3.e(i) = a1.e(i) + a2.e(i);
193 }
194
195 template <class Col1, class Col2, class AT>
196 inline void add(RowVector<Col1, AT> &a1, const RowVector<Col2, AT> &a2) {
197 FMATVEC_ASSERT(a1.size() == a2.size(), AT);
198 for (int i = 0; i < a1.size(); i++)
199 a1.e(i) += a2.e(i);
200 }
201
202 template <class Col1, class Col2, class Col3, class AT1, class AT2>
203 inline void sub(const RowVector<Col1, AT1> &a1, const RowVector<Col2, AT2> &a2, RowVector<Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &a3) {
204 FMATVEC_ASSERT(a1.size() == a2.size(), AT2);
205 for (int i = 0; i < a1.size(); i++)
206 a3.e(i) = a1.e(i) - a2.e(i);
207 }
208
209 template <class Col1, class Col2, class AT1, class AT2>
210 inline void sub(RowVector<Col1, AT1> &a1, const RowVector<Col2, AT2> &a2) {
211 FMATVEC_ASSERT(a1.size() == a2.size(), AT2);
212 for (int i = 0; i < a1.size(); i++)
213 a1.e(i) -= a2.e(i);
214 }
215
216 // Addition
217 template <class AT1, class AT2, class Col1, class Col2>
218 inline RowVector<Var, typename OperatorResult<AT1, AT2>::Type> operator+(const RowVector<Col1, AT1> &a, const RowVector<Col2, AT2> &b) {
219 RowVector<Var, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
220 add(a, b, c);
221 return c;
222 }
223 template <class AT1, class AT2, class Col>
224 inline RowVector<Col, typename OperatorResult<AT1, AT2>::Type> operator+(const RowVector<Col, AT1> &a, const RowVector<Col, AT2> &b) {
225 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
226 add(a, b, c);
227 return c;
228 }
229 template <class AT1, class AT2, int N, class Col2>
230 inline RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const RowVector<Fixed<N>, AT1> &a, const RowVector<Col2, AT2> &b) {
231 RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
232 add(a, b, c);
233 return c;
234 }
235 template <class AT1, class AT2, int N>
236 inline RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const RowVector<Fixed<N>, AT1> &a, const RowVector<Fixed<N>, AT2> &b) {
237 RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
238 add(a, b, c);
239 return c;
240 }
241 // move
242 template <class Row1, class Row2>
243 inline RowVector<Row2, double> operator+(RowVector<Row1, double> &&a, RowVector<Row2, double> &&b) {
244 add(a, b);
245 return std::move(a);
246 }
247 template <class Row1, class Row2>
248 inline RowVector<Row1, double> operator+(RowVector<Row1, double> &&a, const RowVector<Row2, double> &b) {
249 add(a, b);
250 return std::move(a);
251 }
252 template <class Row1, class Row2>
253 inline RowVector<Row2, double> operator+(const RowVector<Row1, double> &a, RowVector<Row2, double> &&b) {
254 add(b, a);
255 return std::move(b);
256 }
257 template <class AT, class Col1, class Col2>
258 inline RowVector<Col1, AT> operator+=(RowVector<Col1, AT> &a, const RowVector<Col2, AT> &b) {
259 add(a, b);
260 return a;
261 }
262
263 // Subtraction
264 template <class AT1, class AT2, class Col1, class Col2>
265 inline RowVector<Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const RowVector<Col1, AT1> &a, const RowVector<Col2, AT2> &b) {
266 RowVector<Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
267 sub(a, b, c);
268 return c;
269 }
270 template <class AT1, class AT2, class Col>
271 inline RowVector<Col, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const RowVector<Col, AT1> &a, const RowVector<Col, AT2> &b) {
272 RowVector<Col, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
273 sub(a, b, c);
274 return c;
275 }
276 template <class AT1, class AT2, int N, class Col2>
277 inline RowVector<Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const RowVector<Fixed<N>, AT1> &a, const RowVector<Col2, AT2> &b) {
278 RowVector<Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
279 sub(a, b, c);
280 return c;
281 }
282 template <class AT1, class AT2, int N>
283 inline RowVector<Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const RowVector<Fixed<N>, AT1> &a, const RowVector<Fixed<N>, AT2> &b) {
284 RowVector<Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> c(a.size(), NONINIT);
285 sub(a, b, c);
286 return c;
287 }
288 // move
289 template <class Row1, class Row2>
290 inline RowVector<Row2, double> operator-(RowVector<Row1, double> &&a, RowVector<Row2, double> &&b) {
291 sub(a, b);
292 return std::move(a);
293 }
294 template <class Row1, class Row2>
295 inline RowVector<Row1, double> operator-(RowVector<Row1, double> &&a, const RowVector<Row2, double> &b) {
296 sub(a, b);
297 return std::move(a);
298 }
299 template <class Row1, class Row2>
300 inline RowVector<Row2, double> operator-(const RowVector<Row1, double> &a, RowVector<Row2, double> &&b) {
301 sub(a, b, b);
302 return std::move(b);
303 }
304 template <class AT, class Col1, class Col2>
305 inline RowVector<Col1, AT> operator-=(RowVector<Col1, AT> &a, const RowVector<Col2, AT> &b) {
306 sub(a, b);
307 return a;
308 }
309
310 // Matrix-Matrix
311 template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT1, class AT2, class AT3>
312 inline void add(const Matrix<Type1, Row1, Col1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2, Matrix<Type3, Row3, Col3, AT3> &A3) {
313 FMATVEC_ASSERT(A1.rows() == A2.rows(), AT3);
314 FMATVEC_ASSERT(A1.cols() == A2.cols(), AT3);
315 for (int i = 0; i < A1.rows(); i++)
316 for (int j = 0; j < A2.cols(); j++)
317 A3.e(i, j) = A1.e(i, j) + A2.e(i, j);
318 }
319
320 template <class Row1, class Row2, class Row3, class AT>
321 inline void add(const Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2, Matrix<Symmetric, Row3, Row3, AT> &A3) {
322 FMATVEC_ASSERT(A1.size() == A2.size(), AT);
323 for (int i = 0; i < A1.size(); i++)
324 for (int j = i; j < A2.size(); j++)
325 A3.ej(i, j) = A1.ej(i, j) + A2.ej(i, j);
326 }
327
328 template <class Row1, class Row2, class Row3, class AT>
329 inline void add(const Matrix<Diagonal, Row1, Row1, AT> &A1, const Matrix<Diagonal, Row2, Row2, AT> &A2, Matrix<Diagonal, Row3, Row3, AT> &A3) {
330 FMATVEC_ASSERT(A1.size() == A2.size(), AT);
331 for (int i = 0; i < A3.size(); i++)
332 A3.e(i) = A1.e(i) + A2.e(i);
333 }
334
335 template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class AT>
336 inline void add(Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2) {
337 FMATVEC_ASSERT(A1.rows() == A2.rows(), AT);
338 FMATVEC_ASSERT(A1.cols() == A2.cols(), AT);
339 for (int i = 0; i < A1.rows(); i++)
340 for (int j = 0; j < A2.cols(); j++)
341 A1.e(i, j) += A2.e(i, j);
342 }
343
344 template <class Row1, class Row2, class AT>
345 inline void add(Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2) {
346 FMATVEC_ASSERT(A1.size() == A2.size(), AT);
347 for (int i = 0; i < A1.size(); i++)
348 for (int j = i; j < A2.size(); j++)
349 A1.ej(i, j) += A2.ej(i, j);
350 }
351
352 template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT1, class AT2>
353 inline void sub(const Matrix<Type1, Row1, Col1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2, Matrix<Type3, Row3, Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
354 FMATVEC_ASSERT(A1.rows() == A2.rows(), AT2);
355 FMATVEC_ASSERT(A1.cols() == A2.cols(), AT2);
356 for (int i = 0; i < A1.rows(); i++)
357 for (int j = 0; j < A2.cols(); j++)
358 A3.e(i, j) = A1.e(i, j) - A2.e(i, j);
359 }
360
361 template <class Row1, class Row2, class Row3, class AT1, class AT2>
362 inline void sub(const Matrix<Symmetric, Row1, Row1, AT1> &A1, const Matrix<Symmetric, Row2, Row2, AT2> &A2, Matrix<Symmetric, Row3, Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
363 FMATVEC_ASSERT(A1.size() == A2.size(), AT2);
364 for (int i = 0; i < A1.size(); i++)
365 for (int j = i; j < A2.size(); j++)
366 A3.ej(i, j) = A1.ej(i, j) - A2.ej(i, j);
367 }
368
369 template <class Row1, class Row2, class Row3, class AT1, class AT2>
370 inline void sub(const Matrix<Diagonal, Row1, Row1, AT1> &A1, const Matrix<Diagonal, Row2, Row2, AT2> &A2, Matrix<Diagonal, Row3, Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
371 FMATVEC_ASSERT(A1.size() == A2.size(), AT2);
372 for (int i = 0; i < A3.size(); i++)
373 A3.e(i) = A1.e(i) - A2.e(i);
374 }
375
376 template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class AT1, class AT2>
377 inline void sub(Matrix<Type1, Row1, Col1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2) {
378 FMATVEC_ASSERT(A1.rows() == A2.rows(), AT2);
379 FMATVEC_ASSERT(A1.cols() == A2.cols(), AT2);
380 for (int i = 0; i < A1.rows(); i++)
381 for (int j = 0; j < A2.cols(); j++)
382 A1.e(i, j) -= A2.e(i, j);
383 }
384
385 template <class Row1, class Row2, class AT1, class AT2>
386 inline void sub(Matrix<Symmetric, Row1, Row1, AT1> &A1, const Matrix<Symmetric, Row2, Row2, AT2> &A2) {
387 FMATVEC_ASSERT(A1.size() == A2.size(), AT2);
388 for (int i = 0; i < A1.size(); i++)
389 for (int j = i; j < A2.size(); j++)
390 A1.ej(i, j) -= A2.ej(i, j);
391 }
392
393 // Addition
394 // Type1 Type2
395 template <class AT1, class AT2, class Type1, class Type2, class Row1, class Col1, class Row2, class Col2>
396 inline Matrix<General, Var, Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Row1, Col1, AT1> &A, const Matrix<Type2, Row2, Col2, AT2> &B) {
397 Matrix<General, Var, Var, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
398 add(A, B, C);
399 return C;
400 }
401 template <class AT1, class AT2, class Type1, class Type2, class Row, class Col>
402 inline Matrix<General, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Row, Col, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
403 Matrix<General, Row, Col, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
404 add(A, B, C);
405 return C;
406 }
407 template <class AT1, class AT2, int M, class Type1, class Type2, class Row, class Col>
408 inline Matrix<General, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Fixed<M>, Var, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
409 Matrix<General, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
410 add(A, B, C);
411 return C;
412 }
413 template <class AT1, class AT2, int M, class Type1, class Type2>
414 inline Matrix<General, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Fixed<M>, Var, AT1> &A, const Matrix<Type2, Fixed<M>, Var, AT2> &B) {
415 Matrix<General, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
416 add(A, B, C);
417 return C;
418 }
419 template <class AT1, class AT2, int N, class Type1, class Type2, class Row, class Col>
420 inline Matrix<General, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Var, Fixed<N>, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
421 Matrix<General, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
422 add(A, B, C);
423 return C;
424 }
425 template <class AT1, class AT2, int N, class Type1, class Type2>
426 inline Matrix<General, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Var, Fixed<N>, AT1> &A, const Matrix<Type2, Var, Fixed<N>, AT2> &B) {
427 Matrix<General, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
428 add(A, B, C);
429 return C;
430 }
431 template <class AT1, class AT2, int M, int N, class Type1, class Type2, class Row, class Col>
432 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
433 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
434 add(A, B, C);
435 return C;
436 }
437 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
438 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Fixed<M>, Var, AT2> &B) {
439 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
440 add(A, B, C);
441 return C;
442 }
443 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
444 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Var, Fixed<N>, AT2> &B) {
445 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
446 add(A, B, C);
447 return C;
448 }
449 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
450 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Fixed<M>, Fixed<N>, AT2> &B) {
451 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
452 add(A, B, C);
453 return C;
454 }
455 template <class AT1, class AT2, int M, int N, class Type1, class Type2, class Row, class Col>
456 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type2, Row, Col, AT1> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT2> &B) {
457 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
458 add(A, B, C);
459 return C;
460 }
461 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
462 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type2, Fixed<M>, Var, AT1> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT2> &B) {
463 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
464 add(A, B, C);
465 return C;
466 }
467 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
468 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type2, Var, Fixed<N>, AT1> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT2> &B) {
469 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
470 add(A, B, C);
471 return C;
472 }
473 // Type
474 template <class AT1, class AT2, class Type, class Row1, class Col1, class Row2, class Col2>
475 inline Matrix<Type, Var, Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Row1, Col1, AT1> &A, const Matrix<Type, Row2, Col2, AT2> &B) {
476 Matrix<Type, Var, Var, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
477 add(A, B, C);
478 return C;
479 }
480 template <class AT1, class AT2, class Type, class Row, class Col>
481 inline Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Row, Col, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
482 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
483 add(A, B, C);
484 return C;
485 }
486 template <class AT1, class AT2, int M, class Type, class Row, class Col>
487 inline Matrix<Type, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Var, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
488 Matrix<Type, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
489 add(A, B, C);
490 return C;
491 }
492 template <class AT1, class AT2, int M, class Type>
493 inline Matrix<Type, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Var, AT1> &A, const Matrix<Type, Fixed<M>, Var, AT2> &B) {
494 Matrix<Type, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
495 add(A, B, C);
496 return C;
497 }
498 template <class AT1, class AT2, int N, class Type, class Row, class Col>
499 inline Matrix<Type, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Var, Fixed<N>, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
500 Matrix<Type, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
501 add(A, B, C);
502 return C;
503 }
504 template <class AT1, class AT2, int N, class Type>
505 inline Matrix<Type, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Var, Fixed<N>, AT1> &A, const Matrix<Type, Var, Fixed<N>, AT2> &B) {
506 Matrix<Type, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
507 add(A, B, C);
508 return C;
509 }
510 template <class AT1, class AT2, int M, int N, class Type, class Row, class Col>
511 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
512 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
513 add(A, B, C);
514 return C;
515 }
516 template <class AT1, class AT2, int M, int N, class Type>
517 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Fixed<M>, Var, AT2> &B) {
518 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
519 add(A, B, C);
520 return C;
521 }
522 template <class AT1, class AT2, int M, int N, class Type>
523 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Var, Fixed<N>, AT2> &B) {
524 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
525 add(A, B, C);
526 return C;
527 }
528 template <class AT1, class AT2, int M, int N, class Type>
529 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
530 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
531 add(A, B, C);
532 return C;
533 }
534 template <class AT1, class AT2, int M, int N, class Type, class Row, class Col>
535 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Row, Col, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
536 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
537 add(A, B, C);
538 return C;
539 }
540 template <class AT1, class AT2, int M, int N, class Type>
541 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Fixed<M>, Var, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
542 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
543 add(A, B, C);
544 return C;
545 }
546 template <class AT1, class AT2, int M, int N, class Type>
547 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator+(const Matrix<Type, Var, Fixed<N>, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
548 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
549 add(A, B, C);
550 return C;
551 }
552 // move
553 template <class Type1, class Type2, class Row1, class Row2, class Col1, class Col2>
554 inline Matrix<Type2, Row2, Col2, double> operator+(Matrix<Type1, Row1, Col1, double> &&a, Matrix<Type2, Row2, Col2, double> &&b) {
555 add(a, b);
556 return std::move(a);
557 }
558 template <class Type1, class Type2, class Row1, class Row2, class Col1, class Col2>
559 inline Matrix<Type1, Row1, Col1, double> operator+(Matrix<Type1, Row1, Col1, double> &&a, const Matrix<Type2, Row2, Col2, double> &b) {
560 add(a, b);
561 return std::move(a);
562 }
563 template <class Type1, class Type2, class Row1, class Row2, class Col1, class Col2>
564 inline Matrix<Type2, Row2, Col2, double> operator+(const Matrix<Type1, Row1, Col1, double> &a, Matrix<Type2, Row2, Col2, double> &&b) {
565 add(b, a);
566 return std::move(b);
567 }
568 template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
569 inline Matrix<Type1, Row1, Col1, AT>& operator+=(Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
570 add(A, B);
571 return A;
572 }
573
574 // Subtraction
575 // Type1 Type2
576 template <class AT1, class AT2, class Type1, class Type2, class Row1, class Col1, class Row2, class Col2>
577 inline Matrix<General, Var, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Row1, Col1, AT1> &A, const Matrix<Type2, Row2, Col2, AT2> &B) {
578 Matrix<General, Var, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
579 sub(A, B, C);
580 return C;
581 }
582 template <class AT1, class AT2, class Type1, class Type2, class Row, class Col>
583 inline Matrix<General, Row, Col, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Row, Col, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
584 Matrix<General, Row, Col, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
585 sub(A, B, C);
586 return C;
587 }
588 template <class AT1, class AT2, int M, class Type1, class Type2, class Row, class Col>
589 inline Matrix<General, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Fixed<M>, Var, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
590 Matrix<General, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
591 sub(A, B, C);
592 return C;
593 }
594 template <class AT1, class AT2, int M, class Type1, class Type2>
595 inline Matrix<General, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Fixed<M>, Var, AT1> &A, const Matrix<Type2, Fixed<M>, Var, AT2> &B) {
596 Matrix<General, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
597 sub(A, B, C);
598 return C;
599 }
600 template <class AT1, class AT2, int N, class Type1, class Type2, class Row, class Col>
601 inline Matrix<General, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Var, Fixed<N>, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
602 Matrix<General, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
603 sub(A, B, C);
604 return C;
605 }
606 template <class AT1, class AT2, int N, class Type1, class Type2>
607 inline Matrix<General, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Var, Fixed<N>, AT1> &A, const Matrix<Type2, Var, Fixed<N>, AT2> &B) {
608 Matrix<General, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
609 sub(A, B, C);
610 return C;
611 }
612 template <class AT1, class AT2, int M, int N, class Type1, class Type2, class Row, class Col>
613 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Row, Col, AT2> &B) {
614 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
615 sub(A, B, C);
616 return C;
617 }
618 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
619 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Fixed<M>, Var, AT2> &B) {
620 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
621 sub(A, B, C);
622 return C;
623 }
624 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
625 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Var, Fixed<N>, AT2> &B) {
626 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
627 sub(A, B, C);
628 return C;
629 }
630 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
631 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type2, Fixed<M>, Fixed<N>, AT2> &B) {
632 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
633 sub(A, B, C);
634 return C;
635 }
636 template <class AT1, class AT2, int M, int N, class Type1, class Type2, class Row, class Col>
637 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type2, Row, Col, AT1> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT2> &B) {
638 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
639 sub(A, B, C);
640 return C;
641 }
642 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
643 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type2, Fixed<M>, Var, AT1> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT2> &B) {
644 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
645 sub(A, B, C);
646 return C;
647 }
648 template <class AT1, class AT2, int M, int N, class Type1, class Type2>
649 inline Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type2, Var, Fixed<N>, AT1> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT2> &B) {
650 Matrix<General, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
651 sub(A, B, C);
652 return C;
653 }
654 // Type
655 template <class AT1, class AT2, class Type, class Row1, class Col1, class Row2, class Col2>
656 inline Matrix<Type, Var, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Row1, Col1, AT1> &A, const Matrix<Type, Row2, Col2, AT2> &B) {
657 Matrix<Type, Var, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
658 sub(A, B, C);
659 return C;
660 }
661 template <class AT1, class AT2, class Type, class Row, class Col>
662 inline Matrix<Type, Row, Col, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Row, Col, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
663 Matrix<Type, Row, Col, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
664 sub(A, B, C);
665 return C;
666 }
667 template <class AT1, class AT2, int M, class Type, class Row, class Col>
668 inline Matrix<Type, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Var, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
669 Matrix<Type, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
670 sub(A, B, C);
671 return C;
672 }
673 template <class AT1, class AT2, int M, class Type>
674 inline Matrix<Type, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Var, AT1> &A, const Matrix<Type, Fixed<M>, Var, AT2> &B) {
675 Matrix<Type, Fixed<M>, Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
676 sub(A, B, C);
677 return C;
678 }
679 template <class AT1, class AT2, int N, class Type, class Row, class Col>
680 inline Matrix<Type, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Var, Fixed<N>, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
681 Matrix<Type, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
682 sub(A, B, C);
683 return C;
684 }
685 template <class AT1, class AT2, int N, class Type>
686 inline Matrix<Type, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Var, Fixed<N>, AT1> &A, const Matrix<Type, Var, Fixed<N>, AT2> &B) {
687 Matrix<Type, Var, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
688 sub(A, B, C);
689 return C;
690 }
691 template <class AT1, class AT2, int M, int N, class Type, class Row, class Col>
692 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Row, Col, AT2> &B) {
693 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
694 sub(A, B, C);
695 return C;
696 }
697 template <class AT1, class AT2, int M, int N, class Type>
698 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Fixed<M>, Var, AT2> &B) {
699 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
700 sub(A, B, C);
701 return C;
702 }
703 template <class AT1, class AT2, int M, int N, class Type>
704 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Var, Fixed<N>, AT2> &B) {
705 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
706 sub(A, B, C);
707 return C;
708 }
709 template <class AT1, class AT2, int M, int N, class Type>
710 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
711 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
712 sub(A, B, C);
713 return C;
714 }
715 template <class AT1, class AT2, int M, int N, class Type, class Row, class Col>
716 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Row, Col, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
717 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
718 sub(A, B, C);
719 return C;
720 }
721 template <class AT1, class AT2, int M, int N, class Type>
722 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Fixed<M>, Var, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
723 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
724 sub(A, B, C);
725 return C;
726 }
727 template <class AT1, class AT2, int M, int N, class Type>
728 inline Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const Matrix<Type, Var, Fixed<N>, AT1> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT2> &B) {
729 Matrix<Type, Fixed<M>, Fixed<N>, typename fmatvec::OperatorResult<AT1, AT2>::Type> C(A.rows(), A.cols(), NONINIT);
730 sub(A, B, C);
731 return C;
732 }
733 // move
734 template <class Type1, class Type2, class Row1, class Row2, class Col1, class Col2>
735 inline Matrix<Type2, Row2, Col2, double> operator-(Matrix<Type1, Row1, Col1, double> &&a, Matrix<Type2, Row2, Col2, double> &&b) {
736 sub(a, b);
737 return std::move(a);
738 }
739 template <class Type1, class Type2, class Row1, class Row2, class Col1, class Col2>
740 inline Matrix<Type1, Row1, Col1, double> operator-(Matrix<Type1, Row1, Col1, double> &&a, const Matrix<Type2, Row2, Col2, double> &b) {
741 sub(a, b);
742 return std::move(a);
743 }
744 template <class Type1, class Type2, class Row1, class Row2, class Col1, class Col2>
745 inline Matrix<Type2, Row2, Col2, double> operator-(const Matrix<Type1, Row1, Col1, double> &a, Matrix<Type2, Row2, Col2, double> &&b) {
746 sub(a, b, b);
747 return std::move(b);
748 }
749 template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
750 inline Matrix<Type1, Row1, Col1, AT>& operator-=(Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
751 sub(A, B);
752 return A;
753 }
754
755 // SquareMatrix-SquareMatrix
756 template <class AT1, class AT2, class Row1, class Row2>
757 inline SquareMatrix<Var, typename OperatorResult<AT1, AT2>::Type> operator+(const SquareMatrix<Row1, AT1> &A1, const SquareMatrix<Row2, AT2> &A2) {
758 SquareMatrix<Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.size(), NONINIT);
759 add(A1, A2, A3);
760 return A3;
761 }
762 template <class AT1, class AT2, class Row>
763 inline SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> operator+(const SquareMatrix<Row, AT1> &A1, const SquareMatrix<Row, AT2> &A2) {
764 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> A3(A1.size(), NONINIT);
765 add(A1, A2, A3);
766 return A3;
767 }
768 // move
769 template <class Row1, class Row2>
770 inline SquareMatrix<Row1, double> operator+(SquareMatrix<Row1, double> &&A1, SquareMatrix<Row2, double> &&A2) {
771 add(A1, A2);
772 return std::move(A1);;
773 }
774 template <class Row1, class Row2>
775 inline SquareMatrix<Row1, double> operator+(SquareMatrix<Row1, double> &&A1, const SquareMatrix<Row2, double> &A2) {
776 add(A1, A2);
777 return std::move(A1);;
778 }
779 template <class Row1, class Row2>
780 inline SquareMatrix<Row2, double> operator+(const SquareMatrix<Row1, double> &A1, SquareMatrix<Row2, double> &&A2) {
781 add(A2, A1);
782 return std::move(A2);
783 }
784
785 // SquareMatrix-SquareMatrix
786 template <class AT1, class AT2, class Row1, class Row2>
787 inline SquareMatrix<Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const SquareMatrix<Row1, AT1> &A1, const SquareMatrix<Row2, AT2> &A2) {
788 SquareMatrix<Var, typename fmatvec::OperatorResult<AT1, AT2>::Type> A3(A1.size(), NONINIT);
789 sub(A1, A2, A3);
790 return A3;
791 }
792 template <class AT1, class AT2, class Row>
793 inline SquareMatrix<Row, typename fmatvec::OperatorResult<AT1, AT2>::Type> operator-(const SquareMatrix<Row, AT1> &A1, const SquareMatrix<Row, AT2> &A2) {
794 SquareMatrix<Row, typename fmatvec::OperatorResult<AT1, AT2>::Type> A3(A1.size(), NONINIT);
795 sub(A1, A2, A3);
796 return A3;
797 }
798 // move
799 template <class Row1, class Row2>
800 inline SquareMatrix<Row1, double> operator-(SquareMatrix<Row1, double> &&A1, SquareMatrix<Row2, double> &&A2) {
801 sub(A1, A2);
802 return std::move(A1);;
803 }
804 template <class Row1, class Row2>
805 inline SquareMatrix<Row1, double> operator-(SquareMatrix<Row1, double> &&A1, const SquareMatrix<Row2, double> &A2) {
806 sub(A1, A2);
807 return std::move(A1);;
808 }
809 template <class Row1, class Row2>
810 inline SquareMatrix<Row2, double> operator-(const SquareMatrix<Row1, double> &A1, SquareMatrix<Row2, double> &&A2) {
811 sub(A1, A2, A2);
812 return std::move(A2);
813 }
814
820 template <class Type1, class Row1, class Col1, class Row2, class Row3, class AT1, class AT2>
821 inline void mult(const Matrix<Type1, Row1, Col1, AT1> &A, const Vector<Row2, AT2> &x, Vector<Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &y) {
822 FMATVEC_ASSERT(A.cols() == x.size(), AT2);
823 for (int i = 0; i < y.size(); i++) {
824 y.e(i) = 0;
825 for (int j = 0; j < x.size(); j++)
826 y.e(i) += A.e(i, j) * x.e(j);
827 }
828 }
829
830 template <class Row1, class Row2, class Row3, class AT1, class AT2>
831 inline void mult(const Matrix<Symmetric, Row1, Row1, AT1> &A, const Vector<Row2, AT2> &x, Vector<Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &y) {
832 FMATVEC_ASSERT(A.cols() == x.size(), AT2);
833 for (int i = 0; i < y.size(); i++) {
834 y.e(i) = 0;
835 for (int j = 0; j < i; j++)
836 y.e(i) += A.ei(i, j) * x.e(j);
837 for (int j = i; j < x.size(); j++)
838 y.e(i) += A.ej(i, j) * x.e(j);
839 }
840 }
841
842 template <class Row2, class Row3, class AT1, class AT2>
843 inline void mult(const Matrix<Sparse, Ref, Ref, AT1> &A, const Vector<Row2, AT2> &x, Vector<Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &y) {
844 FMATVEC_ASSERT(A.cols() == x.size(), AT2);
845 for (int i = 0; i < y.size(); i++) {
846 y.e(i) = 0;
847 for (int j = A.Ip()[i]; j < A.Ip()[i+1]; j++)
848 y.e(i) += A()[j]*x.e(A.Jp()[j]);
849 }
850 }
851
852 template <class Row2, class Row3, class AT1, class AT2>
853 inline void mult(const Matrix<SymmetricSparse, Ref, Ref, AT1> &A, const Vector<Row2, AT2> &x, Vector<Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &y) {
854 FMATVEC_ASSERT(A.cols() == x.size(), AT2);
855 for (int i = 0; i < y.size(); i++)
856 y.e(i) = A()[A.Ip()[i]]*x.e(A.Jp()[A.Ip()[i]]);
857 for (int i = 0; i < y.size(); i++) {
858 for (int j = A.Ip()[i]+1; j < A.Ip()[i+1]; j++) {
859 y.e(i) += A()[j]*x.e(A.Jp()[j]);
860 y.e(A.Jp()[j]) += A()[j]*x.e(i);
861 }
862 }
863 }
864
865 // Matrix-Vector
866 template <class AT1, class AT2, class Type1, class Row1, class Col1, class Row2>
867 inline Vector<Var, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Row1, Col1, AT1> &A, const Vector<Row2, AT2> &x) {
868 Vector<Var, typename OperatorResult<AT1, AT2>::Type> y(A.rows(), NONINIT);
869 mult(A, x, y);
870 return y;
871 }
872 template <class AT1, class AT2, class Type1, class Row, class Col>
873 inline Vector<Row, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Row, Col, AT1> &A, const Vector<Row, AT2> &x) {
874 Vector<Row, typename OperatorResult<AT1, AT2>::Type> y(A.rows(), NONINIT);
875 mult(A, x, y);
876 return y;
877 }
878 template <class AT1, class AT2, int M, class Type1, class Col1, class Row2>
879 inline Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Fixed<M>, Col1, AT1> &A, const Vector<Row2, AT2> &x) {
880 Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> y(A.rows(), NONINIT);
881 mult(A, x, y);
882 return y;
883 }
884 template <class AT1, class AT2, int M, class Type1>
885 inline Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Fixed<M>, Fixed<M>, AT1> &A, const Vector<Fixed<M>, AT2> &x) {
886 Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> y(A.rows(), NONINIT);
887 mult(A, x, y);
888 return y;
889 }
890 template <class AT1, class AT2, int M, int N, class Type, class Row2>
891 inline Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Vector<Row2, AT2> &x) {
892 Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> y(A.rows(), NONINIT);
893 mult(A, x, y);
894 return y;
895 }
896 template <class AT1, class AT2, int M, int N, class Type>
897 inline Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Fixed<M>, Fixed<N>, AT1> &A, const Vector<Fixed<N>, AT2> &x) {
898 Vector<Fixed<M>, typename OperatorResult<AT1, AT2>::Type> y(A.rows(), NONINIT);
899 mult(A, x, y);
900 return y;
901 }
902
903 // RowVector-Matrix
904 template <class Col1, class Type2, class Row2, class Col2, class Type3, class Col3, class AT1, class AT2>
905 inline void mult(const RowVector<Col1, AT1> &x, const Matrix<Type2, Row2, Col2, AT2> &A, RowVector<Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &y) {
906 FMATVEC_ASSERT(x.size() == A.rows(), AT2);
907 for (int i = 0; i < y.size(); i++) {
908 y.e(i) = 0;
909 for (int j = 0; j < x.size(); j++)
910 y.e(i) += x.e(j) * A.e(j, i);
911 }
912 }
913
914 template <class Col1, class Row2, class Col3, class AT1, class AT2>
915 inline void mult(const RowVector<Col1, AT1> &x, const Matrix<Symmetric, Row2, Row2, AT2> &A, RowVector<Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &y) {
916 FMATVEC_ASSERT(x.size() == A.rows(), AT2);
917 for (int i = 0; i < y.size(); i++) {
918 y.e(i) = 0;
919 for (int j = 0; j < i; j++)
920 y.e(i) += x.e(j) * A.ej(j, i);
921 for (int j = i; j < x.size(); j++)
922 y.e(i) += x.e(j) * A.ei(j, i);
923 }
924 }
925
926 template <class AT1, class AT2, class Type2, class Col2, class Row1, class Col1>
927 inline RowVector<Var, typename OperatorResult<AT1, AT2>::Type> operator*(const RowVector<Col2, AT1> &x, const Matrix<Type2, Row1, Col1, AT2> &A) {
928 RowVector<Var, typename OperatorResult<AT1, AT2>::Type> y(A.cols(), NONINIT);
929 mult(x, A, y);
930 return y;
931 }
932 template <class AT1, class AT2, class Type2, class Row, class Col>
933 inline RowVector<Col, typename OperatorResult<AT1, AT2>::Type> operator*(const RowVector<Col, AT1> &x, const Matrix<Type2, Row, Col, AT2> &A) {
934 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> y(A.cols(), NONINIT);
935 mult(x, A, y);
936 return y;
937 }
938 template <class AT1, class AT2, int N, class Type2, class Col1, class Row2>
939 inline RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator*(const RowVector<Col1, AT1> &x, const Matrix<Type2, Row2, Fixed<N>, AT2> &A) {
940 RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> y(A.cols(), NONINIT);
941 mult(x, A, y);
942 return y;
943 }
944 template <class AT1, class AT2, int N, class Type2>
945 inline RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator*(const RowVector<Fixed<N>, AT1> &x, const Matrix<Type2, Fixed<N>, Fixed<N>, AT2> &A) {
946 RowVector<Fixed<N>, typename OperatorResult<AT1, AT2>::Type> y(A.cols(), NONINIT);
947 mult(x, A, y);
948 return y;
949 }
950
951 // Matrix-Matrix
952 template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT1, class AT2>
953 inline void mult(const Matrix<Type1, Row1, Col1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2, Matrix<Type3, Row3, Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
954 FMATVEC_ASSERT(A1.cols() == A2.rows(), AT2);
955 for (int i = 0; i < A3.rows(); i++) {
956 for (int k = 0; k < A3.cols(); k++) {
957 A3.e(i, k) = 0;
958 for (int j = 0; j < A1.cols(); j++)
959 A3.e(i, k) += A1.e(i, j) * A2.e(j, k);
960 }
961 }
962 }
963 template <class Row1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT1, class AT2>
964 inline void mult(const Matrix<Symmetric, Row1, Row1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2, Matrix<Type3, Row3, Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
965 FMATVEC_ASSERT(A1.size() == A2.rows(), AT2);
966 for (int i = 0; i < A3.rows(); i++) {
967 for (int k = 0; k < A3.cols(); k++) {
968 A3.e(i, k) = 0;
969 for (int j = 0; j < i; j++)
970 A3.e(i, k) += A1.ei(i, j) * A2.e(j, k);
971 for (int j = i; j < A1.cols(); j++)
972 A3.e(i, k) += A1.ej(i, j) * A2.e(j, k);
973 }
974 }
975 }
976 template <class Row1, class Row2, class Row3, class AT1, class AT2>
977 inline void mult(const Matrix<Diagonal, Row1, Row1, AT1> &A1, const Matrix<Diagonal, Row2, Row2, AT2> &A2, Matrix<Diagonal, Row3, Row3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
978 FMATVEC_ASSERT(A1.size() == A2.size(), AT2);
979 for (int i = 0; i < A3.size(); i++)
980 A3.e(i) = A1.e(i) * A2.e(i);
981 }
982
983 template <class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT1, class AT2>
984 inline void mult(const Matrix<Sparse, Ref, Ref, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2, Matrix<Type3, Row3, Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
985 FMATVEC_ASSERT(A1.cols() == A2.rows(), AT2);
986 for (int i = 0; i < A3.rows(); i++) {
987 for (int k = 0; k < A3.cols(); k++) {
988 A3.e(i, k) = 0;
989 for (int j = A1.Ip()[i]; j < A1.Ip()[i+1]; j++)
990 A3.e(i, k) += A1()[j]*A2.e(A1.Jp()[j], k);
991 }
992 }
993 }
994
995 template <class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT1, class AT2>
996 inline void mult(const Matrix<SymmetricSparse, Ref, Ref, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2, Matrix<Type3, Row3, Col3, typename fmatvec::OperatorResult<AT1, AT2>::Type> &A3) {
997 FMATVEC_ASSERT(A1.size() == A2.rows(), AT2);
998 for (int i = 0; i < A3.rows(); i++) {
999 for (int k = 0; k < A3.cols(); k++)
1000 A3.e(i, k) = A1()[A1.Ip()[i]]*A2.e(A1.Jp()[A1.Ip()[i]], k);
1001 }
1002 for (int i = 0; i < A3.rows(); i++) {
1003 for (int k = 0; k < A3.cols(); k++) {
1004 for (int j = A1.Ip()[i]+1; j < A1.Ip()[i+1]; j++) {
1005 A3.e(i, k) += A1()[j]*A2.e(A1.Jp()[j], k);
1006 A3.e(A1.Jp()[j], k) += A1()[j]*A2.e(i, k);
1007 }
1008 }
1009 }
1010 }
1011
1012 template <class AT1, class AT2, class Type1, class Type2, class Row1, class Col1, class Row2, class Col2>
1013 inline Matrix<General, Var, Var, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Row1, Col1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2) {
1014 Matrix<General, Var, Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1015 mult(A1, A2, A3);
1016 return A3;
1017 }
1018 template <class AT1, class AT2, class Type1, class Type2, class Row, class Col>
1019 inline Matrix<General, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Row, Col, AT1> &A1, const Matrix<Type2, Row, Col, AT2> &A2) {
1020 Matrix<General, Row, Col, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1021 mult(A1, A2, A3);
1022 return A3;
1023 }
1024 template <class AT1, class AT2, int M, class Type1, class Type2, class Col1, class Row2, class Col2>
1025 inline Matrix<General, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Fixed<M>, Col1, AT1> &A1, const Matrix<Type2, Row2, Col2, AT2> &A2) {
1026 Matrix<General, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1027 mult(A1, A2, A3);
1028 return A3;
1029 }
1030 template <class AT1, class AT2, int N, class Type1, class Row1, class Col1, class Type2, class Row2>
1031 inline Matrix<General, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Row1, Col1, AT1> &A1, const Matrix<Type2, Row2, Fixed<N>, AT2> &A2) {
1032 Matrix<General, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1033 mult(A1, A2, A3);
1034 return A3;
1035 }
1036 template <class AT1, class AT2, int M, int N, class Type1, class Type2, class Col1, class Row2>
1037 inline Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Fixed<M>, Col1, AT1> &A1, const Matrix<Type2, Row2, Fixed<N>, AT2> &A2) {
1038 Matrix<General, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1039 mult(A1, A2, A3);
1040 return A3;
1041 }
1042 template <class AT1, class AT2, int M, class Type1, class Type2>
1043 inline Matrix<General, Fixed<M>, Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type1, Fixed<M>, Fixed<M>, AT1> &A1, const Matrix<Type2, Fixed<M>, Fixed<M>, AT2> &A2) {
1044 Matrix<General, Fixed<M>, Fixed<M>, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1045 mult(A1, A2, A3);
1046 return A3;
1047 }
1048 template <class AT1, class AT2, class Type, class Row1, class Col1, class Row2, class Col2>
1049 inline Matrix<Type, Var, Var, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Row1, Col1, AT1> &A1, const Matrix<Type, Row2, Col2, AT2> &A2) {
1050 Matrix<Type, Var, Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1051 mult(A1, A2, A3);
1052 return A3;
1053 }
1054 template <class AT1, class AT2, class Type, class Row, class Col>
1055 inline Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Row, Col, AT1> &A1, const Matrix<Type, Row, Col, AT2> &A2) {
1056 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1057 mult(A1, A2, A3);
1058 return A3;
1059 }
1060 template <class AT1, class AT2, int M, class Type, class Col1, class Row2, class Col2>
1061 inline Matrix<Type, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Fixed<M>, Col1, AT1> &A1, const Matrix<Type, Row2, Col2, AT2> &A2) {
1062 Matrix<Type, Fixed<M>, Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1063 mult(A1, A2, A3);
1064 return A3;
1065 }
1066 template <class AT1, class AT2, int N, class Type, class Row1, class Col1, class Row2>
1067 inline Matrix<Type, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Row1, Col1, AT1> &A1, const Matrix<Type, Row2, Fixed<N>, AT2> &A2) {
1068 Matrix<Type, Var, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1069 mult(A1, A2, A3);
1070 return A3;
1071 }
1072 template <class AT1, class AT2, int M, int N, class Type, class Col1, class Row2>
1073 inline Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Fixed<M>, Col1, AT1> &A1, const Matrix<Type, Row2, Fixed<N>, AT2> &A2) {
1074 Matrix<Type, Fixed<M>, Fixed<N>, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1075 mult(A1, A2, A3);
1076 return A3;
1077 }
1078 template <class AT1, class AT2, int M, class Type>
1079 inline Matrix<Type, Fixed<M>, Fixed<M>, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Fixed<M>, Fixed<M>, AT1> &A1, const Matrix<Type, Fixed<M>, Fixed<M>, AT2> &A2) {
1080 Matrix<Type, Fixed<M>, Fixed<M>, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1081 mult(A1, A2, A3);
1082 return A3;
1083 }
1084
1085 // SquareMatrix-SquareMatrix
1086 template <class AT1, class AT2, class Row1, class Row2>
1087 inline SquareMatrix<Var, typename OperatorResult<AT1, AT2>::Type> operator*(const SquareMatrix<Row1, AT1> &A1, const SquareMatrix<Row2, AT2> &A2) {
1088 SquareMatrix<Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.size(), NONINIT);
1089 mult(A1, A2, A3);
1090 return A3;
1091 }
1092 template <class AT1, class AT2, class Row>
1093 inline SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> operator*(const SquareMatrix<Row, AT1> &A1, const SquareMatrix<Row, AT2> &A2) {
1094 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> A3(A1.size(), NONINIT);
1095 mult(A1, A2, A3);
1096 return A3;
1097 }
1098
1099 // SymmetricMatrix-SymmetricMatrix
1100 template <class AT1, class AT2, class Row1, class Row2>
1101 inline Matrix<General, Var, Var, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Symmetric, Row1, Row1, AT1> &A1, const Matrix<Symmetric, Row2, Row2, AT2> &A2) {
1102 Matrix<General, Var, Var, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1103 mult(A1, A2, A3);
1104 return A3;
1105 }
1106 template <class AT1, class AT2, class Row>
1107 inline Matrix<General, Row, Row, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Symmetric, Row, Row, AT1> &A1, const Matrix<Symmetric, Row, Row, AT2> &A2) {
1108 Matrix<General, Row, Row, typename OperatorResult<AT1, AT2>::Type> A3(A1.rows(), A2.cols(), NONINIT);
1109 mult(A1, A2, A3);
1110 return A3;
1111 }
1112
1114
1121 template <class Row, class AT1, class AT2>
1122 Vector<Row, typename OperatorResult<AT1, AT2>::Type> operator*(const Vector<Row, AT1> &x, const AT2& alpha) {
1123
1124 Vector<Row, typename OperatorResult<AT1, AT2>::Type> y(x.size(), NONINIT);
1125
1126 for (int i = 0; i < x.size(); i++)
1127 y.e(i) = x.e(i) * alpha;
1128
1129 return y;
1130 }
1131 // move
1132 template <class Row>
1133 Vector<Row, double> operator*(Vector<Row, double> &&x, const double& alpha) {
1134 for (int i = 0; i < x.size(); i++)
1135 x.e(i) *= alpha;
1136 return std::move(x);
1137 }
1138
1143 template <class Row, class AT1, class AT2>
1144 Vector<Row, typename OperatorResult<AT1, AT2>::Type> operator*(const AT1& alpha, const Vector<Row, AT2> &x) {
1145
1146 Vector<Row, typename OperatorResult<AT1, AT2>::Type> y(x.size(), NONINIT);
1147
1148 for (int i = 0; i < x.size(); i++)
1149 y.e(i) = x.e(i) * alpha;
1150
1151 return y;
1152 }
1153 // move
1154 template <class Row>
1155 Vector<Row, double> operator*(const double& alpha, Vector<Row, double> &&x) {
1156 for (int i = 0; i < x.size(); i++)
1157 x.e(i) *= alpha;
1158 return std::move(x);
1159 }
1160
1161 template <class Row, class AT1, class AT2>
1162 inline Vector<Row, AT1> operator*=(Vector<Row, AT1> &x, const AT2 &alpha) {
1163 for (int i = 0; i < x.size(); i++)
1164 x.e(i) *= alpha;
1165 return x;
1166 }
1167
1168 template <class Row, class AT1, class AT2>
1169 inline Vector<Row, typename OperatorResult<AT1, AT2>::Type> operator/(const Vector<Row, AT1> &x, const AT2 &alpha) {
1170 Vector<Row, typename OperatorResult<AT1, AT2>::Type> y(x.size(), NONINIT);
1171 for (int i = 0; i < x.size(); i++)
1172 y.e(i) = x.e(i) / alpha;
1173 return y;
1174 }
1175 // move
1176 template <class Row>
1177 inline Vector<Row, double> operator/(Vector<Row, double> &&x, const double &alpha) {
1178 for (int i = 0; i < x.size(); i++)
1179 x.e(i) /= alpha;
1180 return std::move(x);
1181 }
1182
1183 template <class Row, class AT1, class AT2>
1184 inline Vector<Row, AT1> operator/=(Vector<Row, AT1> &x, const AT2 &alpha) {
1185 for (int i = 0; i < x.size(); i++)
1186 x.e(i) /= alpha;
1187 return x;
1188 }
1195 template <class Col, class AT1, class AT2>
1196 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> operator*(const RowVector<Col, AT1> &x, const AT2& alpha) {
1197
1198 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> y(x.size(), NONINIT);
1199
1200 for (int i = 0; i < x.size(); i++)
1201 y.e(i) = x.e(i) * alpha;
1202
1203 return y;
1204 }
1205 // move
1206 template <class Col>
1207 RowVector<Col, double> operator*(RowVector<Col, double> &&x, const double& alpha) {
1208 for (int i = 0; i < x.size(); i++)
1209 x.e(i) *= alpha;
1210 return std::move(x);
1211 }
1212
1217 template <class Col, class AT1, class AT2>
1218 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> operator*(const AT1 &alpha, const RowVector<Col, AT2> &x) {
1219
1220 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> y(x.size(), NONINIT);
1221
1222 for (int i = 0; i < x.size(); i++)
1223 y.e(i) = x.e(i) * alpha;
1224
1225 return y;
1226 }
1227 // move
1228 template <class Col>
1229 RowVector<Col, double> operator*(const double &alpha, RowVector<Col, double> &&x) {
1230 for (int i = 0; i < x.size(); i++)
1231 x.e(i) *= alpha;
1232 return std::move(x);
1233 }
1234
1235 template <class Col, class AT1, class AT2>
1236 inline RowVector<Col, AT1> operator*=(RowVector<Col, AT1> &x, const AT2 &alpha) {
1237 for (int i = 0; i < x.size(); i++)
1238 x.e(i) *= alpha;
1239 return x;
1240 }
1241
1242 template <class Col, class AT1, class AT2>
1243 inline RowVector<Col, typename OperatorResult<AT1, AT2>::Type> operator/(const RowVector<Col, AT1> &x, const AT2 &a) {
1244 RowVector<Col, typename OperatorResult<AT1, AT2>::Type> y(x.size(), NONINIT);
1245 for (int i = 0; i < x.size(); i++)
1246 y.e(i) = x.e(i) / a;
1247 return y;
1248 }
1249 // move
1250 template <class Col>
1251 inline RowVector<Col, double> operator/(RowVector<Col, double> &&x, const double &a) {
1252 for (int i = 0; i < x.size(); i++)
1253 x.e(i) /= a;
1254 return std::move(x);
1255 }
1256
1257 template <class Col, class AT1, class AT2>
1258 inline RowVector<Col, AT1> operator/=(RowVector<Col, AT1> &x, const AT2 &a) {
1259 for (int i = 0; i < x.size(); i++)
1260 x.e(i) /= a;
1261 return x;
1262 }
1263
1265
1267
1274 template <class Col, class Row, class AT1, class AT2>
1275 typename OperatorResult<AT1, AT2>::Type operator*(const RowVector<Col, AT1> &x, const Vector<Row, AT2> &y) {
1276
1277 FMATVEC_ASSERT(x.size() == y.size(), AT2);
1278
1279 typename OperatorResult<AT1, AT2>::Type res = 0;
1280
1281 for (int i = 0; i < x.size(); i++)
1282 res += x.e(i) * y.e(i);
1283
1284 return res;
1285 }
1286
1292 template <class Row, class AT>
1293 AT scalarProduct(const Vector<Row, AT> &x, const Vector<Row, AT> &y) {
1294
1295 FMATVEC_ASSERT(x.size() == y.size(), AT);
1296
1297 AT res = 0;
1298
1299 for (int i = 0; i < x.size(); i++)
1300 res += x.e(i) * y.e(i);
1301
1302 return res;
1303 }
1304
1310 template <class Row1, class Row2, class AT1, class AT2>
1311 Vector<Fixed<3>, typename OperatorResult<AT1, AT2>::Type> crossProduct(const Vector<Row1, AT1> &x, const Vector<Row2, AT2> &y) {
1312
1313 FMATVEC_ASSERT(x.size() == 3, AT2);
1314 FMATVEC_ASSERT(y.size() == 3, AT2);
1315
1316 Vector<Fixed<3>, typename OperatorResult<AT1, AT2>::Type> z(3, NONINIT);
1317
1318 z.e(0) = x.e(1) * y.e(2) - x.e(2) * y.e(1);
1319 z.e(1) = x.e(2) * y.e(0) - x.e(0) * y.e(2);
1320 z.e(2) = x.e(0) * y.e(1) - x.e(1) * y.e(0);
1321
1322 return z;
1323 }
1324
1330 template <class Row, class AT>
1331 double tripleProduct(const Vector<Row, AT> &a, const Vector<Row, AT> &x, const Vector<Row, AT> &y) {
1332
1333 FMATVEC_ASSERT(a.size() == 3, AT);
1334 FMATVEC_ASSERT(x.size() == 3, AT);
1335 FMATVEC_ASSERT(y.size() == 3, AT);
1336
1337 return a.e(0) * (x.e(1) * y.e(2) - x.e(2) * y.e(1)) + a.e(1) * (x.e(2) * y.e(0) - x.e(0) * y.e(2)) + a.e(2) * (x.e(0) * y.e(1) - x.e(1) * y.e(0));
1338 }
1339
1341 //
1343
1352 template <class Type, class Row, class Col, class AT1, class AT2>
1353 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Type, Row, Col, AT1> &A, const AT2 &alpha) {
1354
1355 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> B(A.rows(), A.cols(), NONINIT);
1356
1357 for (int i = 0; i < A.rows(); i++)
1358 for (int j = 0; j < A.cols(); j++)
1359 B.e(i, j) = A.e(i, j) * alpha;
1360
1361 return B;
1362 }
1363 // move
1364 template <class Type, class Row, class Col>
1365 Matrix<Type, Row, Col, double> operator*(Matrix<Type, Row, Col, double> &&A, const double &alpha) {
1366 for (int i = 0; i < A.rows(); i++)
1367 for (int j = 0; j < A.cols(); j++)
1368 A.e(i, j) *= alpha;
1369 return std::move(A);
1370 }
1371
1372 template <class Type, class Row, class Col, class AT1, class AT2>
1373 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator*(const AT1 &alpha, const Matrix<Type, Row, Col, AT2> &A) {
1374
1375 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> B(A.rows(), A.cols(), NONINIT);
1376
1377 for (int i = 0; i < A.rows(); i++)
1378 for (int j = 0; j < A.cols(); j++)
1379 B.e(i, j) = A.e(i, j) * alpha;
1380
1381 return B;
1382 }
1383 // move
1384 template <class Type, class Row, class Col>
1385 Matrix<Type, Row, Col, double> operator*(const double &alpha, Matrix<Type, Row, Col, double> &&A) {
1386 for (int i = 0; i < A.rows(); i++)
1387 for (int j = 0; j < A.cols(); j++)
1388 A.e(i, j) *= alpha;
1389 return std::move(A);
1390 }
1391
1392 template <class Row, class AT1, class AT2>
1393 inline Matrix<Symmetric, Row, Row, typename OperatorResult<AT1, AT2>::Type> operator*(const AT1 &alpha, const Matrix<Symmetric, Row, Row, AT2> &A) {
1394 Matrix<Symmetric, Row, Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), A.size(), NONINIT);
1395 for (int i = 0; i < A.size(); i++)
1396 for (int j = i; j < A.size(); j++)
1397 B.ej(i, j) = A.ej(i, j) * alpha;
1398 return B;
1399 }
1400 // move
1401 template <class Row>
1402 inline Matrix<Symmetric, Row, Row, double> operator*(const double &alpha, Matrix<Symmetric, Row, Row, double> &&A) {
1403 for (int i = 0; i < A.size(); i++)
1404 for (int j = i; j < A.size(); j++)
1405 A.ej(i, j) *= alpha;
1406 return std::move(A);
1407 }
1408
1409 template <class Row, class AT1, class AT2>
1410 inline Matrix<Symmetric, Row, Row, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Symmetric, Row, Row, AT1> &A, const AT2 &alpha) {
1411 Matrix<Symmetric, Row, Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), A.size(), NONINIT);
1412 for (int i = 0; i < A.size(); i++)
1413 for (int j = i; j < A.size(); j++)
1414 B.ej(i, j) = A.ej(i, j) * alpha;
1415 return B;
1416 }
1417 // move
1418 template <class Row>
1419 inline Matrix<Symmetric, Row, Row, double> operator*(Matrix<Symmetric, Row, Row, double> &&A, const double &alpha) {
1420 for (int i = 0; i < A.size(); i++)
1421 for (int j = i; j < A.size(); j++)
1422 A.ej(i, j) *= alpha;
1423 return std::move(A);
1424 }
1425
1426 template <class AT1, class AT2>
1427 inline Matrix<Diagonal, Ref, Ref, typename OperatorResult<AT1, AT2>::Type> operator*(const AT1 &alpha, const Matrix<Diagonal, Ref, Ref, AT2> &A) {
1428 Matrix<Diagonal, Ref, Ref, typename OperatorResult<AT1, AT2>::Type> B(A.rows(), A.cols(), NONINIT);
1429 for (int i = 0; i < A.rows(); i++)
1430 B.e(i) = A.e(i) * alpha;
1431 return B;
1432 }
1433 // move
1434 inline Matrix<Diagonal, Ref, Ref, double> operator*(const double &alpha, Matrix<Diagonal, Ref, Ref, double> &&A) {
1435 for (int i = 0; i < A.rows(); i++)
1436 A.e(i) *= alpha;
1437 return A;
1438 }
1439
1440 template <class AT1, class AT2>
1441 inline Matrix<Diagonal, Ref, Ref, typename OperatorResult<AT1, AT2>::Type> operator*(const Matrix<Diagonal, Ref, Ref, AT1> &A, const AT2 &alpha) {
1442 Matrix<Diagonal, Ref, Ref, typename OperatorResult<AT1, AT2>::Type> B(A.rows(), A.cols(), NONINIT);
1443 for (int i = 0; i < A.size(); i++)
1444 B.e(i) = A.e(i) * alpha;
1445 return B;
1446 }
1447 // move
1448 inline Matrix<Diagonal, Ref, Ref, double> operator*(Matrix<Diagonal, Ref, Ref, double> &&A, const double &alpha) {
1449 for (int i = 0; i < A.size(); i++)
1450 A.e(i) *= alpha;
1451 return A;
1452 }
1453
1454 template <class Type, class Row, class Col, class AT1, class AT2>
1455 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator/(const Matrix<Type, Row, Col, AT1> &A, const AT2 &alpha) {
1456
1457 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> B(A.rows(), A.cols(), NONINIT);
1458
1459 for (int i = 0; i < A.rows(); i++)
1460 for (int j = 0; j < A.cols(); j++)
1461 B.e(i, j) = A.e(i, j) / alpha;
1462
1463 return B;
1464 }
1465 // move
1466 template <class Type, class Row, class Col>
1467 Matrix<Type, Row, Col, double> operator/(Matrix<Type, Row, Col, double> &&A, const double &alpha) {
1468 for (int i = 0; i < A.rows(); i++)
1469 for (int j = 0; j < A.cols(); j++)
1470 A.e(i, j) /= alpha;
1471 return std::move(A);
1472 }
1473
1474 template <class Row, class AT1, class AT2>
1475 inline Matrix<Symmetric, Row, Row, typename OperatorResult<AT1, AT2>::Type> operator/(const Matrix<Symmetric, Row, Row, AT1> &A, const AT2 &alpha) {
1476 Matrix<Symmetric, Row, Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), A.size(), NONINIT);
1477 for (int i = 0; i < A.size(); i++)
1478 for (int j = i; j < A.size(); j++)
1479 B.ej(i, j) = A.ej(i, j) / alpha;
1480 return B;
1481 }
1482 // move
1483 template <class Row>
1484 inline Matrix<Symmetric, Row, Row, double> operator/(Matrix<Symmetric, Row, Row, double> &&A, const double &alpha) {
1485 for (int i = 0; i < A.size(); i++)
1486 for (int j = i; j < A.size(); j++)
1487 A.ej(i, j) /= alpha;
1488 return std::move(A);
1489 }
1490
1491 template <class Row, class AT1, class AT2>
1492 inline Matrix<Diagonal, Row, Row, typename OperatorResult<AT1, AT2>::Type> operator/(const Matrix<Diagonal, Row, Row, AT1> &A, const AT2 &alpha) {
1493 Matrix<Diagonal, Row, Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), A.size(), NONINIT);
1494 for (int i = 0; i < A.size(); i++)
1495 B.e(i) = A.e(i) / alpha;
1496 return B;
1497 }
1498 // move
1499 template <class Row>
1500 inline Matrix<Diagonal, Row, Row, double> operator/(Matrix<Diagonal, Row, Row, double> &&A, const double &alpha) {
1501 for (int i = 0; i < A.size(); i++)
1502 A.e(i) /= alpha;
1503 return std::move(A);
1504 }
1505
1506 template <class Row, class AT1, class AT2>
1507 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> operator*(const SquareMatrix<Row, AT1> &A, const AT2 &alpha) {
1508
1509 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), NONINIT);
1510
1511 for (int i = 0; i < A.size(); i++)
1512 for (int j = 0; j < A.size(); j++)
1513 B.e(i, j) = A.e(i, j) * alpha;
1514
1515 return B;
1516 }
1517 // move
1518 template <class Row>
1519 SquareMatrix<Row, double> operator*(SquareMatrix<Row, double> &&A, const double &alpha) {
1520 for (int i = 0; i < A.size(); i++)
1521 for (int j = 0; j < A.size(); j++)
1522 A.e(i, j) *= alpha;
1523 return std::move(A);
1524 }
1525
1526 template <class Row, class AT1, class AT2>
1527 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> operator*(const AT1 &alpha, const SquareMatrix<Row, AT2> &A) {
1528
1529 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), NONINIT);
1530
1531 for (int i = 0; i < A.size(); i++)
1532 for (int j = 0; j < A.size(); j++)
1533 B.e(i, j) = A.e(i, j) * alpha;
1534
1535 return B;
1536 }
1537 // move
1538 template <class Row>
1539 SquareMatrix<Row, double> operator*(const double &alpha, SquareMatrix<Row, double> &&A) {
1540 for (int i = 0; i < A.size(); i++)
1541 for (int j = 0; j < A.size(); j++)
1542 A.e(i, j) *= alpha;
1543 return std::move(A);
1544 }
1545
1546 template <class Row, class AT1, class AT2>
1547 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> operator/(const SquareMatrix<Row, AT1> &A, const AT2 &alpha) {
1548
1549 SquareMatrix<Row, typename OperatorResult<AT1, AT2>::Type> B(A.size(), NONINIT);
1550
1551 for (int i = 0; i < A.size(); i++)
1552 for (int j = 0; j < A.size(); j++)
1553 B.e(i, j) = A.e(i, j) / alpha;
1554
1555 return B;
1556 }
1557 // move
1558 template <class Row>
1559 SquareMatrix<Row, double> operator/(SquareMatrix<Row, double> &&A, const double &alpha) {
1560 for (int i = 0; i < A.size(); i++)
1561 for (int j = 0; j < A.size(); j++)
1562 A.e(i, j) /= alpha;
1563 return std::move(A);
1564 }
1565
1566 template <class Type, class Row, class Col, class AT1, class AT2>
1567 Matrix<Type, Row, Col, typename OperatorResult<AT1, AT2>::Type> operator*=(Matrix<Type, Row, Col, AT1> &A, const AT2 &alpha) {
1568 for (int i = 0; i < A.rows(); i++)
1569 for (int j = 0; j < A.cols(); j++)
1570 A.e(i, j) *= alpha;
1571
1572 return A;
1573 }
1574
1575 template <class Row, class AT1, class AT2>
1576 inline Matrix<Symmetric, Row, Row, AT1> operator*=(Matrix<Symmetric, Row, Row, AT1> &A, const AT2 &alpha) {
1577 for (int i = 0; i < A.size(); i++)
1578 for (int j = i; j < A.size(); j++)
1579 A.ej(i, j) *= alpha;
1580 return A;
1581 }
1582
1583 template <class Row, class AT1, class AT2>
1584 inline Matrix<Diagonal, Row, Row, AT1> operator*=(Matrix<Diagonal, Row, Row, AT1> &A, const AT2 &alpha) {
1585 for (int i = 0; i < A.size(); i++)
1586 A.e(i) *= alpha;
1587 return A;
1588 }
1589
1590 template <class Type, class Row, class Col, class AT1, class AT2>
1591 Matrix<Type, Row, Col, AT1> operator/=(Matrix<Type, Row, Col, AT1> &A, const AT2 &alpha) {
1592 for (int i = 0; i < A.rows(); i++)
1593 for (int j = 0; j < A.cols(); j++)
1594 A.e(i, j) /= alpha;
1595
1596 return A;
1597 }
1598
1599 template <class Row, class AT1, class AT2>
1600 inline Matrix<Symmetric, Row, Row, AT1> operator/=(Matrix<Symmetric, Row, Row, AT1> &A, const AT2 &alpha) {
1601 for (int i = 0; i < A.size(); i++)
1602 for (int j = i; j < A.size(); j++)
1603 A.ej(i, j) /= alpha;
1604 return A;
1605 }
1606
1607 template <class Row, class AT1, class AT2>
1608 inline Matrix<Diagonal, Row, Row, AT1> operator/=(Matrix<Diagonal, Row, Row, AT1> &A, const AT2 &alpha) {
1609 for (int i = 0; i < A.size(); i++)
1610 A.e(i) /= alpha;
1611 return A;
1612 }
1613
1615
1617
1623 template <class Row, class AT>
1624 Vector<Row, typename OperatorResult<AT, AT>::Type> operator-(const Vector<Row, AT> &x) {
1625
1626 Vector<Row, typename OperatorResult<AT, AT>::Type> y(x.size(), NONINIT);
1627
1628 for (int i = 0; i < x.size(); i++)
1629 y.e(i) = -x.e(i);
1630
1631 return y;
1632 }
1633 // move
1634 template <class Row>
1635 Vector<Row, double> operator-(Vector<Row, double> &&x) {
1636 for (int i = 0; i < x.size(); i++)
1637 x.e(i) = -x.e(i);
1638 return std::move(x);
1639 }
1640
1646 template <class Col, class AT>
1647 RowVector<Col, typename OperatorResult<AT, AT>::Type> operator-(const RowVector<Col, AT> &a) {
1648
1649 RowVector<Col, typename OperatorResult<AT, AT>::Type> c(a.size(), NONINIT);
1650
1651 for (int i = 0; i < a.size(); i++)
1652 c.e(i) = -a.e(i);
1653
1654 return c;
1655 }
1656 // move
1657 template <class Col>
1658 RowVector<Col, double> operator-(RowVector<Col, double> &&a) {
1659 for (int i = 0; i < a.size(); i++)
1660 a.e(i) = -a.e(i);
1661 return std::move(a);
1662 }
1663
1669 template <class Row, class AT>
1670 SquareMatrix<Row, typename OperatorResult<AT, AT>::Type> operator-(const SquareMatrix<Row, AT> &A) {
1671
1672 SquareMatrix<Row, typename OperatorResult<AT, AT>::Type> B(A.size(), NONINIT);
1673
1674 for (int i = 0; i < A.size(); i++)
1675 for (int j = 0; j < A.size(); j++)
1676 B.e(i, j) = -A.e(i, j);
1677
1678 return B;
1679 }
1680 // move
1681 template <class Row>
1682 SquareMatrix<Row, double> operator-(SquareMatrix<Row, double> &&A) {
1683 for (int i = 0; i < A.size(); i++)
1684 for (int j = 0; j < A.size(); j++)
1685 A.e(i, j) = -A.e(i, j);
1686 return std::move(A);
1687 }
1688
1694 template <class Type, class Row, class Col, class AT>
1695 Matrix<Type, Row, Col, typename OperatorResult<AT, AT>::Type> operator-(const Matrix<Type, Row, Col, AT> &A) {
1696
1697 Matrix<Type, Row, Col, typename OperatorResult<AT, AT>::Type> B(A.rows(), A.cols(), NONINIT);
1698
1699 for (int i = 0; i < A.rows(); i++)
1700 for (int j = 0; j < A.cols(); j++)
1701 B.e(i, j) = -A.e(i, j);
1702
1703 return B;
1704 }
1705 // move
1706 template <class Type, class Row, class Col>
1707 Matrix<Type, Row, Col, double> operator-(Matrix<Type, Row, Col, double> &&A) {
1708 for (int i = 0; i < A.rows(); i++)
1709 for (int j = 0; j < A.cols(); j++)
1710 A.e(i, j) = -A.e(i, j);
1711 return std::move(A);
1712 }
1713
1716
1722 template <class Row, class AT>
1723 RowVector<Row,AT> trans(const Vector<Row,AT> &x) { return x.T(); }
1724 // move
1725 inline RowVector<Var,double> trans(Vector<Var,double> &&x) { return std::move(x).T(); }
1726
1734 template <class Col, class AT>
1735 Vector<Col,AT> trans(const RowVector<Col,AT> &x) { return x.T(); }
1736 // move
1737 inline Vector<Var,double> trans(RowVector<Var,double> &&x) { return std::move(x).T(); }
1738
1747 template <class Type, class Row, class Col, class AT>
1748 Matrix<Type,Col,Row,AT> trans(const Matrix<Type,Row,Col,AT> &A) { return A.T(); }
1749
1758 template <class Row, class AT>
1759 SquareMatrix<Row,AT> trans(const SquareMatrix<Row,AT> &A) { return A.T(); }
1760
1761 // template <class Type, class Row, class Col, class AT>
1762 // inline Matrix<Type, Col, Row, AT> trans(const Matrix<Type, Row, Col, AT> &A) {
1763 // Matrix<Type, Col, Row, AT> B(A.cols(), A.rows(), NONINIT);
1764 // for (int i = 0; i < B.rows(); i++)
1765 // for (int j = 0; j < B.cols(); j++)
1766 // B.e(i, j) = A.e(j, i);
1767 // return B;
1768 // }
1769 //
1770 // template <class Row, class AT>
1771 // inline SquareMatrix<Row, AT> trans(const SquareMatrix<Row, AT> &A) {
1772 // SquareMatrix<Row, AT> B(A.size(), NONINIT);
1773 // for (int i = 0; i < B.size(); i++)
1774 // for (int j = 0; j < B.size(); j++)
1775 // B.e(i, j) = A.e(j, i);
1776 // return B;
1777 // }
1778 //
1779 // template <class Row, class AT>
1780 // inline RowVector<Row, AT> trans(const Vector<Row, AT> &x) {
1781 // RowVector<Row, AT> y(x.size(), NONINIT);
1782 // for (int i = 0; i < y.size(); i++)
1783 // y.e(i) = x.e(i);
1784 // return y;
1785 // }
1786 //
1787 // template <class Row, class AT>
1788 // inline Vector<Row, AT> trans(const RowVector<Row, AT> &x) {
1789 // Vector<Row, AT> y(x.size(), NONINIT);
1790 // for (int i = 0; i < y.size(); i++)
1791 // y.e(i) = x.e(i);
1792 // return y;
1793 // }
1794
1796
1798 template <class Row, class AT>
1799 inline AT nrmInf(const Vector<Row, AT> &x) {
1800 AT c = 0;
1801 for (int i = 0; i < x.size(); i++)
1802 if (c < fabs(x.e(i)))
1803 c = fabs(x.e(i));
1804 return c;
1805 }
1806
1807 template <class Row, class AT>
1808 inline AT nrmInf(const RowVector<Row, AT> &x) {
1809 AT c = 0;
1810 for (int i = 0; i < x.size(); i++)
1811 if (c < fabs(x.e(i)))
1812 c = fabs(x.e(i));
1813 return c;
1814 }
1815
1816 template <class Row, class AT>
1817 inline typename OperatorResult<AT, AT>::Type nrm2(const Vector<Row, AT> &x) {
1818 typename OperatorResult<AT, AT>::Type c=typename OperatorResult<AT, AT>::Type();
1819 for (int i = 0; i < x.size(); i++)
1820 c += pow(x.e(i), 2);
1821 return sqrt(c);
1822 }
1823
1824 template <class Col, class AT>
1825 inline typename OperatorResult<AT, AT>::Type nrm2(const RowVector<Col, AT> &x) {
1826 typename OperatorResult<AT, AT>::Type c=typename OperatorResult<AT, AT>::Type();
1827 for (int i = 0; i < x.size(); i++)
1828 c += pow(x.e(i), 2);
1829 return sqrt(c);
1830 }
1831
1833
1845 template <class Row, class AT>
1846 SquareMatrix<Row, AT> tilde(const Vector<Row, AT> &x) {
1847 assert(x.size()==3);
1848
1849 SquareMatrix<Row, AT> B(x.size(), NONINIT);
1850
1851 B.e(0, 0) = 0.0;
1852 B.e(1, 1) = 0.0;
1853 B.e(2, 2) = 0.0;
1854 B.e(0, 1) = -x.e(2);
1855 B.e(0, 2) = x.e(1);
1856 B.e(1, 0) = x.e(2);
1857 B.e(1, 2) = -x.e(0);
1858 B.e(2, 0) = -x.e(1);
1859 B.e(2, 1) = x.e(0);
1860
1861 return B;
1862 }
1863
1864 FMATVEC_EXPORT extern double tildeEPS;
1865
1880 template <class Row, class AT>
1881 Vector<Row, AT> tilde(const SquareMatrix<Row, AT> &T, double tol = tildeEPS) {
1882 FMATVEC_ASSERT(T.rows()==3, AT);
1883
1884 Vector<Fixed<3>, AT> x(NONINIT);
1885
1886 x.e(0) = T.e(2, 1);
1887 x.e(1) = T.e(0, 2);
1888 x.e(2) = T.e(1, 0);
1889
1890 #ifndef NDEBUG
1891 if(std::abs(T.e(0, 0)) >= tol ||
1892 std::abs(T.e(1, 1)) >= tol ||
1893 std::abs(T.e(2, 2)) >= tol ||
1894 std::abs(x.e(0) + T.e(1, 2)) >= tol ||
1895 std::abs(x.e(1) + T.e(2, 0)) >= tol ||
1896 std::abs(x.e(2) + T.e(0, 1)) >= tol) {
1897 std::cerr<<"fmatvec::tilde called with a none screw symmetric matrix:"<<std::endl
1898 <<T<<std::endl
1899 <<boost::stacktrace::stacktrace()<<std::endl;
1900 std::abort();
1901 }
1902 #endif
1903
1904 return x;
1905 }
1906
1912 template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
1913 bool operator==(const Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
1914
1915 if (&A == &B)
1916 return true;
1917
1918 if (A.rows() != B.rows() || A.cols() != B.cols())
1919 return false;
1920
1921 for (int i = 0; i < A.rows(); i++)
1922 for (int j = 0; j < A.cols(); j++)
1923 if (A(i, j) != B(i, j))
1924 return false;
1925
1926 return true;
1927 }
1928
1934 template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
1935 bool operator!=(const Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
1936
1937 if (A.rows() != B.rows() || A.cols() != B.cols())
1938 return true;
1939
1940 for (int i = 0; i < A.rows(); i++)
1941 for (int j = 0; j < A.cols(); j++)
1942 if (A(i, j) != B(i, j))
1943 return true;
1944
1945 return false;
1946 }
1947
1953 template <class Row, class AT>
1954 AT max(const Vector<Row, AT> &x) {
1955
1956 FMATVEC_ASSERT(x.size() > 0, AT);
1957 AT maximum = x.e(0);
1958 for (int i = 1; i < x.size(); i++) {
1959 if (x.e(i) > maximum)
1960 maximum = x.e(i);
1961 }
1962 return maximum;
1963 }
1964
1970 template <class Row, class AT>
1971 int maxIndex(const Vector<Row, AT> &x) {
1972
1973 FMATVEC_ASSERT(x.size() > 0, AT);
1974 AT maximum = x.e(0);
1975 int index = 0;
1976 for (int i = 1; i < x.size(); i++) {
1977 if (x.e(i) > maximum) {
1978 maximum = x.e(i);
1979 index = i;
1980 }
1981 }
1982 return index;
1983 }
1984
1990 template <class Row, class AT>
1991 AT min(const Vector<Row, AT> &x) {
1992
1993 FMATVEC_ASSERT(x.size() > 0, AT);
1994 AT minimum = x.e(0);
1995 for (int i = 1; i < x.size(); i++) {
1996 if (x.e(i) < minimum)
1997 minimum = x.e(i);
1998 }
1999 return minimum;
2000 }
2001
2007 template <class Row, class AT>
2008 int minIndex(const Vector<Row, AT> &x) {
2009
2010 FMATVEC_ASSERT(x.size() > 0, AT);
2011 AT minimum = x.e(0);
2012 int index = 0;
2013 for (int i = 1; i < x.size(); i++) {
2014 if (x.e(i) < minimum) {
2015 minimum = x.e(i);
2016 index = i;
2017 }
2018 }
2019 return index;
2020 }
2021
2022 // HR 28.09.2006
2029 template <class AT>
2030 Matrix<General, Ref, Ref, AT> bubbleSort(const Matrix<General, Ref, Ref, AT> &A_, int PivotCol) {
2031
2032 FMATVEC_ASSERT(A_.rows() > 0, AT);
2033 FMATVEC_ASSERT(A_.cols() > 0, AT);
2034 FMATVEC_ASSERT(A_.cols() > PivotCol, AT);
2035 Matrix<General, Ref, Ref, AT> A = A_;
2036 int i, j, N;
2037 RowVector<Ref, AT> tmp(A.cols(), NONINIT);
2038 N = A.rows();
2039 for (i = 1; i <= N - 1; i++) {
2040 for (j = 0; j < N - 1; j++) {
2041 if (A(j, PivotCol) > A(j + 1, PivotCol)) {
2042 tmp = A.row(j);
2043 A.set(j, A.row(j + 1));
2044 A.set(j + 1, tmp);
2045 }
2046 }
2047 }
2048 return A;
2049 }
2050
2051 // HR 28.09.2006
2053 template <class AT>
2054 void quicksortmedian_intern(Matrix<General, Ref, Ref, AT> &A, int PivotCol, RowVector<Ref, AT> &tmp, int l, int r) {
2055
2056 if (r > l) {
2057 int i = l - 1, j = r;
2058 //RowVec tmp;
2059 if (r - l > 3) { //Median of three
2060 int m = l + (r - l) / 2;
2061 if (A(l, PivotCol) > A(m, PivotCol)) {
2062 tmp = A.row(l);
2063 A.set(l, A.row(m));
2064 A.set(m, tmp);
2065 }
2066 if (A(l, PivotCol) > A(r, PivotCol)) {
2067 tmp = A.row(l);
2068 A.set(l, A.row(r));
2069 A.set(r, tmp);
2070 }
2071 else if (A(r, PivotCol) > A(m, PivotCol)) {
2072 tmp = A.row(r);
2073 A.set(r, A.row(m));
2074 A.set(m, tmp);
2075 }
2076 }
2077
2078 for (;;) {
2079 while(A(++i,PivotCol) < A(r,PivotCol));
2080 while(A(--j,PivotCol) > A(r,PivotCol) && j>i);
2081 if(i>=j) break;
2082 tmp = A.row(i);
2083 A.set(i, A.row(j));
2084 A.set(j, tmp);
2085 }
2086 tmp = A.row(i);
2087 A.set(i, A.row(r));
2088 A.set(r, tmp);
2089 quicksortmedian_intern(A, PivotCol, tmp, l, i - 1);
2090 quicksortmedian_intern(A, PivotCol, tmp, i + 1, r);
2091 }
2092 }
2093
2094 // HR 28.09.2006
2103 template <class AT>
2104 Matrix<General, Ref, Ref, AT> quickSortMedian(const Matrix<General, Ref, Ref, AT> &A_, int PivotCol) {
2105 Matrix<General, Ref, Ref, AT> A = A_;
2106 int N = A.rows();
2107 RowVector<Ref, AT> tmp(A.cols(), NONINIT);
2108 quicksortmedian_intern(A, PivotCol, tmp, 0, N - 1);
2109 return A;
2110 }
2111
2112 template <class Row, class Col, class AT>
2113 inline Matrix<Symmetric, Col, Col, AT> JTJ(const Matrix<General, Row, Col, AT> &A) {
2114 Matrix<Symmetric, Col, Col, AT> S(A.cols(), A.cols(), NONINIT);
2115 for (int i = 0; i < A.cols(); i++) {
2116 for (int k = i; k < A.cols(); k++) {
2117 S.ej(i, k) = 0;
2118 for (int j = 0; j < A.rows(); j++)
2119 S.ej(i, k) += A.e(j, i) * A.e(j, k);
2120 }
2121 }
2122 return S;
2123 }
2124
2125 template <class Row1, class Row2, class Col, class AT>
2126 inline Matrix<Symmetric, Col, Col, AT> JTMJ(const Matrix<Symmetric, Row2, Row2, AT> &B, const Matrix<General, Row1, Col, AT> &A) {
2127
2128 Matrix<Symmetric, Col, Col, AT> S(A.cols(), A.cols(), NONINIT);
2129 Matrix<General, Row1, Col, AT> C = B * A;
2130
2131 for (int i = 0; i < A.cols(); i++) {
2132 for (int k = i; k < A.cols(); k++) {
2133 S.ej(i, k) = 0;
2134 for (int j = 0; j < A.rows(); j++)
2135 S.ej(i, k) += A.e(j, i) * C.e(j, k);
2136 }
2137 }
2138 return S;
2139 }
2140
2141 template <class Row, class Col, class AT>
2142 inline Matrix<Symmetric, Col, Col, AT> JTMJ(const Matrix<SymmetricSparse, Ref, Ref, AT> &B, const Matrix<General, Row, Col, AT> &A) {
2143
2144 Matrix<Symmetric, Col, Col, AT> S(A.cols(), A.cols(), NONINIT);
2145 Matrix<General, Row, Col, AT> C = B * A;
2146
2147 for (int i = 0; i < A.cols(); i++) {
2148 for (int k = i; k < A.cols(); k++) {
2149 S.ej(i, k) = 0;
2150 for (int j = 0; j < A.rows(); j++)
2151 S.ej(i, k) += A.e(j, i) * C.e(j, k);
2152 }
2153 }
2154 return S;
2155 }
2156
2157 template <class Row, class Col1, class Col2, class AT>
2158 inline Matrix<Symmetric, Row, Row, AT> JMJT(const Matrix<General, Row, Col1, AT> &A, const Matrix<Symmetric, Col2, Col2, AT> &B) {
2159
2160 Matrix<Symmetric, Row, Row, AT> S(A.rows(), A.rows(), NONINIT);
2161 Matrix<General, Row, Col1, AT> C = A * B;
2162
2163 for (int i = 0; i < S.size(); i++) {
2164 for (int k = i; k < S.size(); k++) {
2165 S.ej(i, k) = 0;
2166 for (int j = 0; j < A.cols(); j++)
2167 S.ej(i, k) += C.e(k, j) * A.e(i, j);
2168 }
2169 }
2170 return S;
2171 }
2172
2173 }
2174
2175 #endif
fmatvec::trans
RowVector< Row, AT > trans(const Vector< Row, AT > &x)
Transpose of a vector.
Definition: linear_algebra.h:1723
fmatvec::quickSortMedian
Matrix< General, Ref, Ref, AT > quickSortMedian(const Matrix< General, Ref, Ref, AT > &A_, int PivotCol)
Modified QuickSort Algorithm (unstable sorting Algorithm )
Definition: linear_algebra.h:2104
fmatvec::Matrix
This is the basic matrix class for arbitrary matrices.
Definition: fmatvec.h:42
fmatvec::Matrix< General, Ref, Ref, AT >::rows
int rows() const
Number of rows.
Definition: general_matrix.h:307
fmatvec::quicksortmedian_intern
void quicksortmedian_intern(Matrix< General, Ref, Ref, AT > &A, int PivotCol, RowVector< Ref, AT > &tmp, int l, int r)
Definition: linear_algebra.h:2054
fmatvec::minIndex
int minIndex(const Vector< Row, AT > &x)
Index of minimum value.
Definition: linear_algebra.h:2008
fmatvec::RowVector
Definition: fmatvec.h:51
fmatvec::crossProduct
Vector< Fixed< 3 >, typename OperatorResult< AT1, AT2 >::Type > crossProduct(const Vector< Row1, AT1 > &x, const Vector< Row2, AT2 > &y)
Cross product.
Definition: linear_algebra.h:1311
fmatvec::RowVector< Ref, AT >
This is a rowvector class of general shape in dense storage format.
Definition: row_vector.h:36
fmatvec::SquareMatrix
Definition: fmatvec.h:45
fmatvec::Vector
Definition: fmatvec.h:48
fmatvec::bubbleSort
Matrix< General, Ref, Ref, AT > bubbleSort(const Matrix< General, Ref, Ref, AT > &A_, int PivotCol)
Bubble Sort Algorithm (stable sorting Algorithm )
Definition: linear_algebra.h:2030
fmatvec::maxIndex
int maxIndex(const Vector< Row, AT > &x)
Index of maximum value.
Definition: linear_algebra.h:1971
fmatvec::Matrix::cols
int cols() const
Number of columns.
fmatvec::Matrix< General, Ref, Ref, AT >
This is a matrix class for general matrices.
Definition: general_matrix.h:42
fmatvec
Namespace fmatvec.
Definition: _memory.cc:28
fmatvec::operator==
bool operator==(const Matrix< Type1, Row1, Col1, AT > &A, const Matrix< Type2, Row2, Col2, AT > &B)
Matrix-matrix comparison.
Definition: linear_algebra.h:1913
fmatvec::Matrix< General, Ref, Ref, AT >::cols
int cols() const
Number of columns.
Definition: general_matrix.h:313
fmatvec::Matrix< General, Ref, Ref, AT >::row
const RowVector< Ref, AT > row(int i) const
Row operator.
Definition: general_matrix.h:525
fmatvec::operator!=
bool operator!=(const Matrix< Type1, Row1, Col1, AT > &A, const Matrix< Type2, Row2, Col2, AT > &B)
Matrix-matrix comparison.
Definition: linear_algebra.h:1935
fmatvec::OperatorResult
Definition: types.h:194
fmatvec::tripleProduct
double tripleProduct(const Vector< Row, AT > &a, const Vector< Row, AT > &x, const Vector< Row, AT > &y)
Triple product.
Definition: linear_algebra.h:1331
fmatvec::tilde
SquareMatrix< Row, AT > tilde(const Vector< Row, AT > &x)
Tilde operator.
Definition: linear_algebra.h:1846
fmatvec::Matrix::rows
int rows() const
Number of rows.
fmatvec::scalarProduct
AT scalarProduct(const Vector< Row, AT > &x, const Vector< Row, AT > &y)
Scalar product.
Definition: linear_algebra.h:1293