rtc: rtc: rtcVec.h Source 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 .......: rtcVec.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-03-13 16:48:41 -0700 (Fri, 13 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 99 $
00018  */
00019 #ifndef RTC_VEC_H
00020 #define RTC_VEC_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include <vector>
00024 #include "rtc/rtcMath.h"
00025 #include "rtc/rtcMat.h"
00026 
00027 #ifdef WIN32
00028 #undef min
00029 #undef max
00030 #endif
00031 //== NAMESPACES ================================================================
00032 namespace rtc {
00033 
00034 // Forward declarations
00035 template <class T, int M> class Vec;        // M-d vector
00036 template <class T, int M, int N> class Mat; // MxN Matrix
00037 template <class T, int M> class SMat;       // MxM Square Matrix
00038 
00043 template <class T, int M>
00044 class Vec {
00045 public:
00046   // Constructors
00047   Vec();
00048   Vec(const T* d);
00049   Vec(const T a);
00050   Vec(const Vec<T,M>& v);
00051 
00052   // Cast Operation
00053   template <class U> Vec(const Vec<U,M>& v);
00054 
00055   // Mutators
00056   void set(const T* d);
00057   void set(const T a);
00058   void set(const Vec<T,M>& v);
00059   Vec<T,M>& operator = (const T a);
00060   Vec<T,M>& operator = (const T* d);
00061   Vec<T,M>& operator = (const Vec<T,M>& v);
00062   template <int N> void setSubVec(const int i, const Vec<T,N>& v);
00063 
00064   // Casting Mutators
00065   template <class U> void set(const Vec<U,M>& v);
00066   template <class U> Vec<T,M>& operator = (const Vec<U,M>& v);
00067 
00068   // Accessors
00069   T operator () (const int i) const;
00070   T& operator () (const int i);
00071   T operator [] (const int i) const;
00072   T& operator [] (const int i);
00073   T& at(const int i);
00074   const T& at(const int i) const;
00075 
00076   template <int N> Vec<T,N> getSubVec(const int i) const;
00077 
00078   // Norms and Normalize
00079   T norm() const;
00080   T normSqr() const;
00081   T pnorm(float p) const;
00082   T normalize();
00083   Vec<T,M> normalized() const;
00084 
00085   // Reductions: Max/Min, Sum/Product
00086   T max() const;
00087   T min() const;
00088   T sum() const;
00089   T prod() const;
00090   Vec<T,M> cumsum() const;
00091 
00092   // General elementwise operations
00093   void perform(T (*mathFun)(T));
00094   void perform(T (*mathFun)(T,T), const T arg2);
00095   void perform(T (*mathFun)(T,T), const Vec<T,M>& v);
00096   Vec<T,M> performed(T (*mathFun)(T));
00097   Vec<T,M> performed(T (*mathFun)(T,T), const T arg2);
00098   Vec<T,M> performed(T (*mathFun)(T,T), const Vec<T,M>& v);
00099   Vec<T,M> minimize(const Vec<T,M>& other);
00100   Vec<T,M> maximize(const Vec<T,M>& other);
00101 
00102   // Dot and Outer Products
00103   T dot(const Vec<T,M>& v) const;
00104   SMat<T,M> outer(const Vec<T,M>& v) const;
00105   SMat<T,M> outer() const;
00106 
00107   // Random vectors and sorting
00108   static Vec<T,M> uniformRand(const T a = T(0), const T b = T(1));
00109   static Vec<T,M> normalRand(const T mean = T(0), const T stdev = T(1));
00110   static Vec<T,M> multivariateGauss(const Vec<T,M>& mean, const SMat<T,M>& cov);
00111 
00112   void sort(bool ascending = true);
00113   Vec<T,M> sorted(bool ascending = true);
00114 
00115   // Addition and subtraction
00116   Vec<T,M>& add(const Vec<T,M>& v);
00117   Vec<T,M>& subtract(const Vec<T,M>& v);
00118 
00119   // Addition and subtraction operator
00120   Vec<T,M> operator + (const Vec<T,M>& v) const;
00121   void operator += (const Vec<T,M>& v);
00122   Vec<T,M> operator - (const Vec<T,M>& v) const;
00123   void operator -= (const Vec<T,M>& v);
00124   Vec<T,M> operator - () const;
00125   Vec<T,M> add(const T a);
00126   Vec<T,M> operator + (const T a) const;
00127   void operator += (const T a);
00128   Vec<T,M> subtract(const T a);
00129   Vec<T,M> operator - (const T a) const;
00130   void operator -= (const T a);
00131 
00132   // Multiplication and division operator
00133   Vec<T,M> operator * (const T a) const;
00134   Vec<T,M> operator / (const T a) const;
00135   void operator *= (const T a);
00136   void operator /= (const T a);
00137 
00138   // Equality and inequality tests
00139   int compareTo(const Vec<T,M>& v) const;
00140   bool equalTo(const Vec<T,M>& v, const T tol = T(0)) const;
00141 
00142   // Equality and inequality tests operator
00143   bool operator == (const Vec<T,M>& v) const;
00144   bool operator != (const Vec<T,M>& v) const;
00145   Vec<bool,M> operator >= (const Vec<T,M>& v) const;
00146   Vec<bool,M> operator <= (const Vec<T,M>& v) const;
00147   Vec<bool,M> operator > (const Vec<T,M>& v) const;
00148   Vec<bool,M> operator < (const Vec<T,M>& v) const;
00149 
00150   // Element-wise operations
00151   Vec<T,M> operator * (const Vec<T,M>& v) const;
00152   Vec<T,M> operator / (const Vec<T,M>& v) const;
00153   void operator *= (const Vec<T,M>& v);
00154   void operator /= (const Vec<T,M>& v);
00155 
00156   // Serialization
00157   bool write(OutputHandler& oh) const;
00158   bool read(InputHandler& ih);
00159 
00160   //data
00161   T x[M]; 
00162 };
00163 
00164 // Global operators for cases where Vec<T,M>
00165 // is the second argument in a binary operator
00166 template <class T, int M> Vec<T,M> operator + (const T a, const Vec<T,M>& v);
00167 template <class T, int M> Vec<T,M> operator - (const T a, const Vec<T,M>& v);
00168 template <class T, int M> Vec<T,M> operator * (const T a, const Vec<T,M>& v);
00169 
00170 // ASCII stream IO
00171 template <class T, int M> std::ostream& operator<<(std::ostream& os, const Vec<T,M>& vec);
00172 template <class T, int M> std::istream& operator>>(std::istream& is, Vec<T,M>& vec);
00173 
00174 //==============================================================================
00175 // Vec<T,M>
00176 //==============================================================================
00177 
00178 // Constructors
00179 
00182 template <class T, int M>
00183 inline Vec<T,M>::Vec() {}
00184 
00188 template <class T, int M>
00189 inline Vec<T,M>::Vec(const T* d) {
00190   set(d);
00191 }
00192 
00196 template <class T, int M>
00197 inline Vec<T,M>::Vec(const T a) {
00198   set(a);
00199 }
00200 
00204 template <class T, int M>
00205 inline Vec<T,M>::Vec(const Vec<T,M>& v) {
00206   set(v);
00207 }
00208 
00209 // Casting Operation
00210 
00214 template <class T, int M> template <class U>
00215 inline Vec<T,M>::Vec(const Vec<U,M>& v) {
00216   set<U>(v);
00217 }
00218 
00219 // Mutators
00220 
00224 template <class T, int M>
00225 inline void Vec<T,M>::set(const T a) {
00226   if (a == T(0))
00227     memset(x,0,M*sizeof(T));
00228   else
00229     for (int i=0;i<M;++i)
00230       x[i] = a;
00231 }
00232 
00236 template <class T, int M>
00237 inline void Vec<T,M>::set(const T* d) {
00238   for (int i=0;i<M;i++) x[i] = d[i];
00239 }
00240 
00244 template <class T, int M>
00245 inline void Vec<T,M>::set(const Vec<T,M>& v) {
00246   memcpy((void*)x,(void*)v.x,M*sizeof(T));
00247 }
00248 
00252 template <class T, int M>
00253 inline Vec<T,M>& Vec<T,M>::operator = (const T a) {
00254   set(a);
00255   return *this;
00256 }
00257 
00261 template <class T, int M>
00262 inline Vec<T,M>& Vec<T,M>::operator = (const T* d) {
00263   set(d);
00264   return *this;
00265 }
00266 
00270 template <class T, int M>
00271 inline Vec<T,M>& Vec<T,M>::operator = (const Vec<T,M>& v) {
00272   set(v);
00273   return *this;
00274 }
00275 
00282 template <class T, int M> template <int N>
00283 inline void Vec<T,M>::setSubVec(const int i, const Vec<T,N>& v) {
00284   for (int j=i,k=0;j<M && k<N;j++,k++) x[j] = v.x[k];
00285 }
00286 
00287 // Casting Mutators
00288 
00292 template <class T, int M> template <class U>
00293 inline void Vec<T,M>::set(const Vec<U,M>& v) {
00294   for (int i=0;i<M;i++) x[i] = T(v.x[i]);
00295 }
00296 
00300 template <class T, int M> template <class U>
00301 inline Vec<T,M>& Vec<T,M>::operator = (const Vec<U,M>& v) {
00302   set<U>(v);
00303   return *this;
00304 }
00305 
00306 // Accessors
00307 
00312 template <class T, int M>
00313 inline T Vec<T,M>::operator () (const int i) const {
00314 #if MATMATH_CHECK_BOUNDS
00315   if (i < 0 || i > M) {
00316     std::cerr << "Vec<" << M << ">::(" << i << "): index out of range\n";
00317     std::cerr << std::flush;
00318     exit(1);
00319 }
00320 #endif
00321   return x[i];
00322 }
00323 
00328 template <class T, int M>
00329 inline T& Vec<T,M>::operator () (const int i) {
00330 #if MATMATH_CHECK_BOUNDS
00331   if (i < 0 || i > M) {
00332     std::cerr << "&Vec<" << M << ">::(" << i << "): index out of range\n";
00333     std::cerr << std::flush;
00334     exit(1);
00335   }
00336 #endif
00337   return x[i];
00338 }
00339 
00344 template <class T, int M>
00345 inline T Vec<T,M>::operator [] (const int i) const {
00346 #if MATMATH_CHECK_BOUNDS
00347   if (i < 0 || i > M) {
00348     std::cerr << "Vec<" << M << ">::(" << i << "): index out of range\n";
00349     std::cerr << std::flush;
00350     exit(1);
00351 }
00352 #endif
00353   return x[i];
00354 }
00355 
00360 template <class T, int M>
00361 inline T& Vec<T,M>::operator [] (const int i) {
00362 #if MATMATH_CHECK_BOUNDS
00363   if (i < 0 || i > M) {
00364     std::cerr << "&Vec<" << M << ">::(" << i << "): index out of range\n";
00365     std::cerr << std::flush;
00366     exit(1);
00367   }
00368 #endif
00369   return x[i];
00370 }
00371 
00376 template <class T, int M>
00377 inline T& Vec<T,M>::at(const int i) {
00378 #if MATMATH_CHECK_BOUNDS
00379   if (i < 0 || i > M) {
00380     std::cerr << "&Vec<" << M << ">::(" << i << "): index out of range\n";
00381     std::cerr << std::flush;
00382     exit(1);
00383   }
00384 #endif
00385   return x[i];
00386 }
00387 
00392 template <class T, int M>
00393 inline const T& Vec<T,M>::at(const int i) const {
00394 #if MATMATH_CHECK_BOUNDS
00395   if (i < 0 || i > M) {
00396     std::cerr << "&Vec<" << M << ">::(" << i << "): index out of range\n";
00397     std::cerr << std::flush;
00398     exit(1);
00399   }
00400 #endif
00401   return x[i];
00402 }
00403 
00407 template <class T, int M> template <int N>
00408 inline Vec<T,N> Vec<T,M>::getSubVec(const int i) const {
00409   Vec<T,N> v(T(0));
00410   for (int j=i,k=0;j<M && k<N;j++,k++) v.x[k] = x[j];
00411   return v;
00412 }
00413 
00414 // Norms and Normalize
00415 
00419 template <class T, int M>
00420 inline T Vec<T,M>::norm() const {
00421   return T(sqrt(double(normSqr())));
00422 }
00423 
00427 template <class T, int M>
00428 inline T Vec<T,M>::normSqr() const {
00429   T sum = T(0);
00430   for (int i=0;i<M;i++) sum += x[i]*x[i];
00431   return sum;
00432 }
00433 
00438 template <class T, int M>
00439 inline T Vec<T,M>::pnorm(float p) const {
00440   T sum = T(0);
00441   for (int i=0;i<M;i++) sum += pow(fabs((double)x[i]), (double)p);
00442   return pow((double)sum, (double)1.0/p);
00443 }
00444 
00448 template <class T, int M>
00449 inline T Vec<T,M>::normalize() {
00450   T l = norm();
00451   if (l > T(0)) for (int i=0;i<M;i++) x[i] /= l;
00452   return l;
00453 }
00454 
00458 template <class T, int M>
00459 inline Vec<T,M> Vec<T,M>::normalized() const {
00460   Vec<T,M> v(x);
00461   v.normalize();
00462   return v;
00463 }
00464 
00465 // Maximum/Minimum value and Sum
00466 
00470 template <class T, int M>
00471 inline T Vec<T,M>::max() const {
00472   T m = x[0];
00473   for (int i=1;i<M;i++) if (x[i]>m) m = x[i];
00474   return m;
00475 }
00476 
00480 template <class T, int M>
00481 inline T Vec<T,M>::min() const {
00482   T m = x[0];
00483   for (int i=1;i<M;i++) if (x[i]<m) m = x[i];
00484   return m;
00485 }
00486 
00490 template <class T, int M>
00491 inline T Vec<T,M>::sum() const {
00492   T s = T(0);
00493   for (int i=0;i<M;i++) s += x[i];
00494   return s;
00495 }
00496 
00500 template <class T, int M>
00501 inline Vec<T,M> Vec<T,M>::cumsum() const {
00502   Vec<T,M> v;
00503   T s = T(0);
00504   for (int i=0;i<M;i++) {
00505     s += x[i];
00506     v.x[i]=s;
00507   }
00508   return v;
00509 }
00510 
00514 template <class T, int M>
00515 inline T Vec<T,M>::prod() const {
00516   T p = T(1);
00517   for (int i=0;i<M;i++) p *= x[i];
00518   return p;
00519 }
00520 
00521 // General elementwise operations
00522 
00526 template <class T, int M>
00527 inline void Vec<T,M>::perform(T (*mathFun)(T)) {
00528   for (int i=0;i<M;i++) {
00529     x[i] = (*mathFun)(x[i]);
00530   }
00531 }
00532 
00536 template <class T, int M>
00537 inline void Vec<T,M>::perform(T (*mathFun)(T,T), const T arg2) {
00538   for (int i=0;i<M;i++) {
00539     x[i] = (*mathFun)(x[i], arg2);
00540   }
00541 }
00542 
00546 template <class T, int M>
00547 inline void Vec<T,M>::perform(T (*mathFun)(T,T), const Vec<T,M>& v) {
00548   for (int i=0;i<M;i++) {
00549     x[i] = (*mathFun)(x[i], v.x[i]);
00550   }
00551 }
00552 
00556 template <class T, int M>
00557 inline Vec<T,M> Vec<T,M>::performed(T (*mathFun)(T)) {
00558   Vec<T,M> v;
00559   for (int i=0;i<M;i++) {
00560     v.x[i] = (*mathFun)(x[i]);
00561   }
00562   return v;
00563 }
00564 
00568 template <class T, int M>
00569 inline Vec<T,M> Vec<T,M>::performed(T (*mathFun)(T,T), const T arg2) {
00570   Vec<T,M> v;
00571   for (int i=0;i<M;i++) {
00572     v.x[i] = (*mathFun)(x[i], arg2);
00573   }
00574   return v;
00575 }
00576 
00580 template <class T, int M>
00581 inline Vec<T,M> Vec<T,M>::performed(T (*mathFun)(T,T), const Vec<T,M>& vp) {
00582   Vec<T,M> v;
00583   for (int i=0;i<M;i++) {
00584     v.x[i] = (*mathFun)(x[i], vp.x[i]);
00585   }
00586   return v;
00587 }
00588 
00592 template <class T, int M>
00593 inline Vec<T,M> Vec<T,M>::minimize(const Vec<T,M>& other) {
00594   for (int i=0;i<M;i++) {
00595     if (other.x[i] < x[i]) x[i] = other.x[i];
00596   }
00597   return *this;
00598 }
00599 
00603 template <class T, int M>
00604 inline Vec<T,M> Vec<T,M>::maximize(const Vec<T,M>& other) {
00605   for (int i=0;i<M;i++) {
00606     if (other.x[i] > x[i]) x[i] = other.x[i];
00607   }
00608   return *this;
00609 }
00610 
00611 // Dot, Cross, and Outer Products
00612 
00617 template <class T, int M>
00618 inline T Vec<T,M>::dot(const Vec<T,M>& v) const {
00619   T sum = T(0);
00620   for (int i=0;i<M;i++) sum += x[i]*v.x[i];
00621   return sum;
00622 }
00623 
00628 template <class T, int M>
00629 inline SMat<T,M> Vec<T,M>::outer(const Vec<T,M>& v) const {
00630   SMat<T,M> m;
00631   for (int i=0,k=0;i<M;i++) for (int j=0;j<M;j++,k++) m.x[k] = x[i]*v.x[j];
00632   return m;
00633 }
00634 
00638 template <class T, int M>
00639 inline SMat<T,M> Vec<T,M>::outer() const {
00640   SMat<T,M> m;
00641   for (int i=0,k=0;i<M;i++) for (int j=0;j<M;j++,k++) m.x[k] = x[i]*x[j];
00642   return m;
00643 }
00644 
00645 // Random vectors and sorting
00646 
00652 template <class T, int M>
00653 inline Vec<T,M> Vec<T,M>::uniformRand(const T a, const T b) {
00654   Vec<T,M> v;
00655   for (int i=0;i<M;i++) v.x[i] = rtc_uniform_rand<T>(a,b);
00656   return v;
00657 }
00658 
00664 template <class T, int M>
00665 inline Vec<T,M> Vec<T,M>::normalRand(const T mean, const T stdev) {
00666   Vec<T,M> v;
00667   for (int i=0;i<M;i++) v.x[i] = rtc_normal_rand<T>(mean, stdev);
00668   return v;
00669 }
00670 
00676 template <class T, int M>
00677 inline Vec<T,M> Vec<T,M>::multivariateGauss(const Vec<T,M>& mean, const SMat<T,M>& cov) {
00678   Vec<T,M> v;
00679   SMat<T,M> S(cov);
00680   int n=S.choleskyDecomp();
00681   S.transpose();
00682   Vec<T,M> X = normalRand();
00683   v = mean + S*X;
00684   return v;
00685 }
00686 
00687 
00691 template <class T, int M>
00692 inline void Vec<T,M>::sort(bool ascending) {
00693   int count;
00694   do {
00695     count = 0;
00696     for (int i=0;i<(M-1);i++) {
00697 if (ascending) {
00698   if (x[i] > x[i+1]) {
00699     rtc_swap(x[i],x[i+1]); count++;
00700   }
00701 } else {
00702   if (x[i] < x[i+1]) {
00703     rtc_swap(x[i+1],x[i]); count++;
00704   }
00705 }
00706     }
00707   } while (count > 0);
00708 }
00709 
00713 template <class T, int M>
00714 inline Vec<T,M> Vec<T,M>::sorted(bool ascending) {
00715   Vec<T,M> v(x);
00716   v.sort(ascending);
00717   return v;
00718 }
00719 
00720 // Addition and subtraction
00721 
00724 template <class T, int M>
00725 inline Vec<T,M>& Vec<T,M>::add(const Vec<T,M>& v) {
00726   for (int i=0;i<M;i++) x[i] = x[i] + v.x[i];
00727   return *this;
00728 }
00729 
00732 template <class T, int M>
00733 inline Vec<T,M>& Vec<T,M>::subtract(const Vec<T,M>& v) {
00734   for (int i=0;i<M;i++) x[i] = x[i] - v.x[i];
00735   return *this;
00736 }
00737 
00738 // Addition and subtraction operator
00739 
00742 template <class T, int M>
00743 inline Vec<T,M> Vec<T,M>::operator + (const Vec<T,M>& v) const {
00744   Vec<T,M> vp;
00745   for (int i=0;i<M;i++) vp.x[i] = x[i] + v.x[i];
00746   return vp;
00747 }
00748 
00751 template <class T, int M>
00752 inline void Vec<T,M>::operator += (const Vec<T,M>& v) {
00753   for (int i=0;i<M;i++) x[i] += v.x[i];
00754 }
00755 
00758 template <class T, int M>
00759 inline Vec<T,M> Vec<T,M>::operator - (const Vec<T,M>& v) const {
00760   Vec<T,M> vp;
00761   for (int i=0;i<M;i++) vp.x[i] = x[i] - v.x[i];
00762   return vp;
00763 }
00764 
00767 template <class T, int M>
00768 inline void Vec<T,M>::operator -= (const Vec<T,M>& v) {
00769   for (int i=0;i<M;i++) x[i] -= v.x[i];
00770 }
00771 
00774 template <class T, int M>
00775 inline Vec<T,M> Vec<T,M>::operator - () const {
00776   Vec<T,M> vp;
00777   for (int i=0;i<M;i++) vp.x[i] = -x[i];
00778   return vp;
00779 }
00780 
00783 template <class T, int M>
00784 inline Vec<T,M> Vec<T,M>::add(const T a) {
00785   for (int i=0;i<M;i++) x[i] = x[i] - a;
00786   return *this;
00787 }
00788 
00792 template <class T, int M>
00793 inline Vec<T,M> Vec<T,M>::operator + (const T a) const {
00794   Vec<T,M> vp;
00795   for (int i=0;i<M;i++) vp.x[i] = x[i] + a;
00796   return vp;
00797 }
00798 
00802 template <class T, int M>
00803 inline void Vec<T,M>::operator += (const T a) {
00804   for (int i=0;i<M;i++) x[i] += a;
00805 }
00806 
00809 template <class T, int M>
00810 inline Vec<T,M> Vec<T,M>::subtract(const T a) {
00811   for (int i=0;i<M;i++) x[i] = x[i] - a;
00812   return *this;
00813 }
00814 
00818 template <class T, int M>
00819 inline Vec<T,M> Vec<T,M>::operator - (const T a) const {
00820   Vec<T,M> vp;
00821   for (int i=0;i<M;i++) vp.x[i] = x[i] - a;
00822   return vp;
00823 }
00824 
00828 template <class T, int M>
00829 inline void Vec<T,M>::operator -= (const T a) {
00830   for (int i=0;i<M;i++) x[i] -= a;
00831 }
00832 
00837 template <class T, int M>
00838 inline Vec<T,M> operator + (const T a, const Vec<T,M>& v) {
00839   Vec<T,M> vp;
00840   for (int i=0;i<M;i++) vp.x[i] = a + v.x[i];
00841   return vp;
00842 }
00843 
00848 template <class T, int M>
00849 inline Vec<T,M> operator - (const T a, const Vec<T,M>& v) {
00850   Vec<T,M> vp;
00851   for (int i=0;i<M;i++) vp.x[i] = a - v.x[i];
00852   return vp;
00853 }
00854 
00855 // Multiplication and division
00856 
00860 template <class T, int M>
00861 inline Vec<T,M> Vec<T,M>::operator * (const T a) const {
00862   Vec<T,M> vp;
00863   for (int i=0;i<M;i++) vp.x[i] = x[i]*a;
00864   return vp;
00865 }
00866 
00870 template <class T, int M>
00871 inline Vec<T,M> Vec<T,M>::operator / (const T a) const {
00872   Vec<T,M> vp;
00873   for (int i=0;i<M;i++) vp.x[i] = x[i]/a;
00874   return vp;
00875 }
00876 
00880 template <class T, int M>
00881 inline void Vec<T,M>::operator *= (const T a) {
00882   for (int i=0;i<M;i++) x[i] *= a;
00883 }
00884 
00888 template <class T, int M>
00889 inline void Vec<T,M>::operator /= (const T a) {
00890   for (int i=0;i<M;i++) x[i] /= a;
00891 }
00892 
00897 template <class T, int M>
00898 inline Vec<T,M> operator * (const T a, const Vec<T,M>& v) {
00899   Vec<T,M> vp;
00900   for (int i=0;i<M;i++) vp.x[i] = a*v.x[i];
00901   return vp;
00902 }
00903 
00904 // Equality and inequality tests.
00905 
00909 template <class T, int M>
00910 inline bool Vec<T,M>::operator == (const Vec<T,M>& v) const {
00911   for (int i=0;i<M;i++)
00912     if (x[i] != v.x[i])
00913       return(false);
00914   return true;
00915 }
00916 
00920 template <class T, int M>
00921 inline bool Vec<T,M>::operator != (const Vec<T,M>& v) const {
00922   for (int i=0;i<M;i++)
00923     if (x[i] != v.x[i])
00924       return(true);
00925   return false;
00926 }
00927 
00928 //  /** Element-wise test for equality.
00929 //   * @return vector of boolean results
00930 //   */
00931 //  template <class T, int M>
00932 //  inline Vec<bool,M> Vec<T,M>::operator == (const Vec<T,M>& v) const {
00933 //    Vec<bool,M> b(false);
00934 //    for (int i=0;i<M;i++) if (x[i] == v.x[i]) b.x[i] = true;
00935 //    return b;
00936 //  }
00937 
00938 //  /** Element-wise test for inequality.
00939 //   * @return vector of boolean results
00940 //   */
00941 //  template <class T, int M>
00942 //  inline Vec<bool,M> Vec<T,M>::operator != (const Vec<T,M>& v) const {
00943 //    Vec<bool,M> b(false);
00944 //    for (int i=0;i<M;i++) if (x[i] != v.x[i]) b.x[i] = true;
00945 //    return b;
00946 //  }
00947 
00951 template <class T, int M>
00952 inline Vec<bool,M> Vec<T,M>::operator >= (const Vec<T,M>& v) const {
00953   Vec<bool,M> b(false);
00954   for (int i=0;i<M;i++) if (x[i] >= v.x[i]) b.x[i] = true;
00955   return b;
00956 }
00957 
00961 template <class T, int M>
00962 inline Vec<bool,M> Vec<T,M>::operator <= (const Vec<T,M>& v) const {
00963   Vec<bool,M> b(false);
00964   for (int i=0;i<M;i++) if (x[i] <= v.x[i]) b.x[i] = true;
00965   return b;
00966 }
00967 
00971 template <class T, int M>
00972 inline Vec<bool,M> Vec<T,M>::operator > (const Vec<T,M>& v) const {
00973   Vec<bool,M> b(false);
00974   for (int i=0;i<M;i++) if (x[i] > v.x[i]) b.x[i] = true;
00975   return b;
00976 }
00977 
00981 template <class T, int M>
00982 inline Vec<bool,M> Vec<T,M>::operator < (const Vec<T,M>& v) const {
00983   Vec<bool,M> b(false);
00984   for (int i=0;i<M;i++) if (x[i] < v.x[i]) b.x[i] = true;
00985   return b;
00986 }
00987 
00992 template <class T, int M>
00993 inline int Vec<T,M>::compareTo(const Vec<T,M>& v) const {
00994   int g=0, l=0;
00995   for (int i=0;i<M;i++) {
00996     if (x[i] < v.x[i]) l++;
00997     if (x[i] > v.x[i]) g++;
00998   }
00999   if (l==M) return -1;
01000   else if (g==M) return 1;
01001   else return 0;
01002 }
01003 
01007 template <class T, int M>
01008 inline bool Vec<T,M>::equalTo(const Vec<T,M>& v, const T tol) const {
01009   bool t = true;
01010   for (int i=0;i<M;i++) if (rtc_abs(x[i] - v.x[i]) > tol) t = false;
01011   return t;
01012 }
01013 
01014 // Element-wise multiplication and division
01015 
01018 template <class T, int M>
01019 inline Vec<T,M> Vec<T,M>::operator * (const Vec<T,M>& v) const {
01020   Vec<T,M> vp;
01021   for (int i=0;i<M;i++) vp.x[i] = x[i]*v.x[i];
01022   return vp;
01023 }
01024 
01027 template <class T, int M>
01028 inline Vec<T,M> Vec<T,M>::operator / (const Vec<T,M>& v) const {
01029   Vec<T,M> vp;
01030   for (int i=0;i<M;i++) vp.x[i] = x[i]/v.x[i];
01031   return vp;
01032 }
01033 
01036 template <class T, int M>
01037 inline void Vec<T,M>::operator *= (const Vec<T,M>& v) {
01038   for (int i=0;i<M;i++) x[i] *= v.x[i];
01039 }
01040 
01043 template <class T, int M>
01044 inline void Vec<T,M>::operator /= (const Vec<T,M>& v) {
01045   for (int i=0;i<M;i++) x[i] /= v.x[i];
01046 }
01047 
01048 // Serialization routines
01049 
01052 template <class T, int M>
01053 inline bool Vec<T,M>::write(OutputHandler& oh) const {
01054   return oh.write((char *)(x),M*sizeof(T));
01055 }
01056 
01059 template <class T, int M>
01060 inline bool Vec<T,M>::read(InputHandler& ih) {
01061   return ih.read((char *)(x),M*sizeof(T));
01062 }
01063 
01066 template <class T, int M>
01067 std::ostream& operator<<(std::ostream& os, const Vec<T,M>& vec) {
01068 int minFieldWidth = os.precision()+2;
01069 
01070 os << "[";
01071 for (int i=0; i<M; ++i)
01072   os << std::setw(minFieldWidth) << vec.x[i] << " ";
01073 os << "]";
01074 
01075 return os;
01076 }
01077 
01080 template <class T, int M>
01081 std::istream& operator>>(std::istream& is, Vec<T,M>& vec) {
01082   using namespace std;
01083   vector<T> data;
01084   string vecString;
01085   stringstream vecStringStream;
01086 
01087   getline(is, vecString, ']');
01088   int sPos = (int)vecString.find('[');
01089   if (sPos == (int)string::npos)
01090     throw Exception("format error: expecting formated vector to start with '['");
01091 
01092   //erase the starting '['
01093   //note the ending ']' was removed by the getline function as the delim
01094   vecString.erase(0,sPos+1);
01095   trim(vecString);
01096   vecStringStream.str(vecString);
01097 
01098   //determine num of rows and cols
01099   int colCount = 0;
01100   T tmpVal;
01101   while(vecStringStream.good()){
01102     vecStringStream >> tmpVal;
01103     data.push_back(tmpVal);
01104     ++colCount;
01105   }
01106 
01107   //check that dimensions agree
01108   if (colCount != M){
01109     std::stringstream ss;
01110     ss << "format error: formated text has " << colCount << " columns";
01111     ss << " while destination vector has " << M  << " columns" << endl;
01112     throw Exception(ss.str());
01113   }
01114 
01115   //copy extracted data
01116   for (int i=0;i<M;i++){
01117     vec.x[i] = data[i];
01118   }
01119 
01120   return is;
01121 }
01122 
01126 template <class T, int M>
01127 bool rtc_write(OutputHandler& oh, const Vec<T,M>& data)
01128 {
01129   return data.write(oh);
01130 };
01131 
01135 template <class T, int M>
01136 bool rtc_read(InputHandler& ih, Vec<T,M>& data)
01137 {
01138   return data.read(ih);
01139 };
01140 
01141 //==============================================================================
01142 } // NAMESPACE rtc
01143 //==============================================================================
01144 #endif // RTC_VEC_H defined
01145 //==============================================================================