00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef Vector_H
00020 #define Vector_H
00021
00022 #include <stdlib.h>
00023 #include <inttypes.h>
00024 #include <WProgram.h>
00025
00026 #ifdef ASSERT
00027 const static char vectorSource[] ="Vector.hpp";
00028 #endif
00029
00030
00031 template <class dataType,class sumType=dataType>
00032 class Vector
00033 {
00034 private:
00035 size_t size;
00036 size_t extraAllocationSize;
00037 size_t sizeAllocated;
00038 dataType* data;
00039 public:
00040
00041 Vector(const size_t & size=0, const size_t & extraAllocationSize=0) : size(0), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
00042 setSize(size);
00043 }
00044
00045 Vector(const dataType & a, const dataType & b, const dataType & c) : size(3), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
00046 setSize(size);
00047 (*this)[0]=a;
00048 (*this)[1]=b;
00049 (*this)[2]=c;
00050 }
00051
00052
00053 Vector(const dataType* array, const size_t & size, const size_t & extraAllocationSize=0) : size(0), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
00054 setSize(size);
00055 for (size_t i=0; i<getSize(); i++)
00056 (*this)[i]=array[i];
00057 }
00058
00059 Vector(const Vector &v) : size(0), extraAllocationSize(0), sizeAllocated(0), data(NULL) {
00060 setSize(v.getSize());
00061 for (size_t i=0; i<getSize(); i++)
00062 (*this)[i]=v[i];
00063 }
00064
00065 Vector<float> toFloat() const {
00066 Vector<float> v(getSize());
00067 for (size_t i=0; i<getSize(); i++)
00068 v[i]=(*this)[i];
00069 return v;
00070 }
00071
00072 virtual ~Vector() {
00073 empty();
00074 }
00075 void empty() {
00076 if (data) delete [] data;
00077 data = NULL;
00078 sizeAllocated=0;
00079 size=0;
00080 }
00081
00082 void setSize(const size_t & n) {
00083 if (n==0) {
00084 if (data) delete [] data;
00085 data = NULL;
00086 sizeAllocated=0;
00087 }
00088 if (n>sizeAllocated) {
00089 dataType * newData = new dataType[n+extraAllocationSize];
00090 memcpy(newData,data,sizeof(dataType)/sizeof(char)*getSize());
00091 memset(newData+size,0,sizeof(dataType)/sizeof(char)*(n-getSize()));
00092 delete[] data;
00093 data = newData;
00094 sizeAllocated=n+extraAllocationSize;
00095 }
00096 size=n;
00097 }
00098
00099 const size_t & getSize() const {
00100 return size;
00101 }
00102
00103 void insert(const size_t index, const dataType value) {
00104
00105 #ifdef ASSERT
00106 assert(index<size+1,vectorSource,__LINE__);
00107 #endif
00108
00109
00110 setSize(getSize()+1);
00111
00112
00113
00114
00115 if (index != getSize()-1) {
00116 memmove(data+index+1,data+index,sizeof(dataType)/sizeof(char)*(getSize()-1-index));
00117
00118 }
00119 (*this)[index]=value;
00120
00121 }
00122
00123 void remove(const size_t & index) {
00124 #ifdef ASSERT
00125 assert(index<size,vectorSource,__LINE__);
00126 #endif
00127 memmove(data+index,data+index+1,getSize()-index-1);
00128 setSize(getSize()-1);
00129 }
00130
00131 void push_back(const dataType & value) {
00132
00133 insert(getSize(),value);
00134
00135 }
00136
00137 dataType & pop_front() {
00138 dataType tmp = (*this)[0];
00139 remove(0);
00140 return tmp;
00141 }
00142
00143 void push_back(const Vector & vector) {
00144 for (size_t i=0; i<vector.getSize(); i++)
00145 push_back(vector[i]);
00146 }
00147
00148 const dataType & operator[](const size_t & index) const {
00149 #ifdef ASSERT
00150 assert(index<getSize(),vectorSource,__LINE__);
00151 #endif
00152 return data[index];
00153 }
00154
00155 dataType & operator[](const size_t & index) {
00156 #ifdef ASSERT
00157 assert(index<getSize(),vectorSource,__LINE__);
00158 #endif
00159 return data[index];
00160 }
00161
00162 Vector & operator=(const Vector & v) {
00163 setSize(v.getSize());
00164 for (size_t i=0; i<getSize(); i++)
00165 (*this)[i]=v[i];
00166 return *this;
00167 }
00168
00169 const bool operator==(const Vector& v) const {
00170 #ifdef ASSERT
00171 assert(getSize()==v.getSize(),vectorSource,__LINE__);
00172 #endif
00173 for (size_t i=0; i<getSize(); i++) {
00174 if ((*this)[i]!=v[i]) return false;
00175 }
00176 return true;
00177 }
00178
00179 const bool operator!=(const Vector& v) const {
00180 return !((*this)==v);
00181 }
00182
00183 const Vector operator+(const Vector& v) const {
00184 #ifdef ASSERT
00185 assert(v.getSize() == getSize(),vectorSource,__LINE__);
00186 #endif
00187 Vector result(getSize());
00188 for (size_t i=0; i<getSize(); i++)
00189 result(i)=(*this)[i]+v[i];
00190 return result;
00191 }
00192
00193 const Vector operator+(const dataType& s) const {
00194 Vector result(getSize());
00195 for (size_t i=0; i<getSize(); i++)
00196 result[i]=(*this)[i]+s;
00197 return result;
00198 }
00199
00200 const Vector operator-(const Vector& v) const {
00201 #ifdef ASSERT
00202 assert(v.getSize() == getSize(),vectorSource,__LINE__);
00203 #endif
00204 Vector result(getSize());
00205 for (size_t i=0; i<getSize(); i++)
00206 result[i]=(*this)[i]-v[i];
00207 return result;
00208 }
00209
00210 const Vector operator-() const {
00211 Vector result(getSize());
00212 for (size_t i=0; i<getSize(); i++)
00213 result[i]=-(*this)[i];
00214 return result;
00215 }
00216
00217 Vector& operator+=(const Vector& v) {
00218 #ifdef ASSERT
00219 assert(v.getSize() == getSize(),vectorSource,__LINE__);
00220 #endif
00221 Vector result(getSize());
00222 for (size_t i=0; i<getSize(); i++)
00223 (*this)(i)+=v(i);
00224 return *this;
00225 }
00226
00227 Vector& operator-=( const Vector& v) {
00228 #ifdef ASSERT
00229 assert(v.getSize() == getSize(),vectorSource,__LINE__);
00230 #endif
00231 Vector result(getSize());
00232 for (size_t i=0; i<getSize(); i++)
00233 (*this)(i)-=v(i);
00234 return *this;
00235 }
00236
00237 const Vector operator*(const Vector & v) const {
00238 Vector result(getSize());
00239 for (size_t i=0; i<getSize(); i++)
00240 result(i)=(*this)(i)*v(i);
00241 return result;
00242 }
00243
00244
00245 const Vector operator*(const dataType & s) const {
00246 Vector result(getSize());
00247 for (size_t i=0; i<getSize(); i++)
00248 result(i)=(*this)(i)*s;
00249 return result;
00250 }
00251
00252 const Vector operator/(const dataType & s) const {
00253 Vector result(getSize());
00254 for (size_t i=0; i<getSize(); i++)
00255 result(i)=(*this)(i)/s;
00256 return result;
00257 }
00258
00259 const Vector operator/(const Vector & v) const {
00260 Vector result(getSize());
00261 for (size_t i=0; i<getSize(); i++)
00262 result(i)=(*this)(i)/v(i);
00263 return result;
00264 }
00265
00266
00267 Vector & operator/=(const dataType & s) {
00268 for (size_t i=0; i<getSize(); i++)
00269 (*this)(i)/=s;
00270 return *this;
00271 }
00272
00273 Vector & operator*=(const dataType & s) {
00274 for (size_t i=0; i<getSize(); i++)
00275 (*this)(i)*=s;
00276 return *this;
00277 }
00278
00279 const Vector cross(const Vector& v) const {
00280 Vector result(3), u=*this;
00281 #ifdef ASSERT
00282 assert(u.getSize()==3 && v.getSize()==3,vectorSource,__LINE__);
00283 #endif
00284 result(0) = u(1)*v(2)-u(2)*v(1);
00285 result(1) = -u(0)*v(2)+u(2)*v(0);
00286 result(2) = u(0)*v(1)-u(1)*v(0);
00287 return result;
00288 }
00289
00290 const dataType dot(const Vector& v) const {
00291 #ifdef ASSERT
00292 assert(getSize()==v.getSize(),vectorSource,__LINE__);
00293 #endif
00294 dataType result;
00295 for (size_t i=0; i<getSize(); i++) result += (*this)(i)*v(i);
00296 return result;
00297 }
00298
00299 const dataType norm() const {
00300 return sqrt(dot(*this));
00301 }
00302
00303 const Vector unit() const {
00304 return (*this)*(1/norm());
00305 }
00306
00307 const sumType sum(const size_t & start=0,const int & end=-1) const {
00308 size_t end2;
00309 if (end==-1) end2=getSize()-1;
00310 else end2=end;
00311 sumType sum = 0;
00312 for (size_t i=start; i<=end2; i++) sum += (*this)(i);
00313 return sum;
00314 }
00315 void sumFletcher(uint8_t & CK_A, uint8_t & CK_B, const size_t & start=0,const int & end=-1) const {
00316 size_t end2;
00317 if (end==-1) end2=getSize()-1;
00318 else end2=end;
00319
00320 for (size_t i = start; i<=end2; i++) {
00321 CK_A += (*this)(i);
00322 CK_B += CK_A;
00323 }
00324 }
00325
00326 const Vector range(const size_t & start, const size_t & stop) const {
00327 Vector v(stop-start+1);
00328 for (size_t i=start; i<=stop; i++) v(i-start) = (*this)(i);
00329 return v;
00330 }
00331
00332 const dataType* toArray() const {
00333 dataType array[getSize()];
00334 for (size_t i=0; i<getSize(); i++) array[i] = (*this)(i);
00335 return array;
00336 }
00337
00338 void print(Stream & serial=Serial, const char * msg="", size_t format=0) const {
00339 serial.print(msg);
00340 for (size_t i=0; i<getSize(); i++) {
00341 serial.print((*this)(i),format);
00342 serial.print(" ");
00343 }
00344 serial.println();
00345 }
00346
00347 static bool selfTest(Stream & serial=Serial) {
00348 serial.println("Vector self test.");
00349 Vector u(3),v(3),w(3);
00350 u(0) = 1;
00351 u(1) = 2;
00352 u(2) = 3;
00353 v(0) = -4;
00354 v(1) = -5;
00355 v(2) = -6;
00356 u.print(serial,"u: ");
00357 v.print(serial,"v: ");
00358 (u+v).print(serial,"u + v: ");
00359 (u-v).print(serial,"u - v: ");
00360 Serial.print("u dot v: ");
00361 Serial.println(u.dot(v));
00362 Serial.print("size of u: ");
00363 Serial.println(u.getSize());
00364 Serial.print("size of v: ");
00365 Serial.println(v.getSize());
00366 w=u.cross(v);
00367 w.print(serial,"u cross v: ");
00368 Serial.print("size of u cross v: ");
00369 Serial.println(w.getSize());
00370 return true;
00371 }
00372
00373 };
00374
00375 #endif
00376
00377