rtc: rtc: rtcArray.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 .......: rtcArray.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 14 $
00018  */
00019 #ifndef RTC_ARRAY_H
00020 #define RTC_ARRAY_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcVec.h"
00025 
00026 //== NAMESPACES ================================================================
00027 namespace rtc {
00028 
00029 // Forward declarations
00030 template <class T, int M> class Vec; // M-d vector
00031 template <class T, int K> class Array; // Arbitrary-dimensional array
00032 class Exception;
00033 
00037 template <class T, int K>
00038 class Array {
00039 public:
00040   // Constructors/Destructor
00041   Array();
00042   Array(const Vec<int,K> dim);
00043   Array(const Array<T,K>& a);
00044   ~Array();
00045 
00046   // Mutators
00047   void deleteArray() { reset(); }
00048   void reset();
00049   void setSize(const Vec<int,K> dim_);
00050   void set(const T v);
00051   void set(const T* v, int i0 = 0, int vlen = -1);
00052   void set(const Array<T,K>& a);
00053   void addTo(const T* v, int i0 = 0, int vlen = -1);
00054 
00055   // Accessors
00056   Vec<int,K> size() const;
00057   int size(int i) const;
00058   int length() const;
00059   T& at(int i);
00060   T& at(const Vec<int,K> ind);
00061   const T& at(int i) const;
00062   const T& at(const Vec<int,K> ind) const;
00063   T operator () (int i) const;
00064   T& operator () (int i);
00065   T operator () (const Vec<int,K> ind) const;
00066   T& operator () (const Vec<int,K> ind);
00067   T operator [] (int i) const;
00068   T& operator [] (int i);
00069   T operator [] (const Vec<int,K> ind) const;
00070   T& operator [] (const Vec<int,K> ind);
00071 
00072   // Assignment operator
00073   Array<T,K>& operator = (const Array<T,K>& a);
00074 
00075   // Equality and inequality tests
00076   bool equalTo(const Array<T,K>& a) const;
00077 
00078   // Serialization
00079   bool write(OutputHandler& oh) const;
00080   bool read(InputHandler& ih);
00081 
00082   // Helper functions
00083   int indexOf(Vec<int,K> ind) const;
00084 
00085   // Data
00086   T* x; 
00087 protected:
00088   Vec<int,K> dim; 
00089   Vec<int,K> mul; 
00090   int len; 
00091 };
00092 
00093 // ASCII stream IO
00094 template <class T, int K> std::ostream& operator << (std::ostream& os, const Array<T,K>& a);
00095 template <class T, int K> std::istream& operator >> (std::istream& is, Array<T,K>& a);
00096 
00097 //==============================================================================
00098 // Array<T,K>
00099 //==============================================================================
00100 
00101 // Constructors/Destructor
00102 
00105 template <class T, int K>
00106 inline Array<T,K>::Array() {
00107   x = NULL;
00108   len = 0;
00109   dim.set(0);
00110   mul.set(0);
00111 }
00112 
00116 template <class T, int K>
00117 inline Array<T,K>::Array(const Vec<int,K> dim_) {
00118   x=NULL;
00119   len = 0;
00120   dim.set(0);
00121   mul.set(0);
00122   setSize(dim_);
00123 }
00124 
00128 template <class T, int K>
00129 inline Array<T,K>::Array(const Array<T,K>& a) {
00130   x=NULL;
00131   len = 0;
00132   dim.set(0);
00133   mul.set(0);
00134   set(a);
00135 }
00136 
00139 template <class T, int K>
00140 inline Array<T,K>::~Array() {
00141   reset();
00142 }
00143 
00144 // Mutators
00145 
00148 template <class T, int K>
00149 inline void Array<T,K>::reset() {
00150   len = 0;
00151   dim.set(0);
00152   mul.set(0);
00153   if (x) {
00154     delete [] x;
00155     x = NULL;
00156   }
00157 }
00158 
00162 template <class T, int K>
00163 inline void Array<T,K>::setSize(const Vec<int,K> dim_) {
00164   if (!dim_.equalTo(dim)) {
00165     if (x != NULL) reset();
00166 //    for (int k=0;k<K;k++) if (dim_(k) <= 0) {
00167 //      if (AR_PO>1) {
00168 //        std::cout << dim_;
00169 //        std::cout << "Warning, negative or zero dimensions not allowed." << std::endl << std::flush;
00170 //      }
00171 //    }
00172     dim = dim_;
00173     len = dim.prod();
00174     mul(K-1) = 1;
00175     for (int d=1,k=K-2;d<K;d++,k--) mul(k) = mul(k+1)*dim(d);
00176     if (len>0) {
00177       x = new T[len];
00178     }
00179   }
00180 }
00181 
00185 template <class T, int K>
00186 inline void Array<T,K>::set(const T v) {
00187   for (int i=0;i<len;i++) x[i] = v;
00188 }
00189 
00195 template <class T, int K>
00196 inline void Array<T,K>::set(const T* v, int i0, int vlen) {
00197   int i1 = (vlen == -1)? len:rtc_clamp(i0+vlen,0,len);
00198   for (int i=i0,j=0;i<i1;i++,j++) x[i] = v[j];
00199 }
00200 
00204 template <class T, int K>
00205 inline void Array<T,K>::set(const Array<T,K>& a) {
00206   Vec<int,K> dim_ = a.Array<T,K>::size();
00207   Array<T,K>::setSize(dim_);
00208   Array<T,K>::set(a.x);
00209 }
00210 
00216 template <class T, int K>
00217 inline void Array<T,K>::addTo(const T* v, int i0, int vlen) {
00218   int i1 = (vlen == -1)? len:rtc_clamp(i0+vlen,0,len);
00219   for (int i=i0,j=0;i<i1;i++,j++) x[i] += v[j];
00220 }
00221 
00226 template <class T, int K>
00227 inline T& Array<T,K>::at(int i) {
00228   return x[i];
00229 }
00230 
00235 template <class T, int K>
00236 inline T& Array<T,K>::at(const Vec<int,K> ind) {
00237   return x[indexOf(ind)];
00238 }
00239 
00244 template <class T, int K>
00245 inline const T& Array<T,K>::at(int i) const {
00246   return x[i];
00247 }
00248 
00253 template <class T, int K>
00254 inline const T& Array<T,K>::at(const Vec<int,K> ind) const {
00255   return x[indexOf(ind)];
00256 }
00257 
00262 template <class T, int K>
00263 inline T& Array<T,K>::operator () (const Vec<int,K> ind) {
00264   return x[indexOf(ind)];
00265 }
00266 
00271 template <class T, int K>
00272 inline T& Array<T,K>::operator () (int i) {
00273 #if AR_CHECK_BOUNDS
00274   if (i<0 || i>=len) {
00275     std::stringstream ss;
00276     ss << "Error. Index " << i << " exceeds bounds [0, ";
00277     ss << len << "]." << std::endl << std::flush;
00278     throw Exception(ss.str());
00279   }
00280 #endif
00281   return x[i];
00282 }
00283 
00288 template <class T, int K>
00289 inline T Array<T,K>::operator () (const Vec<int,K> ind) const {
00290   return x[indexOf(ind)];
00291 }
00292 
00297 template <class T, int K>
00298 inline T Array<T,K>::operator () (int i) const {
00299 #if AR_CHECK_BOUNDS
00300   if (i<0 || i>=len) {
00301     std::stringstream ss;
00302     ss << "Error. Index " << i << " exceeds bounds [0, ";
00303     ss << len << "]." << std::endl << std::flush;
00304     throw Exception(ss.str());
00305   }
00306 #endif
00307   return x[i];
00308 }
00309 
00314 template <class T, int K>
00315 inline T& Array<T,K>::operator [] (const Vec<int,K> ind) {
00316   return x[indexOf(ind)];
00317 }
00318 
00323 template <class T, int K>
00324 inline T& Array<T,K>::operator [] (int i) {
00325 #if AR_CHECK_BOUNDS
00326   if (i<0 || i>=len) {
00327     std::stringstream ss;
00328     ss << "Error. Index " << i << " exceeds bounds [0, ";
00329     ss << len << "]." << std::endl << std::flush;
00330     throw Exception(ss.str());
00331   }
00332 #endif
00333   return x[i];
00334 }
00335 
00340 template <class T, int K>
00341 inline T Array<T,K>::operator [] (const Vec<int,K> ind) const {
00342   return x[indexOf(ind)];
00343 }
00344 
00349 template <class T, int K>
00350 inline T Array<T,K>::operator [] (int i) const {
00351 #if AR_CHECK_BOUNDS
00352   if (i<0 || i>=len) {
00353     std::stringstream ss;
00354     ss << "Error. Index " << i << " exceeds bounds [0, ";
00355     ss << len << "]." << std::endl << std::flush;
00356     throw Exception(ss.str());
00357   }
00358 #endif
00359   return x[i];
00360 }
00361 
00365 template <class T, int K>
00366 inline Array<T,K>& Array<T,K>::operator = (const Array<T,K>& a) {
00367   set(a);
00368   return *this;
00369 }
00370 
00374 template <class T, int K>
00375 inline bool Array<T,K>::equalTo(const Array<T,K>& other) const {
00376   for (int i=0;i<len;i++)
00377     if (x[i]!=other.x[i])
00378       return(false);
00379   return true;
00380 }
00381 
00382 // Accessors
00383 
00387 template <class T, int K>
00388 inline Vec<int,K> Array<T,K>::size() const {
00389   return dim;
00390 }
00391 
00395 template <class T, int K>
00396 inline int Array<T,K>::size(int i) const {
00397   return dim(i);
00398 }
00399 
00403 template <class T, int K>
00404 inline int Array<T,K>::length() const {
00405   return len;
00406 }
00407 
00408 // Serialization routines
00409 
00412 template <class T, int K>
00413 inline bool Array<T,K>::write(OutputHandler& oh) const {
00414   dim.write(oh);
00415   if (len>0) oh.write((char *)(x),len*sizeof(T));
00416   return oh.good();
00417 }
00418 
00421 template <class T, int K>
00422 inline bool Array<T,K>::read(InputHandler& ih) {
00423   Vec<int,K> dim_;
00424   dim_.read(ih);
00425   if (!dim_.equalTo(Array<T,K>::size())) Array<T,K>::setSize(dim_);
00426   if (len>0) ih.read((char *)(x),len*sizeof(T));
00427   return ih.good();
00428 }
00429 
00432 template <class T, int K>
00433 std::ostream& operator << (std::ostream& os, const Array<T,K>& a) {
00434   int minFieldWidth = os.precision()+5;
00435   Vec<int,K> dim = a.template size();
00436   os << dim;
00437   for (int i=0;i<a.length();i++) {
00438     if (i>0 && i%dim(0) == 0) os << std::endl;
00439     if (K>2) if (i>0 && i%(dim(0)*dim(1)) == 0) os << std::endl;
00440     if (K>3) if (i>0 && i%(dim(0)*dim(1)*dim(2)) == 0) os << std::endl;
00441     os << std::setw(minFieldWidth) << a(i) << std::endl;
00442   }
00443   os << std::endl;
00444   return os;
00445 }
00446 
00449 template <class T, int K>
00450 std::istream& operator >> (std::istream& is, Array<T,K>& a) {
00451   Vec<int,K> dim;
00452   is >> dim; // read in dimensions
00453   if (!dim.equalTo(a.template Array<T,K>::size())) a.setSize(dim);
00454   for (int i=0;i<a.length();i++) {
00455     is >> a(i);
00456   }
00457   return is;
00458 }
00459 
00460 // Helper functions (used internally only)
00461 
00466 template <class T, int K>
00467 inline int Array<T,K>::indexOf(Vec<int,K> ind) const {
00468 #if AR_CHECK_BOUNDS
00469   for (int k=0;k<K;k++) if (ind(k) < 0 || ind(k) >= dim(k)) {
00470     std::stringstream ss;
00471     ss << "Error. Index " << ind(k) << " exceeds bounds [0, ";
00472     ss << dim(k) << "]";
00473     throw Exception(ss.str());
00474   }
00475 #endif
00476   return (ind*mul).sum();
00477 }
00478 
00482 template <class T, int K>
00483 bool rtc_write(OutputHandler& oh, const Array<T,K>& data)
00484 {
00485   return data.write(oh);
00486 };
00487 
00491 template <class T, int K>
00492 bool rtc_read(InputHandler& ih, Array<T,K>& data)
00493 {
00494   return data.read(ih);
00495 };
00496 
00497 //==============================================================================
00498 } // namespace rtc
00499 //==============================================================================
00500 #endif // ARRAY_H defined
00501 //==============================================================================
00502