The MRPT project: CMatrixFixedNumeric.h Source File

00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2010  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CMatrixFixedNumeric_H
00029 #define CMatrixFixedNumeric_H
00030 
00031 #include <mrpt/math/CMatrix.h>
00032 #include <mrpt/math/CMatrixD.h>
00033 
00034 #include <mrpt/math/CArray.h>
00035 
00036 namespace mrpt
00037 {
00038         namespace math
00039         {
00040                 using namespace mrpt::system;
00041                 using namespace mrpt::poses;
00042                 using namespace detail;
00043 
00044                 // Forward declarations ----------------
00045                 template <typename MAT_H, typename MAT_C, typename MAT_R> void  multiply_HCHt(const MAT_H &H,const MAT_C &C,MAT_R &R, bool accumResultInOutput = false, bool allow_submatrix_mult = false  );
00046                 template <typename T, size_t N, size_t M> void  multiply_HtCH(const CMatrixFixedNumeric<T,M,N> &H,const CMatrixFixedNumeric<T,M,M> &C,CMatrixFixedNumeric<T,N,N>   &R,bool accumResultInOutput = false  );
00047                 template <typename T,size_t NROWS,size_t NCOLS> void  invMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv );
00048                 template <typename T,size_t NROWS,size_t NCOLS> void  invMatrix_destroySrc( CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv );
00049                 template <typename T,size_t NROWS,size_t NCOLS> void  multiply(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00050                 template <typename T,size_t NROWS,size_t NCOLS,size_t M1C> void  multiply(const CMatrixFixedNumeric<T,NROWS,M1C>& m1,const CMatrixFixedNumeric<T,M1C,NCOLS>& m2,CMatrixFixedNumeric<T,NROWS,NCOLS>& RESULT );
00051                 template <typename T,size_t NROWS,size_t NCOLS> void  multiply_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00052                 template <typename T,size_t NROWS,size_t NCOLS,size_t M1C> void  multiply_SIMD(const CMatrixFixedNumeric<T,NROWS,M1C>& m1,const CMatrixFixedNumeric<T,M1C,NCOLS>& m2,CMatrixFixedNumeric<T,NROWS,NCOLS>& RESULT );
00053                 template <typename T,size_t M1R,size_t M1C> void  multiply_AAt(const CMatrixFixedNumeric<T,M1R,M1C>& m1,CMatrixFixedNumeric<T,M1R,M1R>& RESULT );
00054                 template <typename T,size_t N,size_t M> void  multiply_Ab( const CMatrixFixedNumeric<T,N,M>& A, const std::vector<T>& a, std::vector<T>& out_v );
00055                 template <typename T,size_t NROWS,size_t NCOLS> void  sumInPlace(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00056                 template <typename T,size_t NROWS,size_t NCOLS> void  sumInPlace_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00057                 template <typename T,size_t NROWS,size_t NCOLS> void  sumInPlace(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00058                 template <typename T,size_t NROWS,size_t NCOLS> void  sumInPlace_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00059                 template <typename T,size_t NROWS,size_t NCOLS> void  substractInPlace(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00060                 template <typename T,size_t NROWS,size_t NCOLS> void  substractInPlace_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00061                 template <typename T,size_t NROWS,size_t NCOLS> T  sumMatrixAllElements( const CMatrixFixedNumeric<T,NROWS,NCOLS>& M );
00062                 template <typename T,size_t NROWS,size_t NCOLS> T  sumMatrixAllElements_SIMD( const CMatrixFixedNumeric<T,NROWS,NCOLS>& M );
00063                 template <typename T,size_t NROWS,size_t NCOLS> T  minimumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00064                 template <typename T,size_t NROWS,size_t NCOLS> T  minimumMatrix_SIMD(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00065                 template <typename T,size_t NROWS,size_t NCOLS> T  maximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00066                 template <typename T,size_t NROWS,size_t NCOLS> T  maximumMatrix_SIMD(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00067                 template <typename T,size_t NROWS,size_t NCOLS> void  minimumAndMaximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M, T &val_min, T &val_max);
00068                 template <typename T,size_t NROWS,size_t NCOLS> void  minimumAndMaximumMatrix_SIMD(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M, T &val_min, T &val_max);
00069                 template <typename T,size_t NROWS,size_t NCOLS> T  detMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00070                 template <typename T,size_t NROWS,size_t NCOLS> void  sqrtMatrix(CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00071                 template <typename T,size_t N> void  eigenVectorsMatrix(const CMatrixFixedNumeric<T,N,N> &M,CMatrixFixedNumeric<T,N,N> &Z,CMatrixFixedNumeric<T,N,N> &D );
00072 
00073                 // End of forward declarations ----------------
00074 
00075                 /** For usage in one of the constructors of CMatrixFixedNumeric, if it's not required
00076                      to fill it with zeros at the constructor to save time. */
00077                 #define UNITIALIZED_MATRIX  false,false
00078 
00079                 /**  A numeric matrix of compile-time fixed size.
00080                  *   The template can be instanced for data types "float" or "double"
00081                  *   Virtually all methods have specializations and/or SSE2 optimized implementations, so use this class when time is critical.
00082                  *
00083                  * \note To enable SSE2 optimizations, add the definition "#define MRPT_USE_SSE2" BEFORE including MRPT headers in your code. This is because these optimizations are only applicable to static matrix objects, but not when they are created in dynamic memory.
00084                  *
00085                  * \sa CMatrixTemplateNumeric (for dynamic-size matrices)
00086                  */
00087                 template <typename T,size_t NROWS,size_t NCOLS>
00088                 class CMatrixFixedNumeric    // Must have no "MRPTDLLIMPEXP"
00089                 {
00090                 public:
00091                         typedef T value_type;   //!< The type of the matrix elements
00092 
00093                         /** The stored data of the matrix: elements are saved by rows, left to right, from top to bottom. */
00094 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00095                         MRPT_ALIGN16
00096 #endif
00097                         // Use a CArray wrapper instead of a plain array so
00098                         //  it handles the cases of 0-length arrays without problems.
00099                         CArray<T,NROWS*NCOLS> m_Val;
00100 
00101                 public:
00102                         /** Default constructor, fills the whole matrix with zeros */
00103                         CMatrixFixedNumeric() {
00104 #if defined(_DEBUG) && MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00105                                 if ((uintptr_t(m_Val) & 0x0f) != 0 )
00106                                         THROW_EXCEPTION("16-unaligned memory!")
00107 #endif
00108                                 ::memset(&m_Val[0],0,sizeof(T)*NROWS*NCOLS);
00109                         }
00110 
00111                         /** Constructor which leaves the matrix uninitialized: it uses two bool arguments with ignored values, but they must be present to make the method signature distinctive and make sure that the user wants the matrix to be uninitialized (ie, leaving only one bool argument may lead to unintended conversions from bool values!)
00112                           *  Example of usage: CMatrixFixedNumeric<double,3,2> M(UNITIALIZED_MATRIX);
00113                           */
00114                         CMatrixFixedNumeric(bool ,bool ) {
00115                         }
00116 
00117                         /** Constructor from a given size and a C array. The array length must match cols x row.
00118                           * \code
00119                           *  const double numbers[] = {
00120                           *    1,2,3,
00121                           *    4,5,6 };
00122                           *      CMatrixFixedNumeric<double,3,2>    M(numbers);
00123                           * \endcode
00124                           */
00125                         template <typename V, size_t N>
00126                         CMatrixFixedNumeric ( V (&theArray)[N] )
00127                         {
00128                                 MRPT_COMPILE_TIME_ASSERT(N!=0)
00129                                 MRPT_COMPILE_TIME_ASSERT(N==NROWS * NCOLS)
00130                                 if (sizeof(V)==sizeof(T))
00131 					::memcpy(&m_Val[0],theArray,sizeof(m_Val));
00132                                 else
00133                                 for (size_t i=0;i<N;i++)
00134                                         m_Val[i] = static_cast<T>(theArray[i]);
00135                         }
00136 
00137                         /** Copy constructor from another matrix of a different size: it's explicit so matrices of different sizes are not mixed by mistake.
00138                           */
00139                         template <size_t N,size_t M>
00140                         explicit CMatrixFixedNumeric(const CMatrixFixedNumeric<T,N,M> &B)
00141                         {
00142                                 ::memset(&m_Val[0],0,sizeof(m_Val));
00143                                 const size_t nr = std::min(NROWS,N);
00144                                 const size_t nc = std::min(NCOLS,M);
00145                                 for (size_t r=0;r<nr;r++)
00146                                         ::memcpy(&m_Val[0]+NCOLS*r, &B.m_Val[0]+M*r, sizeof(T)*nc );
00147                         }
00148 
00149                         /** Copy constructor from another matrix of a different type: it's explicit so matrices of different types are not mixed by mistake.
00150                           */
00151                         template <typename R>
00152                         explicit CMatrixFixedNumeric(const CMatrixFixedNumeric<R,NROWS,NCOLS> &B)
00153                         {
00154                                 for (size_t r=0;r<NROWS;r++)
00155                                         for (size_t c=0;c<NCOLS;c++)
00156                                                 get_unsafe(r,c) = static_cast<T>( B.get_unsafe(r,c) );
00157                         }
00158 
00159                         /** Copy constructor from a dynamic-size matrix
00160                           *  An exception will be raised if the sizes do not match, unless "clipToFixedMatrixSize" is true.
00161                          */
00162                         template <typename R>
00163                         explicit CMatrixFixedNumeric(const CMatrixTemplate<R> &B, bool clipToFixedMatrixSize = false )
00164                         {
00165                                 if (!clipToFixedMatrixSize) {
00166                                         *this = B;
00167                                 }
00168                                 else
00169                                 {
00170                                         ::memset(&m_Val[0],0,sizeof(m_Val));
00171                                         const size_t nr = std::min(NROWS,B.getRowCount());
00172                                         const size_t nc = std::min(NCOLS,B.getColCount());
00173                                         for (size_t r=0;r<nr;r++)
00174                                                 for (size_t c=0;c<nc;c++)
00175                                                         get_unsafe(r,c) = B.get_unsafe(r,c);
00176                                 }
00177                         }
00178 
00179                         /** Conversion from a dynamic-size matrix to a fixed-size one.
00180                           * \exception std::exception On wrong sizes
00181                           */
00182                         CMatrixFixedNumeric<T,NROWS,NCOLS>& operator =(const CMatrixTemplate<T> &B)
00183                         {
00184                                 ASSERT_( NROWS==B.getRowCount() )
00185                                 ASSERT_( NCOLS==B.getColCount() )
00186                                 for (size_t r=0;r<NROWS;r++)
00187                                         ::memcpy(&m_Val[0]+r*NCOLS, B.get_unsafe_row(r), sizeof(T)*NCOLS);
00188                                 return *this;
00189                         }
00190 
00191                         /** Conversion from a dynamic-size matrix of a different data type to a fixed-size one.
00192                           * \exception std::exception On wrong sizes
00193                           */
00194                         template <typename R>
00195                         CMatrixFixedNumeric<T,NROWS,NCOLS>& operator =(const CMatrixTemplate<R> &B)
00196                         {
00197                                 ASSERT_( NROWS==B.getRowCount() )
00198                                 ASSERT_( NCOLS==B.getColCount() )
00199                                 for (size_t r=0;r<NROWS;r++)
00200                                         for (size_t c=0;c<NCOLS;c++)
00201                                                 get_unsafe(r,c) = B.get_unsafe(r,c);
00202                                 return *this;
00203                         }
00204 
00205                         /** Get number of rows */
00206                         static inline size_t getRowCount() { return NROWS; }
00207 
00208                         /** Get number of columns */
00209                         static inline size_t getColCount() { return NCOLS; }
00210 
00211                         /** Is square? */
00212                         static inline bool IsSquare() { return NROWS==NCOLS; }
00213 
00214                         /** This method has no effects in this class */
00215                         static inline void setSize(const size_t nRows, const size_t nCols) {
00216                                 if (nRows!=NROWS || nCols!=NCOLS)
00217                                         throw std::logic_error(format("Try to change the size of a %ux%u fixed-sized matrix to %ux%u.",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS),static_cast<unsigned>(nRows),static_cast<unsigned>(nCols)));
00218                         }
00219 
00220                         /** Assigns a pose to a 2x1 or 1x2 matrix */
00221                         CMatrixFixedNumeric(const CPoint2D &p) { matrixFromPoseOrPoint(*this,p); }
00222                         /** Assigns a pose to a 3x1 or 1x3 matrix */
00223                         CMatrixFixedNumeric(const CPoint3D &p) { matrixFromPoseOrPoint(*this,p); }
00224                         /** Assigns a pose to a 3x1 or 1x3 matrix */
00225                         CMatrixFixedNumeric(const CPose2D &p) { matrixFromPoseOrPoint(*this,p); }
00226                         /** Assigns a pose to a 6x1 or 1x6 matrix */
00227                         CMatrixFixedNumeric(const CPose3D &p) { matrixFromPoseOrPoint(*this,p); }
00228 
00229                         /** Assigns a pose to a 2x1 or 1x2 matrix */
00230                         CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPoint2D &p) {
00231                                 return matrixFromPoseOrPoint(*this,p);
00232                         }
00233                         /** Assigns a pose to a 3x1 or 1x3 matrix */
00234                         CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPoint3D &p) {
00235                                 return matrixFromPoseOrPoint(*this,p);
00236                         }
00237                         /** Assigns a pose to a 3x1 or 1x3 matrix */
00238                         CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPose2D &p) {
00239                                 return matrixFromPoseOrPoint(*this,p);
00240                         }
00241                         /** Assigns a pose to a 6x1 or 1x6 matrix */
00242                         CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPose3D &p) {
00243                                 return matrixFromPoseOrPoint(*this,p);
00244                         }
00245 
00246                         /** Make the matrix an identity matrix */
00247                         void unit() {
00248                                 ::memset(&m_Val[0],0,sizeof(m_Val));
00249                                 for (size_t i=0;i<NROWS * NCOLS;i+=(NROWS+1))
00250                                         m_Val[i] = 1;
00251                         }
00252 
00253                         /** Set all elements to zero */
00254                         void zeros() {
00255                                 ::memset(&m_Val[0],0,sizeof(m_Val));
00256                         }
00257 
00258                         /** Set all elements to zero */
00259                         void zeros(size_t N, size_t M) {
00260                                 ASSERT_(N==NROWS && M==NCOLS);
00261                                 this->zeros();
00262                         }
00263 
00264                         /** Read-only access to one element (Use with caution, bounds are not checked!) */
00265                         inline T get_unsafe(const size_t row, const size_t col) const {
00266 #ifdef _DEBUG
00267                                 if (row >= NROWS || col >= NCOLS)
00268                                         THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(NROWS),static_cast<unsigned long>(NCOLS)) );
00269 #endif
00270                                 return m_Val[NCOLS*row+col];
00271                         }
00272 
00273                         /** Reference access to one element (Use with caution, bounds are not checked!) */
00274                         inline T& get_unsafe(const size_t row, const size_t col) {
00275 #ifdef _DEBUG
00276                                 if (row >= NROWS || col >= NCOLS)
00277                                         THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(NROWS),static_cast<unsigned long>(NCOLS)) );
00278 #endif
00279                                 return m_Val[NCOLS*row+col];
00280                         }
00281 
00282                         /** Sets an element  (Use with caution, bounds are not checked!) */
00283                         inline void set_unsafe(const size_t row, const size_t col, const T val) {
00284 #ifdef _DEBUG
00285                                 if (row >= NROWS || col >= NCOLS)
00286                                         THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(NROWS),static_cast<unsigned long>(NCOLS)) );
00287 #endif
00288                                 m_Val[NCOLS*row+col] = val;
00289                         }
00290 
00291                         /** Subscript operator to get/set individual elements
00292                                 */
00293                         inline T& operator () (const size_t row, const size_t col)
00294                         {
00295                 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00296                                 if (row >= NROWS || col >= NCOLS)
00297                                         THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(NROWS),static_cast<unsigned long>(NCOLS)) );
00298                 #endif
00299                                 return m_Val[NCOLS*row+col];
00300                         }
00301 
00302                         /** Subscript operator to get/set individual elements
00303                                 */
00304                         inline T operator () (const size_t row, const size_t col) const
00305                         {
00306                 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00307                                 if (row >= NROWS || col >= NCOLS)
00308                                         THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(NROWS),static_cast<unsigned long>(NCOLS)) );
00309                 #endif
00310                                 return m_Val[NCOLS*row+col];
00311                         }
00312 
00313                         /** Gets the matrix as a string in matlab format, for example: [a11 a12 a12;a21 a22 a23]
00314                           * \sa fromMatlabStringFormat
00315                           */
00316                         std::string  inMatlabFormat() const
00317                         {
00318                                 std::stringstream  s;
00319                                 s << "[";
00320                                 s << std::scientific;
00321                                 for (size_t i=0;i<NROWS;i++)
00322                                 {
00323                                         for (size_t j=0;j<NCOLS;j++)
00324                                                 s << get_unsafe(i,j) << " ";
00325                                         if (i<NROWS-1)  s << ";";
00326                                 }
00327                                 s << "]";
00328                                 return s.str();
00329                         }
00330 
00331                         /** Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]"
00332                           *  The string must start with '[' and end with ']'. Rows are separated by semicolons ';' and
00333                           *  columns in each row by one or more whitespaces ' ' or tabs.
00334                           *
00335                           * This format is also used for CConfigFile::read_matrix.
00336                           *
00337                           *  This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
00338                           *
00339                           * \return true on success. false if the string is malformed, or it is of the wrong size.
00340                           * \sa inMatlabFormat, CConfigFile::read_matrix
00341                           */
00342                         bool fromMatlabStringFormat(const std::string &s)
00343                         {
00344                                 CMatrixTemplate<T>      M;
00345                                 if (!M.fromMatlabStringFormat(s)) return false;
00346                                 if (M.getColCount()!=NCOLS || M.getRowCount()!=NROWS) return false;
00347                                 *this = M;
00348                                 return true; // Ok
00349                         }
00350 
00351 
00352                         /** Returns the inverse of the matrix in "out_inv"
00353                           * \sa inv_fast
00354                           */
00355                         void inv(CMatrixFixedNumeric<T,NROWS,NCOLS>& out_inv) const {
00356                                 mrpt::math::invMatrix(*this,out_inv);
00357                         }
00358 
00359                         /** Returns the inverse of the matrix in "out_inv" , DESTROYING the current matrix
00360                           */
00361                         void inv_fast(CMatrixFixedNumeric<T,NROWS,NCOLS>& out_inv) {
00362                                 mrpt::math::invMatrix_destroySrc(*this,out_inv);
00363                         } // end of "inv_fast"
00364 
00365                         /** this = A*B */
00366                         template <size_t NC1>
00367                         void multiply(const CMatrixFixedNumeric<T,NROWS,NC1> &A, const CMatrixFixedNumeric<T,NC1,NCOLS> &B  ) {
00368 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00369                                 mrpt::math::multiply_SIMD(A,B,*this);
00370 #else
00371                                 mrpt::math::multiply(A,B,*this);
00372 #endif
00373                         }
00374 
00375                         /** this = A * A^t */
00376                         template <size_t NC1>
00377                         void multiply_AAt(const CMatrixFixedNumeric<T,NROWS,NC1> &A) {
00378                                 mrpt::math::multiply_AAt(A,*this);
00379                         }
00380 
00381                         /** Computes the vector v = this * a, where "a" is a column vector of the appropriate length.
00382                           */
00383                         void multiply_Ab( const std::vector<T>& a, std::vector<T>& out_v ) const {
00384                                 mrpt::math::multiply_Ab(*this,a,out_v);
00385                         }
00386 
00387                         /**     Calculate the operation this = A*B*C
00388                         * \sa multiply_ABCT
00389                         */
00390                         template <size_t N1,size_t N2>
00391                         void multiply_ABC(
00392                                 const CMatrixFixedNumeric<T,NROWS,N1> &A,
00393                                 const CMatrixFixedNumeric<T,N1,N2>    &B,
00394                                 const CMatrixFixedNumeric<T,N2,NCOLS> &C)
00395                         {
00396                                 this->zeros();
00397                                 for (size_t i=0;i<NROWS;i++)
00398                                         for (size_t l=0;l<N2;l++)
00399                                         {
00400                                                 T sumAccumInner = 0;
00401                                                 for (size_t k=0;k<N1;k++)
00402                                                         sumAccumInner += A.get_unsafe(i,k) * B.get_unsafe(k,l);
00403                                                 for (size_t j=0;j<NCOLS;j++)
00404                                                         get_unsafe(i,j) += sumAccumInner * C.get_unsafe(l,j);
00405                                         }
00406                         }
00407 
00408                         /**     Calculate the operation this = A*B*Ct
00409                         * \sa multiply_ABC
00410                         */
00411                         template <size_t N1,size_t N2>
00412                         void multiply_ABCt(
00413                                 const CMatrixFixedNumeric<T,NROWS,N1> &A,
00414                                 const CMatrixFixedNumeric<T,N1,N2>    &B,
00415                                 const CMatrixFixedNumeric<T,NCOLS,N2> &C)
00416                         {
00417                                 this->zeros();
00418                                 for (size_t i=0;i<NROWS;i++)
00419                                         for (size_t l=0;l<N2;l++)
00420                                         {
00421                                                 T sumAccumInner = 0;
00422                                                 for (size_t k=0;k<N1;k++)
00423                                                         sumAccumInner += A.get_unsafe(i,k) * B.get_unsafe(k,l);
00424                                                 for (size_t j=0;j<NCOLS;j++)
00425                                                         get_unsafe(i,j) += sumAccumInner * C.get_unsafe(j,l);
00426                                         }
00427                         }
00428 
00429                         /** Sum to this matrix A and its transpose: this = this + A + At
00430                         */
00431                         template <size_t M1>
00432                         void add_AAt(const CMatrixFixedNumeric<T,M1,M1> &A)
00433                         {
00434                                 ASSERT_(M1<NROWS && M1<NCOLS)
00435                                 for (size_t i=0;i<M1;i++)
00436                                         for (size_t j=i+1; j < M1; j++)
00437                                         {
00438                                                 const T v = A.get_unsafe(i,j)+A.get_unsafe(j,i);
00439                                                 get_unsafe(i,j) += v;
00440                                                 get_unsafe(j,i) += v;
00441                                         }
00442                                 for (size_t i=0;i<M1;i++)
00443                                         get_unsafe(i,i) += 2*A.get_unsafe(i,i);
00444                         }
00445 
00446                         /** Sum A to this matrix: this = this + A  */
00447                         void add(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A)
00448                         {
00449                                 for (size_t i=0;i<NROWS*NCOLS;i++)
00450                                         m_Val[i]+=A.m_Val[i];
00451                         }
00452 
00453                         /** Substract A from this matrix: this = this - A  */
00454                         void substract(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A)
00455                         {
00456                                 for (size_t i=0;i<NROWS*NCOLS;i++)
00457                                         m_Val[i]-=A.m_Val[i];
00458                         }
00459 
00460                         /** Sum A times a scalar to this matrix: this = this + A*c  */
00461                         void add_Ac(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A, const T c)
00462                         {
00463                                 for (size_t i=0;i<NROWS*NCOLS;i++)
00464                                         m_Val[i]+=A.m_Val[i]*c;
00465                         }
00466 
00467                         /** Substract A times a scalar from this matrix: this = this - A*c  */
00468                         void substract_Ac(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A, const T c)
00469                         {
00470                                 for (size_t i=0;i<NROWS*NCOLS;i++)
00471                                         m_Val[i]-=A.m_Val[i]*c;
00472                         }
00473 
00474                         /** Multiplies all elements by a scalar */
00475                         void operator *= (const T val) {
00476 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00477                                 mrpt::math::multiply_SIMD(*this,val);
00478 #else
00479                                 mrpt::math::multiply(*this,val);
00480 #endif
00481                         }
00482                         /** Multiplies all elements by a scalar */
00483                         void operator /= (const T val) {
00484                                 ASSERTMSG_(val!=0, "division by zero")
00485 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00486                                 mrpt::math::multiply_SIMD(*this,T(1)/val);
00487 #else
00488                                 mrpt::math::multiply(*this,T(1)/val);
00489 #endif
00490                         }
00491                         /** Sum a scalar to all elements */
00492                         void operator += (const T val) {
00493 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00494                                 mrpt::math::sumInPlace_SIMD(*this,val);
00495 #else
00496                                 mrpt::math::sumInPlace(*this,val);
00497 #endif
00498                         }
00499                         /** Substract a scalar to all elements */
00500                         void operator -= (const T val) {
00501 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00502                                 mrpt::math::sumInPlace_SIMD(*this,-val);
00503 #else
00504                                 mrpt::math::sumInPlace(*this,-val);
00505 #endif
00506                         }
00507 
00508                         /** Sum a matrix to this one */
00509                         void operator += (const CMatrixFixedNumeric<T,NROWS,NCOLS>& m) {
00510 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00511                                 mrpt::math::sumInPlace_SIMD(*this,m);
00512 #else
00513                                 mrpt::math::sumInPlace(*this,m);
00514 #endif
00515                         }
00516                         /** Substract a matrix to this one */
00517                         void operator -= (const CMatrixFixedNumeric<T,NROWS,NCOLS>& m) {
00518 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00519                                 mrpt::math::substractInPlace_SIMD(*this,m);
00520 #else
00521                                 mrpt::math::substractInPlace(*this,m);
00522 #endif
00523                         }
00524 
00525                         /** Returns the sum of all the elements
00526                           */
00527                         T sumAll() const {
00528 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00529                                 return mrpt::math::sumMatrixAllElements_SIMD(*this);
00530 #else
00531                                 return mrpt::math::sumMatrixAllElements(*this);
00532 #endif
00533                         }
00534 
00535                         /** Returns the minimum of all the elements \sa minimumAndmaximum */
00536                         T minimum() const {
00537 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00538                                 return mrpt::math::minimumMatrix_SIMD(*this);
00539 #else
00540                                 return mrpt::math::minimumMatrix(*this);
00541 #endif
00542                         }
00543 
00544                         /** Returns the maximum of all the elements  \sa minimumAndmaximum */
00545                         T maximum() const {
00546 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00547                                 return mrpt::math::maximumMatrix_SIMD(*this);
00548 #else
00549                                 return mrpt::math::maximumMatrix(*this);
00550 #endif
00551                         }
00552 
00553                         /** Returns the minimum & maximum of all the elements */
00554                         void minimumAndMaximum(T &val_min, T &val_max) const {
00555 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00556                                 mrpt::math::minimumAndMaximumMatrix_SIMD(*this,val_min,val_max);
00557 #else
00558                                 mrpt::math::minimumAndMaximumMatrix(*this,val_min,val_max);
00559 #endif
00560                         }
00561 
00562                         /** Returns the determinant of the matrix  */
00563                         T det() const {
00564                                 return mrpt::math::detMatrix(*this);
00565                         }
00566 
00567                         /** Applies the sqrt to all the elements  */
00568                         void Sqrt() {
00569                                 mrpt::math::sqrtMatrix(*this);
00570                         }
00571 
00572                         /** Computes the eigenvalues/eigenvector decomposition of a symmetric matrix.
00573                          *    The decomposition is: M = Z · D · Z<sup>T</sup>, where columns in Z are the
00574                          *        eigenvectors and the diagonal matrix D contains the eigenvalues
00575                          *    as diagonal elements, sorted in <i>ascending</i> order.
00576                          *    The algorithm is taken from "Numerical recipes in C", freely available online.
00577                          */
00578                         void eigenVectors( CMatrixFixedNumeric<T,NROWS,NROWS>& Z, CMatrixFixedNumeric<T,NROWS,NROWS>& D) const {
00579                                 mrpt::math::eigenVectorsMatrix(*this, Z,D);
00580                         }
00581 
00582                         /** Copy the upper half of the matrix into the lower half */
00583                         void  force_symmetry() {
00584                                 for (size_t i=0;i<NROWS;i++)
00585                                         for (size_t j=i+1;j<NCOLS;j++)
00586                                                 get_unsafe(i,j) = get_unsafe(j,i);
00587                         }
00588 
00589                         /** @name Import/export as text
00590                                 @{ */
00591 
00592                         /** Save matrix to a text file, compatible with MATLAB text format.
00593                                 * \param file The target filename.
00594                                 * \param fileFormat See TMatrixTextFileFormat. The format of the numbers in the text file.
00595                                 * \param appendMRPTHeader Insert this header to the file "% File generated by MRPT. Load with MATLAB with: VAR=load(FILENAME);"
00596                                 * \param userHeader Additional text to be written at the head of the file. Typically MALAB comments "% This file blah blah". Final end-of-line is not needed.
00597                                 * \sa loadFromTextFile, inMatlabFormat, DEBUG_SAVE_MATRIX
00598                                 */
00599                         void  saveToTextFile(
00600                                 const std::string &file,
00601                                 TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG,
00602                                 bool    appendMRPTHeader = false,
00603                                 const std::string &userHeader = std::string("")
00604                                 ) const
00605                         {
00606                                 mrpt::math::saveMatrixToTextFile(*this, file,fileFormat,appendMRPTHeader,userHeader);
00607                         }
00608 
00609                         /** Load matrix from a text file, compatible with MATLAB text format.
00610                           *  Lines starting with '%' or '#' are interpreted as comments and ignored.
00611                           * \sa saveToTextFile, fromMatlabStringFormat
00612                           */
00613                         void  loadFromTextFile(const std::string &file)
00614                         {
00615                                 // This matrix is NROWS x NCOLS
00616                                 std::ifstream   f(file.c_str());
00617                                 if (f.fail()) THROW_EXCEPTION_CUSTOM_MSG1("loadFromTextFile: can't open file:'%s'",file.c_str());
00618 
00619                                 std::string             str;
00620                                 std::vector<double>     fil(512);
00621 
00622                                 const char      *ptr;
00623                                 char            *ptrEnd;
00624                                 size_t  i;
00625                                 size_t  nRows = 0;
00626 
00627                                 while ( !f.eof() )
00628                                 {
00629                                         std::getline(f,str);
00630 
00631                                         if (str.size() && str[0]!='#' && str[0]!='%')
00632                                         {
00633                                                 // Parse row to floats:
00634                                                 ptr = str.c_str();
00635 
00636                                                 ptrEnd = NULL;
00637                                                 i=0;
00638 
00639                                                 // Process each number in this row:
00640                                                 while ( ptr[0] && ptr!=ptrEnd )
00641                                                 {
00642                                                         // Find next number: (non white-space character):
00643                                                         while (ptr[0] && (ptr[0]==' ' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
00644                                                                 ptr++;
00645 
00646                                                         if (fil.size()<=i)      fil.resize(fil.size()+512);
00647 
00648                                                         // Convert to "double":
00649                                                         fil[i] = strtod(ptr,&ptrEnd);
00650 
00651                                                         // A valid conversion has been done?
00652                                                         if (ptr!=ptrEnd)
00653                                                         {
00654                                                                 i++;    // Yes
00655                                                                 ptr = ptrEnd;
00656                                                                 ptrEnd = NULL;
00657                                                         }
00658                                                 }; // end while procesing this row
00659 
00660                                                 // "i": # of columns:
00661                                                 if (i!=NCOLS) THROW_EXCEPTION(format("The matrix in the text file does not match fixed matrix size %ux%u",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS)));
00662                                                 if (nRows>=NROWS) THROW_EXCEPTION(format("The matrix in the text file does not match fixed matrix size %ux%u",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS)));
00663 
00664                                                 // Copy row to matrix:
00665                                                 for (size_t j=0;j<NCOLS;j++)
00666                                                         get_unsafe(nRows,j) = static_cast<T>(fil[j]);
00667 
00668                                                 nRows++;
00669                                         } // end if fgets
00670                                 } // end while not feof
00671 
00672                                 // Report error as exception
00673                                 if (!nRows)
00674                                         THROW_EXCEPTION("loadFromTextFile: Error loading from text file");
00675                         }
00676 
00677                         /** @} */
00678 
00679                         /** This executes the operation \f$ \mathbf{R} = \mathbf{H} \mathbf{C} \mathbf{H}^t \f$, where 'this' matrix is \f$ \mathbf{H} \f$ and \f$ \mathbf{C} \f$ is symmetric, in an efficient and numerically stable way.
00680                           *  If 'this' matrix is \f$ N \times M \f$, then \f$ \mathbf{C} \f$ must be \f$ M \times M \f$, and the result matrix \f$ R \f$ will be \f$ N \times N \f$.
00681                           * The result from this method is assured to be symmetric (if \f$ \mathbf{C} \f$ is symmetric), whereas executing:
00682                           \code
00683                                  R = H * C * (~H);
00684                           \endcode
00685                           * may lead to non-symmetric matrixes due to numerical rounding errors. In addition, this method is more efficient that the code above (see the MRPT's code examples on matrixes).
00686                           *
00687                           *  If accumResultInOutput=true, the contents of the output matrix will not be cleared, but added to the result of the operations. In this case it must have the correct size
00688                           *   before calling or an exception will be raised since this probably is a bug.
00689                           *
00690                           * \sa multiply_HCHt_scalar
00691                           */
00692                         inline void  multiply_HCHt(
00693                                 const CMatrixFixedNumeric<T,NCOLS,NCOLS> &C,
00694                                 CMatrixFixedNumeric<T,NROWS,NROWS>   &R,
00695                                 bool                                 accumResultInOutput = false  ) const
00696                         {
00697                                 mrpt::math::multiply_HCHt(*this,C,R,accumResultInOutput);
00698                         }
00699 
00700                         void multiplyColumnByScalar(
00701                                 size_t c,
00702                                 T scalar)
00703                         {
00704                                 ASSERT_( c < NCOLS );
00705                                 T *c1 = &m_Val[0]+c;
00706                                 for(unsigned int k = 0; k < NCOLS; k++)
00707                                 {
00708                                         *c1 *= scalar;
00709                                         c1 += NCOLS;
00710                                 }
00711                         }
00712 
00713                         /** Like multiply_HCHt but for resulting matrices of size 1x1, which is returned as a scalar */
00714                         T multiply_HCHt_scalar( const CMatrixFixedNumeric<T,NCOLS,NCOLS> &C ) const
00715                         {
00716                                 ASSERT_(NROWS==1)
00717                                 CMatrixFixedNumeric<T,NROWS,NROWS>   R;
00718                                 this->multiply_HCHt(C,R);
00719                                 return R.m_Val[0];
00720                         }
00721 
00722                         /** Like CMatrixFixedNumeric::multiply_HCHt but for Ht being given transpose. */
00723                         inline void  multiply_HtCH(
00724                                 const CMatrixFixedNumeric<T,NROWS,NROWS> &C,
00725                                 CMatrixFixedNumeric<T,NCOLS,NCOLS>   &R,
00726                                 bool                                 accumResultInOutput = false  ) const
00727                         {
00728                                 mrpt::math::multiply_HtCH(*this,C,R,accumResultInOutput);
00729                         }
00730 
00731                         /** Like multiply_HtCH but for resulting matrices of size 1x1, which is returned as a scalar */
00732                         T multiply_HtCH_scalar( const CMatrixFixedNumeric<T,NROWS,NROWS> &C ) const
00733                         {
00734                                 ASSERT_(NCOLS==1)
00735                                 CMatrixFixedNumeric<T,NCOLS,NCOLS>   R;
00736                                 this->multiply_HtCH(C,R);
00737                                 return R.m_Val[0];
00738                         }
00739 
00740                         /** Used for "det" */
00741                         int pivot(const size_t row)
00742                         {
00743                                 size_t k = row;
00744                                 double amax,temp;
00745 
00746                                 amax = -1;
00747                                 for (size_t i=row; i < NROWS; i++)
00748                                         if ( (temp = fabs( get_unsafe(i,row))) > amax && temp != 0)
00749                                         {
00750                                                 amax = temp;
00751                                                 k = i;
00752                                         }
00753                                 if (get_unsafe(k,row) == T(0))
00754                                         return -1;
00755                                 if (k != row)
00756                                 {
00757                                         // Swap rows "k" & "row":
00758                                         swap_rows(k,row);
00759                                         return static_cast<int>( k );
00760                                 }
00761                                 return 0;
00762                         } // end pivot
00763 
00764                         void swap_rows(size_t i1,size_t i2)
00765                         {
00766                                 ASSERT_( i1 < NROWS && i2 < NROWS );
00767 
00768                                 T  tmprow[NCOLS];
00769 				::memcpy(tmprow, &m_Val[0]+i1*NCOLS, sizeof(tmprow));
00770 				::memcpy(&m_Val[0]+i1*NCOLS,&m_Val[0]+i2*NCOLS, sizeof(tmprow));
00771 				::memcpy(&m_Val[0]+i2*NCOLS,tmprow, sizeof(tmprow));
00772                         }
00773 
00774                         void swap_cols(size_t i1,size_t i2)
00775                         {
00776                                 ASSERT_( i1 < NCOLS && i2 < NCOLS );
00777                                 T *c1 = &m_Val[0]+i1, *c2 = &m_Val[0]+i2;
00778 
00779                                 for( unsigned int k = 0; k < NROWS; k++)
00780                                 {
00781                                         T aux = *c1;
00782                                         *c1 = *c2;
00783                                         *c2 = aux;
00784 
00785                                         c1 += NCOLS;
00786                                         c2 += NCOLS;
00787                                 }
00788                         }
00789 
00790                         /** auxiliary member to get element (i,j), starting at (1,1) instead of (0,0) */
00791                         T _E(const size_t row, const size_t col) const {
00792                                 return m_Val[NCOLS*(row-1)+col-1];
00793                         }
00794                         /** auxiliary member to get element (i,j), starting at (1,1) instead of (0,0) */
00795                         T & _E(const size_t row, const size_t col) {
00796                                 return m_Val[NCOLS*(row-1)+col-1];
00797                         }
00798 
00799                         template<size_t N> inline void ASSERT_ENOUGHROOM(size_t r,size_t c) const       {
00800                                 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00801                                         ASSERT_((r>=N)&&(r<NROWS-N)&&(c>=N)&&(c<NCOLS-N));
00802                                 #endif
00803                         }
00804 
00805                         template<size_t N,typename ReturnType> inline ReturnType getVicinity(size_t c,size_t r) const   {
00806                                 return detail::getVicinity<CMatrixFixedNumeric<T,NROWS,NCOLS>,T,ReturnType,N>::get(c,r,*this);
00807                         }
00808 
00809                 }; // end of class definition ------------------------------
00810 
00811 
00812                 /** Computes the vector v = A * a, where "a" is a column vector of the appropriate length. */
00813                 template <typename T,size_t N,size_t M>
00814                 void multiply_Ab( const CMatrixFixedNumeric<T,N,M>& A, const std::vector<T>& a, std::vector<T>& out_v )
00815                 {
00816                         // Matrix A is    NxM
00817                         //  the vector a is  Mx1
00818                         //  the output v is  Nx1
00819                         out_v.resize(N);
00820                         typename std::vector<T>::const_iterator         a_it;
00821                         typename std::vector<T>::iterator               v_it;
00822                         size_t                                                  i,j;
00823                         for (i=0, v_it=out_v.begin(); i < N; i++)
00824                         {
00825                                 T accum = 0;
00826                                 for (j=0, a_it=a.begin(); j < M; j++)
00827                                         accum += *a_it++ * A.get_unsafe(i,j);
00828                                 *v_it++ = accum;
00829                         }
00830                 } // end of multiply_Ab
00831 
00832                 /** Multiply 2 matrices: RESULT = A * A^t
00833                   */
00834                 template <typename T,size_t M1R,size_t M1C>
00835                 void multiply_AAt(
00836                         const CMatrixFixedNumeric<T,M1R,M1C>& m1,
00837                         CMatrixFixedNumeric<T,M1R,M1R>& RESULT )
00838                 {
00839                         // If m1 is me, make a copy:
00840                         if ((void*)&m1==(void*)&RESULT)
00841                         {
00842                                 // Save result in a temporal matrix:
00843                                 T  temp[M1R*M1R];
00844 
00845                                 T  *ptr = temp;
00846                                 size_t i;
00847                                 for (i=0; i < M1R; i++)
00848                                 {
00849                                         for (size_t j=i; j < M1R; j++)
00850                                         {
00851                                                 T accum = 0;
00852                                                 for (size_t k=0; k < M1C; k++)
00853                                                         accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00854                                                 *(ptr++) = accum;
00855                                         }
00856                                 }
00857                                 // Copy from temp:
00858                                 ptr = temp;
00859                                 for (i=0; i < M1R; i++)
00860                                         for (size_t j=i; j < M1R; j++)
00861                                                 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = *(ptr++);
00862                         }
00863                         else
00864                         {
00865                                 // Work directly over the data:
00866                                 for (size_t i=0; i < M1R; i++)
00867                                 {
00868                                         for (size_t j=i; j < M1R; j++)
00869                                         {
00870                                                 T accum = 0;
00871                                                 for (size_t k=0; k < M1C; k++)
00872                                                         accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00873                                                 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = accum;
00874                                         }
00875                                 }
00876                         }
00877                 } // end multiply_AAt
00878 
00879 
00880                 /** Multiply 2 matrices: RESULT = m1 * m2
00881                   */
00882                 template <typename T,size_t NROWS,size_t NCOLS,size_t M1C>
00883                 void multiply(
00884                         const CMatrixFixedNumeric<T,NROWS,M1C>& m1,
00885                         const CMatrixFixedNumeric<T,M1C,NCOLS>& m2,
00886                         CMatrixFixedNumeric<T,NROWS,NCOLS>& RESULT )
00887                 {
00888                         MRPT_START
00889 
00890                         // If one of the matrices is me, make a copy:
00891                         if ( (CMatrixFixedNumeric<T,NROWS,NCOLS>*)(&m1)==&RESULT || (CMatrixFixedNumeric<T,NROWS,NCOLS>*)(&m2)==&RESULT)
00892                         {
00893                                 // Save result in a temporary matrix:
00894                                 T  temp[NROWS*NCOLS];
00895                                 size_t out_idx = 0;
00896                                 for (size_t i=0; i < NROWS; i++)
00897                                 {
00898                                         for (size_t j=0; j < NCOLS; j++)
00899                                         {
00900                                                 T accum = 0;
00901                                                 for (size_t k=0; k < M1C; k++)
00902                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00903                                                 temp[out_idx++] = accum;
00904                                         }
00905                                 }
00906 
00907                                 // Copy from temp:
00908 				::memcpy(&RESULT.m_Val[0],temp,sizeof(RESULT.m_Val));
00909                         }
00910                         else
00911                         {
00912                                 // Work directly over the data:
00913                                 for (size_t i=0; i < NROWS; i++)
00914                                 {
00915                                         for (size_t j=0; j < NCOLS; j++)
00916                                         {
00917                                                 T accum = 0;
00918                                                 for (size_t k=0; k < M1C; k++)
00919                                                         accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00920                                                 RESULT.get_unsafe(i,j)=accum;
00921                                         }
00922                                 }
00923                         }
00924 
00925                         MRPT_END
00926                 } // end multiply
00927 
00928                 /** Multiply by scalar */
00929                 template <typename T,size_t NROWS,size_t NCOLS>
00930                 void multiply(
00931                         CMatrixFixedNumeric<T,NROWS,NCOLS>& m,
00932                         const T val)
00933                 {
00934                         for (size_t i=0;i<NROWS*NCOLS;i++)
00935                                 m.m_Val[i]*=val;
00936                 }
00937 
00938                 /** Sum a scalar to all elements */
00939                 template <typename T,size_t NROWS,size_t NCOLS>
00940                 void sumInPlace(
00941                         CMatrixFixedNumeric<T,NROWS,NCOLS>& m,
00942                         const T val)
00943                 {
00944                         for (size_t i=0;i<NROWS*NCOLS;i++)
00945                                 m.m_Val[i]+=val;
00946                 }
00947 
00948                 /** Sum two matrices: M+=A */
00949                 template <typename T,size_t NROWS,size_t NCOLS>
00950                 void sumInPlace(
00951                         CMatrixFixedNumeric<T,NROWS,NCOLS>& M,
00952                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& A)
00953                 {
00954                         for (size_t i=0;i<NROWS*NCOLS;i++)
00955                                 M.m_Val[i]+=A.m_Val[i];
00956                 }
00957 
00958                 /** Sum all the elements in the matrix */
00959                 template <typename T,size_t NROWS,size_t NCOLS>
00960                 T sumMatrixAllElements( const CMatrixFixedNumeric<T,NROWS,NCOLS>& M ) {
00961                         T  r = 0;
00962                         for (size_t k=0;k<NROWS*NCOLS;k++)
00963                                 r+=M.m_Val[k];
00964                         return r;
00965                 }
00966 
00967                 /** Sum all the elements in the matrix */
00968                 template <typename T,size_t NROWS,size_t NCOLS>
00969                 T minimumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M) {
00970                         T mi = std::numeric_limits<T>::max();
00971                         for (size_t i=0;i<NROWS*NCOLS;i++)
00972                                 mi = std::min(mi,M.m_Val[i]);
00973                         return mi;
00974                 }
00975 
00976                 /** Sum all the elements in the matrix */
00977                 template <typename T,size_t NROWS,size_t NCOLS>
00978                 T maximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M) {
00979                         T ma = std::numeric_limits<T>::min();
00980                         for (size_t i=0;i<NROWS*NCOLS;i++)
00981                                 ma = std::max(ma,M.m_Val[i]);
00982                         return ma;
00983                 }
00984 
00985                 /** Sum all the elements in the matrix */
00986                 template <typename T,size_t NROWS,size_t NCOLS>
00987                 void minimumAndMaximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M, T &val_min, T &val_max) {
00988                         T mi = std::numeric_limits<T>::max();
00989                         T ma = std::numeric_limits<T>::min();
00990                         for (size_t i=0;i<NROWS*NCOLS;i++)
00991                         {
00992                                 mi = std::min(mi,M.m_Val[i]);
00993                                 ma = std::max(ma,M.m_Val[i]);
00994                         }
00995                         val_min = mi;
00996                         val_max = ma;
00997                 }
00998 
00999                 /** Sum two matrices: M-=A */
01000                 template <typename T,size_t NROWS,size_t NCOLS>
01001                 void substractInPlace(
01002                         CMatrixFixedNumeric<T,NROWS,NCOLS>& M,
01003                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& A)
01004                 {
01005                         for (size_t i=0;i<NROWS*NCOLS;i++)
01006                                 M.m_Val[i]-=A.m_Val[i];
01007                 }
01008 
01009                 /** Return the sum of two matrices: RET = A+B */
01010                 template <typename T,size_t NROWS,size_t NCOLS>
01011                 CMatrixFixedNumeric<T,NROWS,NCOLS> sum(
01012                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& A,
01013                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& B)
01014                 {
01015                         CMatrixFixedNumeric<T,NROWS,NCOLS> ret = A;
01016                         A+=B;
01017                         return A;
01018                 }
01019                 /** Return the sum of two matrices: RET = A-B */
01020                 template <typename T,size_t NROWS,size_t NCOLS>
01021                 CMatrixFixedNumeric<T,NROWS,NCOLS> substract(
01022                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& A,
01023                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& B)
01024                 {
01025                         CMatrixFixedNumeric<T,NROWS,NCOLS> ret = A;
01026                         A-=B;
01027                         return A;
01028                 }
01029 
01030 
01031                 /** The generic function which allow mixing any kind of matrices. */
01032                 template <typename MAT_H, typename MAT_C, typename MAT_R>
01033                 void multiply_HCHt(
01034                         const MAT_H &H,
01035                         const MAT_C &C,
01036                         MAT_R &R,
01037                         bool accumResultInOutput,
01038                         bool allow_submatrix_mult )
01039                 {
01040                         MRPT_START
01041 
01042                         ASSERTMSG_( (void*)&C != (void*)&H, "C and H must be different matrices." )
01043                         ASSERTMSG_( (void*)&R != (void*)&H, "R and H must be different matrices." )
01044                         ASSERTMSG_( (void*)&C != (void*)&R,  "C and R must be different matrices.")
01045                         ASSERT_(C.IsSquare())
01046 
01047                         if (allow_submatrix_mult)
01048                                  ASSERT_(C.getRowCount()>=H.getColCount())
01049                         else ASSERT_(C.getRowCount()==H.getColCount())
01050 
01051                         R.setSize( H.getRowCount(), H.getRowCount()); // Set output size. For fixed size matrices this does nothing.
01052 
01053                         MAT_H R_;
01054 
01055                         const size_t N=H.getRowCount();
01056                         const size_t M=H.getColCount();
01057 
01058                         // First compute R_ = H * C:
01059                         for (size_t i=0;i<N;i++)
01060                                 for (size_t j=0;j<M;j++)
01061                                 {
01062                                         typename MAT_H::value_type sumAccum = 0;
01063                                         for (size_t l=0;l<M;l++)
01064                                                 sumAccum += H.get_unsafe(i,l) * C.get_unsafe(l,j);
01065                                         R_.get_unsafe(i,j)  = sumAccum;
01066                                 }
01067 
01068                         // Now compute R = R_ * (H^t):
01069                         for (size_t i=0;i<N;i++)
01070                                 for (size_t j=i;j<N;j++)
01071                                 {
01072                                         typename MAT_H::value_type sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
01073                                         for (size_t l=0;l<M;l++)
01074                                                 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(j,l);
01075                                         R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
01076                                 }
01077                         MRPT_END
01078                 }
01079 
01080                 /** Use the member method with the same name in matrix classes. */
01081                 template <typename T, size_t N, size_t M>
01082                 void multiply_HtCH(
01083                         const CMatrixFixedNumeric<T,M,N> &H,
01084                         const CMatrixFixedNumeric<T,M,M> &C,
01085                         CMatrixFixedNumeric<T,N,N>   &R,
01086                         bool                                 accumResultInOutput )
01087                 {
01088                         MRPT_START
01089 
01090                         ASSERTMSG_( (void*)&C != (void*)&H, "C and H must be different matrices." )
01091                         ASSERTMSG_( (void*)&R != (void*)&H, "R and H must be different matrices." )
01092                         ASSERTMSG_( (void*)&C != (void*)&R,  "C and R must be different matrices.")
01093 
01094                         CMatrixFixedNumeric<T,N,M>      R_;
01095 
01096                         // First compute R_ = this * C:
01097                         for (size_t i=0;i<N;i++)
01098                                 for (size_t j=0;j<M;j++)
01099                                 {
01100                                         T sumAccum = 0;
01101                                         for (size_t l=0;l<M;l++)
01102                                                 sumAccum += H.get_unsafe(l,i) * C.get_unsafe(l,j);
01103                                         R_.get_unsafe(i,j)  = sumAccum;
01104                                 }
01105 
01106                         // Now compute R = R_ * (~this):
01107                         for (size_t i=0;i<N;i++)
01108                                 for (size_t j=i;j<N;j++)
01109                                 {
01110                                         T sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
01111                                         for (size_t l=0;l<M;l++)
01112                                                 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(l,j);
01113                                         R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
01114                                 }
01115                         MRPT_END
01116                 }
01117 
01118                 /** Computes R = HCH^t, with optional index offsets in C and R. */
01119                 template <typename T, size_t N, size_t M>
01120                 void  multiply_HCHt(
01121                         const CMatrixFixedNumeric<T,N,M> &H,
01122                         const CMatrixTemplateNumeric<T> &C,
01123                         CMatrixTemplateNumeric<T> &R,
01124                         bool resizeOutputR,
01125                         bool accumResultInOutput)
01126                 {
01127                         MRPT_START
01128                         ASSERTMSG_( (void*)&C != (void*)&R,  "C and R must be different matrices.")
01129                         ASSERT_(size(C,1)==size(C,2) && size(C,1)>=N && size(C,1)>=M);
01130 
01131                         if (resizeOutputR)
01132                                 R.setSize(N,N);
01133                         CMatrixFixedNumeric<T,N,M>      R_; // First compute R_ = H * C:
01134                         for (size_t i=0;i<N;i++)
01135                                 for (size_t j=0;j<M;j++)
01136                                 {
01137                                         T sumAccum = 0;
01138                                         for (size_t l=0;l<M;l++)
01139                                                 sumAccum += H.get_unsafe(i,l) * C.get_unsafe(l,j);
01140                                         R_.get_unsafe(i,j)  = sumAccum;
01141                                 }
01142 
01143                         // Now compute R = R_ * (H^t):
01144                         for (size_t i=0;i<N;i++)
01145                                 for (size_t j=i;j<N;j++)
01146                                 {
01147                                         T sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
01148                                         for (size_t l=0;l<M;l++)
01149                                                 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(j,l);
01150                                         R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
01151                                 }
01152                         MRPT_END
01153                 }
01154 
01155                 /** Matrix multiplication of this matrix with a submatrix of 'A', saving the result in a third matrix.
01156                   *   OUT = B * A
01157                   */
01158                 template <class MAT_X,class MAT_A,class MAT_OUT>
01159                 void multiplySubMatrix (
01160                         const MAT_X &X,
01161                         const MAT_A &A,
01162                         MAT_OUT &outResult,
01163                         const size_t  &A_cols_offset,
01164                         const size_t  &A_rows_offset,
01165                         const size_t &A_col_count)
01166                 {
01167                         MRPT_START
01168                         // The output will be NxM:
01169                         const size_t  N = X.getRowCount();
01170                         const size_t  M = A_col_count;
01171                 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01172                         ASSERT_( A.getColCount() >= A_col_count + A_cols_offset );
01173                         ASSERT_( A.getRowCount() >= N + A_rows_offset );
01174                 #endif
01175                         outResult.setSize(N,M);
01176                         for (size_t i=0; i < N; i++)
01177                                 for (size_t j=0; j < M; j++)
01178                                 {
01179                                         typename MAT_OUT::value_type tmp = 0;
01180                                         for (size_t k=0; k < X.getColCount(); k++)
01181                                                 tmp += X.get_unsafe(i,k) * A.get_unsafe(k+A_rows_offset,j+A_cols_offset);
01182                                         outResult.get_unsafe(i,j) = tmp;
01183                                 }
01184                         MRPT_END
01185                 }
01186 
01187                 /** @name Typedefs for common sizes
01188                         @{ */
01189                 typedef CMatrixFixedNumeric<double,2,2> CMatrixDouble22;
01190                 typedef CMatrixFixedNumeric<double,3,3> CMatrixDouble33;
01191                 typedef CMatrixFixedNumeric<double,4,4> CMatrixDouble44;
01192                 typedef CMatrixFixedNumeric<double,6,6> CMatrixDouble66;
01193                 typedef CMatrixFixedNumeric<double,1,3> CMatrixDouble13;
01194                 typedef CMatrixFixedNumeric<double,3,1> CMatrixDouble31;
01195                 typedef CMatrixFixedNumeric<double,1,2> CMatrixDouble12;
01196                 typedef CMatrixFixedNumeric<double,2,1> CMatrixDouble21;
01197                 typedef CMatrixFixedNumeric<double,6,1> CMatrixDouble61;
01198                 typedef CMatrixFixedNumeric<double,1,6> CMatrixDouble16;
01199 
01200                 typedef CMatrixFixedNumeric<float,2,2> CMatrixFloat22;
01201                 typedef CMatrixFixedNumeric<float,3,3> CMatrixFloat33;
01202                 typedef CMatrixFixedNumeric<float,4,4> CMatrixFloat44;
01203                 typedef CMatrixFixedNumeric<float,6,6> CMatrixFloat66;
01204                 typedef CMatrixFixedNumeric<float,1,3> CMatrixFloat13;
01205                 typedef CMatrixFixedNumeric<float,3,1> CMatrixFloat31;
01206                 typedef CMatrixFixedNumeric<float,1,2> CMatrixFloat12;
01207                 typedef CMatrixFixedNumeric<float,2,1> CMatrixFloat21;
01208                 typedef CMatrixFixedNumeric<float,6,1> CMatrixFloat61;
01209                 typedef CMatrixFixedNumeric<float,1,6> CMatrixFloat16;
01210                 /**  @} */
01211 
01212                 /** Multiply a matrix by a scalar
01213                   */
01214                 template <typename T,size_t NROWS,size_t NCOLS>
01215                 CMatrixFixedNumeric<T,NROWS,NCOLS> operator *(const CMatrixFixedNumeric<T,NROWS,NCOLS>& m1,const T v) {
01216                         CMatrixFixedNumeric<T,NROWS,NCOLS> res = m1;
01217                         res*=v;
01218                         return res;
01219                 }
01220 
01221                 /** Multiply 2 matrices with the * operator
01222                   */
01223                 template <typename T,size_t NROWS,size_t NCOLS,size_t M1C>
01224                 CMatrixFixedNumeric<T,NROWS,NCOLS> operator *(
01225                         const CMatrixFixedNumeric<T,NROWS,M1C>& m1,
01226                         const CMatrixFixedNumeric<T,M1C,NCOLS>& m2)
01227                 {
01228                         CMatrixFixedNumeric<T,NROWS,NCOLS> res(false,false);
01229                         multiply(m1,m2,res);
01230                         return res;
01231                 }
01232 
01233                 /** Add 2 matrices with the + operator */
01234                 template <typename T,size_t NROWS,size_t NCOLS>
01235                 CMatrixFixedNumeric<T,NROWS,NCOLS> operator +(
01236                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& m1,
01237                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& m2)
01238                 {
01239                         CMatrixFixedNumeric<T,NROWS,NCOLS> res = m1;
01240                         res+=m2;
01241                         return res;
01242                 }
01243 
01244                 /** Substract 2 matrices with the - operator */
01245                 template <typename T,size_t NROWS,size_t NCOLS>
01246                 CMatrixFixedNumeric<T,NROWS,NCOLS> operator -(
01247                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& m1,
01248                         const CMatrixFixedNumeric<T,NROWS,NCOLS>& m2)
01249                 {
01250                         CMatrixFixedNumeric<T,NROWS,NCOLS> res = m1;
01251                         res-=m2;
01252                         return res;
01253                 }
01254 
01255                 /** unary negative operator -
01256                  */
01257                 template <typename T,size_t NROWS,size_t NCOLS>
01258                 CMatrixFixedNumeric<T,NCOLS,NROWS> operator -(const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
01259                 {
01260                         CMatrixFixedNumeric<T,NCOLS,NROWS>      res(false,false);
01261                         for (size_t i=0; i<NROWS*NCOLS; i++)
01262                                 res.m_Val[i] = -m.m_Val[i];
01263                         return res;
01264                 }
01265 
01266                 /** unary transpose operator ~
01267                  */
01268                 template <typename T,size_t NROWS,size_t NCOLS>
01269                 CMatrixFixedNumeric<T,NCOLS,NROWS> operator ~(const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
01270                 {
01271                         CMatrixFixedNumeric<T,NCOLS,NROWS>      res(false,false);
01272                         for (size_t i=0; i<NROWS; i++)
01273                                 for (size_t j=0; j<NCOLS; j++)
01274                                         res.get_unsafe(j,i) = m.get_unsafe(i,j);
01275                         return res;
01276                 }
01277 
01278                 /** unary inverse operator ! */
01279                 template <typename T,size_t NROWS>
01280                 CMatrixFixedNumeric<T,NROWS,NROWS> operator !(const CMatrixFixedNumeric<T,NROWS,NROWS>& m)
01281                 {
01282                         CMatrixFixedNumeric<T,NROWS,NROWS>      res(false,false);
01283                         m.inv(res);
01284                         return res;
01285                 }
01286 
01287                 /** Returns the determinant of the matrix  */
01288                 template <typename T,size_t NROWS,size_t NCOLS>
01289                 T detMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M)
01290                 {
01291                         // general case:
01292                         ASSERTMSG_(NROWS==NCOLS,"Determinant of non-square matrix")
01293 
01294                         CMatrixFixedNumeric<T,NROWS,NCOLS> temp(M);
01295                         T       piv,detVal = T(1);
01296 
01297                         for (size_t k=0; k < NROWS; k++)
01298                         {
01299                                 int             indx = temp.pivot(k);
01300                                 if (indx == -1)
01301                                         return 0;
01302                                 if (indx != 0)
01303                                         detVal = - detVal;
01304                                 detVal = detVal * temp.get_unsafe(k,k);
01305 
01306                                 for (size_t i=k+1; i < NROWS; i++)
01307                                 {
01308                                         piv = temp.get_unsafe(i,k) / temp.get_unsafe(k,k);
01309                                         for (size_t j=k+1; j < NROWS; j++)
01310                                                 temp.get_unsafe(i,j) -= piv * temp.get_unsafe(k,j);
01311                                 }
01312                         }
01313                         return detVal;
01314                 } // end of "det"
01315 
01316                 /** Returns the determinant of the matrix  */
01317                 template <typename T,size_t NROWS,size_t NCOLS>
01318                 void sqrtMatrix(CMatrixFixedNumeric<T,NROWS,NCOLS>& M)
01319                 {
01320                         for (size_t i=0;i<NROWS*NCOLS;i++)
01321                                 M.m_Val[i] = sqrt(M.m_Val[i]);
01322                 }
01323 
01324                 template <typename T> void MRPTDLLIMPEXP tred2(T **a, size_t nn, T d[], T e[]);
01325                 template <class T> void  MRPTDLLIMPEXP tqli(T d[], T e[], size_t nn, T **z);
01326 
01327                 /** Used from the method from CMatrix classes instead \sa CMatrixFixedNumeric::eigenVectors */
01328                 template <typename T,size_t N>
01329                 void eigenVectorsMatrix(
01330                         const CMatrixFixedNumeric<T,N,N> &M,
01331                         CMatrixFixedNumeric<T,N,N> &Z,
01332                         CMatrixFixedNumeric<T,N,N> &D )
01333                 {
01334                         // TODO: (JL) Rewrite this whole thing, please!
01335                         std::vector<unsigned int>       indxs;
01336                         std::vector<bool>       already;
01337 
01338                         size_t  i,j;
01339                         T               **a;
01340                         T               *d,*e;
01341 
01342                         MRPT_START
01343 
01344                         // Algorithm from "Numerical recipes in C"
01345 
01346                         // Check for symmetry
01347                         // --------------------------------------
01348 #ifdef _DEBUG
01349                         for (i=0;i<N;i++)
01350                                 for (j=i;j<N;j++)
01351                                         if (M.get_unsafe(i,j)!=M.get_unsafe(j,i))
01352                                         {
01353                                                 THROW_EXCEPTION(format("eigenVectors: The matrix is not symmetric! m(%lu,%lu)=%.16e != m(%lu,%lu)=%.16e\n",
01354                                                         static_cast<unsigned long>(i),static_cast<unsigned long>(j), static_cast<double> ( M.get_unsafe(i,j) ),
01355                                                         static_cast<unsigned long>(j),static_cast<unsigned long>(i), static_cast<double> ( M.get_unsafe(j,i) )) )
01356                                         }
01357 #endif
01358 
01359                         // Copy the matrix content to "a":
01360                         // --------------------------------------
01361                         typedef T* matrix_type_ptr;
01362 
01363                         a = new matrix_type_ptr[N+1];
01364                         for (i=1;i<=N;i++)      a[i] = new T[N+1];
01365                         d = new T[N+1];
01366                         e = new T[N+1];
01367 
01368                         for (i=1;i<=N;i++)
01369                                 for (j=1;j<=N;j++)
01370                                         a[i][j] = M.get_unsafe(i-1,j-1);
01371 
01372                         // Algorithm
01373                         // --------------------------------------
01374                         tred2( a, N, d, e);
01375                         tqli(d,e,N,a);
01376 
01377                         // In "d" are the eigenvalues
01378                         // In "a" are the eigenvectors as columns:
01379 
01380                         // SORT: Build a list of the N index in
01381                         //   ascending order of eigenvalue:
01382                         // --------------------------------------
01383                         indxs.resize(N+1);
01384                         already.resize(N+1, false);
01385 
01386                         for (i=1;i<=N;i++)
01387                         {
01388                                 size_t          minIndx = std::numeric_limits<size_t>::max();
01389                                 for (j=1;j<=N;j++)
01390                                         if (!already[j])
01391                                         {
01392                                                 if (minIndx==std::numeric_limits<size_t>::max())                minIndx = j;
01393                                                 else
01394                                                         if (d[j]<d[minIndx])    minIndx = j;
01395                                         }
01396 
01397                                 // The i'th sorted element:
01398                                 indxs[i] = static_cast<unsigned int> ( minIndx );
01399                                 already[minIndx] = true;
01400                         }
01401 
01402                         for (i=1;i<=N;i++)
01403                                 ASSERT_(already[i]);
01404 
01405                         // Copy results to matrices classes
01406                         // --------------------------------------
01407                         for (i=1;i<=N;i++)
01408                                 for (j=1;j<=N;j++)
01409                                 {
01410                                         Z(i-1,j-1) = a[i][indxs[j]];
01411                                         if (i==j)
01412                                         {
01413                                                 if (d[indxs[j]]<0)
01414                                                                 D(i-1,i-1) = -d[indxs[j]];
01415                                                 else    D(i-1,i-1) = d[indxs[j]];
01416                                         }
01417                                         else            D(i-1,j-1) = 0;
01418                                 }
01419 
01420                         // Free
01421                         // --------------------------------------
01422                         for (i=1;i<=N;i++)      delete[] a[i];
01423                         delete[]        a;
01424                         delete[]        d;
01425                         delete[]        e;
01426 
01427                         MRPT_END_WITH_CLEAN_UP( std::cout << "[eigenVectors] The matrix leading to exception is:" << std::endl << M << std::endl; )
01428                 }
01429 
01430                 /** Used from the method from CMatrix classes instead \sa CMatrixFixedNumeric::eigenVectors - 2x2 specializations */
01431                 template <typename T> void MRPTDLLIMPEXP eigenVectorsMatrix(const CMatrixFixedNumeric<T,2,2> &M,CMatrixFixedNumeric<T,2,2> &Z,CMatrixFixedNumeric<T,2,2> &D );
01432 
01433                 /** Returns the determinant of the matrix */
01434                 template <typename T> T detMatrix(const CMatrixFixedNumeric<T,2,2> &M) {
01435                         return M.m_Val[2*0+0]*M.m_Val[2*1+1]-M.m_Val[2*0+1]*M.m_Val[2*1+0];
01436                 }
01437 
01438                 /** Returns the determinant of the matrix
01439                   *  DET  =  a11(a33a22-a32a23)-a21(a33a12-a32a13)+a31(a23a12-a22a13)
01440                   */
01441                 template <typename T> T detMatrix(const CMatrixFixedNumeric<T,3,3> &M) {
01442                         return M._E(1,1)*(M._E(3,3)*M._E(2,2)-M._E(3,2)*M._E(2,3))-
01443                                 M._E(2,1)*(M._E(3,3)*M._E(1,2)-M._E(3,2)*M._E(1,3))+
01444                                 M._E(3,1)*(M._E(2,3)*M._E(1,2)-M._E(2,2)*M._E(1,3));
01445                 }
01446 
01447                 /** Returns the determinant of the matrix
01448                   */
01449                 template <typename T> T detMatrix(const CMatrixFixedNumeric<T,4,4> &M) {
01450                         const float D1 =
01451                                 M._E(1+1,1+1)*(M._E(3+1,3+1)*M._E(2+1,2+1)-M._E(3+1,2+1)*M._E(2+1,3+1))-
01452                                 M._E(2+1,1+1)*(M._E(3+1,3+1)*M._E(1+1,2+1)-M._E(3+1,2+1)*M._E(1+1,3+1))+
01453                                 M._E(3+1,1+1)*(M._E(2+1,3+1)*M._E(1+1,2+1)-M._E(2+1,2+1)*M._E(1+1,3+1));
01454                         const float D2 =
01455                                 M._E(1+1,1)*(M._E(3+1,3+1)*M._E(2+1,2+1)-M._E(3+1,2+1)*M._E(2+1,3+1))-
01456                                 M._E(2+1,1)*(M._E(3+1,3+1)*M._E(1+1,2+1)-M._E(3+1,2+1)*M._E(1+1,3+1))+
01457                                 M._E(3+1,1)*(M._E(2+1,3+1)*M._E(1+1,2+1)-M._E(2+1,2+1)*M._E(1+1,3+1));
01458                         const float D3 =
01459                                 M._E(1+1,1)*(M._E(3+1,3+1)*M._E(2+1,2)-M._E(3+1,2)*M._E(2+1,3+1))-
01460                                 M._E(2+1,1)*(M._E(3+1,3+1)*M._E(1+1,2)-M._E(3+1,2)*M._E(1+1,3+1))+
01461                                 M._E(3+1,1)*(M._E(2+1,3+1)*M._E(1+1,2)-M._E(2+1,2)*M._E(1+1,3+1));
01462                         const float D4 =
01463                                 M._E(1+1,1)*(M._E(3+1,3)*M._E(2+1,2)-M._E(3+1,2)*M._E(2+1,3))-
01464                                 M._E(2+1,1)*(M._E(3+1,3)*M._E(1+1,2)-M._E(3+1,2)*M._E(1+1,3))+
01465                                 M._E(3+1,1)*(M._E(2+1,3)*M._E(1+1,2)-M._E(2+1,2)*M._E(1+1,3));
01466                         return M._E(1,1)*D1 - M._E(1,2)*D2 + M._E(1,3)*D3 - M._E(1,4)*D4;
01467                 }
01468 
01469                 /** Returns the inverse of the matrix in "out_inv"
01470                   *  \sa CMatrixFixedNumeric::inv
01471                   */
01472                 template <typename T,size_t NROWS,size_t NCOLS>
01473                 void  invMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv ) {
01474                         CMatrixFixedNumeric<T,NROWS,NCOLS> temp = M;
01475                         invMatrix_destroySrc(temp,out_inv);  // temp is destroyed in inv_fast
01476                 }
01477 
01478                 /** Returns the inverse of the matrix in "out_inv" , DESTROYING the original matrix M
01479                   *  \sa CMatrixFixedNumeric::inv_fast
01480                   */
01481                 template <typename T,size_t NROWS,size_t NCOLS>
01482                 void invMatrix_destroySrc( CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv ) {
01483                         // Generic implementation (speciations exist for 2x2, 3x3, ...)
01484                         ASSERTMSG_(NROWS==NCOLS,"Inversion of non-square matrix")
01485 
01486                         T a1,a2;
01487                         out_inv.unit();
01488                         for (size_t k=0; k < NROWS; k++)
01489                         {
01490                                 int indx = M.pivot(k);
01491                                 if (indx == -1)
01492                                 {
01493                                         std::cerr << "[inv] Matrix that leaded to error is:" << std::endl << M << std::endl;
01494                                         THROW_EXCEPTION( "Inversion of a singular matrix");
01495                                 }
01496 
01497                                 if (indx != 0)
01498                                 {
01499                                         // Swap rows:
01500                                         M.swap_rows(k,indx);
01501                                 }
01502                                 a1 = M.get_unsafe(k,k);
01503                                 const T a1_i = 1/a1;
01504                                 for (size_t j=0; j < NROWS; j++)
01505                                 {
01506                                         M.get_unsafe(k,j) *= a1_i;
01507                                         out_inv.get_unsafe(k,j) *= a1_i;
01508                                 }
01509                                 for (size_t i=0; i < NROWS; i++)
01510                                 {
01511                                         if (i != k)
01512                                         {
01513                                                 a2 = M.get_unsafe(i,k);
01514                                                 for (size_t j=0; j < NROWS; j++)
01515                                                 {
01516                                                         M.get_unsafe(i,j)  -= a2 * M.get_unsafe(k,j);
01517                                                         out_inv.get_unsafe(i,j) -= a2 * out_inv.get_unsafe(k,j);
01518                                                 }
01519                                         }
01520                                 }
01521                         }
01522                 }
01523 
01524                 /** Returns the inverse of the matrix in "out_inv" */
01525                 template <typename T> void invMatrix( const CMatrixFixedNumeric<T,2,2> &M, CMatrixFixedNumeric<T,2,2> &out_inv )
01526                 {
01527                         // | a11 a12 |-1             |  a22 -a12 |
01528                         // | a21 a22 |    =  1/DET * | -a21  a11 |
01529                         //
01530                         const T det = M.det();
01531                         ASSERTMSG_(det!=0,"Singular matrix")
01532                         const T det_inv = 1.0f / det;
01533                         out_inv.m_Val[2*0+0] =  M.m_Val[2*1+1];
01534                         out_inv.m_Val[2*0+1] = -M.m_Val[2*0+1];
01535                         out_inv.m_Val[2*1+0] = -M.m_Val[2*1+0];
01536                         out_inv.m_Val[2*1+1] =  M.m_Val[2*0+0];
01537                         out_inv*= det_inv;
01538                 }
01539                 template <typename T> void invMatrix_destroySrc(CMatrixFixedNumeric<T,2,2> &M, CMatrixFixedNumeric<T,2,2>& out_inv) { invMatrix(M,out_inv); }
01540 
01541                 /** Returns the inverse of the matrix in "out_inv" */
01542                 template <typename T> void invMatrix(const CMatrixFixedNumeric<T,3,3> &M, CMatrixFixedNumeric<T,3,3>& out_inv)
01543                 {
01544                         // | a11 a12 a13 |-1             |   a33a22-a32a23  -(a33a12-a32a13)   a23a12-a22a13  |
01545                         // | a21 a22 a23 |    =  1/DET * | -(a33a21-a31a23)   a33a11-a31a13  -(a23a11-a21a13) |
01546                         // | a31 a32 a33 |               |   a32a21-a31a22  -(a32a11-a31a12)   a22a11-a21a12  |
01547                         const T det = M.det();
01548                         ASSERTMSG_(det!=0,"Singular matrix")
01549                         const T det_inv = 1.0f / det;
01550                         out_inv._E(1,1) =  (M._E(3,3)*M._E(2,2)-M._E(3,2)*M._E(2,3) );
01551                         out_inv._E(1,2) =  (-M._E(3,3)*M._E(1,2)+M._E(3,2)*M._E(1,3) );
01552                         out_inv._E(1,3) =  (M._E(2,3)*M._E(1,2)-M._E(2,2)*M._E(1,3) );
01553                         out_inv._E(2,1) =  (-M._E(3,3)*M._E(2,1)+M._E(3,1)*M._E(2,3));
01554                         out_inv._E(2,2) =  (M._E(3,3)*M._E(1,1)-M._E(3,1)*M._E(1,3));
01555                         out_inv._E(2,3) =  (-M._E(2,3)*M._E(1,1)+M._E(2,1)*M._E(1,3));
01556                         out_inv._E(3,1) =  (M._E(3,2)*M._E(2,1)-M._E(3,1)*M._E(2,2));
01557                         out_inv._E(3,2) =  (-M._E(3,2)*M._E(1,1)+M._E(3,1)*M._E(1,2));
01558                         out_inv._E(3,3) =  (M._E(2,2)*M._E(1,1)-M._E(2,1)*M._E(1,2));
01559                         out_inv*= det_inv;
01560                 }
01561                 template <typename T> void invMatrix_destroySrc(CMatrixFixedNumeric<T,3,3> &M, CMatrixFixedNumeric<T,3,3>& out_inv) { invMatrix(M,out_inv); }
01562 
01563 
01564                 /** Auxiliary function used in the constructor of dyn. matrices from a fixed one */
01565                 template <typename T,size_t NROWS,size_t NCOLS>
01566                 void fixedToDynMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &SRC, CMatrixTemplateNumeric<T> &DST)
01567                 {
01568                         DST.resize(NROWS,NCOLS);
01569                         for (size_t r=0;r<NROWS;r++)
01570                                 ::memcpy(DST.get_unsafe_row(r), &SRC.m_Val[0]+r*NCOLS,sizeof(T)*NCOLS);
01571                 }
01572 
01573                 /** Auxiliary function used in CMatrixTemplate */
01574                 template <typename T,size_t NROWS,size_t NCOLS>
01575                 void insertMatrixFixTransposeIntoDyn(
01576                         CMatrixTemplate<T> &M,
01577                         const size_t nRow,
01578                         const size_t nCol,
01579                         const CMatrixFixedNumeric<T,NROWS,NCOLS> &in)
01580                 {
01581                         ASSERTMSG_( (nRow+NCOLS <= M.getRowCount() ) && (nCol+NROWS<= M.getColCount()), "insertMatrix: Row or Col index out of bounds")
01582                         for (size_t c=0;c<NCOLS;c++)
01583                                 for (size_t r=0;r<NROWS;r++)
01584                                         M.get_unsafe(nRow+c,nCol+r) = in.get_unsafe(r,c);
01585                 }
01586 
01587                 /** Auxiliary function used in CMatrixTemplate */
01588                 template <typename T,size_t NROWS,size_t NCOLS>
01589                 void insertMatrixFixIntoDyn(
01590                         CMatrixTemplate<T> &M,
01591                         const size_t nRow,
01592                         const size_t nCol,
01593                         const CMatrixFixedNumeric<T,NROWS,NCOLS> &in)
01594                 {
01595                         ASSERTMSG_( (nRow+NROWS <= M.getRowCount() ) && (nCol+NCOLS <= M.getColCount()), "insertMatrix: Row or Col index out of bounds")
01596                         for (size_t r=0;r<NROWS;r++)
01597                                 ::memcpy( M.get_unsafe_row(r+nRow)+nCol, &in.m_Val[0]+r*NCOLS, NCOLS*sizeof(T));
01598                 }
01599 
01600                 /** Used from CMatrixTemplate::extractMatrix */
01601                 template <typename T,size_t NROWS,size_t NCOLS>
01602                 void extractFixMatrixFromDynMatrix(
01603                         const CMatrixTemplate<T> &M,
01604                         const size_t nRow,
01605                         const size_t nCol,
01606                         CMatrixFixedNumeric<T,NROWS,NCOLS> &outMat)
01607                 {
01608                         ASSERTMSG_( (nRow+NROWS <= M.getRowCount() ) && (nCol+NCOLS <= M.getColCount()), "extractMatrix: Row or Col index out of bounds")
01609                         for (size_t r=0;r<NROWS;r++)
01610                                 ::memcpy( &outMat.m_Val[0]+r*NCOLS, M.get_unsafe_row(r+nRow)+nCol, sizeof(T)*NCOLS );
01611                 }
01612 
01613                 template <typename T,size_t N,size_t M>
01614                 inline CMatrixTemplateNumeric<T>& add(CMatrixTemplateNumeric<T> &A, const CMatrixFixedNumeric<T,N,M>& B)
01615                 {
01616 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01617                         ASSERT_(A.getColCount()>=N && A.getRowCount()>=M);
01618 #endif
01619                         for (size_t r=0;r<N;r++)
01620                                 for (size_t c=0;c<M;c++)
01621                                         A.get_unsafe(r,c)+= B.get_unsafe(r,c);
01622                         return A;
01623                 }
01624 
01625                 template <typename T,size_t N,size_t M>
01626                 inline CMatrixTemplateNumeric<T>& substract(CMatrixTemplateNumeric<T> &A, const CMatrixFixedNumeric<T,N,M>& B)
01627                 {
01628 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01629                         ASSERT_(A.getColCount()>=N && A.getRowCount()>=M);
01630 #endif
01631                         for (size_t r=0;r<N;r++)
01632                                 for (size_t c=0;c<M;c++)
01633                                         A.get_unsafe(r,c)-= B.get_unsafe(r,c);
01634                         return A;
01635                 }
01636 
01637                 /** A special case of multiply_HCHt for the case of the resulting matrix being a scalar (that is, a 1x1 matrix) - This method directly returns this as a scalar avoiding the construction of a 1x1 matrix.
01638                   \code
01639                          return = ( H * C * (~H) ) (0,0);
01640                   \endcode
01641                   */
01642                 template <typename T,typename VECTORLIKE,size_t DIM>
01643                 T multiply_HCHt_scalar(
01644                         const VECTORLIKE &H,
01645                         const CMatrixFixedNumeric<T,DIM,DIM> &C)
01646                 {
01647                         ASSERT_(H.size()==DIM);
01648                         T sumAccum = 0;
01649                         for (size_t l=0;l<DIM;l++)
01650                         {
01651                                 T sumAccumInner = 0;
01652                                 for (size_t k=0;k<DIM;k++)
01653                                         sumAccumInner += H[k] * C.get_unsafe(k,l);
01654                                 sumAccum += sumAccumInner * H[l];
01655                         }
01656                         return sumAccum;
01657                 }
01658 
01659 
01660                 /** @name Conversions from point & poses to matrices
01661                          Used from the "operator =" from poses/points to CMatrixFixedNumeric
01662                   @{ */
01663 
01664                 template <typename T,size_t NROWS,size_t NCOLS> CMatrixFixedNumeric<T,NROWS,NCOLS> & matrixFromPoseOrPoint(CMatrixFixedNumeric<T,NROWS,NCOLS>&M, const CPoint2D &p) { THROW_EXCEPTION("Matrix of the wrong size") }
01665                 template <typename T,size_t NROWS,size_t NCOLS> CMatrixFixedNumeric<T,NROWS,NCOLS> & matrixFromPoseOrPoint(CMatrixFixedNumeric<T,NROWS,NCOLS>&M, const CPoint3D &p) { THROW_EXCEPTION("Matrix of the wrong size") }
01666                 template <typename T,size_t NROWS,size_t NCOLS> CMatrixFixedNumeric<T,NROWS,NCOLS> & matrixFromPoseOrPoint(CMatrixFixedNumeric<T,NROWS,NCOLS>&M, const CPose2D &p) { THROW_EXCEPTION("Matrix of the wrong size") }
01667                 template <typename T,size_t NROWS,size_t NCOLS> CMatrixFixedNumeric<T,NROWS,NCOLS> & matrixFromPoseOrPoint(CMatrixFixedNumeric<T,NROWS,NCOLS>&M, const CPose3D &p) { THROW_EXCEPTION("Matrix of the wrong size") }
01668 
01669                 template <> CMatrixDouble21 & matrixFromPoseOrPoint(CMatrixDouble21 &M, const CPoint2D &p);
01670                 template <> CMatrixDouble31 & matrixFromPoseOrPoint(CMatrixDouble31 &M, const CPoint3D &p);
01671                 template <> CMatrixDouble31 & matrixFromPoseOrPoint(CMatrixDouble31 &M, const CPose2D &p);
01672                 template <> CMatrixDouble61 & matrixFromPoseOrPoint(CMatrixDouble61 &M, const CPose3D &p);
01673                 template <> CMatrixDouble12 & matrixFromPoseOrPoint(CMatrixDouble12 &M, const CPoint2D &p);
01674                 template <> CMatrixDouble13 & matrixFromPoseOrPoint(CMatrixDouble13 &M, const CPoint3D &p);
01675                 template <> CMatrixDouble13 & matrixFromPoseOrPoint(CMatrixDouble13 &M, const CPose2D &p);
01676                 template <> CMatrixDouble16 & matrixFromPoseOrPoint(CMatrixDouble16 &M, const CPose3D &p);
01677 
01678                   /** @} */
01679 
01680                 /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
01681                 template <size_t NROWS,size_t NCOLS>
01682                 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
01683                         CMatrix  aux;
01684                         in >> aux;
01685                         M = aux;
01686                         return in;
01687                 }
01688                 /** Read operator from a CStream. The format is compatible with that of CMatrix & CMatrixD */
01689                 template <size_t NROWS,size_t NCOLS>
01690                 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
01691                         CMatrixD  aux;
01692                         in >> aux;
01693                         M = aux;
01694                         return in;
01695                 }
01696 
01697                 /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
01698                 template <size_t NROWS,size_t NCOLS>
01699                 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
01700                         CMatrix aux = CMatrixFloat(M);
01701                         out << aux;
01702                         return out;
01703                 }
01704                 /** Write operator for writing into a CStream. The format is compatible with that of CMatrix & CMatrixD */
01705                 template <size_t NROWS,size_t NCOLS>
01706                 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
01707                         CMatrixD aux = CMatrixDouble(M);
01708                         out << aux;
01709                         return out;
01710                 }
01711 
01712                 /** Textual output stream function.
01713                   *    Use only for text output, for example:  "std::cout << mat;"
01714                   */
01715                 template <class T,size_t NROWS, size_t NCOLS>
01716                 std::ostream& operator << (std::ostream& ostrm, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
01717                 {
01718                         ostrm << std::setprecision(6);
01719 
01720                         for (size_t i=0; i < NROWS; i++)
01721                         {
01722                                 for (size_t j=0; j < NCOLS; j++)
01723                                         ostrm << std::setw(10) << m.get_unsafe(i,j);
01724                                 ostrm << std::endl;
01725                         }
01726                         return ostrm;
01727                 }
01728 
01729                 /** Equal comparison (==)  */
01730                 template <class T,size_t NROWS, size_t NCOLS>
01731                 bool operator == (const CMatrixFixedNumeric<T,NROWS,NCOLS>& M1, const CMatrixFixedNumeric<T,NROWS,NCOLS>& M2)
01732                 {
01733                         for (size_t i=0; i < NROWS; i++)
01734                                 for (size_t j=0; j < NCOLS; j++)
01735                                         if (M1.get_unsafe(i,j)!=M2.get_unsafe(i,j))
01736                                                 return false;
01737                         return true;
01738                 }
01739 
01740                 // ----------------------------- Begin of SSE2 specializations ---------------------------
01741 
01742                 /** @name Optimized functions (with specializations and/or SSE2 code)
01743                   @{ */
01744 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
01745                 /** Default template (not really implemented ) */
01746 //              template <typename T,size_t NROWS,size_t NCOLS>
01747 //              void multiply_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& m, const T val);
01748 //
01749 //              /** Default template (not really implemented ) */
01750 //              template <typename T,size_t NROWS,size_t NCOLS,size_t M1C>
01751 //              void multiply_SIMD( const CMatrixFixedNumeric<T,NROWS,M1C>& m1, const CMatrixFixedNumeric<T,M1C,NCOLS>& m2, CMatrixFixedNumeric<T,NROWS,NCOLS>& RESULT );
01752 //
01753 //              /** Default template (not really implemented ) */
01754 //              template <typename T,size_t NROWS,size_t NCOLS>
01755 //              void sumInPlace_SIMD( CMatrixFixedNumeric<T,NROWS,NCOLS>& m, const T val);
01756 
01757                 /** Multiply by scalar */
01758                 template <size_t NROWS,size_t NCOLS>
01759                 void multiply_SIMD(
01760                         CMatrixFixedNumeric<float,NROWS,NCOLS>& m,
01761                         const float val)
01762                 {
01763                         __m128 cnts = _mm_load1_ps(&val);  // 4 copies of "val"
01764                         const size_t N = NROWS*NCOLS;
01765                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
01766                         float *ptr = m.m_Val;
01767                         for (size_t i=0;i<nBlocks;i++)
01768                         {
01769                                 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) );
01770                                 ptr+=4;
01771                         }
01772                         // The rest, multiply as normal:
01773                         const size_t Nrest = N-(nBlocks<<2);
01774                         for (size_t i=0;i<Nrest;i++, ptr++ )
01775                                 *ptr *= val;
01776                 }
01777 
01778                 /** Multiply two 4x4 matrices - SSE2 specialization */
01779                 template <>
01780                 void multiply_SIMD(
01781                         const CMatrixFixedNumeric<float,4,4>& m1,
01782                         const CMatrixFixedNumeric<float,4,4>& m2,
01783                         CMatrixFixedNumeric<float,4,4>& RESULT )
01784                 {
01785                         ASSERT_(&m1!=&RESULT && &m2!=&RESULT)
01786                         // Columns:
01787                         for (unsigned j=0;j<4;j++)
01788                         {
01789                                 __m128 m2col;
01790                                 ((float*)&m2col)[0] = m2.m_Val[0+j];
01791                                 ((float*)&m2col)[1] = m2.m_Val[4+j];
01792                                 ((float*)&m2col)[2] = m2.m_Val[8+j];
01793                                 ((float*)&m2col)[3] = m2.m_Val[12+j];
01794 
01795                                 for (unsigned i=0;i<4;i++)
01796                                 {
01797                                         __m128 aux = _mm_mul_ps( m2col, _mm_load_ps(&m1.m_Val[i<<2]) );
01798                                         RESULT.m_Val[(i<<2)+j] = ((float*)&aux)[0]+((float*)&aux)[1]+((float*)&aux)[2]+((float*)&aux)[3];
01799                                 }
01800                         }
01801                 }
01802 
01803                 /** Multiply by scalar - SSE2 specialization */
01804                 template <>
01805                 void multiply_SIMD( CMatrixFixedNumeric<float,2,2>& m, const float val)
01806                 {
01807                         __m128 cnts = _mm_load1_ps(&val);  // 4 copies of "val"
01808                         float *ptr = m.m_Val;
01809                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) );
01810                 }
01811                 /** Multiply by scalar - SSE2 specialization */
01812                 template <>
01813                 void multiply_SIMD( CMatrixFixedNumeric<float,3,3>& m, const float val)
01814                 {
01815                         __m128 cnts = _mm_load1_ps(&val);  // 4 copies of "val"
01816                         float *ptr = m.m_Val;
01817                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01818                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01819                         *ptr++ *= val;
01820                 }
01821                 /** Multiply by scalar - SSE2 specialization */
01822                 template <>
01823                 void multiply_SIMD( CMatrixFixedNumeric<float,4,4>& m, const float val)
01824                 {
01825                         __m128 cnts = _mm_load1_ps(&val);  // 4 copies of "val"
01826                         float *ptr = m.m_Val;
01827                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01828                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01829                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01830                         _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) );
01831                 }
01832 
01833                 /** Multiply by scalar - SSE2 specialization */
01834                 template <>
01835                 void multiply_SIMD( CMatrixFixedNumeric<double,2,2>& m, const double val)
01836                 {
01837                         __m128d cnts = _mm_load1_pd(&val);  // 4 copies of "val"
01838                         double *ptr = m.m_Val;
01839                         _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) ); ptr+=2;
01840                         _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01841                 }
01842                 /** Multiply by scalar - SSE2 specialization */
01843                 template <>
01844                 void multiply_SIMD( CMatrixFixedNumeric<double,3,3>& m, const double val)
01845                 {
01846                         __m128d cnts = _mm_load1_pd(&val);  // 4 copies of "val"
01847                         double *ptr = m.m_Val;
01848                         for (size_t i=0;i<8/2;i++)
01849                         {
01850                                 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01851                                 ptr+=2;
01852                         }
01853                         *ptr++ *= val;
01854                 }
01855                 /** Multiply by scalar - SSE2 specialization */
01856                 template <>
01857                 void multiply_SIMD( CMatrixFixedNumeric<double,4,4>& m, const double val)
01858                 {
01859                         __m128d cnts = _mm_load1_pd(&val);  // 4 copies of "val"
01860                         double *ptr = m.m_Val;
01861                         for (size_t i=0;i<16/2;i++)
01862                         {
01863                                 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01864                                 ptr+=2;
01865                         }
01866                 }
01867 
01868                 /** Multiply by scalar */
01869                 template <size_t NROWS,size_t NCOLS>
01870                 void multiply_SIMD(
01871                         CMatrixFixedNumeric<double,NROWS,NCOLS>& m,
01872                         const double val)
01873                 {
01874                         __m128d cnts = _mm_load1_pd(&val);  // 2 copies of "val"
01875                         const size_t N = NROWS*NCOLS;
01876                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
01877                         double *ptr = m.m_Val;
01878                         for (size_t i=0;i<nBlocks;i++)
01879                         {
01880                                 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01881                                 ptr+=2;
01882                         }
01883                         // The rest, multiply as normal:
01884                         const size_t Nrest = N-(nBlocks<<1);
01885                         for (size_t i=0;i<Nrest;i++, ptr++ )
01886                                 *ptr *= val;
01887                 }
01888 
01889                 /** Sum a scalar */
01890                 template <size_t NROWS,size_t NCOLS>
01891                 void sumInPlace_SIMD(
01892                         CMatrixFixedNumeric<float,NROWS,NCOLS>& m,
01893                         const float val)
01894                 {
01895                         __m128 cnts = _mm_load1_ps(&val);  // 4 copies of "val"
01896                         const size_t N = NROWS*NCOLS;
01897                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
01898                         float *ptr = m.m_Val;
01899                         for (size_t i=0;i<nBlocks;i++)
01900                         {
01901                                 _mm_store_ps(ptr, _mm_add_ps(_mm_load_ps(ptr), cnts ) );
01902                                 ptr+=4;
01903                         }
01904                         // The rest, multiply as normal:
01905                         const size_t Nrest = N-(nBlocks<<2);
01906                         for (size_t i=0;i<Nrest;i++, ptr++ )
01907                                 *ptr += val;
01908                 }
01909 
01910                 /** Sum a scalar */
01911                 template <size_t NROWS,size_t NCOLS>
01912                 void sumInPlace_SIMD(
01913                         CMatrixFixedNumeric<double,NROWS,NCOLS>& m,
01914                         const double val)
01915                 {
01916                         __m128d cnts = _mm_load1_pd(&val);  // 2 copies of "val"
01917                         const size_t N = NROWS*NCOLS;
01918                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
01919                         double *ptr = m.m_Val;
01920                         for (size_t i=0;i<nBlocks;i++)
01921                         {
01922                                 _mm_store_pd(ptr, _mm_add_pd(_mm_load_pd(ptr), cnts ) );
01923                                 ptr+=2;
01924                         }
01925                         // The rest, multiply as normal:
01926                         const size_t Nrest = N-(nBlocks<<1);
01927                         for (size_t i=0;i<Nrest;i++, ptr++ )
01928                                 *ptr += val;
01929                 }
01930 
01931                 /** Sum two matrices M+=A */
01932                 template <size_t NROWS,size_t NCOLS>
01933                 void sumInPlace_SIMD(
01934                         CMatrixFixedNumeric<float,NROWS,NCOLS>& M,
01935                         const CMatrixFixedNumeric<float,NROWS,NCOLS>& A)
01936                 {
01937                         const size_t N = NROWS*NCOLS;
01938                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
01939                         float *ptr  = M.m_Val;
01940                         const float *ptr2 = A.m_Val;
01941                         for (size_t i=0;i<nBlocks;i++)
01942                         {
01943                                 _mm_store_ps(ptr, _mm_add_ps(_mm_load_ps(ptr), _mm_load_ps(ptr2)) );
01944                                 ptr+=4;
01945                                 ptr2+=4;
01946                         }
01947                         // The rest, as normal:
01948                         const size_t Nrest = N-(nBlocks<<2);
01949                         for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01950                                 *ptr += *ptr2;
01951                 }
01952 
01953                 /** Sum two matrices M+=A */
01954                 template <size_t NROWS,size_t NCOLS>
01955                 void sumInPlace_SIMD(
01956                         CMatrixFixedNumeric<double,NROWS,NCOLS>& M,
01957                         const CMatrixFixedNumeric<double,NROWS,NCOLS>& A)
01958                 {
01959                         const size_t N = NROWS*NCOLS;
01960                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
01961                         double *ptr  = M.m_Val;
01962                         const double *ptr2 = A.m_Val;
01963                         for (size_t i=0;i<nBlocks;i++)
01964                         {
01965                                 _mm_store_pd(ptr, _mm_add_pd(_mm_load_pd(ptr), _mm_load_pd(ptr2)) );
01966                                 ptr+=2;
01967                                 ptr2+=2;
01968                         }
01969                         // The rest, as normal:
01970                         const size_t Nrest = N-(nBlocks<<1);
01971                         for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01972                                 *ptr += *ptr2;
01973                 }
01974 
01975                 /** Substract two matrices M-=A */
01976                 template <size_t NROWS,size_t NCOLS>
01977                 void substractInPlace_SIMD(
01978                         CMatrixFixedNumeric<float,NROWS,NCOLS>& M,
01979                         const CMatrixFixedNumeric<float,NROWS,NCOLS>& A)
01980                 {
01981                         const size_t N = NROWS*NCOLS;
01982                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
01983                         float *ptr  = M.m_Val;
01984                         const float *ptr2 = A.m_Val;
01985                         for (size_t i=0;i<nBlocks;i++)
01986                         {
01987                                 _mm_store_ps(ptr, _mm_sub_ps(_mm_load_ps(ptr), _mm_load_ps(ptr2)) );
01988                                 ptr+=4;
01989                                 ptr2+=4;
01990                         }
01991                         // The rest, as normal:
01992                         const size_t Nrest = N-(nBlocks<<2);
01993                         for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01994                                 *ptr -= *ptr2;
01995                 }
01996 
01997                 /** Substract two matrices M-=A */
01998                 template <size_t NROWS,size_t NCOLS>
01999                 void substractInPlace_SIMD(
02000                         CMatrixFixedNumeric<double,NROWS,NCOLS>& M,
02001                         const CMatrixFixedNumeric<double,NROWS,NCOLS>& A)
02002                 {
02003                         const size_t N = NROWS*NCOLS;
02004                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
02005                         double *ptr  = M.m_Val;
02006                         const double *ptr2 = A.m_Val;
02007                         for (size_t i=0;i<nBlocks;i++)
02008                         {
02009                                 _mm_store_pd(ptr, _mm_sub_pd(_mm_load_pd(ptr), _mm_load_pd(ptr2)) );
02010                                 ptr+=2;
02011                                 ptr2+=2;
02012                         }
02013                         // The rest, as normal:
02014                         const size_t Nrest = N-(nBlocks<<1);
02015                         for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
02016                                 *ptr -= *ptr2;
02017                 }
02018 
02019                 /** Sum all the elements */
02020                 template <size_t NROWS,size_t NCOLS>
02021                 float sumMatrixAllElements_SIMD( const CMatrixFixedNumeric<float,NROWS,NCOLS>& M )
02022                 {
02023                         const size_t N = NROWS*NCOLS;
02024                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
02025                         const float *ptr  = M.m_Val;
02026                         __m128  acum = _mm_setzero_ps();
02027                         for (size_t i=0;i<nBlocks;i++)
02028                         {
02029                                 acum = _mm_add_ps(acum, _mm_load_ps(ptr));
02030                                 ptr+=4;
02031                         }
02032                         // The rest, as normal:
02033                         float ret = ((float*)&acum)[0]+((float*)&acum)[1]+((float*)&acum)[2]+((float*)&acum)[3];
02034                         const size_t Nrest = N-(nBlocks<<2);
02035                         for (size_t i=0;i<Nrest;i++)
02036                                 ret += *ptr++;
02037                         return ret;
02038                 }
02039 
02040                 /** Sum all the elements */
02041                 template <size_t NROWS,size_t NCOLS>
02042                 double sumMatrixAllElements_SIMD( const CMatrixFixedNumeric<double,NROWS,NCOLS>& M )
02043                 {
02044                         const size_t N = NROWS*NCOLS;
02045                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
02046                         const double *ptr  = M.m_Val;
02047                         __m128d  acum = _mm_setzero_pd();
02048                         for (size_t i=0;i<nBlocks;i++)
02049                         {
02050                                 acum = _mm_add_pd(acum, _mm_load_pd(ptr));
02051                                 ptr+=2;
02052                         }
02053                         // The rest, as normal:
02054                         double ret = ((double*)&acum)[0]+((double*)&acum)[1];
02055                         const size_t Nrest = N-(nBlocks<<1);
02056                         for (size_t i=0;i<Nrest;i++)
02057                                 ret += *ptr++;
02058                         return ret;
02059                 }
02060 
02061                 /** The minimum of a matrix */
02062                 template <size_t NROWS,size_t NCOLS>
02063                 float minimumMatrix_SIMD(const CMatrixFixedNumeric<float,NROWS,NCOLS>& M)
02064                 {
02065                         const size_t N = NROWS*NCOLS;
02066                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
02067                         const float *ptr  = M.m_Val;
02068                         static const float cnst_max = std::numeric_limits<float>::max();
02069                         __m128  acum = _mm_load1_ps(&cnst_max);
02070                         for (size_t i=0;i<nBlocks;i++)
02071                         {
02072                                 acum = _mm_min_ps(acum, _mm_load_ps(ptr));
02073                                 ptr+=4;
02074                         }
02075                         // The rest, as normal:
02076                         float ret = std::min( std::min( ((float*)&acum)[0],((float*)&acum)[1]), std::min(((float*)&acum)[2],((float*)&acum)[3]));
02077                         const size_t Nrest = N-(nBlocks<<2);
02078                         for (size_t i=0;i<Nrest;i++)
02079                                 ret = std::min( ret, *ptr++ );
02080                         return ret;
02081                 }
02082 
02083                 /** The minimum of a matrix */
02084                 template <size_t NROWS,size_t NCOLS>
02085                 double minimumMatrix_SIMD(const CMatrixFixedNumeric<double,NROWS,NCOLS>& M)
02086                 {
02087                         const size_t N = NROWS*NCOLS;
02088                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
02089                         const double *ptr  = M.m_Val;
02090                         static const double cnst_max = std::numeric_limits<double>::max();
02091                         __m128d  acum = _mm_load1_pd(&cnst_max);
02092                         for (size_t i=0;i<nBlocks;i++)
02093                         {
02094                                 acum = _mm_min_pd(acum, _mm_load_pd(ptr));
02095                                 ptr+=2;
02096                         }
02097                         // The rest, as normal:
02098                         double ret = std::min( ((double*)&acum)[0],((double*)&acum)[1]);
02099                         const size_t Nrest = N-(nBlocks<<1);
02100                         for (size_t i=0;i<Nrest;i++)
02101                                 ret = std::min( ret, *ptr++ );
02102                         return ret;
02103                 }
02104 
02105                 /** The maximum of a matrix */
02106                 template <size_t NROWS,size_t NCOLS>
02107                 float maximumMatrix_SIMD(const CMatrixFixedNumeric<float,NROWS,NCOLS>& M)
02108                 {
02109                         const size_t N = NROWS*NCOLS;
02110                         const size_t nBlocks = NROWS*NCOLS >> 2;  // /=4
02111                         const float *ptr  = M.m_Val;
02112                         static const float cnst_max = std::numeric_limits<float>::min();
02113                         __m128  acum = _mm_load1_ps(&cnst_max);
02114                         for (size_t i=0;i<nBlocks;i++)
02115                         {
02116                                 acum = _mm_max_ps(acum, _mm_load_ps(ptr));
02117                                 ptr+=4;
02118                         }
02119                         // The rest, as normal:
02120                         float ret = std::max( std::max( ((float*)&acum)[0],((float*)&acum)[1]), std::max(((float*)&acum)[2],((float*)&acum)[3]));
02121                         const size_t Nrest = N-(nBlocks<<2);
02122                         for (size_t i=0;i<Nrest;i++)
02123                                 ret = std::max( ret, *ptr++ );
02124                         return ret;
02125                 }
02126 
02127                 /** The maximum of a matrix */
02128                 template <size_t NROWS,size_t NCOLS>
02129                 double maximumMatrix_SIMD(const CMatrixFixedNumeric<double,NROWS,NCOLS>& M)
02130                 {
02131                         const size_t N = NROWS*NCOLS;
02132                         const size_t nBlocks = NROWS*NCOLS >> 1;  // /=2
02133                         const double *ptr  = M.m_Val;
02134                         static const double cnst_max = std::numeric_limits<double>::min();
02135                         __m128d  acum = _mm_load1_pd(&cnst_max);
02136                         for (size_t i=0;i<nBlocks;i++)
02137                         {
02138                                 acum = _mm_max_pd(acum, _mm_load_pd(ptr));
02139                                 ptr+=2;
02140                         }
02141                         // The rest, as normal:
02142                         double ret = std::max( ((double*)&acum)[0],((double*)&acum)[1]);
02143                         const size_t Nrest = N-(nBlocks<<1);
02144                         for (size_t i=0;i<Nrest;i++)
02145                                 ret = std::max( ret, *ptr++ );
02146                         return ret;
02147                 }
02148 
02149 #endif  // ----------------------------- end of SSE2 specializations -----------------------------
02150                 /**  @} */
02151 
02152         namespace detail        {
02153                 /**
02154                   * Vicinity traits class specialization for fixed size matrices.
02155                   */
02156                 template<typename T,size_t D> class VicinityTraits<CMatrixFixedNumeric<T,D,D> > {
02157                 public:
02158                         inline static void initialize(CMatrixFixedNumeric<T,D,D> &mat,size_t N) {
02159                                 ASSERT_(N==D);
02160                         }
02161                         inline static void insertInContainer(CMatrixFixedNumeric<T,D,D> &mat,size_t r,size_t c,const T &t)      {
02162                                 mat.get_unsafe(r,c)=t;
02163                         }
02164                 };
02165         }       //End of detail namespace.
02166 
02167 
02168         } // End of namespace
02169 } // End of namespace
02170 
02171 #endif