v0.3.5:
[mldemos:mldemos.git] / _3rdParty / MathLib / TVector.h
1 /*
2  * Copyright (C) 2010 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
3  * Author: Eric Sauser
4  * email:   eric.sauser@a3.epf.ch
5  * website: lasa.epfl.ch
6  *
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15  */
16
17 #ifndef __TVECTOR_H__
18 #define __TVECTOR_H__
19
20 #include "MathLibCommon.h"
21 #define  USE_T_EXTENSIONS
22 #include <math.h>
23 #include <iostream>
24
25 #include "Vector.h"
26
27 #ifdef USE_MATHLIB_NAMESPACE
28 namespace MathLib {
29 #endif
30
31 template<unsigned int ROW> class TMatrix;
32
33 /**
34  * \class TVector
35  * 
36  * \ingroup MathLib
37  * 
38  * \brief The basic template vector class. See class Vector for function definition
39  * 
40  * This template vector class can be used for doing various vector manipulation.
41  * This should be combined with the Matrix class for doing almost anything
42  * you ever dreamt of.
43  */
44 template<unsigned int ROW> class TVector
45 {
46 public:
47   /*
48   friend class Vector;
49   friend class Vector3;
50   friend class TMatrix<ROW>;
51   friend class Matrix3;
52   friend class Matrix4;
53   */
54
55 public:
56   static const TVector<ROW>   ZERO;
57   static const unsigned int   row   = ROW;
58   static       REALTYPE          undef;
59    
60
61 public:
62   REALTYPE _[ROW];
63
64     /// The empty constructor
65     inline TVector(bool clear = true){
66         if(clear) Zero();
67     }
68     /// Copy constructor
69     inline TVector(const TVector<ROW> &vector){
70         Set(vector);
71     }
72     /// Constructor with data pointer to be copied
73     inline TVector(const REALTYPE *array){
74         Set(array);
75     }
76
77     /// Sets data using data pointer
78     inline TVector<ROW>& Set(const REALTYPE *array){
79         if(array)
80             memcpy(_,array,ROW*sizeof(REALTYPE));
81         return *this;
82     }
83     /// Makes a copy of the TVector argument
84     inline TVector<ROW>& Set(const TVector<ROW> &vector){
85         memcpy(_,vector._,ROW*sizeof(REALTYPE));
86         return *this;    
87     }   
88     /// Makes a copy of the Vector argument
89     inline TVector<ROW>& Set(const Vector &vector){
90         const unsigned int k = (ROW<=vector.row?ROW:vector.row);
91         memcpy(_,vector._,k*sizeof(REALTYPE));
92         if(k<ROW){
93             memset(_+k,0,(ROW-k)*sizeof(REALTYPE));
94         }
95         return *this;
96     }   
97     /// Set all elements to a given value
98     inline TVector<ROW>& Set(const REALTYPE value){
99         REALTYPE *dst = (REALTYPE*) _; unsigned int len = ROW;
100         while(len--) *(dst++) = value;
101         return *this;
102     }   
103
104     /// Gets the data array
105     inline REALTYPE *Array() const{
106         return (REALTYPE*)_;
107     }  
108
109     /// Sets all values to zero
110     inline void Zero(){
111         memset(_,0,ROW*sizeof(REALTYPE));
112     }
113   
114     /// Gets a reference to the row element
115     inline REALTYPE& operator[] (const unsigned int row){
116         if(row<ROW)
117             return _[row];
118         return undef; 
119     }
120     /// Gets a reference to the row element
121     inline REALTYPE& operator() (const unsigned int row){
122         if(row<ROW)
123             return _[row];
124         return undef; 
125     }
126     inline REALTYPE& Ref(const unsigned int row){
127         if(row<ROW)
128             return _[row];
129         return undef; 
130     }
131     inline REALTYPE& RefNoCheck(const unsigned int row){
132         return _[row];
133     }
134     /// Gets the value of the row element
135     inline REALTYPE At(const unsigned int row) const {
136         if(row<ROW)
137             return _[row];
138         return undef; 
139     }
140     inline REALTYPE AtNoCheck(const unsigned int row) const {
141         return _[row];
142     }
143   
144     /// Assigment operator
145     inline TVector<ROW>& operator = (const TVector<ROW> &vector) {        
146         return Set(vector);    
147     }
148      
149     inline TVector<ROW>& operator = (const REALTYPE &value) {        
150         return Set(value);    
151     }
152
153     /// Inverse operator
154     inline TVector<ROW> operator - () const {
155         TVector<ROW> result(false);
156         return Minus(result);
157     }
158     /// Inverse operator
159     inline TVector<ROW>& Minus(TVector<ROW>& result) const {
160         REALTYPE *src = (REALTYPE*) _;
161         REALTYPE *dst = (REALTYPE*) result._;
162         unsigned int len = ROW;
163         while(len--)
164             *(dst++) = -*(src++);
165         return result;
166     }
167     /// Self Inversion
168     inline TVector<ROW>& SMinus() {
169         REALTYPE *dst = (REALTYPE*) _;
170         unsigned int len = ROW;
171         while(len--){
172             *dst = -*dst;
173             dst++;
174         }
175         return *this;
176     }
177
178
179     /// Assigment data-wise operations 
180     inline TVector<ROW>& operator += (const TVector<ROW> &vector) {
181         REALTYPE *src = (REALTYPE*) vector._, *dst = (REALTYPE*) _; unsigned int len = ROW;
182         while(len--) *(dst++) += *(src++);
183         return *this;
184     }
185     inline TVector<ROW>& operator -= (const TVector<ROW> &vector) {
186         REALTYPE *src = (REALTYPE*) vector._, *dst = (REALTYPE*) _; unsigned int len = ROW;
187         while(len--) *(dst++) -= *(src++);
188         return *this;
189     }
190     inline TVector<ROW>& operator ^= (const TVector<ROW> &vector){
191         REALTYPE *src = (REALTYPE*) vector._, *dst = (REALTYPE*) _; unsigned int len = ROW;
192         while(len--) *(dst++) *= *(src++);
193         return *this;
194     }
195     inline TVector<ROW>& operator /= (const TVector<ROW> &vector){
196         REALTYPE *src = (REALTYPE*) vector._, *dst = (REALTYPE*) _; unsigned int len = ROW;
197         while(len--) *(dst++) /= *(src++);
198         return *this;
199     }
200  
201
202     /// Assigment operations
203     inline TVector<ROW>& operator += (REALTYPE scalar){
204         REALTYPE *dst = (REALTYPE*) _; unsigned int len = ROW;
205         while(len--) *(dst++) += scalar;
206         return *this;
207     }
208     inline TVector<ROW>& operator -= (REALTYPE scalar){
209         REALTYPE *dst = (REALTYPE*) _; unsigned int len = ROW;
210         while(len--) *(dst++) -= scalar;
211         return *this;
212     }
213     inline TVector<ROW>& operator *= (REALTYPE scalar){
214         REALTYPE *dst = (REALTYPE*) _; unsigned int len = ROW;
215         while(len--) *(dst++) *= scalar;
216         return *this;
217     }
218     inline TVector<ROW>& operator /= (REALTYPE scalar){
219         scalar = R_ONE/scalar;
220         REALTYPE *dst = (REALTYPE*) _; unsigned int len = ROW;
221         while(len--) *(dst++) *= scalar;
222         return *this;
223     }
224
225
226
227     /// Vector data-wise operators
228     inline TVector<ROW> operator + (const TVector<ROW> &vector) const {
229         TVector<ROW> result(false);
230         return Add(vector,result);
231     }
232     inline TVector<ROW> operator - (const TVector<ROW> &vector) const {
233         TVector<ROW> result(false);
234         return Sub(vector,result); 
235     }
236     inline TVector<ROW> operator ^ (const TVector<ROW> &vector) const {
237         TVector<ROW> result(false);
238         return Mult(vector,result);
239     }
240     inline TVector<ROW> operator / (const TVector<ROW> &vector) const {
241         TVector<ROW> result(false);
242         return Div(vector,result); 
243     }
244
245     /// Vector data-wise operations (faster than using operators)
246     inline TVector<ROW>& Add(const TVector<ROW> &vector, TVector<ROW> &result) const {
247         REALTYPE *src0 = (REALTYPE*) _, *src1 = (REALTYPE*) vector._, *dst = (REALTYPE*) result._; unsigned int len = ROW;
248         while(len--) *(dst++) = *(src0++) + (*(src1++));
249         return result;
250     }
251     inline TVector<ROW>& Sub(const TVector<ROW> &vector, TVector<ROW> &result) const{
252         REALTYPE *src0 = (REALTYPE*) _, *src1 = (REALTYPE*) vector._, *dst = (REALTYPE*) result._; unsigned int len = ROW;
253         while(len--) *(dst++) = *(src0++) - (*(src1++));
254         return result;
255     }
256     inline TVector<ROW>& Mult(const TVector<ROW> &vector, TVector<ROW> &result) const{
257         REALTYPE *src0 = (REALTYPE*) _, *src1 = (REALTYPE*) vector._, *dst = (REALTYPE*) result._; unsigned int len = ROW;
258         while(len--) *(dst++) = *(src0++) * (*(src1++));
259         return result;
260     }
261     inline TVector<ROW>& Div(const TVector<ROW> &vector, TVector<ROW> &result) const{
262         REALTYPE *src0 = (REALTYPE*) _, *src1 = (REALTYPE*) vector._, *dst = (REALTYPE*) result._; unsigned int len = ROW;
263         while(len--) *(dst++) = *(src0++) / (*(src1++));
264         return result;
265     }
266
267
268     /// Scalar operations using operators
269     inline TVector<ROW> operator + (REALTYPE scalar) const {
270         TVector<ROW> result(false);
271         return Add(scalar,result);
272     }
273     inline TVector<ROW> operator - (REALTYPE scalar) const {
274         TVector<ROW> result(false);
275         return Sub(scalar,result);
276     }
277     inline TVector<ROW> operator * (REALTYPE scalar) const {
278         TVector<ROW> result(false);
279         return Mult(scalar,result);
280     }
281     inline TVector<ROW> operator / (REALTYPE scalar) const {
282         TVector<ROW> result(false);
283         return Div(scalar,result);
284     }
285
286     /// Scalar operations with result as a parameter (faster than pure operators)
287     inline TVector<ROW>& Add(REALTYPE scalar, TVector<ROW>& result) const {
288         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) result._; unsigned int len = ROW;
289         while(len--) *(dst++) = *(src++)  + scalar;
290         return result;
291     }
292     inline TVector<ROW>& Sub(REALTYPE scalar, TVector<ROW>& result) const{
293         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) result._; unsigned int len = ROW;
294         while(len--) *(dst++) = *(src++)  - scalar;
295         return result;
296     }
297     inline TVector<ROW>& Mult(REALTYPE scalar, TVector<ROW>& result) const{
298         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) result._; unsigned int len = ROW;
299         while(len--) *(dst++) = *(src++)  * scalar;
300         return result;
301     }
302     inline TVector<ROW>& Div(REALTYPE scalar, TVector<ROW>& result) const{
303         scalar = R_ONE/scalar;
304         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) result._; unsigned int len = ROW;
305         while(len--) *(dst++) = *(src++)  * scalar;
306         return result;
307     }
308
309     inline TVector<ROW>& ScaleAddTo(REALTYPE scale, TVector<ROW> &result) const{
310         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) result._; unsigned int len = ROW;
311         while(len--) *(dst++) += *(src++)  * scale;
312         return result;
313     }
314
315
316     /// Tests equality of two vectors
317     inline bool operator == (const TVector<ROW>& vector) const {
318         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) vector._; unsigned int len = ROW;
319         while(len--) 
320             if(*(dst++) != *(src++))
321                 return false;
322         return true;
323     }
324     /// tests inequality of two vectors
325     inline bool operator != (const TVector<ROW>& vector) const {
326         return !(*this ==  vector);
327     }
328
329     /// Returns the norm
330     inline REALTYPE Norm() const {
331         return sqrt(Norm2());
332     }
333     /// Returns the squared norm
334     inline REALTYPE Norm2() const {
335         REALTYPE result = R_ZERO;    
336         REALTYPE *src = (REALTYPE*) _; unsigned int len = ROW;
337         while(len--){
338             result += (*src) * (*(src));
339             src++;
340         } 
341         return result;
342     }
343     /// Normalize the data to 1
344     inline void Normalize() {
345         REALTYPE scalar = R_ONE / Norm();
346         (*this)*=scalar;
347     }
348   
349     /// Performs the dot product
350     inline REALTYPE Dot(const TVector<ROW> &vector) const {
351         REALTYPE result = R_ZERO;
352         REALTYPE *src = (REALTYPE*) _, *dst = (REALTYPE*) vector._; unsigned int len = ROW;
353         while(len--) 
354             result += *(dst++) * (*(src++));
355         return result;     
356     }
357
358     inline TMatrix<ROW> & MultTranspose(TVector<ROW> & vector, TMatrix<ROW> & result) const{
359         REALTYPE *src0 = (REALTYPE*) _;
360         REALTYPE *dst  = (REALTYPE*) result._;
361         unsigned int colLen = ROW;
362         while(colLen--) {
363             REALTYPE *src1 = (REALTYPE*) vector._;
364             unsigned int rowLen = ROW;
365             while(rowLen--)
366                 *(dst++) = *(src0) *(*(src1++));
367             src0++;
368         }      
369         return result;
370     }
371
372     /// Truncs the data between minVal and maxVal
373     inline TVector<ROW> Trunc(const REALTYPE minVal, const REALTYPE maxVal) {
374         REALTYPE *src = (REALTYPE*) _; unsigned int len = ROW;
375         while(len--){
376             *src = TRUNC(*src,minVal,maxVal);
377             src++;
378         } 
379         return *this;     
380     }
381
382   /// Truncs each data between each minVal and maxVal vectors
383   inline TVector<ROW> Trunc(const TVector<ROW> &minVal, const TVector<ROW> &maxVal) 
384   {
385     for (unsigned int i = 0; i < ROW; i++)
386       _[i] = TRUNC(_[i],minVal._[i],maxVal._[i]);
387     return *this;     
388   }
389   
390   /// Prints out the vector to stdout 
391   void Print() const
392   {
393     std::cout << "Vector" <<ROW<<std::endl;;
394     for (unsigned int i = 0; i < ROW; i++)
395       std::cout << _[i] <<" ";
396     std::cout << "\n";    
397   }
398 };
399
400 template<unsigned int ROW> const TVector<ROW> TVector<ROW>::ZERO;
401
402 template<unsigned int ROW> REALTYPE TVector<ROW>::undef = R_ZERO;
403
404
405 #ifdef USE_MATHLIB_NAMESPACE
406 }
407 #endif
408 #endif