v0.3.5:
[mldemos:mldemos.git] / _3rdParty / MathLib / TMatrix.h
1 /*
2  * Copyright (C) 2010 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
3  * Author: Eric Sauser
4  * email:   eric.sauser@a3.epf.ch
5  * website: lasa.epfl.ch
6  *
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15  */
16
17 #ifndef __TMATRIX_H__
18 #define __TMATRIX_H__
19
20 #include "MathLibCommon.h"
21
22 #define  USE_T_EXTENSIONS
23
24
25 #include "TVector.h"
26 #include "Matrix.h"
27
28 #ifdef USE_MATHLIB_NAMESPACE
29 namespace MathLib {
30 #endif
31
32 /**
33  * \class TMatrix
34  *
35  * \ingroup MathLib
36  *
37  * \brief The basic template square matrix class. See the Matrix class for the explanation of the functions.
38  *
39  * This template square matrix class can be used for doing various matrix manipulation.
40  * This should be combined with the TVector class for doing almost anything
41  * you ever dreamt of.
42  */
43 template<unsigned int ROW> class TMatrix
44 {
45 protected:
46     static REALTYPE _s[ROW*ROW];
47     static float    _sf[ROW*ROW];
48     static REALTYPE undef;
49
50 public:
51     REALTYPE _[ROW*ROW];
52
53 public:
54     /// Empty constructor
55     inline TMatrix(bool clear = true){
56         if(clear) Zero();
57     }
58
59     /// Copy constructor
60     inline TMatrix(const TMatrix<ROW> & matrix){
61         Set(matrix);
62     }
63
64     /// Constructor with data pointer to be copied in the matrix
65     inline TMatrix(const REALTYPE *_){
66         Set(_);
67     }
68
69
70     /// copy of the argument
71     inline TMatrix<ROW>& Set(const TMatrix<ROW> & matrix){
72         memcpy(_,matrix._,ROW*ROW*sizeof(REALTYPE));
73         return *this;
74     }
75
76     /// Assigment of data pointer
77     inline TMatrix<ROW>& Set(const REALTYPE *array){
78         if(array) memcpy(_,array,ROW*ROW*sizeof(REALTYPE));
79         return *this;
80     }
81
82     /// Assigment of data pointer
83     inline TMatrix<ROW>& SetForceFloat(const float *array){
84         float    *arrayPos = const_cast<float*>(array);
85         REALTYPE *dst      = _;
86         unsigned int len = ROW*ROW;
87         while(len--)
88             *(dst++) = REALTYPE(*(arrayPos++));
89         return *this;
90     }
91
92     /// Assigment of data pointer
93     inline TMatrix<ROW>& Set(const Matrix& matrix){
94         if((matrix.column == ROW)&&(matrix.row>=ROW)){
95             memcpy(_,matrix._,ROW*ROW*sizeof(REALTYPE));
96         }else{
97             unsigned int len = MIN(ROW*ROW,matrix.row*matrix.column);
98             memcpy(_,matrix._,len*sizeof(REALTYPE));
99         }
100         return *this;
101     }
102
103     /// Gets the data array
104     inline REALTYPE* Array() const{
105         return (REALTYPE*)_;
106     }
107
108     /// Sets all values to zero
109     inline TMatrix<ROW>& Zero(){
110         memset(_,0,ROW*ROW*sizeof(REALTYPE));
111         return *this;
112     }
113
114     /// Sets the matrix to identity
115     inline TMatrix<ROW>& Identity(){
116         Zero();
117         REALTYPE *dst = _;
118         unsigned len = ROW;
119         while(len--){
120             *dst = R_ONE;
121             dst += ROW+1;
122         }
123         return *this;
124     }
125
126     /// Gets a reference to the given element
127     inline REALTYPE& operator() (const unsigned int row, const unsigned int col) {
128         if((row<ROW)&&(col<ROW))
129             return _[row*ROW + col];
130         return undef;
131     }
132     inline REALTYPE& Ref(const unsigned int row, const unsigned int col) {
133         if((row<ROW)&&(col<ROW))
134             return _[row*ROW + col];
135         return undef;
136     }
137     inline REALTYPE& RefNoCheck(const unsigned int row, const unsigned int col) {
138         return _[row*ROW + col];
139     }
140     /// Gets the value of the given element
141     inline REALTYPE At(const unsigned int row, const unsigned int col) const{
142         if((row<ROW)&&(col<ROW))
143             return _[row*ROW + col];
144         return undef;
145     }
146     inline REALTYPE AtNoCheck(const unsigned int row, const unsigned int col) const{
147         return _[row*ROW + col];
148     }
149
150     /// Returns the given column
151     inline TVector<ROW> GetColumn(unsigned int col) const{
152         TVector<ROW> vector(false);
153         return GetColumn(col,vector);
154     }
155     /// Returns the given column in the vector
156     inline TVector<ROW>& GetColumn(unsigned int col, TVector<ROW> &result) const {
157         if(col<ROW){
158             REALTYPE *src = (REALTYPE*)_ + col;
159             REALTYPE *dst = result._;
160             unsigned int len = ROW;
161             while(len--){
162                 *(dst++) = *(src);
163                 src+=ROW;
164             }
165         }else{
166             result.Zero();
167         }
168         return result;
169     }
170
171     /// Returns the given row
172     inline TVector<ROW> GetRow(unsigned int row) const {
173         TVector<ROW> vector(false);
174         return GetRow(row,vector);
175     }
176     /// Returns the given column in the vector
177     inline TVector<ROW>& GetRow(unsigned int row, TVector<ROW> &result) const {
178         if(row<ROW){
179             REALTYPE *src = _ + row*ROW;
180             REALTYPE *dst = result._;
181             unsigned int len = ROW;
182             while(len--)
183                 *(dst++) = *(src++);
184         }else{
185             result.Zero();
186         }
187         return result;
188     }
189
190
191     /// Sets the given column
192     inline TMatrix<ROW>& SetColumn(const TVector<ROW> &vector, unsigned int col){
193         if(col<ROW){
194             REALTYPE *dst = _ + col;
195             REALTYPE *src = (REALTYPE*)vector._;
196             unsigned int len = ROW;
197             while(len--){
198                 *(dst) = *(src++);
199                 dst+=ROW;
200             }
201         }
202         return *this;
203     }
204     /// Sets the given row
205     inline TMatrix<ROW>& SetRow(const TVector<ROW> &vector, unsigned int row){
206         if(row<ROW){
207             REALTYPE *dst = _ + row*ROW;
208             REALTYPE *src = (REALTYPE*)vector._;
209             unsigned int len = ROW;
210             while(len--)
211                 *(dst++) = *(src++);
212         }
213         return *this;
214     }
215
216
217     /// Assignment operator
218     inline TMatrix<ROW>& operator = (const TMatrix<ROW> &matrix){
219         return Set(matrix);
220     }
221
222     /// Inverse operator
223     inline TMatrix<ROW> operator - () const{
224         TMatrix<ROW> result(false);
225         return Minus(result);
226     }
227     /// Inverse operator
228     inline TMatrix<ROW>& Minus(TMatrix<ROW>& result) const{
229         REALTYPE *dst = result._, *src =(REALTYPE*) _; unsigned len = ROW*ROW;
230         while(len--) *(dst++) = -*(src++);
231         return result;
232     }
233     /// Inverse operator
234     inline TMatrix<ROW>& SMinus(){
235         REALTYPE *src = _; unsigned len = ROW*ROW;
236         while(len--){
237             *(src) = -*(src);
238             src++;
239         }
240         return *this;
241     }
242
243
244     /// Assignment data-wise operations
245     inline TMatrix<ROW>& operator += (const TMatrix<ROW> &matrix) {
246         REALTYPE *dst = (REALTYPE*)_, *src = (REALTYPE*)matrix._;
247         unsigned int len = ROW*ROW;
248         while(len--) *(dst++) += (*(src++));
249         return *this;
250     }
251     inline TMatrix<ROW>& operator -= (const TMatrix<ROW> &matrix) {
252         REALTYPE *dst = (REALTYPE*)_, *src = (REALTYPE*)matrix._;
253         unsigned int len = ROW*ROW;
254         while(len--) *(dst++) -= (*(src++));
255         return *this;
256     }
257     inline TMatrix<ROW>& operator ^= (const TMatrix<ROW> &matrix) {
258         REALTYPE *dst = (REALTYPE*)_, *src = (REALTYPE*)matrix._;
259         unsigned int len = ROW*ROW;
260         while(len--) *(dst++) *= (*(src++));
261         return *this;
262     }
263     inline TMatrix<ROW>& operator /= (const TMatrix<ROW> &matrix) {
264         REALTYPE *dst = (REALTYPE*)_, *src = (REALTYPE*)matrix._;
265         unsigned int len = ROW*ROW;
266         while(len--) *(dst++) /= (*(src++));
267         return *this;
268     }
269     inline TMatrix<ROW>& operator *= (const TMatrix<ROW> &matrix) {
270         TMatrix<ROW> copy(*this);
271         copy.Mult(matrix,*this);
272         return *this;
273     }
274
275
276
277
278
279
280
281     inline TMatrix<ROW>& operator += (REALTYPE scalar){
282         REALTYPE *src = _; unsigned len = ROW*ROW;
283         while(len--) *(src++) += scalar;
284         return *this;
285     }
286     inline TMatrix<ROW>& operator -= (REALTYPE scalar){
287         REALTYPE *src = _; unsigned len = ROW*ROW;
288         while(len--) *(src++) -= scalar;
289         return *this;
290     }
291     inline TMatrix<ROW>& operator *= (REALTYPE scalar){
292         REALTYPE *src = _; unsigned len = ROW*ROW;
293         while(len--) *(src++) *= scalar;
294         return *this;
295     }
296     inline TMatrix<ROW>& operator /= (REALTYPE scalar){
297         scalar = R_ONE/scalar;
298         REALTYPE *src = _; unsigned len = ROW*ROW;
299         while(len--) *(src++) *= scalar;
300         return *this;
301     }
302
303
304     inline TMatrix<ROW> operator + (const TMatrix<ROW> &matrix) const{
305         TMatrix<ROW> result(false);
306         return Add(matrix,result);
307     }
308     inline TMatrix<ROW> operator - (const TMatrix<ROW> &matrix) const{
309         TMatrix<ROW> result(false);
310         return Sub(matrix,result);
311     }
312     inline TMatrix<ROW> operator ^ (const TMatrix<ROW> &matrix) const{
313         TMatrix<ROW> result(false);
314         return PMult(matrix,result);
315     }
316     inline TMatrix<ROW> operator / (const TMatrix<ROW> &matrix) const{
317         TMatrix<ROW> result(false);
318         return PDiv(matrix,result);
319     }
320     inline TMatrix<ROW> operator * (const TMatrix<ROW> &matrix) const{
321         TMatrix<ROW> result(false);
322         return Mult(matrix,result);
323     }
324
325
326     inline TMatrix<ROW>& Add(const TMatrix<ROW> &matrix, TMatrix<ROW> & result) const{
327         REALTYPE *src0 = (REALTYPE*)_, *src1 = (REALTYPE*)matrix._, *dst = (REALTYPE*)result._;
328         unsigned int len = ROW*ROW;
329         while(len--)
330                 *(dst++) = (*(src0++)) + (*(src1++));
331         return result;
332     }
333     inline TMatrix<ROW>& Sub(const TMatrix<ROW> &matrix, TMatrix<ROW> & result) const{
334         REALTYPE *src0 = (REALTYPE*)_, *src1 = (REALTYPE*)matrix._, *dst = (REALTYPE*)result._;
335         unsigned int len = ROW*ROW;
336         while(len--)
337                 *(dst++) = (*(src0++)) - (*(src1++));
338         return result;
339     }
340     inline TMatrix<ROW>& PMult(const TMatrix<ROW> &matrix, TMatrix<ROW> & result) const{
341         REALTYPE *src0 = (REALTYPE*)_, *src1 = (REALTYPE*)matrix._, *dst = (REALTYPE*)result._;
342         unsigned int len = ROW*ROW;
343         while(len--)
344                 *(dst++) = (*(src0++)) * (*(src1++));
345         return result;
346     }
347     inline TMatrix<ROW>& PDiv(const TMatrix<ROW> &matrix, TMatrix<ROW> & result) const{
348         REALTYPE *src0 = (REALTYPE*)_, *src1 = (REALTYPE*)matrix._, *dst = (REALTYPE*)result._;
349         unsigned int len = ROW*ROW;
350         while(len--)
351                 *(dst++) = (*(src0++)) / (*(src1++));
352         return result;
353     }
354     inline TMatrix<ROW>& Mult(const TMatrix<ROW> &matrix, TMatrix<ROW> & result) const{
355         result.Zero();
356         REALTYPE *cP1   = (REALTYPE*)_;
357         REALTYPE *eP1   = cP1 + ROW*ROW;
358         REALTYPE *cD    = result._;
359         while(cP1!=eP1){
360             REALTYPE *currP1  = cP1;
361             REALTYPE *endP1   = currP1 + ROW;
362             REALTYPE *currP2  = (REALTYPE*)matrix._;
363             while(currP1!=endP1){
364                 REALTYPE *currPD  = cD;
365                 REALTYPE  curr1   = *currP1;
366                 REALTYPE *endP2   = currP2 + ROW;
367                 while(currP2!=endP2){
368                     (*currPD++) += curr1 * (*(currP2++));
369                 }
370                 currP1++;
371             }
372             cD  += ROW;
373             cP1 += ROW;
374         }
375         return result;
376     }
377     inline TMatrix<ROW>& TransposeMult(const TMatrix<ROW>& matrix, TMatrix<ROW>& result) const
378     {
379         result.Zero();
380         REALTYPE *cP1   = _;
381         REALTYPE *cP2   = matrix._;
382         unsigned int kk = ROW;
383         while(kk--){
384             REALTYPE *currD  = result._;
385             REALTYPE *currP1 = cP1;
386             unsigned int len1 = ROW;
387             while(len1--){
388                 REALTYPE *currP2 = cP2;
389                 unsigned int len2 = ROW;
390                 while(len2--){
391                     *(currD++) += *(currP1) *(*(currP2++));
392                 }
393                 currP1++;
394             }
395             cP2 += ROW;
396             cP1 += ROW;
397         }
398         return result;
399     }
400     inline TMatrix<ROW>& MultTranspose(const TMatrix<ROW>& matrix, TMatrix<ROW>& result) const{
401         REALTYPE *cP1   = _;
402         REALTYPE *currD = result._;
403         unsigned int len1 = ROW;
404         while(len1--){
405             REALTYPE *currP2 = matrix._;
406             unsigned int len2 = ROW;
407             while(len2--){
408                 REALTYPE *currP1 = cP1;
409                 REALTYPE sum = R_ZERO;
410                 unsigned int len = ROW;
411                 while(len--)
412                     sum += *(currP1++) * (*(currP2++));
413                 *(currD++) = sum;
414             }
415             cP1 += ROW;
416         }
417         return result;
418     }
419
420
421     inline TMatrix<ROW> operator + (REALTYPE scalar) const {
422         TMatrix<ROW> result(false);
423         return Add(scalar,result);
424     }
425     inline TMatrix<ROW> operator - (REALTYPE scalar) const {
426         TMatrix<ROW> result(false);
427         return Sub(scalar,result);
428     }
429     inline TMatrix<ROW> operator * (REALTYPE scalar) const {
430         TMatrix<ROW> result(false);
431         return Mult(scalar,result);
432     }
433     inline TMatrix<ROW> operator / (REALTYPE scalar) const {
434         TMatrix<ROW> result(false);
435         return Div(scalar,result);
436     }
437
438     inline TMatrix<ROW>& Add(REALTYPE scalar, TMatrix<ROW> & result) const{
439         REALTYPE *dst = (REALTYPE*)result._, *src = (REALTYPE*)_; unsigned len = ROW*ROW;
440         while(len--) *(dst++) = *(src++) + scalar;
441         return result;
442     }
443     inline TMatrix<ROW>& Sub(REALTYPE scalar, TMatrix<ROW> & result) const{
444         REALTYPE *dst = (REALTYPE*)result._, *src = (REALTYPE*)_; unsigned len = ROW*ROW;
445         while(len--) *(dst++) = *(src++) - scalar;
446         return result;
447     }
448     inline TMatrix<ROW>& Mult(REALTYPE scalar, TMatrix<ROW> & result) const{
449         REALTYPE *dst = (REALTYPE*)result._, *src = (REALTYPE*)_; unsigned len = ROW*ROW;
450         while(len--) *(dst++) = *(src++) * scalar;
451         return result;
452     }
453     inline TMatrix<ROW>& Div(REALTYPE scalar, TMatrix<ROW> & result) const{
454         scalar = R_ONE / scalar;
455         REALTYPE *dst = (REALTYPE*)result._, *src = (REALTYPE*)_; unsigned len = ROW*ROW;
456         while(len--) *(dst++) = *(src++) * scalar;
457         return result;
458     }
459
460
461
462     inline TVector<ROW> operator * (const TVector<ROW> &vector) const{
463         TVector<ROW> result(false);
464         return Mult(vector,result);
465     }
466     inline TVector<ROW> Mult(const TVector<ROW> &vector) const {
467         TVector<ROW> result(false);
468         return Mult(vector,result);
469     }
470     inline TVector<ROW>& Mult(const TVector<ROW> &vector, TVector<ROW> & result) const {
471         REALTYPE *src = (REALTYPE*)_;
472         REALTYPE *dst = result._;
473         unsigned int rowLen = ROW;
474         while(rowLen--){
475             REALTYPE sum = R_ZERO;
476             REALTYPE *vSrc = (REALTYPE*)vector._;
477             unsigned int colLen = ROW;
478             while(colLen--)
479                 sum +=*(src++) * (*(vSrc++));
480             *(dst++) = sum;
481         }
482         return result;
483     }
484
485     inline TVector<ROW> TransposeMult(const TVector<ROW> &vector) const{
486         TVector<ROW> result(false);
487         return MultTranspose(vector,result);
488     }
489     inline TVector<ROW>& TransposeMult(const TVector<ROW> &vector, TVector<ROW> & result) const{
490         result.Zero();
491         REALTYPE *src = (REALTYPE*)_;
492         REALTYPE *vSrc = (REALTYPE*)vector._;
493         unsigned int rowLen = ROW;
494         while(rowLen--){
495             REALTYPE *dst = result._;
496             unsigned int colLen = ROW;
497             while(colLen--)
498                 *(dst++) += *(src++) * (*vSrc);
499             vSrc++;
500         }
501         return result;
502     }
503
504     /// Tests equality of two matrices
505     inline bool operator == (const TMatrix<ROW>& matrix) const {
506         REALTYPE *src = (REALTYPE*) _;
507         REALTYPE *dst = (REALTYPE*) matrix._;
508         unsigned int len = ROW*ROW;
509         while(len--)
510             if(*(src++) != *(dst++)) return false;
511         return true;
512     }
513     /// tests inequality of two vectors
514     inline bool operator != (const TMatrix<ROW>& matrix) const {
515         return !(*this ==  matrix);
516     }
517
518     inline TMatrix<ROW> Abs(){
519         TMatrix<ROW> result(false);
520         Abs(result);
521         return result;
522     }
523     inline TMatrix<ROW>& Abs(const TMatrix<ROW>& result){
524         REALTYPE *src = _;
525         REALTYPE *dst = result._;
526         unsigned int len = ROW*ROW;
527         while(len--)
528             *(dst++) = fabs(*(src++));
529         return result;
530     }
531     inline TMatrix<ROW>& SAbs(){
532         REALTYPE *src = _;
533         unsigned int len = ROW*ROW;
534         while(len--){
535             *src = fabs(*src);
536             src++;
537         }
538         return *this;
539     }
540
541     inline REALTYPE Sum(){
542         REALTYPE sum = R_ZERO;
543         REALTYPE *src = _;
544         unsigned int len = ROW*ROW;
545         while(len--)
546             sum += *(src++);
547         return sum;
548     }
549
550
551     /// Self transpose
552     inline TMatrix<ROW> STranspose(){
553         REALTYPE tmp;
554         REALTYPE *src = _+1;
555         REALTYPE *dst = _+ROW;
556         unsigned int rowLen = ROW;
557         while(rowLen--){
558             REALTYPE *cDst = dst;
559             unsigned int colLen = rowLen;
560             while(colLen--){
561                 tmp      = *cDst;
562                 *cDst    = *src;
563                 *(src++) = tmp;
564                 cDst += ROW;
565             }
566             src+=ROW-rowLen+1;
567             dst+=ROW+1;
568         }
569         return *this;
570     }
571
572     /// Returns the transpose
573     inline TMatrix<ROW> Transpose() const{
574         TMatrix<ROW> result(false);
575         return Transpose(result);
576     }
577     /// Returns the transpose in the result
578     inline TMatrix<ROW>& Transpose(TMatrix<ROW>& result) const{
579         REALTYPE *src = (REALTYPE*)_;
580         REALTYPE *dst = result._;
581         unsigned int rowLen = ROW;
582         while(rowLen--){
583             REALTYPE *cDst = dst;
584             unsigned int colLen = ROW;
585             while(colLen--){
586                 *cDst = *(src++);
587                 cDst += ROW;
588             }
589             dst++;
590         }
591         return result;
592     }
593
594     /// Set a diagonal matrix
595     inline TMatrix<ROW>& Diag(const TVector<ROW>& diag){
596         Zero();
597         const REALTYPE *src = diag._;
598         REALTYPE *dst = _;
599         unsigned int len = ROW;
600         while(len--){
601             *dst = *(src++);
602             dst+=ROW+1;
603         }
604         return *this;
605     }
606
607     /**
608      * \brief Return a data pointer with data ordered in rows and not in column. Useful for opengl matrix manipulation
609      * \param result The data array. If null, this function uses the internal static member of the matrix template.
610      * \return The pointer to the data
611      */
612     REALTYPE* RowOrder(REALTYPE *result=NULL) const{
613         REALTYPE *res;
614         REALTYPE *src = _;
615         REALTYPE *dst = res = (result?result:_s);
616         unsigned int rowLen = ROW;
617         while(rowLen--){
618             REALTYPE *cDst = dst;
619             unsigned int colLen = ROW;
620             while(colLen--){
621                 *cDst = *(src++);
622                 cDst += ROW;
623             }
624             dst++;
625         }
626         return res;
627     }
628
629     /// Same as previous but force the target array to be of type float (usefl for OpenGL)
630     float* RowOrderForceFloat(float result[ROW*ROW]=NULL) const {
631         float *res;
632         REALTYPE *src = (REALTYPE*)_;
633         float *dst = res = (result?result:_sf);
634         unsigned int rowLen = ROW;
635         while(rowLen--){
636             float *cDst = dst;
637             unsigned int colLen = ROW;
638             while(colLen--){
639                 *cDst = float(*(src++));
640                 cDst += ROW;
641             }
642             dst++;
643         }
644         return res;
645     }
646
647     /// Prints out the vector to stdout
648     void Print() const {
649         std::cout << "Matrix " <<ROW<<"x"<<ROW<<std::endl;;
650         for (unsigned int j = 0; j < ROW; j++){
651             for (unsigned int i = 0; i < ROW; i++)
652                 std::cout << _[j*ROW+i] <<" ";
653             std::cout << "\n";
654         }
655     }
656
657 };
658
659
660 template<unsigned int ROW> REALTYPE TMatrix<ROW>::_s[ROW*ROW];
661 template<unsigned int ROW> float    TMatrix<ROW>::_sf[ROW*ROW];
662 template<unsigned int ROW> REALTYPE TMatrix<ROW>::undef=R_ZERO;
663
664 #ifdef USE_MATHLIB_NAMESPACE
665 }
666 #endif
667
668 #endif