Vector2.h
Go to the documentation of this file.
00001 /* This file is adapted from octomap/math/vector3.h see original copyright information below
00002  */
00003 
00004 /*
00005  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  *
00008  * Redistribution and use in source and binary forms, with or without
00009  * modification, are permitted provided that the following conditions are met:
00010  *
00011  *     * Redistributions of source code must retain the above copyright
00012  *       notice, this list of conditions and the following disclaimer.
00013  *     * Redistributions in binary form must reproduce the above copyright
00014  *       notice, this list of conditions and the following disclaimer in the
00015  *       documentation and/or other materials provided with the distribution.
00016  *     * Neither the name of the University of Freiburg nor the names of its
00017  *       contributors may be used to endorse or promote products derived from
00018  *       this software without specific prior written permission.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00021  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00022  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00023  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00024  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00025  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00026  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00027  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00028  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00029  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030  * POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #ifndef COLLVOID_VECTOR2_H
00034 #define COLLVOID_VECTOR2_H
00035 
00036 #include <iostream>
00037 #include <cmath>
00038 
00039 namespace collvoid {
00040 
00041   class Vector2 
00042   {
00043   public:
00044 
00045     Vector2 () { data[0] = data[1] = 0; }
00046 
00047 
00048     Vector2 (const Vector2& other) {
00049       data[0] = other(0);
00050       data[1] = other(1);
00051     }
00052 
00053     Vector2 (double x, double y) {
00054       data[0] = x;
00055       data[1] = y;
00056     }
00057 
00058     inline Vector2& operator= (const Vector2& other)  {
00059       data[0] = other(0);
00060       data[1] = other(1);
00061       return *this;
00062     }
00063 
00064     inline const double& operator() (unsigned int i) const
00065     {
00066       return data[i];
00067     }
00068     
00069     inline double& operator() (unsigned int i)
00070     {
00071       return data[i];
00072     }
00073 
00074     inline double& x() 
00075     {
00076       return operator()(0);
00077     }
00078 
00079     inline double& y() 
00080     {
00081       return operator()(1);
00082     }
00083 
00084     inline const double& x() const 
00085     {
00086       return operator()(0);
00087     }
00088 
00089     inline const double& y() const 
00090     {
00091       return operator()(1);
00092     }
00093 
00094     inline Vector2 operator- () const 
00095     {
00096       Vector2 result;
00097       result(0) = -data[0];
00098       result(1) = -data[1];
00099       return result;
00100     }
00101 
00102     inline Vector2 operator+ (const Vector2 &other) const 
00103     {
00104       Vector2 result(*this);
00105       result(0) += other(0);
00106       result(1) += other(1);
00107       return result;
00108     }
00109 
00110     inline double operator* (const Vector2& other) const 
00111     {
00112       return x()*other.x() + y()*other.y();
00113     }
00114     
00115     inline Vector2 operator*  (double x) const {
00116       Vector2 result(*this);
00117       result(0) *= x;
00118       result(1) *= x;
00119       return result;
00120     }
00121 
00122     inline Vector2 operator/  (double x) const {
00123       Vector2 result(*this);
00124       result(0) /= x;
00125       result(1) /= x;
00126       return result;
00127     }
00128 
00129     inline Vector2 operator- (const Vector2 &other) const 
00130     {
00131       Vector2 result(*this);
00132       result(0) -= other(0);
00133       result(1) -= other(1);
00134       return result;
00135     }
00136 
00137     inline void operator+= (const Vector2 &other)
00138     {
00139       data[0] += other(0);
00140       data[1] += other(1);
00141     }
00142 
00143     inline void operator-= (const Vector2& other) {
00144       data[0] -= other(0);
00145       data[1] -= other(1);
00146     }
00147 
00148     inline void operator/= (double x) {
00149       data[0] /= x;
00150       data[1] /= x;
00151     }
00152 
00153     inline void operator*= (double x) {    
00154       data[0] *= x;
00155       data[1] *= x;
00156     }
00157 
00158     inline bool operator== (const Vector2 &other) const {
00159       for (unsigned int i=0; i<2; i++) {
00160         if (operator()(i) != other(i)) 
00161           return false;
00162       }
00163       return true;
00164     }
00165 
00166     inline bool operator!=(const Vector2& vector) const
00167     {
00168       return x() != vector.x() || y() != vector.y();
00169     }
00170 
00171 
00172 
00173     inline double dist (const Vector2& other) const {
00174       double dist_x = x() - other.x();
00175       double dist_y = y() - other.y();
00176       return sqrt(dist_x*dist_x + dist_y*dist_y);
00177     }
00178 
00179 
00180     inline std::ostream& operator<<(std::ostream& os) {
00181       os << "(" << x() << "," << y() << ")";
00182       
00183       return os;
00184     }
00185 
00186   protected:
00187     double data[2];
00188 
00189   };
00190   
00191   inline Vector2 operator*(float s, const Vector2& vector)  {
00192     return Vector2(s * vector.x(), s * vector.y());
00193   }
00194 
00195   
00196   
00197   inline double abs(const Vector2& vector) {
00198     return std::sqrt(vector * vector);
00199   }
00200 
00201 
00202   inline double absSqr(const Vector2& vector) {
00203     return vector * vector;
00204   }
00205 
00206   inline double atan(const Vector2& vector){
00207     return std::atan2(vector.y(), vector.x());
00208   }
00209   
00210   inline double det(const Vector2& vector1, const Vector2& vector2) {
00211     return vector1.x() * vector2.y() - vector1.y() * vector2.x();
00212   }
00213 
00214   inline Vector2 normalize(const Vector2& vector){
00215     return vector / abs(vector);
00216   }
00217 
00218   inline Vector2 normal(const Vector2& vector){
00219     return normalize(Vector2(vector.y(), -(vector.x())));
00220   }
00221   
00222   inline double angleBetween(const Vector2& one, const Vector2& two) {  
00223     double dot_prod = one*two; 
00224     double len1 = abs(one); 
00225     double len2 = abs(two); 
00226     return acos(dot_prod / (len1*len2)); 
00227   } 
00228 
00229  inline Vector2 rotateVectorByAngle(double x, double y, double ang){
00230     double cos_a, sin_a;
00231     cos_a = cos(ang);
00232     sin_a = sin(ang);
00233     return Vector2(cos_a * x - sin_a * y, cos_a * y + sin_a * x);
00234 
00235   }
00236  inline Vector2 rotateVectorByAngle(const Vector2& vec, double ang) {
00237    return rotateVectorByAngle(vec.x(), vec.y(), ang);
00238  }
00239 
00240 }
00241 
00242 
00243 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


collvoid_local_planner
Author(s): Daniel Claes
autogenerated on Sun Aug 25 2013 10:10:23