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