linear_algebra.h
00001 /* Copyright (C) 2003-2005  Martin Förg, Rober Huber
00002 
00003  * This library is free software; you can redistribute it and/or
00004  * modify it under the terms of the GNU Lesser General Public
00005  * License as published by the Free Software Foundation; either
00006  * version 2.1 of the License, or (at your option) any later version.
00007  * 
00008  * This library is distributed in the hope that it will be useful,
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011  * Lesser General Public License for more details.
00012  * 
00013  * You should have received a copy of the GNU Lesser General Public
00014  * License along with this library; if not, write to the Free Software
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00016  *
00017  * Contact:
00018  *   martin.o.foerg@googlemail.com
00019  *
00020  */
00021 
00022 #ifndef linear_algebra_h
00023 #define linear_algebra_h
00024 
00025 #include <math.h>
00026 
00027 #include "square_matrix.h"
00028 #include "vector.h"
00029 #include "row_vector.h"
00030 #include "fixed_square_matrix.h"
00031 #include "fixed_vector.h"
00032 #include "fixed_row_vector.h"
00033 #include "fixed_symmetric_matrix.h"
00034 #include "var_square_matrix.h"
00035 #include "var_symmetric_matrix.h"
00036 #include "var_vector.h"
00037 #include "var_row_vector.h"
00038 #include "var_fixed_general_matrix.h"
00039 #include "fixed_var_general_matrix.h"
00040 #include <cmath>
00041 
00042 namespace fmatvec {
00043 
00045 
00051   template <class AT>
00052   inline Vector<Ref, AT> operator+(const Vector<Ref, AT> &x, const Vector<Ref, AT> &y) {
00053 
00054 #ifndef FMATVEC_NO_SIZE_CHECK
00055     assert(y.size() == x.size());
00056 #endif
00057 
00058     Vector<Ref, AT> z(x.size(), NONINIT);
00059 
00060     if (y.transposed()) {
00061       if (x.transposed())
00062         for (int i = 0; i < x.size(); i++)
00063           z.er(i) = x.et(i) + y.et(i);
00064       else
00065         for (int i = 0; i < x.size(); i++)
00066           z.er(i) = x.er(i) + y.et(i);
00067     }
00068     else {
00069       if (x.transposed())
00070         for (int i = 0; i < x.size(); i++)
00071           z.er(i) = x.et(i) + y.er(i);
00072       else
00073         for (int i = 0; i < x.size(); i++)
00074           z.er(i) = x.er(i) + y.er(i);
00075     }
00076 
00077     return z;
00078   }
00079 
00080   // Vector-Vector
00081   template <class Row1, class Row2, class Row3, class AT>
00082   inline void add(const Vector<Row1, AT> &a1, const Vector<Row2, AT> &a2, Vector<Row3, AT> &a3) {
00083 #ifndef FMATVEC_NO_SIZE_CHECK
00084     assert(a1.size() == a2.size());
00085 #endif
00086     for (int i = 0; i < a1.size(); i++)
00087       a3.e(i) = a1.e(i) + a2.e(i);
00088   }
00089 
00090   template <class Row1, class Row2, class AT>
00091   inline void add(Vector<Row1, AT> &a1, const Vector<Row2, AT> &a2) {
00092 #ifndef FMATVEC_NO_SIZE_CHECK
00093     assert(a1.size() == a2.size());
00094 #endif
00095     for (int i = 0; i < a1.size(); i++)
00096       a1.e(i) += a2.e(i);
00097   }
00098 
00099   template <class Row1, class Row2, class Row3, class AT>
00100   inline void sub(const Vector<Row1, AT> &a1, const Vector<Row2, AT> &a2, Vector<Row3, AT> &a3) {
00101 #ifndef FMATVEC_NO_SIZE_CHECK
00102     assert(a1.size() == a2.size());
00103 #endif
00104     for (int i = 0; i < a1.size(); i++)
00105       a3.e(i) = a1.e(i) - a2.e(i);
00106   }
00107 
00108   template <class Row1, class Row2, class AT>
00109   inline void sub(Vector<Row1, AT> &a1, const Vector<Row2, AT> &a2) {
00110 #ifndef FMATVEC_NO_SIZE_CHECK
00111     assert(a1.size() == a2.size());
00112 #endif
00113     for (int i = 0; i < a1.size(); i++)
00114       a1.e(i) -= a2.e(i);
00115   }
00116 
00117   // Addition
00118   template <class AT, class Row1, class Row2>
00119   inline Vector<Var, AT> operator+(const Vector<Row1, AT> &a, const Vector<Row2, AT> &b) {
00120     Vector<Var, AT> c(a.size(), NONINIT);
00121     add(a, b, c);
00122     return c;
00123   }
00124   template <class AT, class Row>
00125   inline Vector<Row, AT> operator+(const Vector<Row, AT> &a, const Vector<Row, AT> &b) {
00126     Vector<Row, AT> c(a.size(), NONINIT);
00127     add(a, b, c);
00128     return c;
00129   }
00130   template <class AT, int M, class Row>
00131   inline Vector<Fixed<M>, AT> operator+(const Vector<Fixed<M>, AT> &a, const Vector<Row, AT> &b) {
00132     Vector<Fixed<M>, AT> c(a.size(), NONINIT);
00133     add(a, b, c);
00134     return c;
00135   }
00136   template <class AT, int M>
00137   inline Vector<Fixed<M>, AT> operator+(const Vector<Fixed<M>, AT> &a, const Vector<Fixed<M>, AT> &b) {
00138     Vector<Fixed<M>, AT> c(a.size(), NONINIT);
00139     add(a, b, c);
00140     return c;
00141   }
00142   template <class AT, class Row1, class Row2>
00143   inline Vector<Row1, AT> operator+=(const Vector<Row1, AT> &a_, const Vector<Row2, AT> &b) {
00144     Vector<Row1, AT> &a = const_cast<Vector<Row1, AT> &>(a_);
00145     add(a, b);
00146     return a;
00147   }
00148 
00150   template <class AT, class Row1, class Row2>
00151   inline Vector<Var, AT> operator-(const Vector<Row1, AT> &a, const Vector<Row2, AT> &b) {
00152     Vector<Var, AT> c(a.size(), NONINIT);
00153     sub(a, b, c);
00154     return c;
00155   }
00156   template <class AT, class Row>
00157   inline Vector<Row, AT> operator-(const Vector<Row, AT> &a, const Vector<Row, AT> &b) {
00158     Vector<Row, AT> c(a.size(), NONINIT);
00159     sub(a, b, c);
00160     return c;
00161   }
00162   template <class AT, int M, class Row2>
00163   inline Vector<Fixed<M>, AT> operator-(const Vector<Fixed<M>, AT> &a, const Vector<Row2, AT> &b) {
00164     Vector<Fixed<M>, AT> c(a.size(), NONINIT);
00165     sub(a, b, c);
00166     return c;
00167   }
00168   template <class AT, int M>
00169   inline Vector<Fixed<M>, AT> operator-(const Vector<Fixed<M>, AT> &a, const Vector<Fixed<M>, AT> &b) {
00170     Vector<Fixed<M>, AT> c(a.size(), NONINIT);
00171     sub(a, b, c);
00172     return c;
00173   }
00174   template <class AT, class Row1, class Row2>
00175   inline Vector<Row1, AT> operator-=(const Vector<Row1, AT> &a_, const Vector<Row2, AT> &b) {
00176     Vector<Row1, AT> &a = const_cast<Vector<Row1, AT> &>(a_);
00177     sub(a, b);
00178     return a;
00179   }
00180 
00181   // RowVector-Vector
00182   template <class Col1, class Col2, class Col3, class AT>
00183   inline void add(const RowVector<Col1, AT> &a1, const RowVector<Col2, AT> &a2, RowVector<Col3, AT> &a3) {
00184 #ifndef FMATVEC_NO_SIZE_CHECK
00185     assert(a1.size() == a2.size());
00186 #endif
00187     for (int i = 0; i < a1.size(); i++)
00188       a3.e(i) = a1.e(i) + a2.e(i);
00189   }
00190 
00191   template <class Col1, class Col2, class AT>
00192   inline void add(RowVector<Col1, AT> &a1, const RowVector<Col2, AT> &a2) {
00193 #ifndef FMATVEC_NO_SIZE_CHECK
00194     assert(a1.size() == a2.size());
00195 #endif
00196     for (int i = 0; i < a1.size(); i++)
00197       a1.e(i) += a2.e(i);
00198   }
00199 
00200   template <class Col1, class Col2, class Col3, class AT>
00201   inline void sub(const RowVector<Col1, AT> &a1, const RowVector<Col2, AT> &a2, RowVector<Col3, AT> &a3) {
00202 #ifndef FMATVEC_NO_SIZE_CHECK
00203     assert(a1.size() == a2.size());
00204 #endif
00205     for (int i = 0; i < a1.size(); i++)
00206       a3.e(i) = a1.e(i) - a2.e(i);
00207   }
00208 
00209   template <class Col1, class Col2, class AT>
00210   inline void sub(RowVector<Col1, AT> &a1, const RowVector<Col2, AT> &a2) {
00211 #ifndef FMATVEC_NO_SIZE_CHECK
00212     assert(a1.size() == a2.size());
00213 #endif
00214     for (int i = 0; i < a1.size(); i++)
00215       a1.e(i) -= a2.e(i);
00216   }
00217 
00218   // Addition
00219   template <class AT, class Col1, class Col2>
00220   inline RowVector<Var, AT> operator+(const RowVector<Col1, AT> &a, const RowVector<Col2, AT> &b) {
00221     RowVector<Var, AT> c(a.size(), NONINIT);
00222     add(a, b, c);
00223     return c;
00224   }
00225   template <class AT, class Col>
00226   inline RowVector<Col, AT> operator+(const RowVector<Col, AT> &a, const RowVector<Col, AT> &b) {
00227     RowVector<Col, AT> c(a.size(), NONINIT);
00228     add(a, b, c);
00229     return c;
00230   }
00231   template <class AT, int N, class Col2>
00232   inline RowVector<Fixed<N>, AT> operator+(const RowVector<Fixed<N>, AT> &a, const RowVector<Col2, AT> &b) {
00233     RowVector<Fixed<N>, AT> c(a.size(), NONINIT);
00234     add(a, b, c);
00235     return c;
00236   }
00237   template <class AT, int N>
00238   inline RowVector<Fixed<N>, AT> operator+(const RowVector<Fixed<N>, AT> &a, const RowVector<Fixed<N>, AT> &b) {
00239     RowVector<Fixed<N>, AT> c(a.size(), NONINIT);
00240     add(a, b, c);
00241     return c;
00242   }
00243   template <class AT, class Col1, class Col2>
00244   inline RowVector<Col1, AT> operator+=(const RowVector<Col1, AT> &a_, const RowVector<Col2, AT> &b) {
00245     RowVector<Col1, AT> &a = const_cast<RowVector<Col1, AT> &>(a_);
00246     add(a, b);
00247     return a;
00248   }
00249 
00251   template <class AT, class Col1, class Col2>
00252   inline RowVector<Var, AT> operator-(const RowVector<Col1, AT> &a, const RowVector<Col2, AT> &b) {
00253     RowVector<Var, AT> c(a.size(), NONINIT);
00254     sub(a, b, c);
00255     return c;
00256   }
00257   template <class AT, class Col>
00258   inline RowVector<Col, AT> operator-(const RowVector<Col, AT> &a, const RowVector<Col, AT> &b) {
00259     RowVector<Col, AT> c(a.size(), NONINIT);
00260     sub(a, b, c);
00261     return c;
00262   }
00263   template <class AT, int N, class Col2>
00264   inline RowVector<Fixed<N>, AT> operator-(const RowVector<Fixed<N>, AT> &a, const RowVector<Col2, AT> &b) {
00265     RowVector<Fixed<N>, AT> c(a.size(), NONINIT);
00266     sub(a, b, c);
00267     return c;
00268   }
00269   template <class AT, int N>
00270   inline RowVector<Fixed<N>, AT> operator-(const RowVector<Fixed<N>, AT> &a, const RowVector<Fixed<N>, AT> &b) {
00271     RowVector<Fixed<N>, AT> c(a.size(), NONINIT);
00272     sub(a, b, c);
00273     return c;
00274   }
00275   template <class AT, class Col1, class Col2>
00276   inline RowVector<Col1, AT> operator-=(const RowVector<Col1, AT> &a_, const RowVector<Col2, AT> &b) {
00277     RowVector<Col1, AT> &a = const_cast<RowVector<Col1, AT> &>(a_);
00278     sub(a, b);
00279     return a;
00280   }
00281 
00282   // Matrix-Matrix
00283   template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT>
00284   inline void add(const Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2, Matrix<Type3, Row3, Col3, AT> &A3) {
00285 #ifndef FMATVEC_NO_SIZE_CHECK
00286     assert(A1.rows() == A2.rows());
00287     assert(A1.cols() == A2.cols());
00288 #endif
00289     for (int i = 0; i < A1.rows(); i++)
00290       for (int j = 0; j < A2.cols(); j++)
00291         A3.e(i, j) = A1.e(i, j) + A2.e(i, j);
00292   }
00293 
00294   template <class Row1, class Row2, class Row3, class AT>
00295   inline void add(const Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2, Matrix<Symmetric, Row3, Row3, AT> &A3) {
00296 #ifndef FMATVEC_NO_SIZE_CHECK
00297     assert(A1.size() == A2.size());
00298 #endif
00299     for (int i = 0; i < A1.size(); i++)
00300       for (int j = i; j < A2.size(); j++)
00301         A3.ej(i, j) = A1.ej(i, j) + A2.ej(i, j);
00302   }
00303 
00304   template <class Row1, class Row2, class Row3, class AT>
00305   inline void add(const Matrix<Diagonal, Row1, Row1, AT> &A1, const Matrix<Diagonal, Row2, Row2, AT> &A2, Matrix<Diagonal, Row3, Row3, AT> &A3) {
00306 #ifndef FMATVEC_NO_SIZE_CHECK
00307     assert(A1.size() == A2.size());
00308 #endif
00309     for (int i = 0; i < A3.size(); i++)
00310       A3.e(i) = A1.e(i) + A2.e(i);
00311   }
00312 
00313   template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class AT>
00314   inline void add(Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2) {
00315 #ifndef FMATVEC_NO_SIZE_CHECK
00316     assert(A1.rows() == A2.rows());
00317     assert(A1.cols() == A2.cols());
00318 #endif
00319     for (int i = 0; i < A1.rows(); i++)
00320       for (int j = 0; j < A2.cols(); j++)
00321         A1.e(i, j) += A2.e(i, j);
00322   }
00323 
00324   template <class Row1, class Row2, class AT>
00325   inline void add(Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2) {
00326 #ifndef FMATVEC_NO_SIZE_CHECK
00327     assert(A1.size() == A2.size());
00328 #endif
00329     for (int i = 0; i < A1.size(); i++)
00330       for (int j = i; j < A2.size(); j++)
00331         A1.ej(i, j) += A2.ej(i, j);
00332   }
00333 
00334   template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT>
00335   inline void sub(const Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2, Matrix<Type3, Row3, Col3, AT> &A3) {
00336 #ifndef FMATVEC_NO_SIZE_CHECK
00337     assert(A1.rows() == A2.rows());
00338     assert(A1.cols() == A2.cols());
00339 #endif
00340     for (int i = 0; i < A1.rows(); i++)
00341       for (int j = 0; j < A2.cols(); j++)
00342         A3.e(i, j) = A1.e(i, j) - A2.e(i, j);
00343   }
00344 
00345   template <class Row1, class Row2, class Row3, class AT>
00346   inline void sub(const Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2, Matrix<Symmetric, Row3, Row3, AT> &A3) {
00347 #ifndef FMATVEC_NO_SIZE_CHECK
00348     assert(A1.size() == A2.size());
00349 #endif
00350     for (int i = 0; i < A1.size(); i++)
00351       for (int j = i; j < A2.size(); j++)
00352         A3.ej(i, j) = A1.ej(i, j) - A2.ej(i, j);
00353   }
00354 
00355   template <class Row1, class Row2, class Row3, class AT>
00356   inline void sub(const Matrix<Diagonal, Row1, Row1, AT> &A1, const Matrix<Diagonal, Row2, Row2, AT> &A2, Matrix<Diagonal, Row3, Row3, AT> &A3) {
00357 #ifndef FMATVEC_NO_SIZE_CHECK
00358     assert(A1.size() == A2.size());
00359 #endif
00360     for (int i = 0; i < A3.size(); i++)
00361       A3.e(i) = A1.e(i) - A2.e(i);
00362   }
00363 
00364   template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class AT>
00365   inline void sub(Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2) {
00366 #ifndef FMATVEC_NO_SIZE_CHECK
00367     assert(A1.rows() == A2.rows());
00368     assert(A1.cols() == A2.cols());
00369 #endif
00370     for (int i = 0; i < A1.rows(); i++)
00371       for (int j = 0; j < A2.cols(); j++)
00372         A1.e(i, j) -= A2.e(i, j);
00373   }
00374 
00375   template <class Row1, class Row2, class AT>
00376   inline void sub(Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2) {
00377 #ifndef FMATVEC_NO_SIZE_CHECK
00378     assert(A1.size() == A2.size());
00379 #endif
00380     for (int i = 0; i < A1.size(); i++)
00381       for (int j = i; j < A2.size(); j++)
00382         A1.ej(i, j) -= A2.ej(i, j);
00383   }
00384 
00385   // Addition
00386   // Type1 Type2
00387   template <class AT, class Type1, class Type2, class Row1, class Col1, class Row2, class Col2>
00388   inline Matrix<General, Var, Var, AT> operator+(const Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
00389     Matrix<General, Var, Var, AT> C(A.rows(), A.cols(), NONINIT);
00390     add(A, B, C);
00391     return C;
00392   }
00393   template <class AT, class Type1, class Type2, class Row, class Col>
00394   inline Matrix<General, Row, Col, AT> operator+(const Matrix<Type1, Row, Col, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00395     Matrix<General, Row, Col, AT> C(A.rows(), A.cols(), NONINIT);
00396     add(A, B, C);
00397     return C;
00398   }
00399   template <class AT, int M, class Type1, class Type2, class Row, class Col>
00400   inline Matrix<General, Fixed<M>, Var, AT> operator+(const Matrix<Type1, Fixed<M>, Var, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00401     Matrix<General, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00402     add(A, B, C);
00403     return C;
00404   }
00405   template <class AT, int M, class Type1, class Type2>
00406   inline Matrix<General, Fixed<M>, Var, AT> operator+(const Matrix<Type1, Fixed<M>, Var, AT> &A, const Matrix<Type2, Fixed<M>, Var, AT> &B) {
00407     Matrix<General, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00408     add(A, B, C);
00409     return C;
00410   }
00411   template <class AT, int N, class Type1, class Type2, class Row, class Col>
00412   inline Matrix<General, Var, Fixed<N>, AT> operator+(const Matrix<Type1, Var, Fixed<N>, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00413     Matrix<General, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00414     add(A, B, C);
00415     return C;
00416   }
00417   template <class AT, int N, class Type1, class Type2>
00418   inline Matrix<General, Var, Fixed<N>, AT> operator+(const Matrix<Type1, Var, Fixed<N>, AT> &A, const Matrix<Type2, Var, Fixed<N>, AT> &B) {
00419     Matrix<General, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00420     add(A, B, C);
00421     return C;
00422   }
00423   template <class AT, int M, int N, class Type1, class Type2, class Row, class Col>
00424   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00425     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00426     add(A, B, C);
00427     return C;
00428   }
00429   template <class AT, int M, int N, class Type1, class Type2>
00430   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Fixed<M>, Var, AT> &B) {
00431     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00432     add(A, B, C);
00433     return C;
00434   }
00435   template <class AT, int M, int N, class Type1, class Type2>
00436   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Var, Fixed<N>, AT> &B) {
00437     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00438     add(A, B, C);
00439     return C;
00440   }
00441   template <class AT, int M, int N, class Type1, class Type2>
00442   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Fixed<M>, Fixed<N>, AT> &B) {
00443     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00444     add(A, B, C);
00445     return C;
00446   }
00447   template <class AT, int M, int N, class Type1, class Type2, class Row, class Col>
00448   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type2, Row, Col, AT> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &B) {
00449     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00450     add(A, B, C);
00451     return C;
00452   }
00453   template <class AT, int M, int N, class Type1, class Type2>
00454   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type2, Fixed<M>, Var, AT> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &B) {
00455     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00456     add(A, B, C);
00457     return C;
00458   }
00459   template <class AT, int M, int N, class Type1, class Type2>
00460   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type2, Var, Fixed<N>, AT> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &B) {
00461     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00462     add(A, B, C);
00463     return C;
00464   }
00465   // Type
00466   template <class AT, class Type, class Row1, class Col1, class Row2, class Col2>
00467   inline Matrix<Type, Var, Var, AT> operator+(const Matrix<Type, Row1, Col1, AT> &A, const Matrix<Type, Row2, Col2, AT> &B) {
00468     Matrix<Type, Var, Var, AT> C(A.rows(), A.cols(), NONINIT);
00469     add(A, B, C);
00470     return C;
00471   }
00472   template <class AT, class Type, class Row, class Col>
00473   inline Matrix<Type, Row, Col, AT> operator+(const Matrix<Type, Row, Col, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00474     Matrix<Type, Row, Col, AT> C(A.rows(), A.cols(), NONINIT);
00475     add(A, B, C);
00476     return C;
00477   }
00478   template <class AT, int M, class Type, class Row, class Col>
00479   inline Matrix<Type, Fixed<M>, Var, AT> operator+(const Matrix<Type, Fixed<M>, Var, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00480     Matrix<Type, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00481     add(A, B, C);
00482     return C;
00483   }
00484   template <class AT, int M, class Type>
00485   inline Matrix<Type, Fixed<M>, Var, AT> operator+(const Matrix<Type, Fixed<M>, Var, AT> &A, const Matrix<Type, Fixed<M>, Var, AT> &B) {
00486     Matrix<Type, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00487     add(A, B, C);
00488     return C;
00489   }
00490   template <class AT, int N, class Type, class Row, class Col>
00491   inline Matrix<Type, Var, Fixed<N>, AT> operator+(const Matrix<Type, Var, Fixed<N>, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00492     Matrix<Type, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00493     add(A, B, C);
00494     return C;
00495   }
00496   template <class AT, int N, class Type>
00497   inline Matrix<Type, Var, Fixed<N>, AT> operator+(const Matrix<Type, Var, Fixed<N>, AT> &A, const Matrix<Type, Var, Fixed<N>, AT> &B) {
00498     Matrix<Type, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00499     add(A, B, C);
00500     return C;
00501   }
00502   template <class AT, int M, int N, class Type, class Row, class Col>
00503   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00504     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00505     add(A, B, C);
00506     return C;
00507   }
00508   template <class AT, int M, int N, class Type>
00509   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Fixed<M>, Var, AT> &B) {
00510     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00511     add(A, B, C);
00512     return C;
00513   }
00514   template <class AT, int M, int N, class Type>
00515   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Var, Fixed<N>, AT> &B) {
00516     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00517     add(A, B, C);
00518     return C;
00519   }
00520   template <class AT, int M, int N, class Type>
00521   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00522     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00523     add(A, B, C);
00524     return C;
00525   }
00526   template <class AT, int M, int N, class Type, class Row, class Col>
00527   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Row, Col, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00528     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00529     add(A, B, C);
00530     return C;
00531   }
00532   template <class AT, int M, int N, class Type>
00533   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Fixed<M>, Var, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00534     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00535     add(A, B, C);
00536     return C;
00537   }
00538   template <class AT, int M, int N, class Type>
00539   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator+(const Matrix<Type, Var, Fixed<N>, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00540     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00541     add(A, B, C);
00542     return C;
00543   }
00544   template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
00545   inline Matrix<Type1, Row1, Col1, AT>& operator+=(const Matrix<Type1, Row1, Col1, AT> &A_, const Matrix<Type2, Row2, Col2, AT> &B) {
00546     Matrix<Type1, Row1, Col1, AT> &A = const_cast<Matrix<Type1, Row1, Col1, AT> &>(A_);
00547     add(A, B);
00548     return A;
00549   }
00550 
00551   // Subtraction
00552   // Type1 Type2
00553   template <class AT, class Type1, class Type2, class Row1, class Col1, class Row2, class Col2>
00554   inline Matrix<General, Var, Var, AT> operator-(const Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
00555     Matrix<General, Var, Var, AT> C(A.rows(), A.cols(), NONINIT);
00556     sub(A, B, C);
00557     return C;
00558   }
00559   template <class AT, class Type1, class Type2, class Row, class Col>
00560   inline Matrix<General, Row, Col, AT> operator-(const Matrix<Type1, Row, Col, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00561     Matrix<General, Row, Col, AT> C(A.rows(), A.cols(), NONINIT);
00562     sub(A, B, C);
00563     return C;
00564   }
00565   template <class AT, int M, class Type1, class Type2, class Row, class Col>
00566   inline Matrix<General, Fixed<M>, Var, AT> operator-(const Matrix<Type1, Fixed<M>, Var, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00567     Matrix<General, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00568     sub(A, B, C);
00569     return C;
00570   }
00571   template <class AT, int M, class Type1, class Type2>
00572   inline Matrix<General, Fixed<M>, Var, AT> operator-(const Matrix<Type1, Fixed<M>, Var, AT> &A, const Matrix<Type2, Fixed<M>, Var, AT> &B) {
00573     Matrix<General, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00574     sub(A, B, C);
00575     return C;
00576   }
00577   template <class AT, int N, class Type1, class Type2, class Row, class Col>
00578   inline Matrix<General, Var, Fixed<N>, AT> operator-(const Matrix<Type1, Var, Fixed<N>, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00579     Matrix<General, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00580     sub(A, B, C);
00581     return C;
00582   }
00583   template <class AT, int N, class Type1, class Type2>
00584   inline Matrix<General, Var, Fixed<N>, AT> operator-(const Matrix<Type1, Var, Fixed<N>, AT> &A, const Matrix<Type2, Var, Fixed<N>, AT> &B) {
00585     Matrix<General, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00586     sub(A, B, C);
00587     return C;
00588   }
00589   template <class AT, int M, int N, class Type1, class Type2, class Row, class Col>
00590   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Row, Col, AT> &B) {
00591     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00592     sub(A, B, C);
00593     return C;
00594   }
00595   template <class AT, int M, int N, class Type1, class Type2>
00596   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Fixed<M>, Var, AT> &B) {
00597     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00598     sub(A, B, C);
00599     return C;
00600   }
00601   template <class AT, int M, int N, class Type1, class Type2>
00602   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Var, Fixed<N>, AT> &B) {
00603     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00604     sub(A, B, C);
00605     return C;
00606   }
00607   template <class AT, int M, int N, class Type1, class Type2>
00608   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type2, Fixed<M>, Fixed<N>, AT> &B) {
00609     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00610     sub(A, B, C);
00611     return C;
00612   }
00613   template <class AT, int M, int N, class Type1, class Type2, class Row, class Col>
00614   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type2, Row, Col, AT> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &B) {
00615     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00616     sub(A, B, C);
00617     return C;
00618   }
00619   template <class AT, int M, int N, class Type1, class Type2>
00620   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type2, Fixed<M>, Var, AT> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &B) {
00621     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00622     sub(A, B, C);
00623     return C;
00624   }
00625   template <class AT, int M, int N, class Type1, class Type2>
00626   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type2, Var, Fixed<N>, AT> &A, const Matrix<Type1, Fixed<M>, Fixed<N>, AT> &B) {
00627     Matrix<General, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00628     sub(A, B, C);
00629     return C;
00630   }
00631   // Type
00632   template <class AT, class Type, class Row1, class Col1, class Row2, class Col2>
00633   inline Matrix<Type, Var, Var, AT> operator-(const Matrix<Type, Row1, Col1, AT> &A, const Matrix<Type, Row2, Col2, AT> &B) {
00634     Matrix<Type, Var, Var, AT> C(A.rows(), A.cols(), NONINIT);
00635     sub(A, B, C);
00636     return C;
00637   }
00638   template <class AT, class Type, class Row, class Col>
00639   inline Matrix<Type, Row, Col, AT> operator-(const Matrix<Type, Row, Col, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00640     Matrix<Type, Row, Col, AT> C(A.rows(), A.cols(), NONINIT);
00641     sub(A, B, C);
00642     return C;
00643   }
00644   template <class AT, int M, class Type, class Row, class Col>
00645   inline Matrix<Type, Fixed<M>, Var, AT> operator-(const Matrix<Type, Fixed<M>, Var, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00646     Matrix<Type, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00647     sub(A, B, C);
00648     return C;
00649   }
00650   template <class AT, int M, class Type>
00651   inline Matrix<Type, Fixed<M>, Var, AT> operator-(const Matrix<Type, Fixed<M>, Var, AT> &A, const Matrix<Type, Fixed<M>, Var, AT> &B) {
00652     Matrix<Type, Fixed<M>, Var, AT> C(A.rows(), A.cols(), NONINIT);
00653     sub(A, B, C);
00654     return C;
00655   }
00656   template <class AT, int N, class Type, class Row, class Col>
00657   inline Matrix<Type, Var, Fixed<N>, AT> operator-(const Matrix<Type, Var, Fixed<N>, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00658     Matrix<Type, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00659     sub(A, B, C);
00660     return C;
00661   }
00662   template <class AT, int N, class Type>
00663   inline Matrix<Type, Var, Fixed<N>, AT> operator-(const Matrix<Type, Var, Fixed<N>, AT> &A, const Matrix<Type, Var, Fixed<N>, AT> &B) {
00664     Matrix<Type, Var, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00665     sub(A, B, C);
00666     return C;
00667   }
00668   template <class AT, int M, int N, class Type, class Row, class Col>
00669   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Row, Col, AT> &B) {
00670     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00671     sub(A, B, C);
00672     return C;
00673   }
00674   template <class AT, int M, int N, class Type>
00675   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Fixed<M>, Var, AT> &B) {
00676     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00677     sub(A, B, C);
00678     return C;
00679   }
00680   template <class AT, int M, int N, class Type>
00681   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Var, Fixed<N>, AT> &B) {
00682     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00683     sub(A, B, C);
00684     return C;
00685   }
00686   template <class AT, int M, int N, class Type>
00687   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00688     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00689     sub(A, B, C);
00690     return C;
00691   }
00692   template <class AT, int M, int N, class Type, class Row, class Col>
00693   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Row, Col, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00694     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00695     sub(A, B, C);
00696     return C;
00697   }
00698   template <class AT, int M, int N, class Type>
00699   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Fixed<M>, Var, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00700     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00701     sub(A, B, C);
00702     return C;
00703   }
00704   template <class AT, int M, int N, class Type>
00705   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator-(const Matrix<Type, Var, Fixed<N>, AT> &A, const Matrix<Type, Fixed<M>, Fixed<N>, AT> &B) {
00706     Matrix<Type, Fixed<M>, Fixed<N>, AT> C(A.rows(), A.cols(), NONINIT);
00707     sub(A, B, C);
00708     return C;
00709   }
00710   template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
00711   inline Matrix<Type1, Row1, Col1, AT>& operator-=(const Matrix<Type1, Row1, Col1, AT> &A_, const Matrix<Type2, Row2, Col2, AT> &B) {
00712     Matrix<Type1, Row1, Col1, AT> &A = const_cast<Matrix<Type1, Row1, Col1, AT> &>(A_);
00713     sub(A, B);
00714     return A;
00715   }
00716 
00717 //  SquareMatrix-SquareMatrix
00718   template <class AT, class Row1, class Row2>
00719   inline SquareMatrix<Var, AT> operator+(const SquareMatrix<Row1, AT> &A1, const SquareMatrix<Row2, AT> &A2) {
00720     SquareMatrix<Var, AT> A3(A1.size(), NONINIT);
00721     add(A1, A2, A3);
00722     return A3;
00723   }
00724   template <class AT, class Row>
00725   inline SquareMatrix<Row, AT> operator+(const SquareMatrix<Row, AT> &A1, const SquareMatrix<Row, AT> &A2) {
00726     SquareMatrix<Row, AT> A3(A1.size(), NONINIT);
00727     add(A1, A2, A3);
00728     return A3;
00729   }
00730 
00731 //  SquareMatrix-SquareMatrix
00732   template <class AT, class Row1, class Row2>
00733   inline SquareMatrix<Var, AT> operator-(const SquareMatrix<Row1, AT> &A1, const SquareMatrix<Row2, AT> &A2) {
00734     SquareMatrix<Var, AT> A3(A1.size(), NONINIT);
00735     sub(A1, A2, A3);
00736     return A3;
00737   }
00738   template <class AT, class Row>
00739   inline SquareMatrix<Row, AT> operator-(const SquareMatrix<Row, AT> &A1, const SquareMatrix<Row, AT> &A2) {
00740     SquareMatrix<Row, AT> A3(A1.size(), NONINIT);
00741     sub(A1, A2, A3);
00742     return A3;
00743   }
00744 
00750   template <class Type1, class Row1, class Col1, class Row2, class Row3, class AT>
00751   inline void mult(const Matrix<Type1, Row1, Col1, AT> &A, const Vector<Row2, AT> &x, Vector<Row3, AT> &y) {
00752 #ifndef FMATVEC_NO_SIZE_CHECK
00753     assert(A.cols() == x.size());
00754 #endif
00755     for (int i = 0; i < y.size(); i++) {
00756       y.e(i) = 0;
00757       for (int j = 0; j < x.size(); j++)
00758         y.e(i) += A.e(i, j) * x.e(j);
00759     }
00760   }
00761 
00762   template <class Row1, class Row2, class Row3, class AT>
00763   inline void mult(const Matrix<Symmetric, Row1, Row1, AT> &A, const Vector<Row2, AT> &x, Vector<Row3, AT> &y) {
00764 #ifndef FMATVEC_NO_SIZE_CHECK
00765     assert(A.cols() == x.size());
00766 #endif
00767     for (int i = 0; i < y.size(); i++) {
00768       y.e(i) = 0;
00769       for (int j = 0; j < i; j++)
00770         y.e(i) += A.ei(i, j) * x.e(j);
00771       for (int j = i; j < x.size(); j++)
00772         y.e(i) += A.ej(i, j) * x.e(j);
00773     }
00774   }
00775 
00776   // Matrix-Vector
00777   template <class AT, class Type1, class Row1, class Col1, class Row2>
00778   inline Vector<Var, AT> operator*(const Matrix<Type1, Row1, Col1, AT> &A, const Vector<Row2, AT> &x) {
00779     Vector<Var, AT> y(A.rows(), NONINIT);
00780     mult(A, x, y);
00781     return y;
00782   }
00783   template <class AT, class Type1, class Row, class Col>
00784   inline Vector<Row, AT> operator*(const Matrix<Type1, Row, Col, AT> &A, const Vector<Row, AT> &x) {
00785     Vector<Row, AT> y(A.rows(), NONINIT);
00786     mult(A, x, y);
00787     return y;
00788   }
00789   template <class AT, int M, class Type1, class Col1, class Row2>
00790   inline Vector<Fixed<M>, AT> operator*(const Matrix<Type1, Fixed<M>, Col1, AT> &A, const Vector<Row2, AT> &x) {
00791     Vector<Fixed<M>, AT> y(A.rows(), NONINIT);
00792     mult(A, x, y);
00793     return y;
00794   }
00795   template <class AT, int M, class Type1>
00796   inline Vector<Fixed<M>, AT> operator*(const Matrix<Type1, Fixed<M>, Fixed<M>, AT> &A, const Vector<Fixed<M>, AT> &x) {
00797     Vector<Fixed<M>, AT> y(A.rows(), NONINIT);
00798     mult(A, x, y);
00799     return y;
00800   }
00801   template <class AT, int M, int N, class Type, class Row2>
00802   inline Vector<Fixed<M>, AT> operator*(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Vector<Row2, AT> &x) {
00803     Vector<Fixed<M>, AT> y(A.rows(), NONINIT);
00804     mult(A, x, y);
00805     return y;
00806   }
00807   template <class AT, int M, int N, class Type>
00808   inline Vector<Fixed<M>, AT> operator*(const Matrix<Type, Fixed<M>, Fixed<N>, AT> &A, const Vector<Fixed<N>, AT> &x) {
00809     Vector<Fixed<M>, AT> y(A.rows(), NONINIT);
00810     mult(A, x, y);
00811     return y;
00812   }
00813 
00814   // RowVector-Matrix
00815   template <class Col1, class Type2, class Row2, class Col2, class Type3, class Col3, class AT>
00816   inline void mult(const RowVector<Col1, AT> &x, const Matrix<Type2, Row2, Col2, AT> &A, RowVector<Col3, AT> &y) {
00817 #ifndef FMATVEC_NO_SIZE_CHECK
00818     assert(x.size() == A.rows());
00819 #endif
00820     for (int i = 0; i < y.size(); i++) {
00821       y.e(i) = 0;
00822       for (int j = 0; j < x.size(); j++)
00823         y.e(i) += x.e(j) * A.e(j, i);
00824     }
00825   }
00826 
00827   template <class Col1, class Row2, class Col3, class AT>
00828   inline void mult(const RowVector<Col1, AT> &x, const Matrix<Symmetric, Row2, Row2, AT> &A, RowVector<Col3, AT> &y) {
00829 #ifndef FMATVEC_NO_SIZE_CHECK
00830     assert(x.size() == A.rows());
00831 #endif
00832     for (int i = 0; i < y.size(); i++) {
00833       y.e(i) = 0;
00834       for (int j = 0; j < i; j++)
00835         y.e(i) += x.e(j) * A.ej(j, i);
00836       for (int j = i; j < x.size(); j++)
00837         y.e(i) += x.e(j) * A.ei(j, i);
00838     }
00839   }
00840 
00841   template <class AT, class Type2, class Col2, class Row1, class Col1>
00842   inline RowVector<Var, AT> operator*(const RowVector<Col2, AT> &x, const Matrix<Type2, Row1, Col1, AT> &A) {
00843     RowVector<Var, AT> y(A.cols(), NONINIT);
00844     mult(x, A, y);
00845     return y;
00846   }
00847   template <class AT, class Type2, class Row, class Col>
00848   inline RowVector<Col, AT> operator*(const RowVector<Col, AT> &x, const Matrix<Type2, Row, Col, AT> &A) {
00849     RowVector<Col, AT> y(A.cols(), NONINIT);
00850     mult(x, A, y);
00851     return y;
00852   }
00853   template <class AT, int N, class Type2, class Col1, class Row2>
00854   inline RowVector<Fixed<N>, AT> operator*(const RowVector<Col1, AT> &x, const Matrix<Type2, Row2, Fixed<N>, AT> &A) {
00855     RowVector<Fixed<N>, AT> y(A.cols(), NONINIT);
00856     mult(x, A, y);
00857     return y;
00858   }
00859   template <class AT, int N, class Type2>
00860   inline RowVector<Fixed<N>, AT> operator*(const RowVector<Fixed<N>, AT> &x, const Matrix<Type2, Fixed<N>, Fixed<N>, AT> &A) {
00861     RowVector<Fixed<N>, AT> y(A.cols(), NONINIT);
00862     mult(x, A, y);
00863     return y;
00864   }
00865 
00866   // Matrix-Matrix
00867   template <class Type1, class Row1, class Col1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT>
00868   inline void mult(const Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2, Matrix<Type3, Row3, Col3, AT> &A3) {
00869 #ifndef FMATVEC_NO_SIZE_CHECK
00870     assert(A1.cols() == A2.rows());
00871 #endif
00872     for (int i = 0; i < A3.rows(); i++) {
00873       for (int k = 0; k < A3.cols(); k++) {
00874         A3.e(i, k) = 0;
00875         for (int j = 0; j < A1.cols(); j++)
00876           A3.e(i, k) += A1.e(i, j) * A2.e(j, k);
00877       }
00878     }
00879   }
00880   template <class Row1, class Type2, class Row2, class Col2, class Type3, class Row3, class Col3, class AT>
00881   inline void mult(const Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2, Matrix<Type3, Row3, Col3, AT> &A3) {
00882 #ifndef FMATVEC_NO_SIZE_CHECK
00883     assert(A1.size() == A2.rows());
00884 #endif
00885     for (int i = 0; i < A3.rows(); i++) {
00886       for (int k = 0; k < A3.cols(); k++) {
00887         A3.e(i, k) = 0;
00888         for (int j = 0; j < i; j++)
00889           A3.e(i, k) += A1.ei(i, j) * A2.e(j, k);
00890         for (int j = i; j < A1.cols(); j++)
00891           A3.e(i, k) += A1.ej(i, j) * A2.e(j, k);
00892       }
00893     }
00894   }
00895   template <class Row1, class Row2, class Row3, class AT>
00896   inline void mult(const Matrix<Diagonal, Row1, Row1, AT> &A1, const Matrix<Diagonal, Row2, Row2, AT> &A2, Matrix<Diagonal, Row3, Row3, AT> &A3) {
00897 #ifndef FMATVEC_NO_SIZE_CHECK
00898     assert(A1.size() == A2.size());
00899 #endif
00900     for (int i = 0; i < A3.size(); i++)
00901       A3.e(i) = A1.e(i) * A2.e(i);
00902   }
00903 
00904   template <class AT, class Type1, class Type2, class Row1, class Col1, class Row2, class Col2>
00905   inline Matrix<General, Var, Var, AT> operator*(const Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2) {
00906     Matrix<General, Var, Var, AT> A3(A1.rows(), A2.cols(), NONINIT);
00907     mult(A1, A2, A3);
00908     return A3;
00909   }
00910   template <class AT, class Type1, class Type2, class Row, class Col>
00911   inline Matrix<General, Row, Col, AT> operator*(const Matrix<Type1, Row, Col, AT> &A1, const Matrix<Type2, Row, Col, AT> &A2) {
00912     Matrix<General, Row, Col, AT> A3(A1.rows(), A2.cols(), NONINIT);
00913     mult(A1, A2, A3);
00914     return A3;
00915   }
00916   template <class AT, int M, class Type1, class Type2, class Col1, class Row2, class Col2>
00917   inline Matrix<General, Fixed<M>, Var, AT> operator*(const Matrix<Type1, Fixed<M>, Col1, AT> &A1, const Matrix<Type2, Row2, Col2, AT> &A2) {
00918     Matrix<General, Fixed<M>, Var, AT> A3(A1.rows(), A2.cols(), NONINIT);
00919     mult(A1, A2, A3);
00920     return A3;
00921   }
00922   template <class AT, int N, class Type1, class Row1, class Col1, class Type2, class Row2>
00923   inline Matrix<General, Var, Fixed<N>, AT> operator*(const Matrix<Type1, Row1, Col1, AT> &A1, const Matrix<Type2, Row2, Fixed<N>, AT> &A2) {
00924     Matrix<General, Var, Fixed<N>, AT> A3(A1.rows(), A2.cols(), NONINIT);
00925     mult(A1, A2, A3);
00926     return A3;
00927   }
00928   template <class AT, int M, int N, class Type1, class Type2, class Col1, class Row2>
00929   inline Matrix<General, Fixed<M>, Fixed<N>, AT> operator*(const Matrix<Type1, Fixed<M>, Col1, AT> &A1, const Matrix<Type2, Row2, Fixed<N>, AT> &A2) {
00930     Matrix<General, Fixed<M>, Fixed<N>, AT> A3(A1.rows(), A2.cols(), NONINIT);
00931     mult(A1, A2, A3);
00932     return A3;
00933   }
00934   template <class AT, int M, class Type1, class Type2>
00935   inline Matrix<General, Fixed<M>, Fixed<M>, AT> operator*(const Matrix<Type1, Fixed<M>, Fixed<M>, AT> &A1, const Matrix<Type2, Fixed<M>, Fixed<M>, AT> &A2) {
00936     Matrix<General, Fixed<M>, Fixed<M>, AT> A3(A1.rows(), A2.cols(), NONINIT);
00937     mult(A1, A2, A3);
00938     return A3;
00939   }
00940   template <class AT, class Type, class Row1, class Col1, class Row2, class Col2>
00941   inline Matrix<Type, Var, Var, AT> operator*(const Matrix<Type, Row1, Col1, AT> &A1, const Matrix<Type, Row2, Col2, AT> &A2) {
00942     Matrix<Type, Var, Var, AT> A3(A1.rows(), A2.cols(), NONINIT);
00943     mult(A1, A2, A3);
00944     return A3;
00945   }
00946   template <class AT, class Type, class Row, class Col>
00947   inline Matrix<Type, Row, Col, AT> operator*(const Matrix<Type, Row, Col, AT> &A1, const Matrix<Type, Row, Col, AT> &A2) {
00948     Matrix<Type, Row, Col, AT> A3(A1.rows(), A2.cols(), NONINIT);
00949     mult(A1, A2, A3);
00950     return A3;
00951   }
00952   template <class AT, int M, class Type, class Col1, class Row2, class Col2>
00953   inline Matrix<Type, Fixed<M>, Var, AT> operator*(const Matrix<Type, Fixed<M>, Col1, AT> &A1, const Matrix<Type, Row2, Col2, AT> &A2) {
00954     Matrix<Type, Fixed<M>, Var, AT> A3(A1.rows(), A2.cols(), NONINIT);
00955     mult(A1, A2, A3);
00956     return A3;
00957   }
00958   template <class AT, int N, class Type, class Row1, class Col1, class Row2>
00959   inline Matrix<Type, Var, Fixed<N>, AT> operator*(const Matrix<Type, Row1, Col1, AT> &A1, const Matrix<Type, Row2, Fixed<N>, AT> &A2) {
00960     Matrix<Type, Var, Fixed<N>, AT> A3(A1.rows(), A2.cols(), NONINIT);
00961     mult(A1, A2, A3);
00962     return A3;
00963   }
00964   template <class AT, int M, int N, class Type, class Col1, class Row2>
00965   inline Matrix<Type, Fixed<M>, Fixed<N>, AT> operator*(const Matrix<Type, Fixed<M>, Col1, AT> &A1, const Matrix<Type, Row2, Fixed<N>, AT> &A2) {
00966     Matrix<Type, Fixed<M>, Fixed<N>, AT> A3(A1.rows(), A2.cols(), NONINIT);
00967     mult(A1, A2, A3);
00968     return A3;
00969   }
00970   template <class AT, int M, class Type>
00971   inline Matrix<Type, Fixed<M>, Fixed<M>, AT> operator*(const Matrix<Type, Fixed<M>, Fixed<M>, AT> &A1, const Matrix<Type, Fixed<M>, Fixed<M>, AT> &A2) {
00972     Matrix<Type, Fixed<M>, Fixed<M>, AT> A3(A1.rows(), A2.cols(), NONINIT);
00973     mult(A1, A2, A3);
00974     return A3;
00975   }
00976   
00977 //  SquareMatrix-SquareMatrix
00978   template <class AT, class Row1, class Row2>
00979   inline SquareMatrix<Var, AT> operator*(const SquareMatrix<Row1, AT> &A1, const SquareMatrix<Row2, AT> &A2) {
00980     SquareMatrix<Var, AT> A3(A1.size(), NONINIT);
00981     mult(A1, A2, A3);
00982     return A3;
00983   }
00984   template <class AT, class Row>
00985   inline SquareMatrix<Row, AT> operator*(const SquareMatrix<Row, AT> &A1, const SquareMatrix<Row, AT> &A2) {
00986     SquareMatrix<Row, AT> A3(A1.size(), NONINIT);
00987     mult(A1, A2, A3);
00988     return A3;
00989   }
00990 
00991   // SymmetricMatrix-SymmetricMatrix
00992   template <class AT, class Row1, class Row2>
00993   inline Matrix<General, Var, Var, AT> operator*(const Matrix<Symmetric, Row1, Row1, AT> &A1, const Matrix<Symmetric, Row2, Row2, AT> &A2) {
00994     Matrix<General, Var, Var, AT> A3(A1.rows(), A2.cols(), NONINIT);
00995     mult(A1, A2, A3);
00996     return A3;
00997   }
00998   template <class AT, class Row>
00999   inline Matrix<General, Row, Row, AT> operator*(const Matrix<Symmetric, Row, Row, AT> &A1, const Matrix<Symmetric, Row, Row, AT> &A2) {
01000     Matrix<General, Row, Row, AT> A3(A1.rows(), A2.cols(), NONINIT);
01001     mult(A1, A2, A3);
01002     return A3;
01003   }
01004 
01006 
01013   template <class Row, class AT>
01014   Vector<Row, AT> operator*(const Vector<Row, AT> &x, const AT& alpha) {
01015 
01016     Vector<Row, AT> y(x.size(), NONINIT);
01017 
01018     for (int i = 0; i < x.size(); i++)
01019       y.e(i) = x.e(i) * alpha;
01020 
01021     return y;
01022   }
01023 
01028   template <class Row, class AT>
01029   Vector<Row, AT> operator*(const AT& alpha, const Vector<Row, AT> &x) {
01030 
01031     Vector<Row, AT> y(x.size(), NONINIT);
01032 
01033     for (int i = 0; i < x.size(); i++)
01034       y.e(i) = x.e(i) * alpha;
01035 
01036     return y;
01037   }
01038 
01039   template <class Row, class AT>
01040   inline Vector<Row, AT> operator*=(const Vector<Row, AT> &x_, const AT &alpha) {
01041     Vector<Row, AT> &x = const_cast<Vector<Row, AT> &>(x_);
01042     for (int i = 0; i < x.size(); i++)
01043       x.e(i) *= alpha;
01044     return x;
01045   }
01046 
01047   template <class Row, class AT>
01048   inline Vector<Row, AT> operator/(const Vector<Row, AT> &x, const AT &alpha) {
01049     Vector<Row, AT> y(x.size(), NONINIT);
01050     for (int i = 0; i < x.size(); i++)
01051       y.e(i) = x.e(i) / alpha;
01052     return y;
01053   }
01054 
01055   template <class Row, class AT>
01056   inline Vector<Row, AT> operator/=(const Vector<Row, AT> &x_, const AT &alpha) {
01057     Vector<Row, AT> &x = const_cast<Vector<Row, AT> &>(x_);
01058     for (int i = 0; i < x.size(); i++)
01059       x.e(i) /= alpha;
01060     return x;
01061   }
01068   template <class Col, class AT>
01069   RowVector<Col, AT> operator*(const RowVector<Col, AT> &x, const AT& alpha) {
01070 
01071     RowVector<Col, AT> y(x.size(), NONINIT);
01072 
01073     for (int i = 0; i < x.size(); i++)
01074       y.e(i) = x.e(i) * alpha;
01075 
01076     return y;
01077   }
01078 
01083   template <class Col, class AT>
01084   RowVector<Col, AT> operator*(const AT &alpha, const RowVector<Col, AT> &x) {
01085 
01086     RowVector<Col, AT> y(x.size(), NONINIT);
01087 
01088     for (int i = 0; i < x.size(); i++)
01089       y.e(i) = x.e(i) * alpha;
01090 
01091     return y;
01092   }
01093 
01094   template <class Col, class AT>
01095   inline RowVector<Col, AT> operator*=(const RowVector<Col, AT> &x_, const AT &alpha) {
01096     RowVector<Col, AT> &x = const_cast<RowVector<Col, AT> &>(x_);
01097     for (int i = 0; i < x.size(); i++)
01098       x.e(i) *= alpha;
01099     return x;
01100   }
01101 
01102   template <class Col, class AT>
01103   inline RowVector<Col, AT> operator/(const RowVector<Col, AT> &x, const AT &a) {
01104     RowVector<Col, AT> y(x.size(), NONINIT);
01105     for (int i = 0; i < x.size(); i++)
01106       y.e(i) = x.e(i) / a;
01107     return y;
01108   }
01109 
01110   template <class Col, class AT>
01111   inline RowVector<Col, AT> operator/=(const RowVector<Col, AT> &x_, const AT &a) {
01112     RowVector<Col, AT> &x = const_cast<RowVector<Col, AT> &>(x_);
01113     for (int i = 0; i < x.size(); i++)
01114       x.e(i) /= a;
01115     return x;
01116   }
01117 
01119 
01121 
01128   template <class Col, class Row, class AT>
01129   AT operator*(const RowVector<Col, AT> &x, const Vector<Row, AT> &y) {
01130 
01131 #ifndef FMATVEC_NO_SIZE_CHECK
01132     assert(x.size() == y.size());
01133 #endif
01134 
01135     AT res = 0;
01136 
01137     for (int i = 0; i < x.size(); i++)
01138       res += x.e(i) * y.e(i);
01139 
01140     return res;
01141   }
01142 
01148   template <class Row, class AT>
01149   AT scalarProduct(const Vector<Row, AT> &x, const Vector<Row, AT> &y) {
01150 
01151 #ifndef FMATVEC_NO_SIZE_CHECK
01152     assert(x.size() == y.size());
01153 #endif
01154 
01155     AT res = 0;
01156 
01157     for (int i = 0; i < x.size(); i++)
01158       res += x.e(i) * y.e(i);
01159 
01160     return res;
01161   }
01162 
01168   template <class Row, class AT>
01169   Vector<Row, AT> crossProduct(const Vector<Row, AT> &x, const Vector<Row, AT> &y) {
01170 
01171 #ifndef FMATVEC_NO_SIZE_CHECK
01172     assert(x.size() == 3);
01173     assert(y.size() == 3);
01174 #endif
01175 
01176     Vector<Row, AT> z(3, NONINIT);
01177 
01178     z.e(0) = x.e(1) * y.e(2) - x.e(2) * y.e(1);
01179     z.e(1) = x.e(2) * y.e(0) - x.e(0) * y.e(2);
01180     z.e(2) = x.e(0) * y.e(1) - x.e(1) * y.e(0);
01181 
01182     return z;
01183   }
01184 
01190   template <class Row, class AT>
01191   double tripleProduct(const Vector<Row, AT> &a, const Vector<Row, AT> &x, const Vector<Row, AT> &y) {
01192 
01193 #ifndef FMATVEC_NO_SIZE_CHECK
01194     assert(a.size() == 3);
01195     assert(x.size() == 3);
01196     assert(y.size() == 3);
01197 #endif
01198 
01199     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));
01200   }
01201 
01203   //
01205 
01214   template <class Type, class Row, class Col, class AT>
01215   Matrix<Type, Row, Col, AT> operator*(const Matrix<Type, Row, Col, AT> &A, const AT &alpha) {
01216 
01217     Matrix<Type, Row, Col, AT> B(A.rows(), A.cols(), NONINIT);
01218 
01219     for (int i = 0; i < A.rows(); i++)
01220       for (int j = 0; j < A.cols(); j++)
01221         B.e(i, j) = A.e(i, j) * alpha;
01222 
01223     return B;
01224   }
01225 
01226   template <class Type, class Row, class Col, class AT>
01227   Matrix<Type, Row, Col, AT> operator*(const AT &alpha, const Matrix<Type, Row, Col, AT> &A) {
01228 
01229     Matrix<Type, Row, Col, AT> B(A.rows(), A.cols(), NONINIT);
01230 
01231     for (int i = 0; i < A.rows(); i++)
01232       for (int j = 0; j < A.cols(); j++)
01233         B.e(i, j) = A.e(i, j) * alpha;
01234 
01235     return B;
01236   }
01237 
01238   template <class Row, class AT>
01239   inline Matrix<Symmetric, Row, Row, AT> operator*(const AT &alpha, const Matrix<Symmetric, Row, Row, AT> &A) {
01240     Matrix<Symmetric, Row, Row, AT> B(A.size(), A.size(), NONINIT);
01241     for (int i = 0; i < A.size(); i++)
01242       for (int j = i; j < A.size(); j++)
01243         B.ej(i, j) = A.ej(i, j) * alpha;
01244     return B;
01245   }
01246 
01247   template <class Row, class AT>
01248   inline Matrix<Symmetric, Row, Row, AT> operator*(const Matrix<Symmetric, Row, Row, AT> &A, const AT &alpha) {
01249     Matrix<Symmetric, Row, Row, AT> B(A.size(), A.size(), NONINIT);
01250     for (int i = 0; i < A.size(); i++)
01251       for (int j = i; j < A.size(); j++)
01252         B.ej(i, j) = A.ej(i, j) * alpha;
01253     return B;
01254   }
01255 
01256   template <class Row, class AT>
01257   inline Matrix<Diagonal, Row, Row, AT> operator*(const AT &alpha, const Matrix<Diagonal, Row, Row, AT> &A) {
01258     Matrix<Diagonal, Row, Row, AT> B(A.size(), A.size(), NONINIT);
01259     for (int i = 0; i < A.size(); i++)
01260       B.e(i) = A.e(i) * alpha;
01261     return B;
01262   }
01263 
01264   template <class Row, class AT>
01265   inline Matrix<Diagonal, Row, Row, AT> operator*(const Matrix<Diagonal, Row, Row, AT> &A, const AT &alpha) {
01266     Matrix<Diagonal, Row, Row, AT> B(A.size(), A.size(), NONINIT);
01267     for (int i = 0; i < A.size(); i++)
01268       B.e(i) = A.e(i) * alpha;
01269     return B;
01270   }
01271 
01272   template <class Type, class Row, class Col, class AT>
01273   Matrix<Type, Row, Col, AT> operator/(const Matrix<Type, Row, Col, AT> &A, const AT &alpha) {
01274 
01275     Matrix<Type, Row, Col, AT> B(A.rows(), A.cols(), NONINIT);
01276 
01277     for (int i = 0; i < A.rows(); i++)
01278       for (int j = 0; j < A.cols(); j++)
01279         B.e(i, j) = A.e(i, j) / alpha;
01280 
01281     return B;
01282   }
01283 
01284   template <class Row, class AT>
01285   inline Matrix<Symmetric, Row, Row, AT> operator/(const Matrix<Symmetric, Row, Row, AT> &A, const AT &alpha) {
01286     Matrix<Symmetric, Row, Row, AT> B(A.size(), A.size(), NONINIT);
01287     for (int i = 0; i < A.size(); i++)
01288       for (int j = i; j < A.size(); j++)
01289         B.ej(i, j) = A.ej(i, j) / alpha;
01290     return B;
01291   }
01292   
01293   template <class Row, class AT>
01294   inline Matrix<Diagonal, Row, Row, AT> operator/(const Matrix<Diagonal, Row, Row, AT> &A, const AT &alpha) {
01295     Matrix<Diagonal, Row, Row, AT> B(A.size(), A.size(), NONINIT);
01296     for (int i = 0; i < A.size(); i++)
01297       B.e(i) = A.e(i) / alpha;
01298     return B;
01299   }
01300 
01301   template <class Row, class AT>
01302   SquareMatrix<Row, AT> operator*(const SquareMatrix<Row, AT> &A, const AT &alpha) {
01303 
01304     SquareMatrix<Row, AT> B(A.size(), NONINIT);
01305 
01306     for (int i = 0; i < A.size(); i++)
01307       for (int j = 0; j < A.size(); j++)
01308         B.e(i, j) = A.e(i, j) * alpha;
01309 
01310     return B;
01311   }
01312 
01313   template <class Row, class AT>
01314   SquareMatrix<Row, AT> operator*(const AT &alpha, const SquareMatrix<Row, AT> &A) {
01315 
01316     SquareMatrix<Row, AT> B(A.size(), NONINIT);
01317 
01318     for (int i = 0; i < A.size(); i++)
01319       for (int j = 0; j < A.size(); j++)
01320         B.e(i, j) = A.e(i, j) * alpha;
01321 
01322     return B;
01323   }
01324 
01325   template <class Row, class AT>
01326   SquareMatrix<Row, AT> operator/(const SquareMatrix<Row, AT> &A, const AT &alpha) {
01327 
01328     SquareMatrix<Row, AT> B(A.size(), NONINIT);
01329 
01330     for (int i = 0; i < A.size(); i++)
01331       for (int j = 0; j < A.size(); j++)
01332         B.e(i, j) = A.e(i, j) / alpha;
01333 
01334     return B;
01335   }
01336 
01337   template <class Type, class Row, class Col, class AT>
01338   Matrix<Type, Row, Col, AT> operator*=(const Matrix<Type, Row, Col, AT> &A_, const AT &alpha) {
01339     Matrix<Type, Row, Col, AT> A = const_cast<Matrix<Type, Row, Col, AT> &>(A_);
01340     for (int i = 0; i < A.rows(); i++)
01341       for (int j = 0; j < A.cols(); j++)
01342         A.e(i, j) *= alpha;
01343 
01344     return A;
01345   }
01346 
01347   template <class Row, class AT>
01348   inline Matrix<Symmetric, Row, Row, AT> operator*=(const Matrix<Symmetric, Row, Row, AT> &A_, const AT &alpha) {
01349     Matrix<Symmetric, Row, Row, AT> A = const_cast<Matrix<Symmetric, Row, Row, AT> &>(A_);
01350     for (int i = 0; i < A.size(); i++)
01351       for (int j = i; j < A.size(); j++)
01352         A.ej(i, j) *= alpha;
01353     return A;
01354   }
01355 
01356   template <class Row, class AT>
01357   inline Matrix<Diagonal, Row, Row, AT> operator*=(const Matrix<Diagonal, Row, Row, AT> &A_, const AT &alpha) {
01358     Matrix<Diagonal, Row, Row, AT> A = const_cast<Matrix<Diagonal, Row, Row, AT> &>(A_);
01359     for (int i = 0; i < A.size(); i++)
01360       A.e(i) *= alpha;
01361     return A;
01362   }
01363 
01364   template <class Type, class Row, class Col, class AT>
01365   Matrix<Type, Row, Col, AT> operator/=(const Matrix<Type, Row, Col, AT> &A_, const AT &alpha) {
01366     Matrix<Type, Row, Col, AT> A = const_cast<Matrix<Type, Row, Col, AT> &>(A_);
01367     for (int i = 0; i < A.rows(); i++)
01368       for (int j = 0; j < A.cols(); j++)
01369         A.e(i, j) /= alpha;
01370 
01371     return A;
01372   }
01373 
01374   template <class Row, class AT>
01375   inline Matrix<Symmetric, Row, Row, AT> operator/=(const Matrix<Symmetric, Row, Row, AT> &A_, const AT &alpha) {
01376     Matrix<Symmetric, Row, Row, AT> A = const_cast<Matrix<Symmetric, Row, Row, AT> &>(A_);
01377     for (int i = 0; i < A.size(); i++)
01378       for (int j = i; j < A.size(); j++)
01379         A.ej(i, j) /= alpha;
01380     return A;
01381   }
01382 
01383   template <class Row, class AT>
01384   inline Matrix<Diagonal, Row, Row, AT> operator/=(const Matrix<Diagonal, Row, Row, AT> &A_, const AT &alpha) {
01385     Matrix<Diagonal, Row, Row, AT> A = const_cast<Matrix<Diagonal, Row, Row, AT> &>(A_);
01386     for (int i = 0; i < A.size(); i++)
01387       A.e(i) /= alpha;
01388     return A;
01389   }
01390 
01392   
01394 
01400   template <class Row, class AT>
01401   Vector<Row, AT> operator-(const Vector<Row, AT> &x) {
01402 
01403     Vector<Row, AT> y(x.size(), NONINIT);
01404 
01405     for (int i = 0; i < x.size(); i++)
01406       y.e(i) = -x.e(i);
01407 
01408     return y;
01409   }
01410 
01416   template <class Col, class AT>
01417   RowVector<Col, AT> operator-(const RowVector<Col, AT> &a) {
01418 
01419     RowVector<Col, AT> c(a.size(), NONINIT);
01420 
01421     for (int i = 0; i < a.size(); i++)
01422       c.e(i) = -a.e(i);
01423 
01424     return c;
01425   }
01426 
01432   template <class Row, class AT>
01433   SquareMatrix<Row, AT> operator-(const SquareMatrix<Row, AT> &A) {
01434 
01435     SquareMatrix<Row, AT> B(A.size(), NONINIT);
01436 
01437     for (int i = 0; i < A.size(); i++)
01438       for (int j = 0; j < A.size(); j++)
01439         B.e(i, j) = -A.e(i, j);
01440 
01441     return B;
01442   }
01443 
01449   template <class Type, class Row, class Col, class AT>
01450   Matrix<Type, Row, Col, AT> operator-(const Matrix<Type, Row, Col, AT> &A) {
01451 
01452     Matrix<Type, Row, Col, AT> B(A.rows(), A.cols(), NONINIT);
01453 
01454     for (int i = 0; i < A.rows(); i++)
01455       for (int j = 0; j < A.cols(); j++)
01456         B.e(i, j) = -A.e(i, j);
01457 
01458     return B;
01459   }
01460 
01463 
01469   template <class AT>
01470   RowVector<Ref, AT> trans(const Vector<Ref, AT> &x) {
01471 
01472     return RowVector<Ref, AT>(x.m, x.lda, x.tp ? false : true, x.memory, x.ele).copy();
01473   }
01474 
01482   template <class AT>
01483   Vector<Ref, AT> trans(const RowVector<Ref, AT> &x) {
01484 
01485     return Vector<Ref, AT>(x.n, x.lda, x.tp ? false : true, x.memory, x.ele).copy();
01486   }
01487 
01496   template <class AT>
01497   Matrix<General, Ref, Ref, AT> trans(const Matrix<General, Ref, Ref, AT> &A) {
01498 
01499     return Matrix<General, Ref, Ref, AT>(A.n, A.m, A.lda, A.tp ? false : true, A.memory, A.ele).copy();
01500   }
01501 
01510   template <class AT>
01511   SquareMatrix<Ref, AT> trans(const SquareMatrix<Ref, AT> &A) {
01512 
01513     return SquareMatrix<Ref, AT>(A.n, A.lda, A.tp ? false : true, A.memory, A.ele).copy();
01514   }
01515 
01516   template <class Type, class Row, class Col, class AT>
01517   inline Matrix<Type, Col, Row, AT> trans(const Matrix<Type, Row, Col, AT> &A) {
01518     Matrix<Type, Col, Row, AT> B(A.cols(), A.rows(), NONINIT);
01519     for (int i = 0; i < B.rows(); i++)
01520       for (int j = 0; j < B.cols(); j++)
01521         B.e(i, j) = A.e(j, i);
01522     return B;
01523   }
01524 
01525   template <class Row, class AT>
01526   inline SquareMatrix<Row, AT> trans(const SquareMatrix<Row, AT> &A) {
01527     SquareMatrix<Row, AT> B(A.size(), NONINIT);
01528     for (int i = 0; i < B.size(); i++)
01529       for (int j = 0; j < B.size(); j++)
01530         B.e(i, j) = A.e(j, i);
01531     return B;
01532   }
01533 
01534   template <class Row, class AT>
01535   inline RowVector<Row, AT> trans(const Vector<Row, AT> &x) {
01536     RowVector<Row, AT> y(x.cols(), NONINIT);
01537     for (int i = 0; i < y.size(); i++)
01538       y.e(i) = x.e(i);
01539     return y;
01540   }
01541 
01542   template <class Row, class AT>
01543   inline Vector<Row, AT> trans(const RowVector<Row, AT> &x) {
01544     Vector<Row, AT> y(x.cols(), NONINIT);
01545     for (int i = 0; i < y.size(); i++)
01546       y.e(i) = x.e(i);
01547     return y;
01548   }
01549 
01551 
01553   template <class Row, class AT>
01554   inline AT nrmInf(const Vector<Row, AT> &x) {
01555     AT c = 0;
01556     for (int i = 0; i < x.size(); i++)
01557       if (c < fabs(x.e(i)))
01558         c = fabs(x.e(i));
01559     return c;
01560   }
01561 
01562   template <class Row, class AT>
01563   inline AT nrmInf(const RowVector<Row, AT> &x) {
01564     AT c = 0;
01565     for (int i = 0; i < x.size(); i++)
01566       if (c < fabs(x.e(i)))
01567         c = fabs(x.e(i));
01568     return c;
01569   }
01570 
01571   template <class Row, class AT>
01572   inline AT nrm2(const Vector<Row, AT> &x) {
01573     AT c = 0;
01574     for (int i = 0; i < x.size(); i++)
01575       c += pow(x.e(i), 2);
01576     return sqrt(c);
01577   }
01578 
01579   template <class Col, class AT>
01580   inline AT nrm2(const RowVector<Col, AT> &x) {
01581     AT c = 0;
01582     for (int i = 0; i < x.size(); i++)
01583       c += pow(x.e(i), 2);
01584     return sqrt(c);
01585   }
01586 
01588 
01600   template <class Row, class AT>
01601   SquareMatrix<Row, AT> tilde(const Vector<Row, AT> &x) {
01602 
01603     SquareMatrix<Row, AT> B(x.size(), NONINIT);
01604 
01605     B.e(0, 0) = 0;
01606     B.e(1, 1) = 0;
01607     B.e(2, 2) = 0;
01608     B.e(0, 1) = -x.e(2);
01609     B.e(0, 2) = x.e(1);
01610     B.e(1, 0) = x.e(2);
01611     B.e(1, 2) = -x.e(0);
01612     B.e(2, 0) = -x.e(1);
01613     B.e(2, 1) = x.e(0);
01614 
01615     return B;
01616   }
01617 
01623   template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
01624   bool operator==(const Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
01625 
01626     if (&A == &B)
01627       return true;
01628 
01629     if (A.rows() != B.rows() || A.cols() != B.cols())
01630       return false;
01631 
01632     for (int i = 0; i < A.rows(); i++)
01633       for (int j = 0; j < A.cols(); j++)
01634         if (A(i, j) != B(i, j))
01635           return false;
01636 
01637     return true;
01638   }
01639 
01645   template <class AT, class Type1, class Row1, class Col1, class Type2, class Row2, class Col2>
01646   bool operator!=(const Matrix<Type1, Row1, Col1, AT> &A, const Matrix<Type2, Row2, Col2, AT> &B) {
01647 
01648     if (A.rows() != B.rows() || A.cols() != B.cols())
01649       return true;
01650 
01651     for (int i = 0; i < A.rows(); i++)
01652       for (int j = 0; j < A.cols(); j++)
01653         if (A(i, j) != B(i, j))
01654           return true;
01655 
01656     return false;
01657   }
01658 
01664   template <class Row, class AT>
01665   AT max(const Vector<Row, AT> &x) {
01666 
01667 #ifndef FMATVEC_NO_SIZE_CHECK
01668     assert(x.size() > 0);
01669 #endif
01670     AT maximum = x.e(0);
01671     for (int i = 1; i < x.size(); i++) {
01672       if (x.e(i) > maximum)
01673         maximum = x.e(i);
01674     }
01675     return maximum;
01676   }
01677 
01683   template <class Row, class AT>
01684   int maxIndex(const Vector<Row, AT> &x) {
01685 
01686 #ifndef FMATVEC_NO_SIZE_CHECK
01687     assert(x.size() > 0);
01688 #endif
01689     AT maximum = x.e(0);
01690     int index = 0;
01691     for (int i = 1; i < x.size(); i++) {
01692       if (x.e(i) > maximum) {
01693         maximum = x.e(i);
01694         index = i;
01695       }
01696     }
01697     return index;
01698   }
01699 
01705   template <class Row, class AT>
01706   AT min(const Vector<Row, AT> &x) {
01707 
01708 #ifndef FMATVEC_NO_SIZE_CHECK
01709     assert(x.size() > 0);
01710 #endif
01711     AT minimum = x.e(0);
01712     for (int i = 1; i < x.size(); i++) {
01713       if (x.e(i) < minimum)
01714         minimum = x.e(i);
01715     }
01716     return minimum;
01717   }
01718 
01724   template <class Row, class AT>
01725   int minIndex(const Vector<Row, AT> &x) {
01726 
01727 #ifndef FMATVEC_NO_SIZE_CHECK
01728     assert(x.size() > 0);
01729 #endif
01730     AT minimum = x.e(0);
01731     int index = 0;
01732     for (int i = 1; i < x.size(); i++) {
01733       if (x.e(i) < minimum) {
01734         minimum = x.e(i);
01735         index = i;
01736       }
01737     }
01738     return index;
01739   }
01740 
01741   // HR 28.09.2006
01748   template <class AT>
01749   Matrix<General, Ref, Ref, AT> bubbleSort(const Matrix<General, Ref, Ref, AT> &A_, int PivotCol) {
01750 
01751 #ifndef FMATVEC_NO_SIZE_CHECK
01752     assert(A_.rows() > 0);
01753     assert(A_.cols() > 0);
01754     assert(A_.cols() > PivotCol);
01755 #endif
01756     Matrix<General, Ref, Ref, AT> A = A_.copy();
01757     int i, j, N;
01758     RowVector<Ref, AT> tmp(A.cols(), NONINIT);
01759     N = A.rows();
01760     for (i = 1; i <= N - 1; i++) {
01761       for (j = 0; j < N - 1; j++) {
01762         if (A(j, PivotCol) > A(j + 1, PivotCol)) {
01763           tmp = A.row(j);
01764           A.row(j) = A.row(j + 1);
01765           A.row(j + 1) = tmp;
01766         }
01767       }
01768     }
01769     return A;
01770   }
01771 
01772   // HR 28.09.2006
01774   template <class AT>
01775   void quicksortmedian_intern(Matrix<General, Ref, Ref, AT> &A, int PivotCol, RowVector<Ref, AT> &tmp, int l, int r) {
01776 
01777     if (r > l) {
01778       int i = l - 1, j = r;
01779       //RowVec tmp;
01780       if (r - l > 3) { //Median of three
01781         int m = l + (r - l) / 2;
01782         if (A(l, PivotCol) > A(m, PivotCol)) {
01783           tmp = A.row(l);
01784           A.row(l) = A.row(m);
01785           A.row(m) = tmp;
01786         }
01787         if (A(l, PivotCol) > A(r, PivotCol)) {
01788           tmp = A.row(l);
01789           A.row(l) = A.row(r);
01790           A.row(r) = tmp;
01791         }
01792         else if (A(r, PivotCol) > A(m, PivotCol)) {
01793           tmp = A.row(r);
01794           A.row(r) = A.row(m);
01795           A.row(m) = tmp;
01796         }
01797       }
01798 
01799       for (;;) {
01800           while(A(++i,PivotCol) < A(r,PivotCol));
01801           while(A(--j,PivotCol) > A(r,PivotCol) && j>i);
01802           if(i>=j) break;
01803         tmp = A.row(i);
01804         A.row(i) = A.row(j);
01805         A.row(j) = tmp;
01806       }
01807       tmp = A.row(i);
01808       A.row(i) = A.row(r);
01809       A.row(r) = tmp;
01810       quicksortmedian_intern(A, PivotCol, tmp, l, i - 1);
01811       quicksortmedian_intern(A, PivotCol, tmp, i + 1, r);
01812     }
01813   }
01814 
01815   // HR 28.09.2006
01824   template <class AT>
01825   Matrix<General, Ref, Ref, AT> quickSortMedian(const Matrix<General, Ref, Ref, AT> &A_, int PivotCol) {
01826     Matrix<General, Ref, Ref, AT> A = A_.copy();
01827     int N = A.rows();
01828     RowVector<Ref, AT> tmp(A.cols(), NONINIT);
01829     quicksortmedian_intern(A, PivotCol, tmp, 0, N - 1);
01830     return A;
01831   }
01832 
01839   template <class AT> int countElements(const SquareMatrix<Ref, AT> &A) {
01840     int k = 0;
01841     for (int i = 0; i < A.size(); i++) {
01842       for (int j = 0; j < A.size(); j++) {
01843         if (fabs(A(i, j)) > 1e-16 || i == j) {
01844           k++;
01845         }
01846       }
01847     }
01848     return k;
01849   }
01850 
01851   template <class Row, class Col, class AT>
01852   inline Matrix<Symmetric, Col, Col, AT> JTJ(const Matrix<General, Row, Col, AT> &A) {
01853     Matrix<Symmetric, Col, Col, AT> S(A.cols(), A.cols(), NONINIT);
01854     for (int i = 0; i < A.cols(); i++) {
01855       for (int k = i; k < A.cols(); k++) {
01856         S.ej(i, k) = 0;
01857         for (int j = 0; j < A.rows(); j++)
01858           S.ej(i, k) += A.e(j, i) * A.e(j, k);
01859       }
01860     }
01861     return S;
01862   }
01863 
01864   template <class Row, class Col, class AT>
01865   inline Matrix<Symmetric, Col, Col, AT> JTMJ(const Matrix<Symmetric, Row, Row, AT> &B, const Matrix<General, Row, Col, AT> &A) {
01866 
01867     Matrix<Symmetric, Col, Col, AT> S(A.cols(), A.cols(), NONINIT);
01868     Matrix<General, Row, Col, AT> C = B * A;
01869 
01870     for (int i = 0; i < A.cols(); i++) {
01871       for (int k = i; k < A.cols(); k++) {
01872         S.ej(i, k) = 0;
01873         for (int j = 0; j < A.rows(); j++)
01874           S.ej(i, k) += A.e(j, i) * C.e(j, k);
01875       }
01876     }
01877     return S;
01878   }
01879 
01880   template <class Row, class Col, class AT>
01881   inline Matrix<Symmetric, Row, Row, AT> JMJT(const Matrix<General, Row, Col, AT> &A, const Matrix<Symmetric, Col, Col, AT> &B) {
01882 
01883     Matrix<Symmetric, Row, Row, AT> S(A.rows(), A.rows(), NONINIT);
01884     Matrix<General, Row, Col, AT> C = A * B;
01885 
01886     for (int i = 0; i < S.size(); i++) {
01887       for (int k = i; k < S.size(); k++) {
01888         S.ej(i, k) = 0;
01889         for (int j = 0; j < A.cols(); j++)
01890           S.ej(i, k) += C.e(k, j) * A.e(i, j);
01891       }
01892     }
01893     return S;
01894   }
01895 
01896 }
01897 
01898 #endif