d4f7c2f40210a2ab7481aa7635c31061b36edea3
[blender.git] / intern / iksolver / intern / TNT / vec.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 // Basic TNT  numerical vector (0-based [i] AND 1-based (i) indexing )
31 //
32
33 #ifndef VEC_H
34 #define VEC_H
35
36 #include "subscript.h"
37 #include <stdlib.h>
38 #include <assert.h>
39 #include <iostream>
40 #include <strstream>
41
42 namespace TNT
43 {
44
45 template <class T>
46 class Vector 
47 {
48
49
50   public:
51
52     typedef Subscript   size_type;
53     typedef         T   value_type;
54     typedef         T   element_type;
55     typedef         T*  pointer;
56     typedef         T*  iterator;
57     typedef         T&  reference;
58     typedef const   T*  const_iterator;
59     typedef const   T&  const_reference;
60
61     Subscript lbound() const { return 1;}
62  
63   protected:
64     T* v_;                  
65     T* vm1_;        // pointer adjustment for optimzied 1-offset indexing
66     Subscript n_;
67
68     // internal helper function to create the array
69     // of row pointers
70
71     void initialize(Subscript N)
72     {
73         // adjust pointers so that they are 1-offset:
74         // v_[] is the internal contiguous array, it is still 0-offset
75         //
76         assert(v_ == NULL);
77         v_ = new T[N];
78         assert(v_  != NULL);
79         vm1_ = v_-1;
80         n_ = N;
81     }
82    
83     void copy(const T*  v)
84     {
85         Subscript N = n_;
86         Subscript i;
87
88 #ifdef TNT_UNROLL_LOOPS
89         Subscript Nmod4 = N & 3;
90         Subscript N4 = N - Nmod4;
91
92         for (i=0; i<N4; i+=4)
93         {
94             v_[i] = v[i];
95             v_[i+1] = v[i+1];
96             v_[i+2] = v[i+2];
97             v_[i+3] = v[i+3];
98         }
99
100         for (i=N4; i< N; i++)
101             v_[i] = v[i];
102 #else
103
104         for (i=0; i< N; i++)
105             v_[i] = v[i];
106 #endif      
107     }
108
109     void set(const T& val)
110     {
111         Subscript N = n_;
112         Subscript i;
113
114 #ifdef TNT_UNROLL_LOOPS
115         Subscript Nmod4 = N & 3;
116         Subscript N4 = N - Nmod4;
117
118         for (i=0; i<N4; i+=4)
119         {
120             v_[i] = val;
121             v_[i+1] = val;
122             v_[i+2] = val;
123             v_[i+3] = val; 
124         }
125
126         for (i=N4; i< N; i++)
127             v_[i] = val;
128 #else
129
130         for (i=0; i< N; i++)
131             v_[i] = val;
132         
133 #endif      
134     }
135     
136
137
138     void destroy()
139     {     
140         /* do nothing, if no memory has been previously allocated */
141         if (v_ == NULL) return ;
142
143         /* if we are here, then matrix was previously allocated */
144         delete [] (v_);     
145
146         v_ = NULL;
147         vm1_ = NULL;
148     }
149
150
151   public:
152
153     // access
154
155     iterator begin() { return v_;}
156     iterator end()   { return v_ + n_; }
157     const iterator begin() const { return v_;}
158     const iterator end() const  { return v_ + n_; }
159
160     // destructor
161
162     ~Vector() 
163     {
164         destroy();
165     }
166
167     // constructors
168
169     Vector() : v_(0), vm1_(0), n_(0)  {};
170
171     Vector(const Vector<T> &A) : v_(0), vm1_(0), n_(0)
172     {
173         initialize(A.n_);
174         copy(A.v_);
175     }
176
177     Vector(Subscript N, const T& value = T()) :  v_(0), vm1_(0), n_(0)
178     {
179         initialize(N);
180         set(value);
181     }
182
183     Vector(Subscript N, const T* v) :  v_(0), vm1_(0), n_(0)
184     {
185         initialize(N);
186         copy(v);
187     }
188
189     Vector(Subscript N, char *s) :  v_(0), vm1_(0), n_(0)
190     {
191         initialize(N);
192         std::istrstream ins(s);
193
194         Subscript i;
195
196         for (i=0; i<N; i++)
197                 ins >> v_[i];
198     }
199
200
201     // methods
202     // 
203     Vector<T>& newsize(Subscript N)
204     {
205         if (n_ == N) return *this;
206
207         destroy();
208         initialize(N);
209
210         return *this;
211     }
212
213
214     // assignments
215     //
216     Vector<T>& operator=(const Vector<T> &A)
217     {
218         if (v_ == A.v_)
219             return *this;
220
221         if (n_ == A.n_)         // no need to re-alloc
222             copy(A.v_);
223
224         else
225         {
226             destroy();
227             initialize(A.n_);
228             copy(A.v_);
229         }
230
231         return *this;
232     }
233         
234     Vector<T>& operator=(const T& scalar)
235     { 
236         set(scalar);  
237         return *this;
238     }
239
240     inline Subscript dim() const 
241     {
242         return  n_; 
243     }
244
245     inline Subscript size() const 
246     {
247         return  n_; 
248     }
249
250
251     inline reference operator()(Subscript i)
252     { 
253 #ifdef TNT_BOUNDS_CHECK
254         assert(1<=i);
255         assert(i <= n_) ;
256 #endif
257         return vm1_[i]; 
258     }
259
260     inline const_reference operator() (Subscript i) const
261     {
262 #ifdef TNT_BOUNDS_CHECK
263         assert(1<=i);
264         assert(i <= n_) ;
265 #endif
266         return vm1_[i]; 
267     }
268
269     inline reference operator[](Subscript i)
270     { 
271 #ifdef TNT_BOUNDS_CHECK
272         assert(0<=i);
273         assert(i < n_) ;
274 #endif
275         return v_[i]; 
276     }
277
278     inline const_reference operator[](Subscript i) const
279     {
280 #ifdef TNT_BOUNDS_CHECK
281         assert(0<=i) ;
282         assert(i < n_) ;
283 #endif
284         return v_[i]; 
285     }
286
287
288
289 };
290
291
292 /* ***************************  I/O  ********************************/
293
294 template <class T>
295 std::ostream& operator<<(std::ostream &s, const Vector<T> &A)
296 {
297     Subscript N=A.dim();
298
299     s <<  N << std::endl;
300
301     for (Subscript i=0; i<N; i++)
302         s   << A[i] << " " << std::endl;
303     s << std::endl;
304
305     return s;
306 }
307
308 template <class T>
309 std::istream & operator>>(std::istream &s, Vector<T> &A)
310 {
311
312     Subscript N;
313
314     s >> N;
315
316     if ( !(N == A.size() ))
317     {
318         A.newsize(N);
319     }
320
321
322     for (Subscript i=0; i<N; i++)
323             s >>  A[i];
324
325
326     return s;
327 }
328
329 // *******************[ basic matrix algorithms ]***************************
330
331
332 template <class T>
333 Vector<T> operator+(const Vector<T> &A, 
334     const Vector<T> &B)
335 {
336     Subscript N = A.dim();
337
338     assert(N==B.dim());
339
340     Vector<T> tmp(N);
341     Subscript i;
342
343     for (i=0; i<N; i++)
344             tmp[i] = A[i] + B[i];
345
346     return tmp;
347 }
348
349 template <class T>
350 Vector<T> operator-(const Vector<T> &A, 
351     const Vector<T> &B)
352 {
353     Subscript N = A.dim();
354
355     assert(N==B.dim());
356
357     Vector<T> tmp(N);
358     Subscript i;
359
360     for (i=0; i<N; i++)
361             tmp[i] = A[i] - B[i];
362
363     return tmp;
364 }
365
366 template <class T>
367 Vector<T> operator*(const Vector<T> &A, 
368     const Vector<T> &B)
369 {
370     Subscript N = A.dim();
371
372     assert(N==B.dim());
373
374     Vector<T> tmp(N);
375     Subscript i;
376
377     for (i=0; i<N; i++)
378             tmp[i] = A[i] * B[i];
379
380     return tmp;
381 }
382
383 template <class T>
384 Vector<T> operator*(const Vector<T> &A, 
385     const T &B)
386 {
387     Subscript N = A.dim();
388
389     Vector<T> tmp(N);
390     Subscript i;
391
392     for (i=0; i<N; i++)
393             tmp[i] = A[i] * B;
394
395     return tmp;
396 }
397
398
399 template <class T>
400 T dot_prod(const Vector<T> &A, const Vector<T> &B)
401 {
402     Subscript N = A.dim();
403     assert(N == B.dim());
404
405     Subscript i;
406     T sum = 0;
407
408     for (i=0; i<N; i++)
409         sum += A[i] * B[i];
410
411     return sum;
412 }
413
414 // inplace versions of the above template functions
415
416 // A = A + B
417
418 template <class T>
419 void vectoradd(
420         Vector<T> &A, 
421     const Vector<T> &B)
422 {
423     const Subscript N = A.dim();
424     assert(N==B.dim());
425     Subscript i;
426
427     for (i=0; i<N; i++)
428             A[i] += B[i];
429 }
430
431 // same with seperate output vector
432
433 template <class T>
434 void vectoradd(
435         Vector<T> &C,
436         const Vector<T> &A, 
437     const Vector<T> &B)
438 {
439     const Subscript N = A.dim();
440     assert(N==B.dim());
441     Subscript i;
442
443     for (i=0; i<N; i++)
444             C[i] = A[i] + B[i];
445 }
446
447 // A = A - B
448
449 template <class T>
450 void vectorsub(
451         Vector<T> &A, 
452     const Vector<T> &B)
453 {
454     const Subscript N = A.dim();
455     assert(N==B.dim());
456     Subscript i;
457
458     for (i=0; i<N; i++)
459             A[i] -= B[i];
460 }
461
462 template <class T>
463 void vectorsub(
464         Vector<T> &C,
465         const Vector<T> &A, 
466     const Vector<T> &B)
467 {
468     const Subscript N = A.dim();
469     assert(N==B.dim());
470     Subscript i;
471
472     for (i=0; i<N; i++)
473             C[i] = A[i] - B[i];
474 }
475
476 template <class T>
477 void vectorscale(
478         Vector<T> &C,
479         const Vector<T> &A, 
480     const T &B)
481 {
482     const Subscript N = A.dim();
483     Subscript i;
484
485     for (i=0; i<N; i++)
486             C[i] = A[i] * B;
487 }
488
489 template <class T>
490 void vectorscale(
491         Vector<T> &A, 
492     const T &B)
493 {
494     const Subscript N = A.dim();
495     Subscript i;
496
497     for (i=0; i<N; i++)
498             A[i] *= B;
499 }
500
501 }   /* namespace TNT */
502
503 #endif // VEC_H
504