Robotsystem from Robot Group Leipzig: vector.h Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# vector.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: vector.h,v $
    00023  *   Revision 1.4.4.1  2005/12/06 10:13:26  martius
    00024  *   openscenegraph integration started
    00025  *
    00026  *   Revision 1.4  2005/11/09 13:31:51  martius
    00027  *   GPL'ised
    00028  *
    00029  ***************************************************************************/
    00030 #ifndef vector_h
    00031 #define vector_h
    00032
    00033
    00034 #include <vector>
    00035 #include <math.h>
    00036 #include "exceptions.h"
    00037
    00038
    00039 namespace lpzrobots {
    00040
    00041
    00042 template<typename T>
    00043 class Vector {
    00044  public:
    00045   //typedef std::vector<T> Data;
    00046
    00047  protected:
    00048   std::vector<T> data;
    00049
    00050  public:
    00051   Vector(unsigned dimension = 0);
    00052
    00053   void resize(unsigned new_dimension);
    00054
    00055   unsigned get_dimension() const;
    00056   T& operator() (unsigned i);
    00057   const T& operator() (unsigned i) const;
    00058
    00059   Vector<T> operator - (const Vector<T> &r_rhs) const;
    00060
    00061   T length() const;
    00062   T square_length() const;
    00063 };
    00064
    00065
    00066
    00067
    00068
    00069
    00070 template <typename T>
    00071 Vector<T>::Vector(unsigned dimension) :
    00072   data(dimension)
    00073 {
    00074 }
    00075
    00076 template <typename T>
    00077 void Vector<T>::resize(unsigned new_dimension)
    00078 {
    00079   data.resize(new_dimension);
    00080 }
    00081
    00082
    00083 template <typename T>
    00084 unsigned Vector<T>::get_dimension() const
    00085 {
    00086   return data.size();
    00087 }
    00088
    00089
    00090 template <typename T>
    00091 T& Vector<T>::operator() (unsigned i)
    00092 {
    00093   if(i >= data.size())
    00094     IndexOutOfBoundsException().raise();
    00095
    00096
    00097   return data[i];
    00098 }
    00099
    00100
    00101 template <typename T>
    00102 Vector<T> Vector<T>::operator - (const Vector<T> &r_rhs) const
    00103 {
    00104   if(get_dimension() != r_rhs.get_dimension())
    00105     IndexOutOfBoundsException().raise();
    00106
    00107   unsigned n = get_dimension();
    00108
    00109   Vector result(n);
    00110
    00111   for(unsigned i = 0; i < n; ++i)
    00112     result(i) = (*this)(i) - r_rhs(i);
    00113
    00114   return result;
    00115 }
    00116
    00117 /*
    00118 template <typename T>
    00119 Vector<T> Vector<T>::operator + (const Vector<T> &r_rhs) const
    00120 {
    00121   if(get_dimension() != r_rhs.get_dimension())
    00122     IndexOutOfBoundsException().raise();
    00123
    00124   unsigned n = get_dimension();
    00125
    00126   Vector result(n);
    00127
    00128   for(unsigned i = 0; i < n; ++i)
    00129     result(i) = (*this)(i) + r_rhs(i);
    00130
    00131   return result;
    00132 }
    00133 */
    00134
    00135
    00136 template <typename T>
    00137 T Vector<T>::square_length() const
    00138 {
    00139   unsigned n = get_dimension();
    00140
    00141   T result = static_cast<T>(0);
    00142   for(unsigned i = 0; i < n; ++i)
    00143     result += (*this)(i) * (*this)(i);
    00144
    00145   return result;
    00146 }
    00147
    00148
    00149 template <typename T>
    00150 T Vector<T>::length() const
    00151 {
    00152   return sqrt(square_length());
    00153 }
    00154
    00155
    00156 template <typename T>
    00157 const T& Vector<T>::operator() (unsigned i) const
    00158 {
    00159   if(i >= data.size())
    00160     IndexOutOfBoundsException().raise();
    00161
    00162
    00163   return data[i];
    00164 }
    00165
    00166
    00167
    00168
    00169
    00170
    00171
    00172 // note: Vector3 should be a partial specialization of Vector
    00173 template<class T> class Vector3 {
    00174  protected:
    00175   union {
    00176     struct {
    00177       T x;
    00178       T y;
    00179       T z;
    00180     };
    00181     T c[3]; // c = component
    00182   };
    00183
    00184
    00185  public:
    00186   Vector3(const T &r_x = static_cast<T>(0),
    00187           const T &r_y = static_cast<T>(0),
    00188           const T &r_z = static_cast<T>(0)) :
    00189     x(r_x),
    00190     y(r_y),
    00191     z(r_z)
    00192   {
    00193   }
    00194
    00195
    00196   Vector3(const T* p)
    00197   {
    00198     memcpy(c, p, sizeof(c));
    00199   }
    00200
    00201
    00202   template <class U> Vector3(const Vector3<U> &r_other) :
    00203     x(r_other.x),
    00204     y(r_other.y),
    00205     z(r_other.z)
    00206   {
    00207   }
    00208
    00209
    00210   template <class U> Vector3<T> operator - (const Vector3<U> &r_rhs) const
    00211   {
    00212     return Vector3<T>(x - r_rhs.x,
    00213                       y - r_rhs.y,
    00214                       z - r_rhs.z);
    00215   }
    00216
    00217
    00218   template <class U> Vector3<T> operator + (const Vector3<U> &r_rhs) const
    00219   {
    00220     return Vector3<T>(x + r_rhs.x,
    00221                       y + r_rhs.y,
    00222                       z + r_rhs.z);
    00223   }
    00224
    00225
    00226   template <class U> Vector3<T> operator / (const U &r_rhs) const
    00227   {
    00228     return Vector3<T>(x / r_rhs,
    00229                       y / r_rhs,
    00230                       z / r_rhs);
    00231   }
    00232
    00233
    00234
    00235   Vector3<T> get_unit_vector() const
    00236   {
    00237     double l = length();
    00238     if(0.0 == l)
    00239       return *this;
    00240
    00241     return Vector3<T>(x / l, y / l, z / l);
    00242   }
    00243
    00244
    00245
    00246   Vector3<T>& make_unit_length()
    00247   {
    00248     double l = length();
    00249     if(0.0 == l)
    00250       return *this;
    00251
    00252     x /= l;
    00253     y /= l;
    00254     z /= l;
    00255
    00256     return *this;
    00257   }
    00258
    00259
    00260   template <class U> Vector3<T>& operator = (const Vector3<U> &r_rhs)
    00261   {
    00262     x = r_rhs.x;
    00263     y = r_rhs.y;
    00264     z = r_rhs.z;
    00265
    00266     return *this;
    00267   }
    00268
    00269
    00270   /*
    00271   const Vector3& operator = (const Vector3 &r_rhs) {
    00272     x = r_rhs.x;
    00273     y = r_rhs.y;
    00274     z = r_rhs.z;
    00275
    00276     return *this;
    00277   }
    00278   */
    00279
    00280   double square_length() const
    00281   {
    00282     return static_cast<double>(x*x + y*y + z*z);
    00283   }
    00284
    00285
    00286   double length() const
    00287   {
    00288     return sqrt(square_length());
    00289   }
    00290
    00291   operator T* () {
    00292     return &c[0];
    00293   }
    00294 };
    00295
    00296
    00297 typedef Vector3<double> DVector3;
    00298 typedef Vector3<float>  FVector3;
    00299
    00300
    00301 }
    00302
    00303
    00304
    00305 #endif