Vector.h

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Eric Sauser, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #ifndef __VECTOR_H__
00020 #define __VECTOR_H__
00021 
00022 #include "Macros.h"
00023 #include <math.h>
00024 #include <iostream>
00025 
00026 
00027 #ifndef NULL
00028 #define NULL 0
00029 #endif
00030 
00031 #ifdef  USE_T_EXTENSIONS
00032 template<unsigned int ROW> class TVector;
00033 #endif
00034 
00035 class Vector
00036 {
00037   friend class Matrix;
00038 #ifdef  USE_T_EXTENSIONS  
00039   template<unsigned int ROW> friend class TVector;  
00040 #endif
00041   
00042 protected:
00043   static  float undef;
00044    
00045   unsigned int   row;
00046   float         *_;
00047 
00048 public:
00049 
00050   
00051         inline Vector() {
00052     row = 0;
00053     _   = NULL;
00054    }
00055   
00056   inline virtual ~Vector(){
00057     Release(); 
00058   }
00059 
00060   inline Vector(const Vector &vector)
00061   {
00062     row = 0;
00063     _   = NULL;
00064     Resize(vector.row,false);
00065     for (unsigned int i = 0; i < row; i++)
00066       _[i] = vector._[i];
00067   }
00068 
00069   inline Vector(unsigned int size, bool clear = true)
00070   {
00071     row = 0;
00072     _   = NULL;
00073     Resize(size,false);
00074     if(clear)
00075       Zero();
00076   }
00077   
00078   inline Vector(const float _[], unsigned int size)
00079   {
00080     row       = 0;
00081     this->_   = NULL;
00082     Resize(size,false);
00083     for (unsigned int i = 0; i < row; i++)
00084       this->_[i] = _[i];
00085   }
00086    
00087 #ifdef  USE_T_EXTENSIONS
00088   template<unsigned int ROW> inline Vector(const TVector<ROW> &vector)
00089   {
00090     row = 0;
00091     _   = NULL;
00092     Resize(ROW,false);
00093     for (unsigned int i = 0; i < row; i++)
00094       _[i] = vector._[i];        
00095   }
00096 #endif     
00097    
00098   inline Vector& Zero()
00099   {
00100     for (unsigned int i = 0; i < row; i++)
00101       _[i] = 0.0f;
00102     return *this;    
00103   }
00104 
00105   inline Vector& One()
00106   {
00107     for (unsigned int i = 0; i < row; i++)
00108       _[i] = 1.0f;
00109     return *this;    
00110   }
00111 
00112   inline Vector& Random(){
00113     for (unsigned int j = 0; j < row; j++)
00114       _[j] =((float)rand())/((float)(RAND_MAX+1.0));    
00115     return *this;    
00116   }
00117     
00118   inline unsigned int Size() const{
00119     return row;
00120   }
00121   
00122   inline float *GetArray() const{
00123     return _;
00124   }
00125   
00126   inline float& operator[] (const unsigned int row)
00127   {
00128     if(row<this->row)
00129       return _[row];
00130     return undef; 
00131   }
00132   
00133   inline float& operator() (const unsigned int row)
00134   {
00135     if(row<this->row)
00136       return _[row];
00137     return undef; 
00138   }
00139   
00140   inline Vector operator - () const
00141   {
00142     Vector result(row,false);
00143     for (unsigned int i = 0; i < row; i++)
00144       result._[i] = -_[i];
00145     return result;
00146   }
00147   
00148   inline Vector& Set(const Vector &vector){
00149     return (*this)=vector;  
00150   }
00151   
00152   inline Vector& operator = (const Vector &vector)
00153   {
00154     Resize(vector.row,false);
00155     const unsigned int k = (row<=vector.row?row:vector.row);
00156     for (unsigned int i = 0; i < k; i++)
00157       _[i] = vector._[i];
00158     for (unsigned int i = k; i < row; i++)
00159       _[i] = 0;
00160     return *this;    
00161   }
00162   
00163         inline Vector& operator += (const Vector &vector)
00164         {
00165     const unsigned int k = (row<=vector.row?row:vector.row);
00166                 for (unsigned int i = 0; i < k; i++)
00167                         _[i] += vector._[i];
00168     return *this;
00169         }
00170 
00171         inline Vector& operator -= (const Vector &vector)
00172         {
00173     const unsigned int k = (row<=vector.row?row:vector.row);
00174                 for (unsigned int i = 0; i < k; i++)
00175                         _[i] -= vector._[i];
00176     return *this;
00177         }
00178 
00179   inline Vector& operator ^= (const Vector &vector)
00180   {
00181     const unsigned int k = (row<=vector.row?row:vector.row);
00182     for (unsigned int i = 0; i < k; i++)
00183       _[i] *= vector._[i];
00184     return *this;
00185   }
00186 
00187   inline Vector& operator /= (const Vector &vector)
00188   {
00189     const unsigned int k = (row<=vector.row?row:vector.row);
00190     for (unsigned int i = 0; i < k; i++)
00191       _[i] /= vector._[i];
00192     return *this;
00193   }
00194 
00195   inline Vector& operator += (float scalar)
00196   {
00197     for (unsigned int i = 0; i < row; i++)
00198       _[i] += scalar;
00199     return *this;
00200   }
00201 
00202   inline Vector& operator -= (float scalar)
00203   {
00204     for (unsigned int i = 0; i < row; i++)
00205       _[i] -= scalar;
00206     return *this;
00207   }
00208 
00209         inline Vector& operator *= (float scalar)
00210         {
00211                 for (unsigned int i = 0; i < row; i++)
00212                         _[i] *= scalar;
00213     return *this;
00214         }
00215 
00216         inline Vector& operator /= (float scalar)
00217         {
00218                 scalar = 1.0f / scalar;
00219                 for (unsigned int i = 0; i < row; i++)
00220                         _[i] *= scalar;
00221     return *this;
00222         }
00223 
00224   inline Vector operator + (const Vector &vector) const
00225   {
00226     Vector result(row,false);
00227     return Add(vector,result);    
00228   }
00229   
00230   inline Vector& Add(const Vector &vector, Vector& result) const
00231   {   
00232     result.Resize(row,false);
00233     const unsigned int k = (row<=vector.row?row:vector.row);
00234     for (unsigned int i = 0; i < k; i++)
00235       result._[i] = _[i] + vector._[i];
00236     for (unsigned int i = k; i < row; i++)
00237       result._[i] = _[i];      
00238     return result;
00239   }
00240 
00241   inline Vector operator - (const Vector &vector) const
00242   {
00243     Vector result(row,false);
00244     return Sub(vector,result);    
00245   }
00246   
00247   inline Vector& Sub(const Vector &vector, Vector& result) const
00248   {
00249     result.Resize(row,false);
00250     const unsigned int k = (row<=vector.row?row:vector.row);
00251     for (unsigned int i = 0; i < k; i++)
00252       result._[i] = _[i] - vector._[i];
00253     for (unsigned int i = k; i < row; i++)
00254       result._[i] = _[i];      
00255     return result; 
00256   }
00257 
00258   inline Vector operator ^ (const Vector &vector) const
00259   {
00260     Vector result(row,false);
00261     return PMult(vector,result);    
00262   }
00263   
00264   inline Vector& PMult(const Vector &vector, Vector& result) const
00265   {   
00266     result.Resize(row,false);
00267     const unsigned int k = (row<=vector.row?row:vector.row);
00268     for (unsigned int i = 0; i < k; i++)
00269       result._[i] = _[i] * vector._[i];
00270     for (unsigned int i = k; i < row; i++)
00271       result._[i] = _[i];      
00272     return result;
00273   }
00274 
00275   inline Vector operator / (const Vector &vector) const
00276   {
00277     Vector result(row,false);
00278     return PDiv(vector,result);    
00279   }
00280   
00281   inline Vector& PDiv(const Vector &vector, Vector& result) const
00282   {
00283     result.Resize(row,false);
00284     const unsigned int k = (row<=vector.row?row:vector.row);
00285     for (unsigned int i = 0; i < k; i++)
00286       result._[i] = _[i] / vector._[i];
00287     for (unsigned int i = k; i < row; i++)
00288       result._[i] = _[i];      
00289     return result; 
00290   }
00291 
00292   inline float operator * (const Vector &vector) const
00293   { 
00294     return this->Dot(vector);  
00295   }
00296   
00297   inline Vector operator + (float scalar) const
00298   {
00299     Vector result(row,false);
00300     return Add(scalar,result);    
00301   }
00302   
00303   inline Vector& Add(float scalar, Vector& result) const
00304   {
00305     result.Resize(row,false);
00306     for (unsigned int i = 0; i < row; i++)
00307       result._[i] = _[i] + scalar;
00308     return result;
00309   }
00310 
00311   inline Vector operator - (float scalar) const
00312   {
00313     Vector result(row,false);
00314     return Sub(scalar,result);    
00315   }
00316   
00317   inline Vector& Sub(float scalar, Vector& result) const
00318   {
00319     result.Resize(row,false);
00320     for (unsigned int i = 0; i < row; i++)
00321       result._[i] = _[i] - scalar;
00322     return result;
00323   }
00324 
00325   inline Vector operator * (float scalar) const
00326   {
00327     Vector result(row,false);
00328     return Mult(scalar,result);    
00329   }
00330   
00331   inline Vector& Mult(float scalar, Vector& result) const
00332   {
00333     result.Resize(row,false);
00334     for (unsigned int i = 0; i < row; i++)
00335       result._[i] = _[i] * scalar;
00336     return result;
00337   }
00338 
00339   inline Vector operator / (float scalar) const
00340   {
00341     Vector result(row,false);
00342     return Div(scalar,result);    
00343   }
00344   
00345   inline Vector& Div(float scalar, Vector& result) const
00346   {
00347     result.Resize(row,false);
00348     scalar = 1.0f/scalar;
00349     for (unsigned int i = 0; i < row; i++)
00350       result._[i] = _[i] * scalar;
00351     return result;
00352   }
00353 
00354   inline bool operator == (const Vector& vector) const
00355   {
00356     if(row!=vector.row) return false;
00357     for (unsigned int i = 0; i < row; i++)
00358       if(_[i] != vector._[i]) return false;
00359     return true;
00360   }
00361 
00362   inline bool operator != (const Vector& vector) const
00363   {
00364     return !(*this ==  vector);
00365   }
00366 
00367 
00368   inline float Sum() const 
00369   {
00370     float result = 0.0f;
00371     for (unsigned int i = 0; i < row; i++)
00372       result += _[i];
00373     return result;
00374   }
00375 
00376   inline float Norm() const 
00377   {
00378     return sqrtf(Norm2());
00379   }
00380 
00381   inline float Norm2() const 
00382   {
00383     float result = 0.0f;
00384     for (unsigned int i = 0; i < row; i++)
00385       result += _[i]*_[i];
00386     return result;
00387   }
00388 
00389         inline void Normalize()
00390         {
00391                 float scalar = 1.0f / Norm();
00392     (*this)*=scalar;
00393         }
00394   
00395   inline float Distance(const Vector &vector) const
00396   {
00397     return (*this-vector).Norm();
00398   }
00399   
00400   inline float Distance2(const Vector &vector) const
00401   {
00402     return (*this-vector).Norm2();  
00403   }
00404 
00405   inline float Dot(const Vector &vector) const
00406   {
00407     const unsigned int k = (row<=vector.row?row:vector.row);
00408     float result = 0.0f;
00409     for (unsigned int i = 0; i < k; i++)
00410       result += _[i]*vector._[i];
00411     return result;     
00412   }
00413 
00414   inline Vector& SetSubVector(unsigned int startPos, const Vector &vector)
00415   {
00416     if(startPos<row){
00417       const unsigned int k = (row-startPos<=vector.row?row-startPos:vector.row);
00418       for (unsigned int i = 0; i < k; i++){
00419         _[startPos+i] = vector._[i];  
00420       }
00421     }
00422     return *this;   
00423   }
00424 
00425   inline Vector GetSubVector(unsigned int startPos, unsigned int len)
00426   {
00427     Vector result(len,false);
00428     return GetSubVector(startPos,len,result);
00429   }
00430   
00431 
00432   inline Vector& GetSubVector(unsigned int startPos, unsigned int len, Vector &result)
00433   {
00434     result.Resize(len,false);    
00435     if(startPos<row){
00436       const unsigned int k = (row-startPos<=len?row-startPos:len);
00437       for (unsigned int i = 0; i < k; i++){
00438         result[i] = _[startPos+i]; 
00439       }
00440       for (unsigned int i = k; i < len; i++){
00441         result[i] = 0.0f;
00442       }
00443       
00444     }else{
00445       result.Zero();  
00446     }
00447     return result;   
00448   }
00449 
00450   inline float Max(){
00451     if(row==0)
00452       return 0.0f;
00453       
00454     float res=_[0];
00455     for(unsigned int i=1;i<row;i++){
00456       if(_[i]>res) res = _[i];  
00457     }
00458     return res;
00459   }
00460 
00461   inline int MaxId(){
00462     if(row==0)
00463       return -1;
00464       
00465     float mx  = _[0];
00466     int   res = 0;
00467     for(unsigned int i=1;i<row;i++){
00468       if(_[i]>mx){ mx = _[i]; res = i;}  
00469     }
00470     return res;
00471   }
00472 
00473   inline Vector Abs(){
00474     Vector result(row);
00475     return Abs(result);
00476   }
00477 
00478   inline Vector& Abs(Vector &result) const{
00479     result.Resize(row,false);
00480     for(unsigned int i=0;i<row;i++){
00481       result._[i] = fabs(_[i]);
00482     }
00483     return result;
00484   }
00485 
00486   inline Vector& GetSubVector(const Vector &ids, Vector &result) const
00487   {
00488     const unsigned int k=ids.Size();
00489     result.Resize(k);
00490     for(unsigned int i=0;i<k;i++){
00491       const unsigned int g = (unsigned int)(fabs(ROUND(ids._[i])));
00492       if(g<row){
00493         result._[i] = _[g];
00494       }else{
00495         result._[i] = 0.0f;
00496       }
00497     }
00498     return result;     
00499   }
00500 
00501 
00502   void Print() const
00503   {
00504     std::cout << "Vector " <<row<<std::endl;;
00505     for (unsigned int i = 0; i < row; i++)
00506       std::cout << _[i] <<" ";
00507     std::cout << std::endl;
00508   }
00509   
00510 inline std::ostream& StreamOut(std::ostream& out) const
00511 {
00512    for(unsigned int i=0;i<row;i++){
00513      out<< _[i]<<" ";
00514    }
00515    return out;
00516  }
00517 
00518 protected:
00519     inline void Release(){
00520     if(_!=NULL) delete [] _; 
00521     row = 0;
00522     _   = NULL;
00523   }  
00524 public:  
00525   inline virtual void Resize(unsigned int size, bool copy = true){
00526     if(row!=size){
00527       if(size){
00528         float *arr = new float[size];
00529         if(copy){
00530           unsigned int m = (row<size?row:size);
00531           for(unsigned int i=0; i<m; i++)
00532             arr[i] = _[i];
00533           for(unsigned int i=m; i<size; i++)
00534             arr[i] = 0.0f;
00535         }
00536         if(_!=NULL) delete [] _; 
00537         _   = arr;
00538         row = size;        
00539       }else{
00540         Release();
00541       }
00542     }
00543   }
00544 };
00545 
00546 
00547 inline std::ostream& operator<<(std::ostream& out, const Vector& v){
00548   return v.StreamOut(out);
00549 }
00550 #endif
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:25 2010 for Body_Schema_Learning by  doxygen 1.6.1