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