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