Wiselib
|
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