rtcVarVec.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcVarVec.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-03-19 11:10:08 -0700 (Thu, 19 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 101 $
00018  */
00019 #ifndef RTC_VARVEC_H
00020 #define RTC_VARVEC_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcArray1.h"
00025 
00026 //== NAMESPACES ================================================================
00027 namespace rtc {
00028 
00029 // Forward declarations
00030 template <class T> class VarVec; // len-d vector
00031 template <class T> class VarSMat; // MxM Square Matrix
00032 
00036 template <class T>
00037 class VarVec : public Array1<T> {
00038 public:
00039   // Constructors
00040   VarVec();
00041   VarVec(int len);
00042   VarVec(int len, const T* d);
00043   VarVec(int len, const T a);
00044   VarVec(const Array1<T>& a);
00045 
00046   // Cast Operation
00047   template <class U> VarVec(const VarVec<U>& v);
00048   template <class U, int N> VarVec(const Vec<U,N>& v);
00049 
00050   // Mutators
00051   void set(const T* d);
00052   void set(const T a);
00053   void set(const VarVec<T>& v);
00054   VarVec<T>& operator = (const T a);
00055   VarVec<T>& operator = (const T* d);
00056   VarVec<T>& operator = (const VarVec<T>& v);
00057   void setSubVec(const int i, const VarVec<T>& v);
00058 
00059   // Casting Mutators
00060   template <class U> void set(const VarVec<U>& v);
00061   template <class U, int N> void set(const Vec<U,N>& v);
00062   template <class U> VarVec<T>& operator = (const VarVec<U>& v);
00063 
00064   // Accessors
00065   T operator [] (const int i) const;
00066   T& operator [] (const int i);
00067   VarVec<T> getSubVec(const int i, int sub_len) const;
00068 
00069   // Norms and Normalize
00070   T norm() const;
00071   T normSqr() const;
00072   T pnorm(float p) const;
00073   T normalize();
00074   VarVec<T> normalized() const;
00075 
00076   // Reductions: Max/Min, Sum/Product
00077   T max() const;
00078   T min() const;
00079   T sum() const;
00080   T prod() const;
00081   VarVec<T> cumsum() const;
00082 
00083   // General elementwise operations
00084   void perform(T (*mathFun)(T));
00085   void perform(T (*mathFun)(T,T), const T arg2);
00086   void perform(T (*mathFun)(T,T), const VarVec<T>& v);
00087   VarVec<T> performed(T (*mathFun)(T));
00088   VarVec<T> performed(T (*mathFun)(T,T), const T arg2);
00089   VarVec<T> performed(T (*mathFun)(T,T), const VarVec<T>& v);
00090   VarVec<T> minimize(const VarVec<T>& other);
00091   VarVec<T> maximize(const VarVec<T>& other);
00092 
00093   // Dot and Outer Products
00094   T dot(const VarVec<T>& v) const;
00095   VarSMat<T> outer(const VarVec<T>& v) const;
00096   VarSMat<T> outer() const;
00097 
00098   // Random vectors and sorting
00099 //    static VarVec<T> uniformRand(const T a = T(0), const T b = T(1));
00100 //    static VarVec<T> normalRand(const T mean = T(0), const T stdev = T(1));
00101 //    static VarVec<T> multivariateGauss(const VarVec<T>& mean, const SMat<T,len>& cov);
00102 
00103   void sort(bool ascending = true);
00104   VarVec<T> sorted(bool ascending = true);
00105 
00106   // Addition and subtraction
00107   VarVec<T>& add(const VarVec<T>& v);
00108   void addSubVec(const int i, const VarVec<T>& v);
00109   VarVec<T>& subtract(const VarVec<T>& v);
00110   void subtractSubVec(const int i, const VarVec<T>& v);
00111 
00112   // Addition and subtraction operator
00113   VarVec<T> operator + (const VarVec<T>& v) const;
00114   void operator += (const VarVec<T>& v);
00115   VarVec<T> operator - (const VarVec<T>& v) const;
00116   void operator -= (const VarVec<T>& v);
00117   VarVec<T> operator - () const;
00118   VarVec<T> add(const T a);
00119   VarVec<T> operator + (const T a) const;
00120   void operator += (const T a);
00121   VarVec<T> subtract(const T a);
00122   VarVec<T> operator - (const T a) const;
00123   void operator -= (const T a);
00124 
00125   // Multiplication and division operator
00126   VarVec<T> operator * (const T a) const;
00127   VarVec<T> operator / (const T a) const;
00128   void operator *= (const T a);
00129   void operator /= (const T a);
00130 
00131   // Equality and inequality tests
00132   int compareTo(const VarVec<T>& v) const;
00133   bool equalTo(const VarVec<T>& v, const T tol = T(0)) const;
00134 
00135   // Equality and inequality tests operator
00136   bool operator == (const VarVec<T>& v) const;
00137   bool operator != (const VarVec<T>& v) const;
00138 //    VarVec<bool> operator == (const VarVec<T>& v) const;
00139 //    VarVec<bool> operator != (const VarVec<T>& v) const;
00140   VarVec<bool> operator >= (const VarVec<T>& v) const;
00141   VarVec<bool> operator <= (const VarVec<T>& v) const;
00142   VarVec<bool> operator > (const VarVec<T>& v) const;
00143   VarVec<bool> operator < (const VarVec<T>& v) const;
00144 
00145   // Element-wise operations
00146   VarVec<T> operator * (const VarVec<T>& v) const;
00147   VarVec<T> operator / (const VarVec<T>& v) const;
00148   void operator *= (const VarVec<T>& v);
00149   void operator /= (const VarVec<T>& v);
00150 
00151   // Serialization
00152   bool write(std::ostream& os) const;
00153   bool read(std::istream& is);
00154 
00155   // inherit member data and functions of parent
00156   using Array1<T>::x;
00157   using Array1<T>::reset;
00158   using Array1<T>::at;
00159   using Array1<T>::setSize;
00160 
00161 protected:
00162   // inherit member data and functions of parent
00163   using Array1<T>::dim;
00164   using Array1<T>::mul;
00165   using Array1<T>::len;
00166 };
00167 
00168 // Declare a few common typdefs
00169 typedef VarVec<bool> VarVecb;
00170 typedef VarVec<char> VarVecc;
00171 typedef VarVec<unsigned char> VarVecuc;
00172 typedef VarVec<int> VarVeci;
00173 typedef VarVec<float> VarVecf;
00174 typedef VarVec<double> VarVecd;
00175 
00176 // Global operators for cases where VarVec<T>
00177 // is the second argument in a binary operator
00178 template <class T> VarVec<T> operator + (const T a, const VarVec<T>& v);
00179 template <class T> VarVec<T> operator - (const T a, const VarVec<T>& v);
00180 template <class T> VarVec<T> operator * (const T a, const VarVec<T>& v);
00181 
00182 // ASCII stream IO
00183 template <class T> std::ostream& operator<<(std::ostream& os, const VarVec<T>& vec);
00184 template <class T> std::istream& operator>>(std::istream& is, VarVec<T>& vec);
00185 
00186 //==============================================================================
00187 // VarVec<T>
00188 //==============================================================================
00189 
00190 // Constructors
00191 
00194 template <class T>
00195 inline VarVec<T>::VarVec() : Array1<T>() {}
00196 
00199 template <class T>
00200 inline VarVec<T>::VarVec(int _len) : Array1<T>(_len) {}
00201 
00206 template <class T>
00207 inline VarVec<T>::VarVec(int _len, const T* d) : Array1<T>(_len,d) {}
00208 
00213 template <class T>
00214 inline VarVec<T>::VarVec(int _len, const T a) : Array1<T>(_len,a) {}
00215 
00219 template <class T>
00220 inline VarVec<T>::VarVec(const Array1<T>& a) : Array1<T>(a) {}
00221 
00222 // Casting Operation
00223 
00227 template <class T> template <class U>
00228 inline VarVec<T>::VarVec(const VarVec<U>& v) {
00229   set<U>(v);
00230 }
00231 
00235 template <class T> template <class U, int N>
00236 inline VarVec<T>::VarVec(const Vec<U,N>& v) : Array1<T>() {
00237   setSize(N);
00238   set<U,N>(v);
00239 }
00240 
00241 // Mutators
00242 
00246 template <class T>
00247 inline void VarVec<T>::set(const T a) {
00248   Array1<T>::set(a);
00249 }
00250 
00254 template <class T>
00255 inline void VarVec<T>::set(const T* d) {
00256   Array1<T>::set(d);
00257 }
00258 
00262 template <class T>
00263 inline void VarVec<T>::set(const VarVec<T>& v) {
00264   if(len!=v.len) setSize(v.len);
00265   Array1<T>::set(v);
00266 }
00267 
00274 template <class T>
00275 inline void VarVec<T>::setSubVec(const int i, const VarVec<T>& v) {
00276 #if MATMATH_CHECK_BOUNDS
00277   if (i < 0 || i+v.len > len) {
00278     std::stringstream ss;
00279     ss << "VarVec<" << len << ">::setSubVec(" << i << ", ";
00280     ss << "VarVec<" << v.len << ">): not a valid operation\n";
00281     throw Exception(ss.str());
00282   }
00283 #endif
00284   for (int j=i,k=0;j<len && k<v.len;j++,k++) x[j] = v.x[k];
00285 }
00286 
00290 template <class T>
00291 inline VarVec<T>& VarVec<T>::operator = (const T a) {
00292   set(a);
00293   return *this;
00294 }
00295 
00299 template <class T>
00300 inline VarVec<T>& VarVec<T>::operator = (const T* d) {
00301   set(d);
00302   return *this;
00303 }
00304 
00308 template <class T>
00309 inline VarVec<T>& VarVec<T>::operator = (const VarVec<T>& v) {
00310   set(v);
00311   return *this;
00312 }
00313 // Casting Mutators
00314 
00318 template <class T> template <class U>
00319 inline void VarVec<T>::set(const VarVec<U>& v) {
00320   if(len!=v.len) setSize(v.len);
00321   for (int i=0;i<len;i++) x[i] = T(v.x[i]);
00322 }
00323 
00327 template <class T> template <class U, int N>
00328 inline void VarVec<T>::set(const Vec<U,N>& v) {
00329   if(len!=N) setSize(N);
00330   for (int k=0;k<len;k++) x[k] = T(v.x[k]);
00331 }
00332 
00336 template <class T> template <class U>
00337 inline VarVec<T>& VarVec<T>::operator = (const VarVec<U>& v) {
00338   set<U>(v);
00339   return *this;
00340 }
00341 
00342 // Accessors
00343 
00348 template <class T>
00349 inline T VarVec<T>::operator [] (const int i) const {
00350 #if MATMATH_CHECK_BOUNDS
00351   if (i < 0 || i > len) {
00352     std::stringstream ss;
00353     ss << "Vec<" << len << ">::(" << i << "): index out of range\n";
00354     ss << std::flush;
00355     throw Exception(ss.str());
00356 }
00357 #endif
00358   return x[i];
00359 }
00360 
00365 template <class T>
00366 inline T& VarVec<T>::operator [] (const int i) {
00367 #if MATMATH_CHECK_BOUNDS
00368   if (i < 0 || i > len) {
00369     std::stringstream ss;
00370     ss << "&Vec<" << len << ">::(" << i << "): index out of range";
00371     throw Exception(ss.str());
00372   }
00373 #endif
00374   return x[i];
00375 }
00376 
00382 template <class T>
00383 inline VarVec<T> VarVec<T>::getSubVec(const int i, int sub_len) const {
00384 #if MATMATH_CHECK_BOUNDS
00385   if (i < 0 || i+sub_len > len) {
00386     std::stringstream ss;
00387     ss << "&Vec<" << len << ">::(" << i << "): index out of range";
00388     throw Exception(ss.str());
00389   }
00390 #endif
00391   VarVec<T> v(sub_len,T(0));
00392   for (int j=i,k=0;j<len && k<sub_len;j++,k++) v.x[k] = x[j];
00393   return v;
00394 }
00395 
00396 // Norms and Normalize
00397 
00401 template <class T>
00402 inline T VarVec<T>::norm() const {
00403   return T(sqrt(double(normSqr())));
00404 }
00405 
00409 template <class T>
00410 inline T VarVec<T>::normSqr() const {
00411   T sum = T(0);
00412   for (int i=0;i<len;i++) sum += x[i]*x[i];
00413   return sum;
00414 }
00415 
00420 template <class T>
00421 inline T VarVec<T>::pnorm(float p) const {
00422   T sum = T(0);
00423   for (int i=0;i<len;i++) sum += pow(fabs((double)x[i]), (double)p);
00424   return pow((double)sum, (double)1.0/p);
00425 }
00426 
00430 template <class T>
00431 inline T VarVec<T>::normalize() {
00432   T l = norm();
00433   if (l > T(0)) for (int i=0;i<len;i++) x[i] /= l;
00434   return l;
00435 }
00436 
00440 template <class T>
00441 inline VarVec<T> VarVec<T>::normalized() const {
00442   VarVec<T> v(len,x);
00443   v.normalize();
00444   return v;
00445 }
00446 
00447 // Maximum/Minimum value and Sum
00448 
00452 template <class T>
00453 inline T VarVec<T>::max() const {
00454   T m = x[0];
00455   for (int i=1;i<len;i++) if (x[i]>m) m = x[i];
00456   return m;
00457 }
00458 
00462 template <class T>
00463 inline T VarVec<T>::min() const {
00464   T m = x[0];
00465   for (int i=1;i<len;i++) if (x[i]<m) m = x[i];
00466   return m;
00467 }
00468 
00472 template <class T>
00473 inline T VarVec<T>::sum() const {
00474   T s = T(0);
00475   for (int i=0;i<len;i++) s += x[i];
00476   return s;
00477 }
00478 
00482 template <class T>
00483 inline VarVec<T> VarVec<T>::cumsum() const {
00484   VarVec<T> v(len);
00485   T s = T(0);
00486   for (int i=0;i<len;i++) {
00487     s += x[i];
00488     v.x[i]=s;
00489   }
00490   return v;
00491 }
00492 
00496 template <class T>
00497 inline T VarVec<T>::prod() const {
00498   T p = T(1);
00499   for (int i=0;i<len;i++) p *= x[i];
00500   return p;
00501 }
00502 
00503 // General elementwise operations
00504 
00508 template <class T>
00509 inline void VarVec<T>::perform(T (*mathFun)(T)) {
00510   for (int i=0;i<len;i++) {
00511     x[i] = (*mathFun)(x[i]);
00512   }
00513 }
00514 
00518 template <class T>
00519 inline void VarVec<T>::perform(T (*mathFun)(T,T), const T arg2) {
00520   for (int i=0;i<len;i++) {
00521     x[i] = (*mathFun)(x[i], arg2);
00522   }
00523 }
00524 
00528 template <class T>
00529 inline void VarVec<T>::perform(T (*mathFun)(T,T), const VarVec<T>& v) {
00530 #if MATMATH_CHECK_BOUNDS
00531   if (len!=v.len) {
00532     std::stringstream ss;
00533     ss << "VarVec<" << len << ">::perform(" <<
00534     ss << "VarVec<" << v.len << "): not a valid operation\n";
00535     ss << std::flush;
00536     throw Exception(ss.str());
00537   }
00538 #endif
00539   for (int i=0;i<len;i++) {
00540     x[i] = (*mathFun)(x[i], v.x[i]);
00541   }
00542 }
00543 
00547 template <class T>
00548 inline VarVec<T> VarVec<T>::performed(T (*mathFun)(T)) {
00549   VarVec<T> v(len);
00550   for (int i=0;i<len;i++) {
00551     v.x[i] = (*mathFun)(x[i]);
00552   }
00553   return v;
00554 }
00555 
00559 template <class T>
00560 inline VarVec<T> VarVec<T>::performed(T (*mathFun)(T,T), const T arg2) {
00561   VarVec<T> v(len);
00562   for (int i=0;i<len;i++) {
00563     v.x[i] = (*mathFun)(x[i], arg2);
00564   }
00565   return v;
00566 }
00567 
00571 template <class T>
00572 inline VarVec<T> VarVec<T>::performed(T (*mathFun)(T,T), const VarVec<T>& vp) {
00573 #if MATMATH_CHECK_BOUNDS
00574   if (len!=vp.len) {
00575     std::stringstream ss;
00576     ss << "VarVec<" << len << ">::performed(" <<
00577     ss << "VarVec<" << vp.len << "): not a valid operation\n";
00578     throw Exception(ss.str());
00579   }
00580 #endif
00581   VarVec<T> v(len);
00582   for (int i=0;i<len;i++) {
00583     v.x[i] = (*mathFun)(x[i], vp.x[i]);
00584   }
00585   return v;
00586 }
00587 
00591 template <class T>
00592 inline VarVec<T> VarVec<T>::minimize(const VarVec<T>& other) {
00593 #if MATMATH_CHECK_BOUNDS
00594   if (len!=other.len) {
00595     std::stringstream ss;
00596     ss << "VarVec<" << len << ">::minimize(" <<
00597     ss << "VarVec<" << other.len << "): not a valid operation\n";
00598     throw Exception(ss.str());
00599   }
00600 #endif
00601   for (int i=0;i<len;i++) {
00602     if (other.x[i] < x[i]) x[i] = other.x[i];
00603   }
00604   return *this;
00605 }
00606 
00610 template <class T>
00611 inline VarVec<T> VarVec<T>::maximize(const VarVec<T>& other) {
00612 #if MATMATH_CHECK_BOUNDS
00613   if (len!=other.len) {
00614     std::stringstream ss;
00615     ss << "VarVec<" << len << ">::maximize(" <<
00616     ss << "VarVec<" << other.len << "): not a valid operation\n";
00617     throw Exception(ss.str());
00618   }
00619 #endif
00620   for (int i=0;i<len;i++) {
00621     if (other.x[i] > x[i]) x[i] = other.x[i];
00622   }
00623   return *this;
00624 }
00625 
00626 // Dot, Cross, and Outer Products
00627 
00632 template <class T>
00633 inline T VarVec<T>::dot(const VarVec<T>& v) const {
00634 #if MATMATH_CHECK_BOUNDS
00635   if (len!=v.len) {
00636     std::stringstream ss;
00637     ss << "VarVec<" << len << ">::dot(" <<
00638     ss << "VarVec<" << v.len << "): not a valid operation\n";
00639     throw Exception(ss.str());
00640   }
00641 #endif
00642   T sum = T(0);
00643   for (int i=0;i<len;i++) sum += x[i]*v.x[i];
00644   return sum;
00645 }
00646 
00651 template <class T>
00652 inline VarSMat<T> VarVec<T>::outer(const VarVec<T>& v) const {
00653 #if MATMATH_CHECK_BOUNDS
00654   if (len!=v.len) {
00655     std::stringstream ss;
00656     ss << "VarVec<" << len << ">::outer(" <<
00657     ss << "VarVec<" << v.len << ">): not a valid operation\n";
00658     throw Exception(ss.str());
00659   }
00660 #endif
00661   VarSMat<T> m(len);
00662   for (int i=0,k=0;i<len;i++) for (int j=0;j<len;j++,k++) m.x[k] = x[i]*v.x[j];
00663   return m;
00664 }
00665 
00669 template <class T>
00670 inline VarSMat<T> VarVec<T>::outer() const {
00671   VarSMat<T> m(len);
00672   for (int i=0,k=0;i<len;i++) for (int j=0;j<len;j++,k++) m.x[k] = x[i]*x[j];
00673   return m;
00674 }
00675 
00676 // Random vectors and sorting
00677 
00678 //  /** Creates vector with samples from a uniform distribution on [a,b].
00679 //   * @param a start of range
00680 //   * @param b end of range
00681 //   * @return vector of uniform samples
00682 //   */
00683 //  template <class T>
00684 //  inline VarVec<T> VarVec<T>::uniformRand(const T a, const T b) {
00685 //    VarVec<T> v;
00686 //    for (int i=0;i<len;i++) v.x[i] = rtc::uniformRand<T>(a,b);
00687 //    return v;
00688 //  }
00689 //
00690 //  /** Create vector with samples from a normal distribution.
00691 //   * @param mean mean of normal distribution
00692 //   * @param stdev standard deviation of normal distribution
00693 //   * @return vector of normal samples
00694 //   */
00695 //  template <class T>
00696 //  inline VarVec<T> VarVec<T>::normalRand(const T mean, const T stdev) {
00697 //    VarVec<T> v;
00698 //    for (int i=0;i<len;i++) v.x[i] = rtc::normalRand<T>(mean, stdev);
00699 //    return v;
00700 //  }
00701 //
00702 //  /** Create vector from a multivariate gaussian distribution.
00703 //   * @param mean mean of normal distribution
00704 //   * @param stdev standard deviation of normal distribution
00705 //   * @return vector of a multivariate gaussian distribution
00706 //   */
00707 //  template <class T>
00708 //  inline VarVec<T> VarVec<T>::multivariateGauss(const VarVec<T>& mean, const SMat<T,len>& cov) {
00709 //    VarVec<T> v;
00710 //    SMat<T,len> S(cov);
00711 //    int n=S.choleskyDecomp();
00712 //    assert(n==0);
00713 //    S.transpose();
00714 //    VarVec<T> X = normalRand();
00715 //    v = mean + S*X;
00716 //    return v;
00717 //  }
00718 
00719 
00723 template <class T>
00724 inline void VarVec<T>::sort(bool ascending) {
00725   int count;
00726   do {
00727     count = 0;
00728     for (int i=0;i<(len-1);i++) {
00729       if (ascending) {
00730         if (x[i]> x[i+1]) {
00731           rtc_swap(x[i],x[i+1]); count++;
00732         }
00733       } else {
00734         if (x[i] < x[i+1]) {
00735           rtc_swap(x[i+1],x[i]); count++;
00736         }
00737       }
00738     }
00739   } while (count> 0);
00740 }
00741 
00746 template <class T>
00747 inline VarVec<T> VarVec<T>::sorted(bool ascending) {
00748   VarVec<T> v(len,x);
00749   v.sort(ascending);
00750   return v;
00751 }
00752 
00753 // Addition and subtraction
00754 
00757 template <class T>
00758 inline VarVec<T>& VarVec<T>::add(const VarVec<T>& v) {
00759 #if MATMATH_CHECK_BOUNDS
00760   if (len!=v.len) {
00761     std::stringstream ss;
00762     ss << "VarVec<" << len << ">::add(" <<
00763     ss << "VarVec<" << v.len << "): not a valid operation\n";
00764     throw Exception(ss.str());
00765   }
00766 #endif
00767   for (int i=0;i<len;i++) x[i] = x[i] + v.x[i];
00768   return *this;
00769 }
00770 
00776 template <class T>
00777 inline void VarVec<T>::addSubVec(const int i, const VarVec<T>& v) {
00778 #if MATMATH_CHECK_BOUNDS
00779   if (i < 0 || i+v.len > len) {
00780     std::stringstream ss;
00781     ss << "VarVec<" << len << ">::addSubVec(" << i << ", ";
00782     ss << "VarVec<" << v.len << ">): not a valid operation\n";
00783     throw Exception(ss.str());
00784   }
00785 #endif
00786   for (int j=i,k=0;j<len && k<v.len;j++,k++) x[j] += v.x[k];
00787 }
00788 
00791 template <class T>
00792 inline VarVec<T>& VarVec<T>::subtract(const VarVec<T>& v) {
00793 #if MATMATH_CHECK_BOUNDS
00794   if (len!=v.len) {
00795     std::stringstream ss;
00796     ss << "VarVec<" << len << ">::subtract(" <<
00797     ss << "VarVec<" << v.len << "): not a valid operation\n";
00798     throw Exception(ss.str());
00799   }
00800 #endif
00801   for (int i=0;i<len;i++) x[i] = x[i] - v.x[i];
00802   return *this;
00803 }
00804 
00810 template <class T>
00811 inline void VarVec<T>::subtractSubVec(const int i, const VarVec<T>& v) {
00812 #if MATMATH_CHECK_BOUNDS
00813   if (i < 0 || i+v.len > len) {
00814     std::stringstream ss;
00815     ss << "VarVec<" << len << ">::addSubVec(" << i << ", ";
00816     ss << "VarVec<" << v.len << ">): not a valid operation\n";
00817     throw Exception(ss.str());
00818   }
00819 #endif
00820   for (int j=i,k=0;j<len && k<v.len;j++,k++) x[j] -= v.x[k];
00821 }
00822 
00823 // Addition and subtraction operator
00824 
00827 template <class T>
00828 inline VarVec<T> VarVec<T>::operator + (const VarVec<T>& v) const {
00829 #if MATMATH_CHECK_BOUNDS
00830   if (len!=v.len) {
00831     std::stringstream ss;
00832     ss << "VarVec<" << len << ">::operator + (" <<
00833     ss << "VarVec<" << v.len << "): not a valid operation\n";
00834     throw Exception(ss.str());
00835   }
00836 #endif
00837   VarVec<T> vp(len);
00838   for (int i=0;i<len;i++) vp.x[i] = x[i] + v.x[i];
00839   return vp;
00840 }
00841 
00844 template <class T>
00845 inline void VarVec<T>::operator += (const VarVec<T>& v) {
00846 #if MATMATH_CHECK_BOUNDS
00847   if (len!=v.len) {
00848     std::stringstream ss;
00849     ss << "VarVec<" << len << ">::operator += (" <<
00850     ss << "VarVec<" << v.len << "): not a valid operation\n";
00851     throw Exception(ss.str());
00852   }
00853 #endif
00854   for (int i=0;i<len;i++) x[i] += v.x[i];
00855 }
00856 
00859 template <class T>
00860 inline VarVec<T> VarVec<T>::operator - (const VarVec<T>& v) const {
00861 #if MATMATH_CHECK_BOUNDS
00862   if (len!=v.len) {
00863     std::stringstream ss;
00864     ss << "VarVec<" << len << ">::operator - (" <<
00865     ss << "VarVec<" << v.len << "): not a valid operation\n";
00866     throw Exception(ss.str());
00867   }
00868 #endif
00869   VarVec<T> vp(len);
00870   for (int i=0;i<len;i++) vp.x[i] = x[i] - v.x[i];
00871   return vp;
00872 }
00873 
00876 template <class T>
00877 inline void VarVec<T>::operator -= (const VarVec<T>& v) {
00878 #if MATMATH_CHECK_BOUNDS
00879   if (len!=v.len) {
00880     std::stringstream ss;
00881     ss << "VarVec<" << len << ">::operator -= (" <<
00882     ss << "VarVec<" << v.len << "): not a valid operation\n";
00883     throw Exception(ss.str());
00884   }
00885 #endif
00886   for (int i=0;i<len;i++) x[i] -= v.x[i];
00887 }
00888 
00891 template <class T>
00892 inline VarVec<T> VarVec<T>::operator - () const {
00893   VarVec<T> vp(len);
00894   for (int i=0;i<len;i++) vp.x[i] = -x[i];
00895   return vp;
00896 }
00897 
00900 template <class T>
00901 inline VarVec<T> VarVec<T>::add(const T a) {
00902   for (int i=0;i<len;i++) x[i] = x[i] - a;
00903   return *this;
00904 }
00905 
00909 template <class T>
00910 inline VarVec<T> VarVec<T>::operator + (const T a) const {
00911   VarVec<T> vp(len);
00912   for (int i=0;i<len;i++) vp.x[i] = x[i] + a;
00913   return vp;
00914 }
00915 
00919 template <class T>
00920 inline void VarVec<T>::operator += (const T a) {
00921   for (int i=0;i<len;i++) x[i] += a;
00922 }
00923 
00926 template <class T>
00927 inline VarVec<T> VarVec<T>::subtract(const T a) {
00928   for (int i=0;i<len;i++) x[i] = x[i] - a;
00929   return *this;
00930 }
00931 
00935 template <class T>
00936 inline VarVec<T> VarVec<T>::operator - (const T a) const {
00937   VarVec<T> vp(len);
00938   for (int i=0;i<len;i++) vp.x[i] = x[i] - a;
00939   return vp;
00940 }
00941 
00945 template <class T>
00946 inline void VarVec<T>::operator -= (const T a) {
00947   for (int i=0;i<len;i++) x[i] -= a;
00948 }
00949 
00954 template <class T>
00955 inline VarVec<T> operator + (const T a, const VarVec<T>& v) {
00956   VarVec<T> vp(v.len);
00957   for (int i=0;i<v.len;i++) vp.x[i] = a + v.x[i];
00958   return vp;
00959 }
00960 
00965 template <class T>
00966 inline VarVec<T> operator - (const T a, const VarVec<T>& v) {
00967   VarVec<T> vp(v.len);
00968   for (int i=0;i<v.len;i++) vp.x[i] = a - v.x[i];
00969   return vp;
00970 }
00971 
00972 // Multiplication and division
00973 
00977 template <class T>
00978 inline VarVec<T> VarVec<T>::operator * (const T a) const {
00979   VarVec<T> vp(len);
00980   for (int i=0;i<len;i++) vp.x[i] = x[i]*a;
00981   return vp;
00982 }
00983 
00987 template <class T>
00988 inline VarVec<T> VarVec<T>::operator / (const T a) const {
00989   VarVec<T> vp(len);
00990   for (int i=0;i<len;i++) vp.x[i] = x[i]/a;
00991   return vp;
00992 }
00993 
00997 template <class T>
00998 inline void VarVec<T>::operator *= (const T a) {
00999   for (int i=0;i<len;i++) x[i] *= a;
01000 }
01001 
01005 template <class T>
01006 inline void VarVec<T>::operator /= (const T a) {
01007   for (int i=0;i<len;i++) x[i] /= a;
01008 }
01009 
01014 template <class T>
01015 inline VarVec<T> operator * (const T a, const VarVec<T>& v) {
01016   VarVec<T> vp(v.len);
01017   for (int i=0;i<v.len;i++) vp.x[i] = a*v.x[i];
01018   return vp;
01019 }
01020 
01021 // Equality and inequality tests.
01022 
01026 template <class T>
01027 inline bool VarVec<T>::operator == (const VarVec<T>& v) const {
01028 #if MATMATH_CHECK_BOUNDS
01029   if (len!=v.len) {
01030     std::stringstream ss;
01031     ss << "VarVec<" << len << ">::operator == (" <<
01032     ss << "VarVec<" << v.len << "): not a valid operation\n";
01033     ss << std::flush;
01034     throw Exception(ss.str());
01035   }
01036 #endif
01037   for (int i=0;i<len;i++)
01038     if (x[i] != v.x[i])
01039       return(false);
01040   return true;
01041 }
01042 
01046 template <class T>
01047 inline bool VarVec<T>::operator != (const VarVec<T>& v) const {
01048 #if MATMATH_CHECK_BOUNDS
01049   if (len!=v.len) {
01050     std::stringstream ss;
01051     ss << "VarVec<" << len << ">::operator != (" <<
01052     ss << "VarVec<" << v.len << "): not a valid operation\n";
01053     ss << std::flush;
01054     throw Exception(ss.str());
01055   }
01056 #endif
01057   for (int i=0;i<len;i++)
01058     if (x[i] != v.x[i])
01059       return(true);
01060   return false;
01061 }
01062 
01063 //  /** Element-wise test for equality.
01064 //   * @return vector of boolean results
01065 //   */
01066 //  template <class T>
01067 //  inline VarVec<bool> VarVec<T>::operator == (const VarVec<T>& v) const {
01068 //    VarVec<bool> b(len,false);
01069 //    for (int i=0;i<len;i++) if (x[i] == v.x[i]) b.x[i] = true;
01070 //    return b;
01071 //  }
01072 
01073 //  /** Element-wise test for inequality.
01074 //   * @return vector of boolean results
01075 //   */
01076 //  template <class T>
01077 //  inline VarVec<bool> VarVec<T>::operator != (const VarVec<T>& v) const {
01078 //    VarVec<bool> b(len,false);
01079 //    for (int i=0;i<len;i++) if (x[i] != v.x[i]) b.x[i] = true;
01080 //    return b;
01081 //  }
01082 
01086 template <class T>
01087 inline VarVec<bool> VarVec<T>::operator >= (const VarVec<T>& v) const {
01088 #if MATMATH_CHECK_BOUNDS
01089   if (len!=v.len) {
01090     std::stringstream ss;
01091     ss << "VarVec<" << len << ">::operator >= (" <<
01092     ss << "VarVec<" << v.len << "): not a valid operation\n";
01093     ss << std::flush;
01094     throw Exception(ss.str());
01095   }
01096 #endif
01097   VarVec<bool> b(len,false);
01098   for (int i=0;i<len;i++) if (x[i] >= v.x[i]) b.x[i] = true;
01099   return b;
01100 }
01101 
01105 template <class T>
01106 inline VarVec<bool> VarVec<T>::operator <= (const VarVec<T>& v) const {
01107 #if MATMATH_CHECK_BOUNDS
01108   if (len!=v.len) {
01109     std::stringstream ss;
01110     ss << "VarVec<" << len << ">::operator <= (" <<
01111     ss << "VarVec<" << v.len << "): not a valid operation\n";
01112     ss << std::flush;
01113     throw Exception(ss.str());
01114   }
01115 #endif
01116   VarVec<bool> b(len,false);
01117   for (int i=0;i<len;i++) if (x[i] <= v.x[i]) b.x[i] = true;
01118   return b;
01119 }
01120 
01124 template <class T>
01125 inline VarVec<bool> VarVec<T>::operator > (const VarVec<T>& v) const {
01126 #if MATMATH_CHECK_BOUNDS
01127   if (len!=v.len) {
01128     std::stringstream ss;
01129     ss << "VarVec<" << len << ">::operator > (" <<
01130     ss << "VarVec<" << v.len << "): not a valid operation\n";
01131     ss << std::flush;
01132     throw Exception(ss.str());
01133   }
01134 #endif
01135   VarVec<bool> b(len,false);
01136   for (int i=0;i<len;i++) if (x[i] > v.x[i]) b.x[i] = true;
01137   return b;
01138 }
01139 
01143 template <class T>
01144 inline VarVec<bool> VarVec<T>::operator < (const VarVec<T>& v) const {
01145 #if MATMATH_CHECK_BOUNDS
01146   if (len!=v.len) {
01147     std::stringstream ss;
01148     ss << "VarVec<" << len << ">::operator < (" <<
01149     ss << "VarVec<" << v.len << "): not a valid operation\n";
01150     ss << std::flush;
01151     throw Exception(ss.str());
01152   }
01153 #endif
01154   VarVec<bool> b(len,false);
01155   for (int i=0;i<len;i++) if (x[i] < v.x[i]) b.x[i] = true;
01156   return b;
01157 }
01158 
01163 template <class T>
01164 inline int VarVec<T>::compareTo(const VarVec<T>& v) const {
01165 #if MATMATH_CHECK_BOUNDS
01166   if (len!=v.len) {
01167     std::stringstream ss;
01168     ss << "VarVec<" << len << ">::compareTo (" <<
01169     ss << "VarVec<" << v.len << "): not a valid operation\n";
01170     ss << std::flush;
01171     throw Exception(ss.str());
01172   }
01173 #endif
01174   int g=0, l=0;
01175   for (int i=0;i<len;i++) {
01176     if (x[i] < v.x[i]) l++;
01177     if (x[i] > v.x[i]) g++;
01178   }
01179   if (l==len) return -1;
01180   else if (g==len) return 1;
01181   else return 0;
01182 }
01183 
01187 template <class T>
01188 inline bool VarVec<T>::equalTo(const VarVec<T>& v, const T tol) const {
01189 #if MATMATH_CHECK_BOUNDS
01190   if (len!=v.len) {
01191     std::stringstream ss;
01192     ss << "VarVec<" << len << ">::equalTo (" <<
01193     ss << "VarVec<" << v.len << "): not a valid operation\n";
01194     throw Exception(ss.str());
01195   }
01196 #endif
01197   bool t = true;
01198   for (int i=0;i<len;i++) if (rtc_abs(x[i] - v.x[i]) > tol) t = false;
01199   return t;
01200 }
01201 
01202 // Element-wise multiplication and division
01203 
01206 template <class T>
01207 inline VarVec<T> VarVec<T>::operator * (const VarVec<T>& v) const {
01208   VarVec<T> vp(v.len);
01209   for (int i=0;i<len;i++) vp.x[i] = x[i]*v.x[i];
01210   return vp;
01211 }
01212 
01215 template <class T>
01216 inline VarVec<T> VarVec<T>::operator / (const VarVec<T>& v) const {
01217   VarVec<T> vp(v.len);
01218   for (int i=0;i<v.len;i++) vp.x[i] = x[i]/v.x[i];
01219   return vp;
01220 }
01221 
01224 template <class T>
01225 inline void VarVec<T>::operator *= (const VarVec<T>& v) {
01226 #if MATMATH_CHECK_BOUNDS
01227   if (len!=v.len) {
01228     std::stringstream ss;
01229     ss << "VarVec<" << len << ">::operator *= (" <<
01230     ss << "VarVec<" << v.len << "): not a valid operation\n";
01231     throw Exception(ss.str());
01232   }
01233 #endif
01234   for (int i=0;i<len;i++) x[i] *= v.x[i];
01235 }
01236 
01239 template <class T>
01240 inline void VarVec<T>::operator /= (const VarVec<T>& v) {
01241 #if MATMATH_CHECK_BOUNDS
01242   if (len!=v.len) {
01243     std::stringstream ss;
01244     ss << "VarVec<" << len << ">::operator /= (" <<
01245     ss << "VarVec<" << v.len << "): not a valid operation\n";
01246     throw Exception(ss.str());
01247   }
01248 #endif
01249   for (int i=0;i<len;i++) x[i] /= v.x[i];
01250 }
01251 
01252 // Serialization routines
01253 
01256 template <class T>
01257 inline bool VarVec<T>::write(std::ostream& os) const {
01258   os.write((char *)(len),sizeof(int));
01259   os.write((char *)(x),len*sizeof(T));
01260   return os.good();
01261 }
01262 
01265 template <class T>
01266 inline bool VarVec<T>::read(std::istream& is) {
01267   int new_len;
01268   is.read((char *)(new_len),sizeof(int));
01269   if(new_len!=len) setSize(len);
01270   is.read((char *)(x),len*sizeof(T));
01271   return is.good();
01272 }
01273 
01276 template <class T>
01277 std::ostream& operator<<(std::ostream& os, const VarVec<T>& vec) {
01278 int minFieldWidth = os.precision()+2;
01279 
01280 os << "[";
01281 for (int i=0; i<vec.length(); ++i)
01282   os << std::setw(minFieldWidth) << vec.x[i] << " ";
01283 os << "]";
01284 
01285 return os;
01286 }
01287 
01290 template <class T>
01291 std::istream& operator>>(std::istream& is, VarVec<T>& vec) {
01292   using namespace std;
01293   vector<T> data;
01294   string vecString;
01295   stringstream vecStringStream;
01296 
01297   getline(is, vecString, ']');
01298   int sPos = (int)vecString.find('[');
01299   if (sPos == (int)string::npos)
01300     throw Exception("format error: expecting formated matrix to start with '['");
01301 
01302   //erase the starting '['
01303   //note the ending ']' was removed by the getline function as the delim
01304   vecString.erase(0,sPos+1);
01305   trim(vecString);
01306   vecStringStream.str(vecString);
01307 
01308   //determine num of rows and cols
01309   int colCount = 0;
01310   T tmpVal;
01311   while(vecStringStream.good()){
01312     vecStringStream >> tmpVal;
01313     data.push_back(tmpVal);
01314     ++colCount;
01315   }
01316 
01317   //check that dimensions agree
01318   if (colCount != vec.len)
01319     vec.setSize(colCount);
01320 
01321   //copy extracted data
01322   for (int i=0;i<vec.len;i++){
01323     vec.x[i] = data[i];
01324   }
01325 
01326   return is;
01327 }
01328 
01329 //==============================================================================
01330 } // NAMESPACE rtc
01331 //==============================================================================
01332 #endif // RTC_VARVEC_H defined
01333 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Mon Oct 6 2014 10:07:35