Final merge of HEAD (bf-blender) into the orange branch.
[blender.git] / intern / iksolver / intern / TNT / cmat.h
1 /**
2  * $Id$
3  */
4
5 /*
6
7 *
8 * Template Numerical Toolkit (TNT): Linear Algebra Module
9 *
10 * Mathematical and Computational Sciences Division
11 * National Institute of Technology,
12 * Gaithersburg, MD USA
13 *
14 *
15 * This software was developed at the National Institute of Standards and
16 * Technology (NIST) by employees of the Federal Government in the course
17 * of their official duties. Pursuant to title 17 Section 105 of the
18 * United States Code, this software is not subject to copyright protection
19 * and is in the public domain.  The Template Numerical Toolkit (TNT) is
20 * an experimental system.  NIST assumes no responsibility whatsoever for
21 * its use by other parties, and makes no guarantees, expressed or implied,
22 * about its quality, reliability, or any other characteristic.
23 *
24 * BETA VERSION INCOMPLETE AND SUBJECT TO CHANGE
25 * see http://math.nist.gov/tnt for latest updates.
26 *
27 */
28
29
30
31 // C compatible matrix: row-oriented, 0-based [i][j] and 1-based (i,j) indexing
32 //
33
34 #ifndef CMAT_H
35 #define CMAT_H
36
37 #include "subscript.h"
38 #include "vec.h"
39 #include <stdlib.h>
40 #include <assert.h>
41 #include <iostream>
42 #include <strstream>
43 #ifdef TNT_USE_REGIONS
44 #include "region2d.h"
45 #endif
46
47 namespace TNT
48 {
49
50 template <class T>
51 class Matrix 
52 {
53
54
55   public:
56
57     typedef Subscript   size_type;
58     typedef         T   value_type;
59     typedef         T   element_type;
60     typedef         T*  pointer;
61     typedef         T*  iterator;
62     typedef         T&  reference;
63     typedef const   T*  const_iterator;
64     typedef const   T&  const_reference;
65
66     Subscript lbound() const { return 1;}
67  
68   protected:
69     Subscript m_;
70     Subscript n_;
71     Subscript mn_;      // total size
72     T* v_;                  
73     T** row_;           
74     T* vm1_ ;       // these point to the same data, but are 1-based 
75     T** rowm1_;
76
77     // internal helper function to create the array
78     // of row pointers
79
80     void initialize(Subscript M, Subscript N)
81     {
82         mn_ = M*N;
83         m_ = M;
84         n_ = N;
85
86         v_ = new T[mn_]; 
87         row_ = new T*[M];
88         rowm1_ = new T*[M];
89
90         assert(v_  != NULL);
91         assert(row_  != NULL);
92         assert(rowm1_ != NULL);
93
94         T* p = v_;              
95         vm1_ = v_ - 1;
96         for (Subscript i=0; i<M; i++)
97         {
98             row_[i] = p;
99             rowm1_[i] = p-1;
100             p += N ;
101             
102         }
103
104         rowm1_ -- ;     // compensate for 1-based offset
105     }
106    
107     void copy(const T*  v)
108     {
109         Subscript N = m_ * n_;
110         Subscript i;
111
112 #ifdef TNT_UNROLL_LOOPS
113         Subscript Nmod4 = N & 3;
114         Subscript N4 = N - Nmod4;
115
116         for (i=0; i<N4; i+=4)
117         {
118             v_[i] = v[i];
119             v_[i+1] = v[i+1];
120             v_[i+2] = v[i+2];
121             v_[i+3] = v[i+3];
122         }
123
124         for (i=N4; i< N; i++)
125             v_[i] = v[i];
126 #else
127
128         for (i=0; i< N; i++)
129             v_[i] = v[i];
130 #endif      
131     }
132
133     void set(const T& val)
134     {
135         Subscript N = m_ * n_;
136         Subscript i;
137
138 #ifdef TNT_UNROLL_LOOPS
139         Subscript Nmod4 = N & 3;
140         Subscript N4 = N - Nmod4;
141
142         for (i=0; i<N4; i+=4)
143         {
144             v_[i] = val;
145             v_[i+1] = val;
146             v_[i+2] = val;
147             v_[i+3] = val; 
148         }
149
150         for (i=N4; i< N; i++)
151             v_[i] = val;
152 #else
153
154         for (i=0; i< N; i++)
155             v_[i] = val;
156         
157 #endif      
158     }
159     
160
161     
162     void destroy()
163     {     
164         /* do nothing, if no memory has been previously allocated */
165         if (v_ == NULL) return ;
166
167         /* if we are here, then matrix was previously allocated */
168         if (v_ != NULL) delete [] (v_);     
169         if (row_ != NULL) delete [] (row_);
170
171         /* return rowm1_ back to original value */
172         rowm1_ ++;
173         if (rowm1_ != NULL ) delete [] (rowm1_);
174     }
175
176
177   public:
178
179     operator T**(){ return  row_; }
180     operator T**() const { return row_; }
181
182
183     Subscript size() const { return mn_; }
184
185     // constructors
186
187     Matrix() : m_(0), n_(0), mn_(0), v_(0), row_(0), vm1_(0), rowm1_(0) {};
188
189     Matrix(const Matrix<T> &A)
190     {
191         initialize(A.m_, A.n_);
192         copy(A.v_);
193     }
194
195     Matrix(Subscript M, Subscript N, const T& value = T())
196     {
197         initialize(M,N);
198         set(value);
199     }
200
201     Matrix(Subscript M, Subscript N, const T* v)
202     {
203         initialize(M,N);
204         copy(v);
205     }
206
207     Matrix(Subscript M, Subscript N, const char *s)
208     {
209         initialize(M,N);
210         std::istrstream ins(s);
211
212         Subscript i, j;
213
214         for (i=0; i<M; i++)
215             for (j=0; j<N; j++)
216                 ins >> row_[i][j];
217     }
218
219                 
220     // destructor
221     //
222     ~Matrix()
223     {
224         destroy();
225     }
226
227
228     // reallocating
229     //
230     Matrix<T>& newsize(Subscript M, Subscript N)
231     {
232         if (num_rows() == M && num_cols() == N)
233             return *this;
234
235         destroy();
236         initialize(M,N);
237         
238         return *this;
239     }
240
241                 void
242         diagonal(Vector<T> &diag)
243         {
244                 int sz = diag.dim();
245                 newsize(sz,sz);
246                 set(0);
247
248                 Subscript i;
249                 for (i = 0; i < sz; i++) {
250                         row_[i][i] = diag[i];
251                 }
252         }
253
254
255
256     // assignments
257     //
258     Matrix<T>& operator=(const Matrix<T> &A)
259     {
260         if (v_ == A.v_)
261             return *this;
262
263         if (m_ == A.m_  && n_ == A.n_)      // no need to re-alloc
264             copy(A.v_);
265
266         else
267         {
268             destroy();
269             initialize(A.m_, A.n_);
270             copy(A.v_);
271         }
272
273         return *this;
274     }
275         
276     Matrix<T>& operator=(const T& scalar)
277     { 
278         set(scalar); 
279         return *this;
280     }
281
282
283     Subscript dim(Subscript d) const 
284     {
285 #ifdef TNT_BOUNDS_CHECK
286        assert( d >= 1);
287         assert( d <= 2);
288 #endif
289         return (d==1) ? m_ : ((d==2) ? n_ : 0); 
290     }
291
292     Subscript num_rows() const { return m_; }
293     Subscript num_cols() const { return n_; }
294
295
296
297
298     inline T* operator[](Subscript i)
299     {
300 #ifdef TNT_BOUNDS_CHECK
301         assert(0<=i);
302         assert(i < m_) ;
303 #endif
304         return row_[i];
305     }
306
307     inline const T* operator[](Subscript i) const
308     {
309 #ifdef TNT_BOUNDS_CHECK
310         assert(0<=i);
311         assert(i < m_) ;
312 #endif
313         return row_[i];
314     }
315
316     inline reference operator()(Subscript i)
317     { 
318 #ifdef TNT_BOUNDS_CHECK
319         assert(1<=i);
320         assert(i <= mn_) ;
321 #endif
322         return vm1_[i]; 
323     }
324
325     inline const_reference operator()(Subscript i) const
326     { 
327 #ifdef TNT_BOUNDS_CHECK
328         assert(1<=i);
329         assert(i <= mn_) ;
330 #endif
331         return vm1_[i]; 
332     }
333
334
335
336     inline reference operator()(Subscript i, Subscript j)
337     { 
338 #ifdef TNT_BOUNDS_CHECK
339         assert(1<=i);
340         assert(i <= m_) ;
341         assert(1<=j);
342         assert(j <= n_);
343 #endif
344         return  rowm1_[i][j]; 
345     }
346
347
348     
349     inline const_reference operator() (Subscript i, Subscript j) const
350     {
351 #ifdef TNT_BOUNDS_CHECK
352         assert(1<=i);
353         assert(i <= m_) ;
354         assert(1<=j);
355         assert(j <= n_);
356 #endif
357         return rowm1_[i][j]; 
358     }
359
360
361
362 #ifdef TNT_USE_REGIONS
363
364     typedef Region2D<Matrix<T> > Region;
365     
366
367     Region operator()(const Index1D &I, const Index1D &J)
368     {
369         return Region(*this, I,J);
370     }
371
372
373     typedef const_Region2D< Matrix<T> > const_Region;
374     const_Region operator()(const Index1D &I, const Index1D &J) const
375     {
376         return const_Region(*this, I,J);
377     }
378
379 #endif
380
381
382 };
383
384
385 /* ***************************  I/O  ********************************/
386
387 template <class T>
388 std::ostream& operator<<(std::ostream &s, const Matrix<T> &A)
389 {
390     Subscript M=A.num_rows();
391     Subscript N=A.num_cols();
392
393     s << M << " " << N << "\n";
394
395     for (Subscript i=0; i<M; i++)
396     {
397         for (Subscript j=0; j<N; j++)
398         {
399             s << A[i][j] << " ";
400         }
401         s << "\n";
402     }
403
404
405     return s;
406 }
407
408 template <class T>
409 std::istream& operator>>(std::istream &s, Matrix<T> &A)
410 {
411
412     Subscript M, N;
413
414     s >> M >> N;
415
416     if ( !(M == A.num_rows() && N == A.num_cols() ))
417     {
418         A.newsize(M,N);
419     }
420
421
422     for (Subscript i=0; i<M; i++)
423         for (Subscript j=0; j<N; j++)
424         {
425             s >>  A[i][j];
426         }
427
428
429     return s;
430 }
431
432 // *******************[ basic matrix algorithms ]***************************
433
434 template <class T>
435 Matrix<T> operator+(const Matrix<T> &A, 
436     const Matrix<T> &B)
437 {
438     Subscript M = A.num_rows();
439     Subscript N = A.num_cols();
440
441     assert(M==B.num_rows());
442     assert(N==B.num_cols());
443
444     Matrix<T> tmp(M,N);
445     Subscript i,j;
446
447     for (i=0; i<M; i++)
448         for (j=0; j<N; j++)
449             tmp[i][j] = A[i][j] + B[i][j];
450
451     return tmp;
452 }
453
454 template <class T>
455 Matrix<T> operator-(const Matrix<T> &A, 
456     const Matrix<T> &B)
457 {
458     Subscript M = A.num_rows();
459     Subscript N = A.num_cols();
460
461     assert(M==B.num_rows());
462     assert(N==B.num_cols());
463
464     Matrix<T> tmp(M,N);
465     Subscript i,j;
466
467     for (i=0; i<M; i++)
468         for (j=0; j<N; j++)
469             tmp[i][j] = A[i][j] - B[i][j];
470
471     return tmp;
472 }
473
474 template <class T>
475 Matrix<T> mult_element(const Matrix<T> &A, 
476     const Matrix<T> &B)
477 {
478     Subscript M = A.num_rows();
479     Subscript N = A.num_cols();
480
481     assert(M==B.num_rows());
482     assert(N==B.num_cols());
483
484     Matrix<T> tmp(M,N);
485     Subscript i,j;
486
487     for (i=0; i<M; i++)
488         for (j=0; j<N; j++)
489             tmp[i][j] = A[i][j] * B[i][j];
490
491     return tmp;
492 }
493
494 template <class T>
495 void transpose(const Matrix<T> &A, Matrix<T> &S)
496 {
497     Subscript M = A.num_rows();
498     Subscript N = A.num_cols();
499
500     assert(M==S.num_cols());
501     assert(N==S.num_rows());
502
503     Subscript i, j;
504
505     for (i=0; i<M; i++)
506         for (j=0; j<N; j++)
507             S[j][i] = A[i][j];
508
509 }
510
511
512 template <class T>
513 inline void matmult(Matrix<T>& C, const Matrix<T>  &A, 
514     const Matrix<T> &B)
515 {
516
517     assert(A.num_cols() == B.num_rows());
518
519     Subscript M = A.num_rows();
520     Subscript N = A.num_cols();
521     Subscript K = B.num_cols();
522
523     C.newsize(M,K);
524
525     T sum;
526
527     const T* row_i;
528     const T* col_k;
529
530     for (Subscript i=0; i<M; i++)
531     for (Subscript k=0; k<K; k++)
532     {
533         row_i  = &(A[i][0]);
534         col_k  = &(B[0][k]);
535         sum = 0;
536         for (Subscript j=0; j<N; j++)
537         {
538             sum  += *row_i * *col_k;
539             row_i++;
540             col_k += K;
541         }
542         C[i][k] = sum; 
543     }
544
545 }
546
547 template <class T>
548 void matmult(Vector<T> &y, const Matrix<T>  &A, const Vector<T> &x)
549 {
550
551 #ifdef TNT_BOUNDS_CHECK
552     assert(A.num_cols() == x.dim());
553         assert(A.num_rows() == y.dim());
554 #endif
555
556     Subscript M = A.num_rows();
557     Subscript N = A.num_cols();
558
559     T sum;
560
561     for (Subscript i=0; i<M; i++)
562     {
563         sum = 0;
564         const T* rowi = A[i];
565         for (Subscript j=0; j<N; j++)
566             sum = sum +  rowi[j] * x[j];
567
568         y[i] = sum; 
569     }
570 }
571
572 template <class T>
573 inline void matmultdiag(
574         Matrix<T>& C, 
575         const Matrix<T> &A, 
576     const Vector<T> &diag
577 ){
578 #ifdef TNT_BOUNDS_CHECK
579     assert(A.num_cols() ==A.num_rows()== diag.dim());
580 #endif
581
582     Subscript M = A.num_rows();
583     Subscript K = diag.dim();
584
585     C.newsize(M,K);
586
587     const T* row_i;
588     const T* col_k;
589
590     for (Subscript i=0; i<M; i++) {
591                 for (Subscript k=0; k<K; k++)
592                 {
593                         C[i][k] = A[i][k] * diag[k];            
594                 }
595         }
596 }
597         
598
599 template <class T>
600 inline void matmultdiag(
601         Matrix<T>& C, 
602     const Vector<T> &diag,
603         const Matrix<T> &A 
604 ){
605 #ifdef TNT_BOUNDS_CHECK
606     assert(A.num_cols() ==A.num_rows()== diag.dim());
607 #endif
608
609     Subscript M = A.num_rows();
610     Subscript K = diag.dim();
611
612     C.newsize(M,K);
613
614     for (Subscript i=0; i<M; i++) {
615         
616                 const T diag_element = diag[i];
617
618                 for (Subscript k=0; k<K; k++)
619                 {
620                         C[i][k] = A[i][k] * diag_element;               
621                 }
622         }
623 }
624
625 } // namespace TNT
626
627 #endif // CMAT_H
628