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