• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

AP_Vector.h

Go to the documentation of this file.
00001 /*
00002  * Vector.h
00003  * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
00004  *
00005  * This file is free software: you can redistribute it and/or modify it
00006  * under the terms of the GNU General Public License as published by the
00007  * Free Software Foundation, either version 3 of the License, or
00008  * (at your option) any later version.
00009  *
00010  * This file is distributed in the hope that it will be useful, but
00011  * WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00013  * See the GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License along
00016  * with this program.  If not, see <http://www.gnu.org/licenses/>.
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 // vector
00031 template <class dataType,class sumType=dataType>
00032 class Vector
00033 {
00034 private:
00035     size_t size;
00036     size_t extraAllocationSize; // extra space to add after each allocation
00037     size_t sizeAllocated; // total allocated size
00038     dataType* data;
00039 public:
00040     // default constructor
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     // 3 vector constructor
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     // construct from array
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     // copy constructor
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     // convert to float vector
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     // destructor
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     // set the size
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     // return size
00099     const size_t & getSize() const {
00100         return size;
00101     }
00102     // insert
00103     void insert(const size_t index, const dataType value) {
00104         //Serial.println("insert called");
00105 #ifdef ASSERT
00106         assert(index<size+1,vectorSource,__LINE__);
00107 #endif
00108         //Serial.print("Old Size: ");
00109         //Serial.println(getSize());
00110         setSize(getSize()+1);
00111         //Serial.print("New Size: ");
00112         //Serial.println(getSize());
00113         //Serial.print("Size of dataType");
00114         //Serial.println(sizeof(dataType));
00115         if (index != getSize()-1) {
00116             memmove(data+index+1,data+index,sizeof(dataType)/sizeof(char)*(getSize()-1-index));
00117             //Serial.println("memmove called and completed");
00118         }
00119         (*this)[index]=value;
00120         //Serial.println("insert done");
00121     }
00122     // remove
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     // push_back
00131     void push_back(const dataType & value) {
00132         //Serial.println("push_back called");
00133         insert(getSize(),value);
00134         //Serial.println("push_back done");
00135     }
00136     // pop_front
00137     dataType & pop_front() {
00138         dataType tmp = (*this)[0];
00139         remove(0);
00140         return tmp;
00141     }
00142     // push_back a vector
00143     void push_back(const Vector & vector) {
00144         for (size_t i=0; i<vector.getSize(); i++)
00145             push_back(vector[i]);
00146     }
00147     // const array access operator
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     // array access operator
00155     dataType & operator[](const size_t & index) {
00156 #ifdef ASSERT
00157         assert(index<getSize(),vectorSource,__LINE__);
00158 #endif
00159         return data[index];
00160     }
00161     // assignment operator
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     // equal
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     // not equal
00179     const bool operator!=(const Vector& v) const {
00180         return !((*this)==v);
00181     }
00182     // addition
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     // addition
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     // subtraction
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     // negation
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     // elementwise  mult.
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     // mult. by a scalar
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     // div. by a scalar
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     // elementwise div.
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     // div. by a scalar
00267     Vector & operator/=(const dataType & s) {
00268         for (size_t i=0; i<getSize(); i++)
00269             (*this)(i)/=s;
00270         return *this;
00271     }
00272     // mult. by a scalar
00273     Vector & operator*=(const dataType & s) {
00274         for (size_t i=0; i<getSize(); i++)
00275             (*this)(i)*=s;
00276         return *this;
00277     }
00278     // cross/vector product
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     // dot/scalar product
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     // norm
00299     const dataType norm() const {
00300         return sqrt(dot(*this));
00301     }
00302     // unit vector
00303     const Vector unit() const {
00304         return (*this)*(1/norm());
00305     }
00306     // sum
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     // range
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     // to Array
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     // printing
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     // self test
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 // vim:ts=4:sw=4:expandtab

Generated for ArduPilot Libraries by doxygen