00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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 namespace mrpt
00035 {
00036 namespace math
00037 {
00038 using namespace mrpt::system;
00039 using namespace mrpt::poses;
00040
00041
00042 template <typename T, size_t N, size_t M> void multiply_HCHt(const CMatrixFixedNumeric<T,N,M> &H,const CMatrixFixedNumeric<T,M,M> &C,CMatrixFixedNumeric<T,N,N> &R,bool accumResultInOutput = false );
00043 template <typename T,size_t NROWS,size_t NCOLS> void invMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv );
00044 template <typename T,size_t NROWS,size_t NCOLS> void invMatrix_destroySrc( CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv );
00045 template <typename T,size_t NROWS,size_t NCOLS> void multiply(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00046 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 );
00047 template <typename T,size_t NROWS,size_t NCOLS> void multiply_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00048 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 );
00049 template <typename T,size_t M1R,size_t M1C> void multiply_AAt(const CMatrixFixedNumeric<T,M1R,M1C>& m1,CMatrixFixedNumeric<T,M1R,M1R>& RESULT );
00050 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 );
00051 template <typename T,size_t NROWS,size_t NCOLS> void sumInPlace(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00052 template <typename T,size_t NROWS,size_t NCOLS> void sumInPlace_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& m,const T val);
00053 template <typename T,size_t NROWS,size_t NCOLS> void sumInPlace(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00054 template <typename T,size_t NROWS,size_t NCOLS> void sumInPlace_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00055 template <typename T,size_t NROWS,size_t NCOLS> void substractInPlace(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00056 template <typename T,size_t NROWS,size_t NCOLS> void substractInPlace_SIMD(CMatrixFixedNumeric<T,NROWS,NCOLS>& M,const CMatrixFixedNumeric<T,NROWS,NCOLS>& A);
00057 template <typename T,size_t NROWS,size_t NCOLS> T sumMatrixAllElements( const CMatrixFixedNumeric<T,NROWS,NCOLS>& M );
00058 template <typename T,size_t NROWS,size_t NCOLS> T sumMatrixAllElements_SIMD( const CMatrixFixedNumeric<T,NROWS,NCOLS>& M );
00059 template <typename T,size_t NROWS,size_t NCOLS> T minimumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00060 template <typename T,size_t NROWS,size_t NCOLS> T minimumMatrix_SIMD(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00061 template <typename T,size_t NROWS,size_t NCOLS> T maximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00062 template <typename T,size_t NROWS,size_t NCOLS> T maximumMatrix_SIMD(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00063 template <typename T,size_t NROWS,size_t NCOLS> void minimumAndMaximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M, T &val_min, T &val_max);
00064 template <typename T,size_t NROWS,size_t NCOLS> void minimumAndMaximumMatrix_SIMD(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M, T &val_min, T &val_max);
00065 template <typename T,size_t NROWS,size_t NCOLS> T detMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00066 template <typename T,size_t NROWS,size_t NCOLS> void sqrtMatrix(CMatrixFixedNumeric<T,NROWS,NCOLS>& M);
00067 template <typename T,size_t N> void eigenVectorsMatrix(const CMatrixFixedNumeric<T,N,N> &M,CMatrixFixedNumeric<T,N,N> &Z,CMatrixFixedNumeric<T,N,N> &D );
00068 template <typename T, size_t N, size_t M> void multiply_HtCH(const CMatrixFixedNumeric<T,M,N> &H,
00069 const CMatrixFixedNumeric<T,M,M> &C,CMatrixFixedNumeric<T,N,N> &R,bool accumResultInOutput = false );
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 template <typename T,size_t NROWS,size_t NCOLS>
00083 class CMatrixFixedNumeric
00084 {
00085 public:
00086 typedef T value_type;
00087
00088
00089 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00090 MRPT_ALIGN16
00091 #endif
00092 T m_Val[ NROWS * NCOLS ];
00093
00094 public:
00095
00096 CMatrixFixedNumeric() {
00097 #if defined(_DEBUG) && MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00098 if ((uintptr_t(m_Val) & 0x0f) != 0 )
00099 THROW_EXCEPTION("16-unaligned memory!")
00100 #endif
00101 ::memset(m_Val,0,sizeof(m_Val));
00102 }
00103
00104
00105 CMatrixFixedNumeric(bool ,bool ) {
00106 }
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 template <typename V, size_t N>
00117 CMatrixFixedNumeric ( V (&theArray)[N] )
00118 {
00119 MRPT_COMPILE_TIME_ASSERT(N!=0)
00120 MRPT_COMPILE_TIME_ASSERT(N==NROWS * NCOLS)
00121 if (sizeof(V)==sizeof(T))
00122 ::memcpy(m_Val,theArray,sizeof(m_Val));
00123 else
00124 for (size_t i=0;i<N;i++)
00125 m_Val[i] = static_cast<T>(theArray[i]);
00126 }
00127
00128
00129
00130 template <size_t N,size_t M>
00131 explicit CMatrixFixedNumeric(const CMatrixFixedNumeric<T,N,M> &B)
00132 {
00133 ::memset(m_Val,0,sizeof(m_Val));
00134 const size_t nr = std::min(NROWS,N);
00135 const size_t nc = std::min(NCOLS,M);
00136 for (size_t r=0;r<nr;r++)
00137 ::memcpy(m_Val+NCOLS*r, B.m_Val+M*r, sizeof(T)*nc );
00138 }
00139
00140
00141
00142 template <typename R>
00143 explicit CMatrixFixedNumeric(const CMatrixFixedNumeric<R,NROWS,NCOLS> &B)
00144 {
00145 for (size_t r=0;r<NROWS;r++)
00146 for (size_t c=0;c<NCOLS;c++)
00147 get_unsafe(r,c) = static_cast<T>( B.get_unsafe(r,c) );
00148 }
00149
00150
00151
00152
00153 template <typename R>
00154 explicit CMatrixFixedNumeric(const CMatrixTemplate<R> &B, bool clipToFixedMatrixSize = false )
00155 {
00156 if (!clipToFixedMatrixSize) {
00157 *this = B;
00158 }
00159 else
00160 {
00161 ::memset(m_Val,0,sizeof(m_Val));
00162 const size_t nr = std::min(NROWS,B.getRowCount());
00163 const size_t nc = std::min(NCOLS,B.getColCount());
00164 for (size_t r=0;r<nr;r++)
00165 for (size_t c=0;c<nc;c++)
00166 get_unsafe(r,c) = B.get_unsafe(r,c);
00167 }
00168 }
00169
00170
00171
00172
00173 CMatrixFixedNumeric<T,NROWS,NCOLS>& operator =(const CMatrixTemplate<T> &B)
00174 {
00175 ASSERT_( NROWS==B.getRowCount() )
00176 ASSERT_( NCOLS==B.getColCount() )
00177 for (size_t r=0;r<NROWS;r++)
00178 ::memcpy(m_Val+r*NCOLS, B.get_unsafe_row(r), sizeof(T)*NCOLS);
00179 return *this;
00180 }
00181
00182
00183
00184
00185 template <typename R>
00186 CMatrixFixedNumeric<T,NROWS,NCOLS>& operator =(const CMatrixTemplate<R> &B)
00187 {
00188 ASSERT_( NROWS==B.getRowCount() )
00189 ASSERT_( NCOLS==B.getColCount() )
00190 for (size_t r=0;r<NROWS;r++)
00191 for (size_t c=0;c<NCOLS;c++)
00192 get_unsafe(r,c) = B.get_unsafe(r,c);
00193 return *this;
00194 }
00195
00196
00197 static size_t getRowCount() {
00198 return NROWS;
00199 }
00200
00201 static size_t getColCount() {
00202 return NCOLS;
00203 }
00204
00205
00206 CMatrixFixedNumeric(const CPoint2D &p) { matrixFromPoseOrPoint(*this,p); }
00207
00208 CMatrixFixedNumeric(const CPoint3D &p) { matrixFromPoseOrPoint(*this,p); }
00209
00210 CMatrixFixedNumeric(const CPose2D &p) { matrixFromPoseOrPoint(*this,p); }
00211
00212 CMatrixFixedNumeric(const CPose3D &p) { matrixFromPoseOrPoint(*this,p); }
00213
00214
00215 CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPoint2D &p) {
00216 return matrixFromPoseOrPoint(*this,p);
00217 }
00218
00219 CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPoint3D &p) {
00220 return matrixFromPoseOrPoint(*this,p);
00221 }
00222
00223 CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPose2D &p) {
00224 return matrixFromPoseOrPoint(*this,p);
00225 }
00226
00227 CMatrixFixedNumeric<T,NROWS,NCOLS> & operator = (const CPose3D &p) {
00228 return matrixFromPoseOrPoint(*this,p);
00229 }
00230
00231
00232 void unit() {
00233 ::memset(m_Val,0,sizeof(m_Val));
00234 for (size_t i=0;i<NROWS * NCOLS;i+=(NROWS+1))
00235 m_Val[i] = 1;
00236 }
00237
00238
00239 void zeros() {
00240 ::memset(m_Val,0,sizeof(m_Val));
00241 }
00242
00243
00244 T get_unsafe(const size_t row, const size_t col) const {
00245 return m_Val[NCOLS*row+col];
00246 }
00247
00248
00249 T& get_unsafe(const size_t row, const size_t col) {
00250 return m_Val[NCOLS*row+col];
00251 }
00252
00253
00254 void set_unsafe(const size_t row, const size_t col, const T val) {
00255 m_Val[NCOLS*row+col] = val;
00256 }
00257
00258
00259
00260 inline T& operator () (const size_t row, const size_t col)
00261 {
00262 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00263 if (row >= NROWS || col >= NCOLS)
00264 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)) );
00265 #endif
00266 return m_Val[NCOLS*row+col];
00267 }
00268
00269
00270
00271 inline T operator () (const size_t row, const size_t col) const
00272 {
00273 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00274 if (row >= NROWS || col >= NCOLS)
00275 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)) );
00276 #endif
00277 return m_Val[NCOLS*row+col];
00278 }
00279
00280
00281
00282
00283 std::string inMatlabFormat() const
00284 {
00285 std::stringstream s;
00286 s << "[";
00287 s << std::scientific;
00288 for (size_t i=0;i<NROWS;i++)
00289 {
00290 for (size_t j=0;j<NCOLS;j++)
00291 s << get_unsafe(i,j) << " ";
00292 if (i<NROWS-1) s << ";";
00293 }
00294 s << "]";
00295 return s.str();
00296 }
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 bool fromMatlabStringFormat(const std::string &s)
00310 {
00311 CMatrixTemplate<T> M;
00312 if (!M.fromMatlabStringFormat(s)) return false;
00313 if (M.getColCount()!=NCOLS || M.getRowCount()!=NROWS) return false;
00314 *this = M;
00315 return true;
00316 }
00317
00318
00319
00320
00321
00322 void inv(CMatrixFixedNumeric<T,NROWS,NCOLS>& out_inv) const {
00323 mrpt::math::invMatrix(*this,out_inv);
00324 }
00325
00326
00327
00328 void inv_fast(CMatrixFixedNumeric<T,NROWS,NCOLS>& out_inv) {
00329 mrpt::math::invMatrix_destroySrc(*this,out_inv);
00330 }
00331
00332
00333 template <size_t NC1>
00334 void multiply(const CMatrixFixedNumeric<T,NROWS,NC1> &A, const CMatrixFixedNumeric<T,NC1,NCOLS> &B ) {
00335 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00336 mrpt::math::multiply_SIMD(A,B,*this);
00337 #else
00338 mrpt::math::multiply(A,B,*this);
00339 #endif
00340 }
00341
00342
00343 template <size_t NC1>
00344 void multiply_AAt(const CMatrixFixedNumeric<T,NROWS,NC1> &A) {
00345 mrpt::math::multiply_AAt(A,*this);
00346 }
00347
00348
00349
00350 void multiply_Ab( const std::vector<T>& a, std::vector<T>& out_v ) const {
00351 mrpt::math::multiply_Ab(*this,a,out_v);
00352 }
00353
00354
00355
00356
00357 template <size_t N1,size_t N2>
00358 void multiply_ABC(
00359 const CMatrixFixedNumeric<T,NROWS,N1> &A,
00360 const CMatrixFixedNumeric<T,N1,N2> &B,
00361 const CMatrixFixedNumeric<T,N2,NCOLS> &C)
00362 {
00363 this->zeros();
00364 for (size_t i=0;i<NROWS;i++)
00365 for (size_t l=0;l<N2;l++)
00366 {
00367 T sumAccumInner = 0;
00368 for (size_t k=0;k<N1;k++)
00369 sumAccumInner += A.get_unsafe(i,k) * B.get_unsafe(k,l);
00370 for (size_t j=0;j<NCOLS;j++)
00371 get_unsafe(i,j) += sumAccumInner * C.get_unsafe(l,j);
00372 }
00373 }
00374
00375
00376
00377
00378 template <size_t N1,size_t N2>
00379 void multiply_ABCt(
00380 const CMatrixFixedNumeric<T,NROWS,N1> &A,
00381 const CMatrixFixedNumeric<T,N1,N2> &B,
00382 const CMatrixFixedNumeric<T,NCOLS,N2> &C)
00383 {
00384 this->zeros();
00385 for (size_t i=0;i<NROWS;i++)
00386 for (size_t l=0;l<N2;l++)
00387 {
00388 T sumAccumInner = 0;
00389 for (size_t k=0;k<N1;k++)
00390 sumAccumInner += A.get_unsafe(i,k) * B.get_unsafe(k,l);
00391 for (size_t j=0;j<NCOLS;j++)
00392 get_unsafe(i,j) += sumAccumInner * C.get_unsafe(j,l);
00393 }
00394 }
00395
00396
00397
00398 template <size_t M1>
00399 void add_AAt(const CMatrixFixedNumeric<T,M1,M1> &A)
00400 {
00401 ASSERT_(M1<NROWS && M1<NCOLS)
00402 for (size_t i=0;i<M1;i++)
00403 for (size_t j=i+1; j < M1; j++)
00404 {
00405 const T v = A.get_unsafe(i,j)+A.get_unsafe(j,i);
00406 get_unsafe(i,j) += v;
00407 get_unsafe(j,i) += v;
00408 }
00409 for (size_t i=0;i<M1;i++)
00410 get_unsafe(i,i) += 2*A.get_unsafe(i,i);
00411 }
00412
00413
00414 void add(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A)
00415 {
00416 for (size_t i=0;i<NROWS*NCOLS;i++)
00417 m_Val[i]+=A.m_Val[i];
00418 }
00419
00420
00421 void substract(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A)
00422 {
00423 for (size_t i=0;i<NROWS*NCOLS;i++)
00424 m_Val[i]-=A.m_Val[i];
00425 }
00426
00427
00428 void add_Ac(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A, const T c)
00429 {
00430 for (size_t i=0;i<NROWS*NCOLS;i++)
00431 m_Val[i]+=A.m_Val[i]*c;
00432 }
00433
00434
00435 void substract_Ac(const CMatrixFixedNumeric<T,NROWS,NCOLS> &A, const T c)
00436 {
00437 for (size_t i=0;i<NROWS*NCOLS;i++)
00438 m_Val[i]-=A.m_Val[i]*c;
00439 }
00440
00441
00442 void operator *= (const T val) {
00443 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00444 mrpt::math::multiply_SIMD(*this,val);
00445 #else
00446 mrpt::math::multiply(*this,val);
00447 #endif
00448 }
00449
00450 void operator /= (const T val) {
00451 ASSERTMSG_(val!=0, "division by zero")
00452 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00453 mrpt::math::multiply_SIMD(*this,T(1)/val);
00454 #else
00455 mrpt::math::multiply(*this,T(1)/val);
00456 #endif
00457 }
00458
00459 void operator += (const T val) {
00460 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00461 mrpt::math::sumInPlace_SIMD(*this,val);
00462 #else
00463 mrpt::math::sumInPlace(*this,val);
00464 #endif
00465 }
00466
00467 void operator -= (const T val) {
00468 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00469 mrpt::math::sumInPlace_SIMD(*this,-val);
00470 #else
00471 mrpt::math::sumInPlace(*this,-val);
00472 #endif
00473 }
00474
00475
00476 void operator += (const CMatrixFixedNumeric<T,NROWS,NCOLS>& m) {
00477 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00478 mrpt::math::sumInPlace_SIMD(*this,m);
00479 #else
00480 mrpt::math::sumInPlace(*this,m);
00481 #endif
00482 }
00483
00484 void operator -= (const CMatrixFixedNumeric<T,NROWS,NCOLS>& m) {
00485 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00486 mrpt::math::substractInPlace_SIMD(*this,m);
00487 #else
00488 mrpt::math::substractInPlace(*this,m);
00489 #endif
00490 }
00491
00492
00493
00494 T sumAll() const {
00495 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00496 return mrpt::math::sumMatrixAllElements_SIMD(*this);
00497 #else
00498 return mrpt::math::sumMatrixAllElements(*this);
00499 #endif
00500 }
00501
00502
00503 T minimum() const {
00504 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00505 return mrpt::math::minimumMatrix_SIMD(*this);
00506 #else
00507 return mrpt::math::minimumMatrix(*this);
00508 #endif
00509 }
00510
00511
00512 T maximum() const {
00513 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00514 return mrpt::math::maximumMatrix_SIMD(*this);
00515 #else
00516 return mrpt::math::maximumMatrix(*this);
00517 #endif
00518 }
00519
00520
00521 void minimumAndMaximum(T &val_min, T &val_max) const {
00522 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00523 mrpt::math::minimumAndMaximumMatrix_SIMD(*this,val_min,val_max);
00524 #else
00525 mrpt::math::minimumAndMaximumMatrix(*this,val_min,val_max);
00526 #endif
00527 }
00528
00529
00530 T det() const {
00531 return mrpt::math::detMatrix(*this);
00532 }
00533
00534
00535 void Sqrt() {
00536 mrpt::math::sqrtMatrix(*this);
00537 }
00538
00539
00540
00541
00542
00543
00544
00545 void eigenVectors( CMatrixFixedNumeric<T,NROWS,NROWS>& Z, CMatrixFixedNumeric<T,NROWS,NROWS>& D) const {
00546 mrpt::math::eigenVectorsMatrix(*this, Z,D);
00547 }
00548
00549
00550 void force_symmetry() {
00551 for (size_t i=0;i<NROWS;i++)
00552 for (size_t j=i+1;j<NCOLS;j++)
00553 get_unsafe(i,j) = get_unsafe(j,i);
00554 }
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566 void saveToTextFile(
00567 const std::string &file,
00568 TMatrixTextFileFormat fileFormat = MATRIX_FORMAT_ENG,
00569 bool appendMRPTHeader = false,
00570 const std::string &userHeader = std::string("")
00571 ) const
00572 {
00573 mrpt::math::saveMatrixToTextFile(*this, file,fileFormat,appendMRPTHeader,userHeader);
00574 }
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591 void multiply_HCHt(
00592 const CMatrixFixedNumeric<T,NCOLS,NCOLS> &C,
00593 CMatrixFixedNumeric<T,NROWS,NROWS> &R,
00594 bool accumResultInOutput = false ) const
00595 {
00596 mrpt::math::multiply_HCHt(*this,C,R,accumResultInOutput);
00597 }
00598
00599
00600 T multiply_HCHt_scalar( const CMatrixFixedNumeric<T,NCOLS,NCOLS> &C ) const
00601 {
00602 ASSERT_(NROWS==1)
00603 CMatrixFixedNumeric<T,NROWS,NROWS> R;
00604 this->multiply_HCHt(C,R);
00605 return R.m_Val[0];
00606 }
00607
00608
00609 void multiply_HtCH(
00610 const CMatrixFixedNumeric<T,NROWS,NROWS> &C,
00611 CMatrixFixedNumeric<T,NCOLS,NCOLS> &R,
00612 bool accumResultInOutput = false ) const
00613 {
00614 mrpt::math::multiply_HtCH(*this,C,R,accumResultInOutput);
00615 }
00616
00617
00618 T multiply_HtCH_scalar( const CMatrixFixedNumeric<T,NROWS,NROWS> &C ) const
00619 {
00620 ASSERT_(NCOLS==1)
00621 CMatrixFixedNumeric<T,NCOLS,NCOLS> R;
00622 this->multiply_HtCH(C,R);
00623 return R.m_Val[0];
00624 }
00625
00626
00627 int pivot(const size_t row)
00628 {
00629 size_t k = row;
00630 double amax,temp;
00631
00632 amax = -1;
00633 for (size_t i=row; i < NROWS; i++)
00634 if ( (temp = fabs( get_unsafe(i,row))) > amax && temp != 0)
00635 {
00636 amax = temp;
00637 k = i;
00638 }
00639 if (get_unsafe(k,row) == T(0))
00640 return -1;
00641 if (k != row)
00642 {
00643
00644 swap_rows(k,row);
00645 return static_cast<int>( k );
00646 }
00647 return 0;
00648 }
00649
00650 void swap_rows(size_t i1,size_t i2)
00651 {
00652 T tmprow[NCOLS];
00653 ::memcpy(tmprow, m_Val+i1*NCOLS, sizeof(tmprow));
00654 ::memcpy(m_Val+i1*NCOLS,m_Val+i2*NCOLS, sizeof(tmprow));
00655 ::memcpy(m_Val+i2*NCOLS,tmprow, sizeof(tmprow));
00656 }
00657
00658
00659 T _E(const size_t row, const size_t col) const {
00660 return m_Val[NCOLS*(row-1)+col-1];
00661 }
00662
00663 T & _E(const size_t row, const size_t col) {
00664 return m_Val[NCOLS*(row-1)+col-1];
00665 }
00666
00667 };
00668
00669
00670
00671 template <typename T,size_t N,size_t M>
00672 void multiply_Ab( const CMatrixFixedNumeric<T,N,M>& A, const std::vector<T>& a, std::vector<T>& out_v )
00673 {
00674
00675
00676
00677 out_v.resize(N);
00678 typename std::vector<T>::const_iterator a_it;
00679 typename std::vector<T>::iterator v_it;
00680 size_t i,j;
00681 for (i=0, v_it=out_v.begin(); i < N; i++)
00682 {
00683 T accum = 0;
00684 for (j=0, a_it=a.begin(); j < M; j++)
00685 accum += *a_it++ * A.get_unsafe(i,j);
00686 *v_it++ = accum;
00687 }
00688 }
00689
00690
00691
00692 template <typename T,size_t M1R,size_t M1C>
00693 void multiply_AAt(
00694 const CMatrixFixedNumeric<T,M1R,M1C>& m1,
00695 CMatrixFixedNumeric<T,M1R,M1R>& RESULT )
00696 {
00697
00698 if ((void*)&m1==(void*)&RESULT)
00699 {
00700
00701 T temp[M1R*M1R];
00702
00703 T *ptr = temp;
00704 size_t i;
00705 for (i=0; i < M1R; i++)
00706 {
00707 for (size_t j=i; j < M1R; j++)
00708 {
00709 T accum = 0;
00710 for (size_t k=0; k < M1C; k++)
00711 accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00712 *(ptr++) = accum;
00713 }
00714 }
00715
00716 ptr = temp;
00717 for (i=0; i < M1R; i++)
00718 for (size_t j=i; j < M1R; j++)
00719 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = *(ptr++);
00720 }
00721 else
00722 {
00723
00724 for (size_t i=0; i < M1R; i++)
00725 {
00726 for (size_t j=i; j < M1R; j++)
00727 {
00728 T accum = 0;
00729 for (size_t k=0; k < M1C; k++)
00730 accum += m1.get_unsafe(i,k) * m1.get_unsafe(j,k);
00731 RESULT.get_unsafe(i,j) = RESULT.get_unsafe(j,i) = accum;
00732 }
00733 }
00734 }
00735 }
00736
00737
00738
00739
00740 template <typename T,size_t NROWS,size_t NCOLS,size_t M1C>
00741 void multiply(
00742 const CMatrixFixedNumeric<T,NROWS,M1C>& m1,
00743 const CMatrixFixedNumeric<T,M1C,NCOLS>& m2,
00744 CMatrixFixedNumeric<T,NROWS,NCOLS>& RESULT )
00745 {
00746 MRPT_TRY_START
00747
00748
00749 if ( (CMatrixFixedNumeric<T,NROWS,NCOLS>*)(&m1)==&RESULT || (CMatrixFixedNumeric<T,NROWS,NCOLS>*)(&m2)==&RESULT)
00750 {
00751
00752 T temp[NROWS*NCOLS];
00753 size_t out_idx = 0;
00754 for (size_t i=0; i < NROWS; i++)
00755 {
00756 for (size_t j=0; j < NCOLS; j++)
00757 {
00758 T accum = 0;
00759 for (size_t k=0; k < M1C; k++)
00760 accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00761 temp[out_idx++] = accum;
00762 }
00763 }
00764
00765
00766 ::memcpy(RESULT.m_Val,temp,sizeof(RESULT.m_Val));
00767 }
00768 else
00769 {
00770
00771 for (size_t i=0; i < NROWS; i++)
00772 {
00773 for (size_t j=0; j < NCOLS; j++)
00774 {
00775 T accum = 0;
00776 for (size_t k=0; k < M1C; k++)
00777 accum += m1.get_unsafe(i,k) * m2.get_unsafe(k,j);
00778 RESULT.get_unsafe(i,j)=accum;
00779 }
00780 }
00781 }
00782
00783 MRPT_TRY_END
00784 }
00785
00786
00787 template <typename T,size_t NROWS,size_t NCOLS>
00788 void multiply(
00789 CMatrixFixedNumeric<T,NROWS,NCOLS>& m,
00790 const T val)
00791 {
00792 for (size_t i=0;i<NROWS*NCOLS;i++)
00793 m.m_Val[i]*=val;
00794 }
00795
00796
00797 template <typename T,size_t NROWS,size_t NCOLS>
00798 void sumInPlace(
00799 CMatrixFixedNumeric<T,NROWS,NCOLS>& m,
00800 const T val)
00801 {
00802 for (size_t i=0;i<NROWS*NCOLS;i++)
00803 m.m_Val[i]+=val;
00804 }
00805
00806
00807 template <typename T,size_t NROWS,size_t NCOLS>
00808 void sumInPlace(
00809 CMatrixFixedNumeric<T,NROWS,NCOLS>& M,
00810 const CMatrixFixedNumeric<T,NROWS,NCOLS>& A)
00811 {
00812 for (size_t i=0;i<NROWS*NCOLS;i++)
00813 M.m_Val[i]+=A.m_Val[i];
00814 }
00815
00816
00817 template <typename T,size_t NROWS,size_t NCOLS>
00818 T sumMatrixAllElements( const CMatrixFixedNumeric<T,NROWS,NCOLS>& M ) {
00819 T r = 0;
00820 for (size_t k=0;k<NROWS*NCOLS;k++)
00821 r+=M.m_Val[k];
00822 return r;
00823 }
00824
00825
00826 template <typename T,size_t NROWS,size_t NCOLS>
00827 T minimumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M) {
00828 T mi = std::numeric_limits<T>::max();
00829 for (size_t i=0;i<NROWS*NCOLS;i++)
00830 mi = std::min(mi,M.m_Val[i]);
00831 return mi;
00832 }
00833
00834
00835 template <typename T,size_t NROWS,size_t NCOLS>
00836 T maximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M) {
00837 T ma = std::numeric_limits<T>::min();
00838 for (size_t i=0;i<NROWS*NCOLS;i++)
00839 ma = std::max(ma,M.m_Val[i]);
00840 return ma;
00841 }
00842
00843
00844 template <typename T,size_t NROWS,size_t NCOLS>
00845 void minimumAndMaximumMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M, T &val_min, T &val_max) {
00846 T mi = std::numeric_limits<T>::max();
00847 T ma = std::numeric_limits<T>::min();
00848 for (size_t i=0;i<NROWS*NCOLS;i++)
00849 {
00850 mi = std::min(mi,M.m_Val[i]);
00851 ma = std::max(ma,M.m_Val[i]);
00852 }
00853 val_min = mi;
00854 val_max = ma;
00855 }
00856
00857
00858 template <typename T,size_t NROWS,size_t NCOLS>
00859 void substractInPlace(
00860 CMatrixFixedNumeric<T,NROWS,NCOLS>& M,
00861 const CMatrixFixedNumeric<T,NROWS,NCOLS>& A)
00862 {
00863 for (size_t i=0;i<NROWS*NCOLS;i++)
00864 M.m_Val[i]-=A.m_Val[i];
00865 }
00866
00867
00868 template <typename T,size_t NROWS,size_t NCOLS>
00869 CMatrixFixedNumeric<T,NROWS,NCOLS> sum(
00870 const CMatrixFixedNumeric<T,NROWS,NCOLS>& A,
00871 const CMatrixFixedNumeric<T,NROWS,NCOLS>& B)
00872 {
00873 CMatrixFixedNumeric<T,NROWS,NCOLS> ret = A;
00874 A+=B;
00875 return A;
00876 }
00877
00878 template <typename T,size_t NROWS,size_t NCOLS>
00879 CMatrixFixedNumeric<T,NROWS,NCOLS> substract(
00880 const CMatrixFixedNumeric<T,NROWS,NCOLS>& A,
00881 const CMatrixFixedNumeric<T,NROWS,NCOLS>& B)
00882 {
00883 CMatrixFixedNumeric<T,NROWS,NCOLS> ret = A;
00884 A-=B;
00885 return A;
00886 }
00887
00888
00889
00890 template <typename T, size_t N, size_t M>
00891 void multiply_HCHt(
00892 const CMatrixFixedNumeric<T,N,M> &H,
00893 const CMatrixFixedNumeric<T,M,M> &C,
00894 CMatrixFixedNumeric<T,N,N> &R,
00895 bool accumResultInOutput )
00896 {
00897 MRPT_TRY_START
00898
00899 ASSERTMSG_( (void*)&C != (void*)&H, "C and H must be different matrices." )
00900 ASSERTMSG_( (void*)&R != (void*)&H, "R and H must be different matrices." )
00901 ASSERTMSG_( (void*)&C != (void*)&R, "C and R must be different matrices.")
00902
00903 CMatrixFixedNumeric<T,N,M> R_;
00904
00905
00906 for (size_t i=0;i<N;i++)
00907 for (size_t j=0;j<M;j++)
00908 {
00909 T sumAccum = 0;
00910 for (size_t l=0;l<M;l++)
00911 sumAccum += H.get_unsafe(i,l) * C.get_unsafe(l,j);
00912 R_.get_unsafe(i,j) = sumAccum;
00913 }
00914
00915
00916 for (size_t i=0;i<N;i++)
00917 for (size_t j=i;j<N;j++)
00918 {
00919 T sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
00920 for (size_t l=0;l<M;l++)
00921 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(j,l);
00922 R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
00923 }
00924 MRPT_TRY_END
00925 }
00926
00927
00928 template <typename T, size_t N, size_t M>
00929 void multiply_HtCH(
00930 const CMatrixFixedNumeric<T,M,N> &H,
00931 const CMatrixFixedNumeric<T,M,M> &C,
00932 CMatrixFixedNumeric<T,N,N> &R,
00933 bool accumResultInOutput )
00934 {
00935 MRPT_TRY_START
00936
00937 ASSERTMSG_( (void*)&C != (void*)&H, "C and H must be different matrices." )
00938 ASSERTMSG_( (void*)&R != (void*)&H, "R and H must be different matrices." )
00939 ASSERTMSG_( (void*)&C != (void*)&R, "C and R must be different matrices.")
00940
00941 CMatrixFixedNumeric<T,N,M> R_;
00942
00943
00944 for (size_t i=0;i<N;i++)
00945 for (size_t j=0;j<M;j++)
00946 {
00947 T sumAccum = 0;
00948 for (size_t l=0;l<M;l++)
00949 sumAccum += H.get_unsafe(l,i) * C.get_unsafe(l,j);
00950 R_.get_unsafe(i,j) = sumAccum;
00951 }
00952
00953
00954 for (size_t i=0;i<N;i++)
00955 for (size_t j=i;j<N;j++)
00956 {
00957 T sumAccum = accumResultInOutput ? R.get_unsafe(i,j) : 0;
00958 for (size_t l=0;l<M;l++)
00959 sumAccum += R_.get_unsafe(i,l) * H.get_unsafe(l,j);
00960 R.get_unsafe(i,j) = R.get_unsafe(j,i) = sumAccum;
00961 }
00962 MRPT_TRY_END
00963 }
00964
00965
00966
00967 typedef CMatrixFixedNumeric<double,2,2> CMatrixDouble22;
00968 typedef CMatrixFixedNumeric<double,3,3> CMatrixDouble33;
00969 typedef CMatrixFixedNumeric<double,4,4> CMatrixDouble44;
00970 typedef CMatrixFixedNumeric<double,6,6> CMatrixDouble66;
00971 typedef CMatrixFixedNumeric<double,1,3> CMatrixDouble13;
00972 typedef CMatrixFixedNumeric<double,3,1> CMatrixDouble31;
00973 typedef CMatrixFixedNumeric<double,1,2> CMatrixDouble12;
00974 typedef CMatrixFixedNumeric<double,2,1> CMatrixDouble21;
00975 typedef CMatrixFixedNumeric<double,6,1> CMatrixDouble61;
00976 typedef CMatrixFixedNumeric<double,1,6> CMatrixDouble16;
00977
00978 typedef CMatrixFixedNumeric<float,2,2> CMatrixFloat22;
00979 typedef CMatrixFixedNumeric<float,3,3> CMatrixFloat33;
00980 typedef CMatrixFixedNumeric<float,4,4> CMatrixFloat44;
00981 typedef CMatrixFixedNumeric<float,6,6> CMatrixFloat66;
00982 typedef CMatrixFixedNumeric<float,1,3> CMatrixFloat13;
00983 typedef CMatrixFixedNumeric<float,3,1> CMatrixFloat31;
00984 typedef CMatrixFixedNumeric<float,1,2> CMatrixFloat12;
00985 typedef CMatrixFixedNumeric<float,2,1> CMatrixFloat21;
00986 typedef CMatrixFixedNumeric<float,6,1> CMatrixFloat61;
00987 typedef CMatrixFixedNumeric<float,1,6> CMatrixFloat16;
00988
00989
00990
00991
00992 template <typename T,size_t NROWS,size_t NCOLS>
00993 CMatrixFixedNumeric<T,NROWS,NCOLS> operator *(const CMatrixFixedNumeric<T,NROWS,NCOLS>& m1,const T v) {
00994 CMatrixFixedNumeric<T,NROWS,NCOLS> res = m1;
00995 res*=v;
00996 return res;
00997 }
00998
00999
01000
01001 template <typename T,size_t NROWS,size_t NCOLS,size_t M1C>
01002 CMatrixFixedNumeric<T,NROWS,NCOLS> operator *(
01003 const CMatrixFixedNumeric<T,NROWS,M1C>& m1,
01004 const CMatrixFixedNumeric<T,M1C,NCOLS>& m2)
01005 {
01006 CMatrixFixedNumeric<T,NROWS,NCOLS> res(false,false);
01007 multiply(m1,m2,res);
01008 return res;
01009 }
01010
01011
01012 template <typename T,size_t NROWS,size_t NCOLS>
01013 CMatrixFixedNumeric<T,NROWS,NCOLS> operator +(
01014 const CMatrixFixedNumeric<T,NROWS,NCOLS>& m1,
01015 const CMatrixFixedNumeric<T,NROWS,NCOLS>& m2)
01016 {
01017 CMatrixFixedNumeric<T,NROWS,NCOLS> res = m1;
01018 res+=m2;
01019 return res;
01020 }
01021
01022
01023 template <typename T,size_t NROWS,size_t NCOLS>
01024 CMatrixFixedNumeric<T,NROWS,NCOLS> operator -(
01025 const CMatrixFixedNumeric<T,NROWS,NCOLS>& m1,
01026 const CMatrixFixedNumeric<T,NROWS,NCOLS>& m2)
01027 {
01028 CMatrixFixedNumeric<T,NROWS,NCOLS> res = m1;
01029 res-=m2;
01030 return res;
01031 }
01032
01033
01034
01035 template <typename T,size_t NROWS,size_t NCOLS>
01036 CMatrixFixedNumeric<T,NCOLS,NROWS> operator -(const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
01037 {
01038 CMatrixFixedNumeric<T,NCOLS,NROWS> res(false,false);
01039 for (size_t i=0; i<NROWS*NCOLS; i++)
01040 res.m_Val[i] = -m.m_Val[i];
01041 return res;
01042 }
01043
01044
01045
01046 template <typename T,size_t NROWS,size_t NCOLS>
01047 CMatrixFixedNumeric<T,NCOLS,NROWS> operator ~(const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
01048 {
01049 CMatrixFixedNumeric<T,NCOLS,NROWS> res(false,false);
01050 for (size_t i=0; i<NROWS; i++)
01051 for (size_t j=0; j<NCOLS; j++)
01052 res.get_unsafe(j,i) = m.get_unsafe(i,j);
01053 return res;
01054 }
01055
01056
01057 template <typename T,size_t NROWS>
01058 CMatrixFixedNumeric<T,NROWS,NROWS> operator !(const CMatrixFixedNumeric<T,NROWS,NROWS>& m)
01059 {
01060 CMatrixFixedNumeric<T,NROWS,NROWS> res(false,false);
01061 m.inv(res);
01062 return res;
01063 }
01064
01065
01066 template <typename T,size_t NROWS,size_t NCOLS>
01067 T detMatrix(const CMatrixFixedNumeric<T,NROWS,NCOLS>& M)
01068 {
01069
01070 ASSERTMSG_(NROWS==NCOLS,"Determinant of non-square matrix")
01071
01072 CMatrixFixedNumeric<T,NROWS,NCOLS> temp(M);
01073 T piv,detVal = T(1);
01074
01075 for (size_t k=0; k < NROWS; k++)
01076 {
01077 int indx = temp.pivot(k);
01078 if (indx == -1)
01079 return 0;
01080 if (indx != 0)
01081 detVal = - detVal;
01082 detVal = detVal * temp.get_unsafe(k,k);
01083
01084 for (size_t i=k+1; i < NROWS; i++)
01085 {
01086 piv = temp.get_unsafe(i,k) / temp.get_unsafe(k,k);
01087 for (size_t j=k+1; j < NROWS; j++)
01088 temp.get_unsafe(i,j) -= piv * temp.get_unsafe(k,j);
01089 }
01090 }
01091 return detVal;
01092 }
01093
01094
01095 template <typename T,size_t NROWS,size_t NCOLS>
01096 void sqrtMatrix(CMatrixFixedNumeric<T,NROWS,NCOLS>& M)
01097 {
01098 for (size_t i=0;i<NROWS*NCOLS;i++)
01099 M.m_Val[i] = sqrt(M.m_Val[i]);
01100 }
01101
01102 template <typename T> void MRPTDLLIMPEXP tred2(T **a, size_t nn, T d[], T e[]);
01103 template <class T> void MRPTDLLIMPEXP tqli(T d[], T e[], size_t nn, T **z);
01104
01105
01106 template <typename T,size_t N>
01107 void eigenVectorsMatrix(
01108 const CMatrixFixedNumeric<T,N,N> &M,
01109 CMatrixFixedNumeric<T,N,N> &Z,
01110 CMatrixFixedNumeric<T,N,N> &D )
01111 {
01112
01113 std::vector<unsigned int> indxs;
01114 std::vector<bool> already;
01115
01116 size_t i,j;
01117 T **a;
01118 T *d,*e;
01119
01120 MRPT_TRY_START
01121
01122
01123
01124
01125
01126 #ifdef _DEBUG
01127 for (i=0;i<N;i++)
01128 for (j=i;j<N;j++)
01129 if (M.get_unsafe(i,j)!=M.get_unsafe(j,i))
01130 {
01131 THROW_EXCEPTION(format("eigenVectors: The matrix is not symmetric! m(%lu,%lu)=%.16e != m(%lu,%lu)=%.16e\n",
01132 static_cast<unsigned long>(i),static_cast<unsigned long>(j), static_cast<double> ( M.get_unsafe(i,j) ),
01133 static_cast<unsigned long>(j),static_cast<unsigned long>(i), static_cast<double> ( M.get_unsafe(j,i) )) )
01134 }
01135 #endif
01136
01137
01138
01139 typedef T* matrix_type_ptr;
01140
01141 a = new matrix_type_ptr[N+1];
01142 for (i=1;i<=N;i++) a[i] = new T[N+1];
01143 d = new T[N+1];
01144 e = new T[N+1];
01145
01146 for (i=1;i<=N;i++)
01147 for (j=1;j<=N;j++)
01148 a[i][j] = M.get_unsafe(i-1,j-1);
01149
01150
01151
01152 tred2( a, N, d, e);
01153 tqli(d,e,N,a);
01154
01155
01156
01157
01158
01159
01160
01161 indxs.resize(N+1);
01162 already.resize(N+1, false);
01163
01164 for (i=1;i<=N;i++)
01165 {
01166 size_t minIndx = std::numeric_limits<size_t>::max();
01167 for (j=1;j<=N;j++)
01168 if (!already[j])
01169 {
01170 if (minIndx==std::numeric_limits<size_t>::max()) minIndx = j;
01171 else
01172 if (d[j]<d[minIndx]) minIndx = j;
01173 }
01174
01175
01176 indxs[i] = static_cast<unsigned int> ( minIndx );
01177 already[minIndx] = true;
01178 }
01179
01180 for (i=1;i<=N;i++)
01181 ASSERT_(already[i]);
01182
01183
01184
01185 for (i=1;i<=N;i++)
01186 for (j=1;j<=N;j++)
01187 {
01188 Z(i-1,j-1) = a[i][indxs[j]];
01189 if (i==j)
01190 {
01191 if (d[indxs[j]]<0)
01192 D(i-1,i-1) = -d[indxs[j]];
01193 else D(i-1,i-1) = d[indxs[j]];
01194 }
01195 else D(i-1,j-1) = 0;
01196 }
01197
01198
01199
01200 for (i=1;i<=N;i++) delete[] a[i];
01201 delete[] a;
01202 delete[] d;
01203 delete[] e;
01204
01205 MRPT_TRY_END_WITH_CLEAN_UP( std::cout << "[eigenVectors] The matrix leading to exception is:" << std::endl << M << std::endl; )
01206 }
01207
01208
01209 template <typename T> void MRPTDLLIMPEXP eigenVectorsMatrix(const CMatrixFixedNumeric<T,2,2> &M,CMatrixFixedNumeric<T,2,2> &Z,CMatrixFixedNumeric<T,2,2> &D );
01210
01211
01212 template <typename T> T detMatrix(const CMatrixFixedNumeric<T,2,2> &M) {
01213 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];
01214 }
01215
01216
01217
01218
01219 template <typename T> T detMatrix(const CMatrixFixedNumeric<T,3,3> &M) {
01220 return M._E(1,1)*(M._E(3,3)*M._E(2,2)-M._E(3,2)*M._E(2,3))-
01221 M._E(2,1)*(M._E(3,3)*M._E(1,2)-M._E(3,2)*M._E(1,3))+
01222 M._E(3,1)*(M._E(2,3)*M._E(1,2)-M._E(2,2)*M._E(1,3));
01223 }
01224
01225
01226
01227 template <typename T> T detMatrix(const CMatrixFixedNumeric<T,4,4> &M) {
01228 const float D1 =
01229 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))-
01230 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))+
01231 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));
01232 const float D2 =
01233 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))-
01234 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))+
01235 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));
01236 const float D3 =
01237 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))-
01238 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))+
01239 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));
01240 const float D4 =
01241 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))-
01242 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))+
01243 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));
01244 return M._E(1,1)*D1 - M._E(1,2)*D2 + M._E(1,3)*D3 - M._E(1,4)*D4;
01245 }
01246
01247
01248
01249
01250 template <typename T,size_t NROWS,size_t NCOLS>
01251 void invMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv ) {
01252 CMatrixFixedNumeric<T,NROWS,NCOLS> temp = M;
01253 invMatrix_destroySrc(temp,out_inv);
01254 }
01255
01256
01257
01258
01259 template <typename T,size_t NROWS,size_t NCOLS>
01260 void invMatrix_destroySrc( CMatrixFixedNumeric<T,NROWS,NCOLS> &M, CMatrixFixedNumeric<T,NROWS,NCOLS> &out_inv ) {
01261
01262 ASSERTMSG_(NROWS==NCOLS,"Inversion of non-square matrix")
01263
01264 T a1,a2;
01265 out_inv.unit();
01266 for (size_t k=0; k < NROWS; k++)
01267 {
01268 int indx = M.pivot(k);
01269 if (indx == -1)
01270 {
01271 std::cerr << "[inv] Matrix that leaded to error is:" << std::endl << M << std::endl;
01272 THROW_EXCEPTION( "Inversion of a singular matrix");
01273 }
01274
01275 if (indx != 0)
01276 {
01277
01278 M.swap_rows(k,indx);
01279 }
01280 a1 = M.get_unsafe(k,k);
01281 const T a1_i = 1/a1;
01282 for (size_t j=0; j < NROWS; j++)
01283 {
01284 M.get_unsafe(k,j) *= a1_i;
01285 out_inv.get_unsafe(k,j) *= a1_i;
01286 }
01287 for (size_t i=0; i < NROWS; i++)
01288 {
01289 if (i != k)
01290 {
01291 a2 = M.get_unsafe(i,k);
01292 for (size_t j=0; j < NROWS; j++)
01293 {
01294 M.get_unsafe(i,j) -= a2 * M.get_unsafe(k,j);
01295 out_inv.get_unsafe(i,j) -= a2 * out_inv.get_unsafe(k,j);
01296 }
01297 }
01298 }
01299 }
01300 }
01301
01302
01303 template <typename T> void invMatrix( const CMatrixFixedNumeric<T,2,2> &M, CMatrixFixedNumeric<T,2,2> &out_inv )
01304 {
01305
01306
01307
01308 const T det = M.det();
01309 ASSERTMSG_(det!=0,"Singular matrix")
01310 const T det_inv = 1.0f / det;
01311 out_inv.m_Val[2*0+0] = M.m_Val[2*1+1];
01312 out_inv.m_Val[2*0+1] = -M.m_Val[2*0+1];
01313 out_inv.m_Val[2*1+0] = -M.m_Val[2*1+0];
01314 out_inv.m_Val[2*1+1] = M.m_Val[2*0+0];
01315 out_inv*= det_inv;
01316 }
01317 template <typename T> void invMatrix_destroySrc(CMatrixFixedNumeric<T,2,2> &M, CMatrixFixedNumeric<T,2,2>& out_inv) { invMatrix(M,out_inv); }
01318
01319
01320 template <typename T> void invMatrix(const CMatrixFixedNumeric<T,3,3> &M, CMatrixFixedNumeric<T,3,3>& out_inv)
01321 {
01322
01323
01324
01325 const T det = M.det();
01326 ASSERTMSG_(det!=0,"Singular matrix")
01327 const T det_inv = 1.0f / det;
01328 out_inv._E(1,1) = (M._E(3,3)*M._E(2,2)-M._E(3,2)*M._E(2,3) );
01329 out_inv._E(1,2) = (-M._E(3,3)*M._E(1,2)+M._E(3,2)*M._E(1,3) );
01330 out_inv._E(1,3) = (M._E(2,3)*M._E(1,2)-M._E(2,2)*M._E(1,3) );
01331 out_inv._E(2,1) = (-M._E(3,3)*M._E(2,1)+M._E(3,1)*M._E(2,3));
01332 out_inv._E(2,2) = (M._E(3,3)*M._E(1,1)-M._E(3,1)*M._E(1,3));
01333 out_inv._E(2,3) = (-M._E(2,3)*M._E(1,1)+M._E(2,1)*M._E(1,3));
01334 out_inv._E(3,1) = (M._E(3,2)*M._E(2,1)-M._E(3,1)*M._E(2,2));
01335 out_inv._E(3,2) = (-M._E(3,2)*M._E(1,1)+M._E(3,1)*M._E(1,2));
01336 out_inv._E(3,3) = (M._E(2,2)*M._E(1,1)-M._E(2,1)*M._E(1,2));
01337 out_inv*= det_inv;
01338 }
01339 template <typename T> void invMatrix_destroySrc(CMatrixFixedNumeric<T,3,3> &M, CMatrixFixedNumeric<T,3,3>& out_inv) { invMatrix(M,out_inv); }
01340
01341
01342
01343 template <typename T,size_t NROWS,size_t NCOLS>
01344 void fixedToDynMatrix( const CMatrixFixedNumeric<T,NROWS,NCOLS> &SRC, CMatrixTemplateNumeric<T> &DST)
01345 {
01346 DST.resize(NROWS,NCOLS);
01347 for (size_t r=0;r<NROWS;r++)
01348 ::memcpy(DST.get_unsafe_row(r), SRC.m_Val+r*NCOLS,sizeof(T)*NCOLS);
01349 }
01350
01351
01352 template <typename T,size_t NROWS,size_t NCOLS>
01353 void insertMatrixFixTransposeIntoDyn(
01354 CMatrixTemplate<T> &M,
01355 const size_t nRow,
01356 const size_t nCol,
01357 const CMatrixFixedNumeric<T,NROWS,NCOLS> &in)
01358 {
01359 ASSERTMSG_( (nRow+NCOLS <= M.getRowCount() ) && (nCol+NROWS<= M.getColCount()), "insertMatrix: Row or Col index out of bounds")
01360 for (size_t c=0;c<NCOLS;c++)
01361 for (size_t r=0;r<NROWS;r++)
01362 M.get_unsafe(nRow+c,nCol+r) = in.get_unsafe(r,c);
01363 }
01364
01365
01366 template <typename T,size_t NROWS,size_t NCOLS>
01367 void insertMatrixFixIntoDyn(
01368 CMatrixTemplate<T> &M,
01369 const size_t nRow,
01370 const size_t nCol,
01371 const CMatrixFixedNumeric<T,NROWS,NCOLS> &in)
01372 {
01373 ASSERTMSG_( (nRow+NROWS <= M.getRowCount() ) && (nCol+NCOLS <= M.getColCount()), "insertMatrix: Row or Col index out of bounds")
01374 for (size_t r=0;r<NROWS;r++)
01375 ::memcpy( M.get_unsafe_row(r+nRow)+nCol, in.m_Val+r*NCOLS, NCOLS*sizeof(T));
01376 }
01377
01378
01379 template <typename T,size_t NROWS,size_t NCOLS>
01380 void extractFixMatrixFromDynMatrix(
01381 const CMatrixTemplate<T> &M,
01382 const size_t nRow,
01383 const size_t nCol,
01384 CMatrixFixedNumeric<T,NROWS,NCOLS> &outMat)
01385 {
01386 ASSERTMSG_( (nRow+NROWS <= M.getRowCount() ) && (nCol+NCOLS <= M.getColCount()), "extractMatrix: Row or Col index out of bounds")
01387 for (size_t r=0;r<NROWS;r++)
01388 ::memcpy( outMat.m_Val+r*NCOLS, M.get_unsafe_row(r+nRow)+nCol, sizeof(T)*NCOLS );
01389 }
01390
01391
01392
01393
01394
01395 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") }
01396 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") }
01397 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") }
01398 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") }
01399
01400 template <> CMatrixDouble21 & matrixFromPoseOrPoint(CMatrixDouble21 &M, const CPoint2D &p);
01401 template <> CMatrixDouble31 & matrixFromPoseOrPoint(CMatrixDouble31 &M, const CPoint3D &p);
01402 template <> CMatrixDouble31 & matrixFromPoseOrPoint(CMatrixDouble31 &M, const CPose2D &p);
01403 template <> CMatrixDouble61 & matrixFromPoseOrPoint(CMatrixDouble61 &M, const CPose3D &p);
01404 template <> CMatrixDouble12 & matrixFromPoseOrPoint(CMatrixDouble12 &M, const CPoint2D &p);
01405 template <> CMatrixDouble13 & matrixFromPoseOrPoint(CMatrixDouble13 &M, const CPoint3D &p);
01406 template <> CMatrixDouble13 & matrixFromPoseOrPoint(CMatrixDouble13 &M, const CPose2D &p);
01407 template <> CMatrixDouble16 & matrixFromPoseOrPoint(CMatrixDouble16 &M, const CPose3D &p);
01408
01409
01410
01411
01412 template <size_t NROWS,size_t NCOLS>
01413 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
01414 CMatrix aux;
01415 in >> aux;
01416 M = aux;
01417 return in;
01418 }
01419
01420 template <size_t NROWS,size_t NCOLS>
01421 mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in, CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
01422 CMatrixD aux;
01423 in >> aux;
01424 M = aux;
01425 return in;
01426 }
01427
01428
01429 template <size_t NROWS,size_t NCOLS>
01430 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<float,NROWS,NCOLS> & M) {
01431 CMatrix aux = CMatrixFloat(M);
01432 out << aux;
01433 return out;
01434 }
01435
01436 template <size_t NROWS,size_t NCOLS>
01437 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const CMatrixFixedNumeric<double,NROWS,NCOLS> & M) {
01438 CMatrixD aux = CMatrixDouble(M);
01439 out << aux;
01440 return out;
01441 }
01442
01443
01444
01445
01446 template <class T,size_t NROWS, size_t NCOLS>
01447 std::ostream& operator << (std::ostream& ostrm, const CMatrixFixedNumeric<T,NROWS,NCOLS>& m)
01448 {
01449 ostrm << std::setprecision(6);
01450
01451 for (size_t i=0; i < NROWS; i++)
01452 {
01453 for (size_t j=0; j < NCOLS; j++)
01454 ostrm << std::setw(10) << m.get_unsafe(i,j);
01455 ostrm << std::endl;
01456 }
01457 return ostrm;
01458 }
01459
01460
01461 template <class T,size_t NROWS, size_t NCOLS>
01462 bool operator == (const CMatrixFixedNumeric<T,NROWS,NCOLS>& M1, const CMatrixFixedNumeric<T,NROWS,NCOLS>& M2)
01463 {
01464 for (size_t i=0; i < NROWS; i++)
01465 for (size_t j=0; j < NCOLS; j++)
01466 if (M1.get_unsafe(i,j)!=M2.get_unsafe(i,j))
01467 return false;
01468 return true;
01469 }
01470
01471
01472
01473
01474
01475 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
01476
01477
01478
01479
01480
01481
01482
01483
01484
01485
01486
01487
01488
01489 template <size_t NROWS,size_t NCOLS>
01490 void multiply_SIMD(
01491 CMatrixFixedNumeric<float,NROWS,NCOLS>& m,
01492 const float val)
01493 {
01494 __m128 cnts = _mm_load1_ps(&val);
01495 const size_t N = NROWS*NCOLS;
01496 const size_t nBlocks = NROWS*NCOLS >> 2;
01497 float *ptr = m.m_Val;
01498 for (size_t i=0;i<nBlocks;i++)
01499 {
01500 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) );
01501 ptr+=4;
01502 }
01503
01504 const size_t Nrest = N-(nBlocks<<2);
01505 for (size_t i=0;i<Nrest;i++, ptr++ )
01506 *ptr *= val;
01507 }
01508
01509
01510 template <>
01511 void multiply_SIMD(
01512 const CMatrixFixedNumeric<float,4,4>& m1,
01513 const CMatrixFixedNumeric<float,4,4>& m2,
01514 CMatrixFixedNumeric<float,4,4>& RESULT )
01515 {
01516 ASSERT_(&m1!=&RESULT && &m2!=&RESULT)
01517
01518 for (unsigned j=0;j<4;j++)
01519 {
01520 __m128 m2col;
01521 ((float*)&m2col)[0] = m2.m_Val[0+j];
01522 ((float*)&m2col)[1] = m2.m_Val[4+j];
01523 ((float*)&m2col)[2] = m2.m_Val[8+j];
01524 ((float*)&m2col)[3] = m2.m_Val[12+j];
01525
01526 for (unsigned i=0;i<4;i++)
01527 {
01528 __m128 aux = _mm_mul_ps( m2col, _mm_load_ps(&m1.m_Val[i<<2]) );
01529 RESULT.m_Val[(i<<2)+j] = ((float*)&aux)[0]+((float*)&aux)[1]+((float*)&aux)[2]+((float*)&aux)[3];
01530 }
01531 }
01532 }
01533
01534
01535 template <>
01536 void multiply_SIMD( CMatrixFixedNumeric<float,2,2>& m, const float val)
01537 {
01538 __m128 cnts = _mm_load1_ps(&val);
01539 float *ptr = m.m_Val;
01540 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) );
01541 }
01542
01543 template <>
01544 void multiply_SIMD( CMatrixFixedNumeric<float,3,3>& m, const float val)
01545 {
01546 __m128 cnts = _mm_load1_ps(&val);
01547 float *ptr = m.m_Val;
01548 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01549 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01550 *ptr++ *= val;
01551 }
01552
01553 template <>
01554 void multiply_SIMD( CMatrixFixedNumeric<float,4,4>& m, const float val)
01555 {
01556 __m128 cnts = _mm_load1_ps(&val);
01557 float *ptr = m.m_Val;
01558 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01559 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01560 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) ); ptr+=4;
01561 _mm_store_ps(ptr, _mm_mul_ps(_mm_load_ps(ptr), cnts ) );
01562 }
01563
01564
01565 template <>
01566 void multiply_SIMD( CMatrixFixedNumeric<double,2,2>& m, const double val)
01567 {
01568 __m128d cnts = _mm_load1_pd(&val);
01569 double *ptr = m.m_Val;
01570 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) ); ptr+=2;
01571 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01572 }
01573
01574 template <>
01575 void multiply_SIMD( CMatrixFixedNumeric<double,3,3>& m, const double val)
01576 {
01577 __m128d cnts = _mm_load1_pd(&val);
01578 double *ptr = m.m_Val;
01579 for (size_t i=0;i<8/2;i++)
01580 {
01581 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01582 ptr+=2;
01583 }
01584 *ptr++ *= val;
01585 }
01586
01587 template <>
01588 void multiply_SIMD( CMatrixFixedNumeric<double,4,4>& m, const double val)
01589 {
01590 __m128d cnts = _mm_load1_pd(&val);
01591 double *ptr = m.m_Val;
01592 for (size_t i=0;i<16/2;i++)
01593 {
01594 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01595 ptr+=2;
01596 }
01597 }
01598
01599
01600 template <size_t NROWS,size_t NCOLS>
01601 void multiply_SIMD(
01602 CMatrixFixedNumeric<double,NROWS,NCOLS>& m,
01603 const double val)
01604 {
01605 __m128d cnts = _mm_load1_pd(&val);
01606 const size_t N = NROWS*NCOLS;
01607 const size_t nBlocks = NROWS*NCOLS >> 1;
01608 double *ptr = m.m_Val;
01609 for (size_t i=0;i<nBlocks;i++)
01610 {
01611 _mm_store_pd(ptr, _mm_mul_pd(_mm_load_pd(ptr), cnts ) );
01612 ptr+=2;
01613 }
01614
01615 const size_t Nrest = N-(nBlocks<<1);
01616 for (size_t i=0;i<Nrest;i++, ptr++ )
01617 *ptr *= val;
01618 }
01619
01620
01621 template <size_t NROWS,size_t NCOLS>
01622 void sumInPlace_SIMD(
01623 CMatrixFixedNumeric<float,NROWS,NCOLS>& m,
01624 const float val)
01625 {
01626 __m128 cnts = _mm_load1_ps(&val);
01627 const size_t N = NROWS*NCOLS;
01628 const size_t nBlocks = NROWS*NCOLS >> 2;
01629 float *ptr = m.m_Val;
01630 for (size_t i=0;i<nBlocks;i++)
01631 {
01632 _mm_store_ps(ptr, _mm_add_ps(_mm_load_ps(ptr), cnts ) );
01633 ptr+=4;
01634 }
01635
01636 const size_t Nrest = N-(nBlocks<<2);
01637 for (size_t i=0;i<Nrest;i++, ptr++ )
01638 *ptr += val;
01639 }
01640
01641
01642 template <size_t NROWS,size_t NCOLS>
01643 void sumInPlace_SIMD(
01644 CMatrixFixedNumeric<double,NROWS,NCOLS>& m,
01645 const double val)
01646 {
01647 __m128d cnts = _mm_load1_pd(&val);
01648 const size_t N = NROWS*NCOLS;
01649 const size_t nBlocks = NROWS*NCOLS >> 1;
01650 double *ptr = m.m_Val;
01651 for (size_t i=0;i<nBlocks;i++)
01652 {
01653 _mm_store_pd(ptr, _mm_add_pd(_mm_load_pd(ptr), cnts ) );
01654 ptr+=2;
01655 }
01656
01657 const size_t Nrest = N-(nBlocks<<1);
01658 for (size_t i=0;i<Nrest;i++, ptr++ )
01659 *ptr += val;
01660 }
01661
01662
01663 template <size_t NROWS,size_t NCOLS>
01664 void sumInPlace_SIMD(
01665 CMatrixFixedNumeric<float,NROWS,NCOLS>& M,
01666 const CMatrixFixedNumeric<float,NROWS,NCOLS>& A)
01667 {
01668 const size_t N = NROWS*NCOLS;
01669 const size_t nBlocks = NROWS*NCOLS >> 2;
01670 float *ptr = M.m_Val;
01671 const float *ptr2 = A.m_Val;
01672 for (size_t i=0;i<nBlocks;i++)
01673 {
01674 _mm_store_ps(ptr, _mm_add_ps(_mm_load_ps(ptr), _mm_load_ps(ptr2)) );
01675 ptr+=4;
01676 ptr2+=4;
01677 }
01678
01679 const size_t Nrest = N-(nBlocks<<2);
01680 for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01681 *ptr += *ptr2;
01682 }
01683
01684
01685 template <size_t NROWS,size_t NCOLS>
01686 void sumInPlace_SIMD(
01687 CMatrixFixedNumeric<double,NROWS,NCOLS>& M,
01688 const CMatrixFixedNumeric<double,NROWS,NCOLS>& A)
01689 {
01690 const size_t N = NROWS*NCOLS;
01691 const size_t nBlocks = NROWS*NCOLS >> 1;
01692 double *ptr = M.m_Val;
01693 const double *ptr2 = A.m_Val;
01694 for (size_t i=0;i<nBlocks;i++)
01695 {
01696 _mm_store_pd(ptr, _mm_add_pd(_mm_load_pd(ptr), _mm_load_pd(ptr2)) );
01697 ptr+=2;
01698 ptr2+=2;
01699 }
01700
01701 const size_t Nrest = N-(nBlocks<<1);
01702 for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01703 *ptr += *ptr2;
01704 }
01705
01706
01707 template <size_t NROWS,size_t NCOLS>
01708 void substractInPlace_SIMD(
01709 CMatrixFixedNumeric<float,NROWS,NCOLS>& M,
01710 const CMatrixFixedNumeric<float,NROWS,NCOLS>& A)
01711 {
01712 const size_t N = NROWS*NCOLS;
01713 const size_t nBlocks = NROWS*NCOLS >> 2;
01714 float *ptr = M.m_Val;
01715 const float *ptr2 = A.m_Val;
01716 for (size_t i=0;i<nBlocks;i++)
01717 {
01718 _mm_store_ps(ptr, _mm_sub_ps(_mm_load_ps(ptr), _mm_load_ps(ptr2)) );
01719 ptr+=4;
01720 ptr2+=4;
01721 }
01722
01723 const size_t Nrest = N-(nBlocks<<2);
01724 for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01725 *ptr -= *ptr2;
01726 }
01727
01728
01729 template <size_t NROWS,size_t NCOLS>
01730 void substractInPlace_SIMD(
01731 CMatrixFixedNumeric<double,NROWS,NCOLS>& M,
01732 const CMatrixFixedNumeric<double,NROWS,NCOLS>& A)
01733 {
01734 const size_t N = NROWS*NCOLS;
01735 const size_t nBlocks = NROWS*NCOLS >> 1;
01736 double *ptr = M.m_Val;
01737 const double *ptr2 = A.m_Val;
01738 for (size_t i=0;i<nBlocks;i++)
01739 {
01740 _mm_store_pd(ptr, _mm_sub_pd(_mm_load_pd(ptr), _mm_load_pd(ptr2)) );
01741 ptr+=2;
01742 ptr2+=2;
01743 }
01744
01745 const size_t Nrest = N-(nBlocks<<1);
01746 for (size_t i=0;i<Nrest;i++, ptr++,ptr2++ )
01747 *ptr -= *ptr2;
01748 }
01749
01750
01751 template <size_t NROWS,size_t NCOLS>
01752 float sumMatrixAllElements_SIMD( const CMatrixFixedNumeric<float,NROWS,NCOLS>& M )
01753 {
01754 const size_t N = NROWS*NCOLS;
01755 const size_t nBlocks = NROWS*NCOLS >> 2;
01756 const float *ptr = M.m_Val;
01757 __m128 acum = _mm_setzero_ps();
01758 for (size_t i=0;i<nBlocks;i++)
01759 {
01760 acum = _mm_add_ps(acum, _mm_load_ps(ptr));
01761 ptr+=4;
01762 }
01763
01764 float ret = ((float*)&acum)[0]+((float*)&acum)[1]+((float*)&acum)[2]+((float*)&acum)[3];
01765 const size_t Nrest = N-(nBlocks<<2);
01766 for (size_t i=0;i<Nrest;i++)
01767 ret += *ptr++;
01768 return ret;
01769 }
01770
01771
01772 template <size_t NROWS,size_t NCOLS>
01773 double sumMatrixAllElements_SIMD( const CMatrixFixedNumeric<double,NROWS,NCOLS>& M )
01774 {
01775 const size_t N = NROWS*NCOLS;
01776 const size_t nBlocks = NROWS*NCOLS >> 1;
01777 const double *ptr = M.m_Val;
01778 __m128d acum = _mm_setzero_pd();
01779 for (size_t i=0;i<nBlocks;i++)
01780 {
01781 acum = _mm_add_pd(acum, _mm_load_pd(ptr));
01782 ptr+=2;
01783 }
01784
01785 double ret = ((double*)&acum)[0]+((double*)&acum)[1];
01786 const size_t Nrest = N-(nBlocks<<1);
01787 for (size_t i=0;i<Nrest;i++)
01788 ret += *ptr++;
01789 return ret;
01790 }
01791
01792
01793 template <size_t NROWS,size_t NCOLS>
01794 float minimumMatrix_SIMD(const CMatrixFixedNumeric<float,NROWS,NCOLS>& M)
01795 {
01796 const size_t N = NROWS*NCOLS;
01797 const size_t nBlocks = NROWS*NCOLS >> 2;
01798 const float *ptr = M.m_Val;
01799 static const float cnst_max = std::numeric_limits<float>::max();
01800 __m128 acum = _mm_load1_ps(&cnst_max);
01801 for (size_t i=0;i<nBlocks;i++)
01802 {
01803 acum = _mm_min_ps(acum, _mm_load_ps(ptr));
01804 ptr+=4;
01805 }
01806
01807 float ret = std::min( std::min( ((float*)&acum)[0],((float*)&acum)[1]), std::min(((float*)&acum)[2],((float*)&acum)[3]));
01808 const size_t Nrest = N-(nBlocks<<2);
01809 for (size_t i=0;i<Nrest;i++)
01810 ret = std::min( ret, *ptr++ );
01811 return ret;
01812 }
01813
01814
01815 template <size_t NROWS,size_t NCOLS>
01816 double minimumMatrix_SIMD(const CMatrixFixedNumeric<double,NROWS,NCOLS>& M)
01817 {
01818 const size_t N = NROWS*NCOLS;
01819 const size_t nBlocks = NROWS*NCOLS >> 1;
01820 const double *ptr = M.m_Val;
01821 static const double cnst_max = std::numeric_limits<double>::max();
01822 __m128d acum = _mm_load1_pd(&cnst_max);
01823 for (size_t i=0;i<nBlocks;i++)
01824 {
01825 acum = _mm_min_pd(acum, _mm_load_pd(ptr));
01826 ptr+=2;
01827 }
01828
01829 double ret = std::min( ((double*)&acum)[0],((double*)&acum)[1]);
01830 const size_t Nrest = N-(nBlocks<<1);
01831 for (size_t i=0;i<Nrest;i++)
01832 ret = std::min( ret, *ptr++ );
01833 return ret;
01834 }
01835
01836
01837 template <size_t NROWS,size_t NCOLS>
01838 float maximumMatrix_SIMD(const CMatrixFixedNumeric<float,NROWS,NCOLS>& M)
01839 {
01840 const size_t N = NROWS*NCOLS;
01841 const size_t nBlocks = NROWS*NCOLS >> 2;
01842 const float *ptr = M.m_Val;
01843 static const float cnst_max = std::numeric_limits<float>::min();
01844 __m128 acum = _mm_load1_ps(&cnst_max);
01845 for (size_t i=0;i<nBlocks;i++)
01846 {
01847 acum = _mm_max_ps(acum, _mm_load_ps(ptr));
01848 ptr+=4;
01849 }
01850
01851 float ret = std::max( std::max( ((float*)&acum)[0],((float*)&acum)[1]), std::max(((float*)&acum)[2],((float*)&acum)[3]));
01852 const size_t Nrest = N-(nBlocks<<2);
01853 for (size_t i=0;i<Nrest;i++)
01854 ret = std::max( ret, *ptr++ );
01855 return ret;
01856 }
01857
01858
01859 template <size_t NROWS,size_t NCOLS>
01860 double maximumMatrix_SIMD(const CMatrixFixedNumeric<double,NROWS,NCOLS>& M)
01861 {
01862 const size_t N = NROWS*NCOLS;
01863 const size_t nBlocks = NROWS*NCOLS >> 1;
01864 const double *ptr = M.m_Val;
01865 static const double cnst_max = std::numeric_limits<double>::min();
01866 __m128d acum = _mm_load1_pd(&cnst_max);
01867 for (size_t i=0;i<nBlocks;i++)
01868 {
01869 acum = _mm_max_pd(acum, _mm_load_pd(ptr));
01870 ptr+=2;
01871 }
01872
01873 double ret = std::max( ((double*)&acum)[0],((double*)&acum)[1]);
01874 const size_t Nrest = N-(nBlocks<<1);
01875 for (size_t i=0;i<Nrest;i++)
01876 ret = std::max( ret, *ptr++ );
01877 return ret;
01878 }
01879
01880 #endif // ----------------------------- end of SSE2 specializations -----------------------------
01881
01882
01883
01884 }
01885 }
01886
01887 #endif