Wiselib
wiselib.testing/algorithms/localization/distance_based/math/vec.h
Go to the documentation of this file.
00001 /***************************************************************************
00002  ** This file is part of the generic algorithm library Wiselib.           **
00003  ** Copyright (C) 2008,2009 by the Wisebed (www.wisebed.eu) project.      **
00004  **                                                                       **
00005  ** The Wiselib is free software: you can redistribute it and/or modify   **
00006  ** it under the terms of the GNU Lesser General Public License as        **
00007  ** published by the Free Software Foundation, either version 3 of the    **
00008  ** License, or (at your option) any later version.                       **
00009  **                                                                       **
00010  ** The Wiselib is distributed in the hope that it will be useful,        **
00011  ** but WITHOUT ANY WARRANTY; without even the implied warranty of        **
00012  ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         **
00013  ** GNU Lesser General Public License for more details.                   **
00014  **                                                                       **
00015  ** You should have received a copy of the GNU Lesser General Public      **
00016  ** License along with the Wiselib.                                       **
00017  ** If not, see <http://www.gnu.org/licenses/>.                           **
00018  ***************************************************************************/
00019 #ifndef __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_MATH_VEC_H
00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_MATH_VEC_H
00021 
00022 #include <math.h>
00023 
00024 namespace wiselib
00025 {
00026 
00027     template<typename Arithmatic_P = double>
00028    class Vec
00029    {
00030 //       static const Arithmatic epsilon;
00031 
00032    public:
00033       typedef Arithmatic_P Arithmatic;
00034       typedef Vec<Arithmatic> self_type;
00035       // --------------------------------------------------------------------
00036       inline Vec( const self_type& p )
00037          : x_(p.x()), y_(p.y()), z_(p.z())
00038       {}
00039       inline Vec( Arithmatic x=0.0, Arithmatic y=0.0, Arithmatic z=0.0 )
00040          : x_(x), y_(y), z_(z)
00041       {}
00042       // --------------------------------------------------------------------
00043       inline Arithmatic x( void ) const
00044       { return x_; }
00045 
00046       inline Arithmatic y( void ) const
00047       { return y_; }
00048 
00049       inline Arithmatic z( void ) const
00050       { return z_; }
00051       // --------------------------------------------------------------------
00052       inline self_type operator - ( const self_type& p ) const
00053       { return Vec( x()-p.x(), y()-p.y(), z()-p.z() ); }
00054       inline self_type operator + ( const self_type& p ) const
00055       { return self_type( x()+p.x(), y()+p.y(), z()+p.z() ); }
00056 
00057       inline self_type& operator -= ( const self_type& p )
00058       { return *this = (*this-p); }
00059       inline self_type& operator += ( const self_type& p )
00060       { return *this = (*this+p); }
00061 
00062       inline self_type operator * ( Arithmatic f ) const
00063       { return self_type( f*x(), f*y(), f*z() ); }
00064       inline self_type operator / ( Arithmatic f ) const
00065       { return self_type( x()/f, y()/f, z()/f ); }
00066 
00067       inline self_type& operator *= ( Arithmatic f )
00068       { return *this = (*this*f); }
00069       inline self_type& operator /= ( Arithmatic f )
00070       { return *this = (*this/f); }
00071       // --------------------------------------------------------------------
00072       inline bool operator == ( const self_type& p ) const
00073       { return (*this - p).euclidean_norm()<=.00000000001; }
00074 
00075       inline bool operator != ( const self_type& p ) const
00076       { return !(*this == p); }
00077 
00078       inline self_type& operator= ( const self_type& p )
00079       { x_=p.x(); y_=p.y(); z_=p.z(); return *this; }
00080 
00081       inline Arithmatic operator * ( const self_type& p ) const
00082       { return x()*p.x() + y()*p.y() + z()*p.z() ; }
00083       // --------------------------------------------------------------------
00084       Arithmatic euclidean_norm( void )
00085          const
00086       { return sqrt( (x_*x_) + (y_*y_) + (z_*z_) ); }
00087 
00088  /*     static Arithmatic euclidean_distance( const self_type& p1,
00089                                    const self_type& p2 );
00090       static self_type cross_product( const self_type& p1, const self_type& p2 );
00091 */
00092 
00093      static inline Arithmatic
00094 
00095          euclidean_distance( const self_type& p1,
00096                              const self_type& p2 )
00097          { return (p1-p2).euclidean_norm(); }
00098          // -----------------------------------------------------------------------
00099 
00100          static inline self_type
00101 
00102          cross_product( const self_type& p1, const self_type& p2 )
00103          { return self_type( p1.y()*p2.z() - p1.z()*p2.y(), p1.z()*p2.x() - p1.x()*p2.z(), p1.x()*p2.y() - p1.y()*p2.x() ); }
00104 
00105    private:
00106       Arithmatic x_, y_, z_;
00107    };
00108    // -----------------------------------------------------------------------
00109 
00110 /*   template<typename Arithmatic_P>
00111 
00112    typename Vec<Arithmatic_P>::Arithmatic
00113          Vec<Arithmatic_P>::
00114    euclidean_distance( const self_type& p1,
00115                        const self_type& p2 )
00116    { return (p1-p2).euclidean_norm(); }
00117    // -----------------------------------------------------------------------
00118     template<typename Arithmatic_P>
00119    typename Vec<Arithmatic_P>::self_type
00120    Vec<Arithmatic_P>::
00121    cross_product( const self_type& p1, const self_type& p2 )
00122    { return Vec<Arithmatic_P>::self_type( p1.y()*p2.z() - p1.z()*p2.y(), p1.z()*p2.x() - p1.x()*p2.z(), p1.x()*p2.y() - p1.y()*p2.x() ); }*/
00123    // -----------------------------------------------------------------------
00124 //    const Arithmatic Vec::epsilon = .00000000001;
00125 }
00126 
00127 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines