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

vector2.h

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 
00003 // Copyright 2010 Michael Smith, all rights reserved.
00004 
00005 //      This library is free software; you can redistribute it and / or
00006 //      modify it under the terms of the GNU Lesser General Public
00007 //      License as published by the Free Software Foundation; either
00008 //      version 2.1 of the License, or (at your option) any later version.
00009 
00010 // Derived closely from:
00011 /****************************************
00012  * 2D Vector Classes
00013  * By Bill Perone (billperone@yahoo.com)
00014  * Original: 9-16-2002
00015  * Revised: 19-11-2003
00016  *          18-12-2003
00017  *          06-06-2004
00018  *
00019  * © 2003, This code is provided "as is" and you can use it freely as long as
00020  * credit is given to Bill Perone in the application it is used in
00021  ****************************************/
00022 
00023 #ifndef VECTOR2_H
00024 #define VECTOR2_H
00025 
00026 #include <math.h>
00027 
00028 template <typename T>
00029 struct Vector2
00030 {
00031         T x, y;
00032 
00033         // trivial ctor
00034         Vector2<T>() { x = y = 0; }
00035 
00036         // setting ctor
00037         Vector2<T>(const T x0, const T y0): x(x0), y(y0) {}
00038 
00039         // function call operator
00040         void operator ()(const T x0, const T y0)
00041         {       x= x0; y= y0;   }
00042 
00043         // test for equality
00044         bool operator==(const Vector2<T> &v)
00045         {       return (x==v.x && y==v.y);      }
00046 
00047         // test for inequality
00048         bool operator!=(const Vector2<T> &v)
00049         {       return (x!=v.x || y!=v.y);      }
00050 
00051         // negation
00052         Vector2<T> operator -(void) const
00053         {       return Vector2<T>(-x, -y);      }
00054 
00055         // addition
00056         Vector2<T> operator +(const Vector2<T> &v) const
00057         {       return Vector2<T>(x+v.x, y+v.y);        }
00058 
00059         // subtraction
00060         Vector2<T> operator -(const Vector2<T> &v) const
00061         {   return Vector2<T>(x-v.x, y-v.y);    }
00062 
00063         // uniform scaling
00064         Vector2<T> operator *(const T num) const
00065         {
00066                 Vector2<T> temp(*this);
00067                 return temp*=num;
00068         }
00069 
00070         // uniform scaling
00071         Vector2<T> operator /(const T num) const
00072         {
00073                 Vector2<T> temp(*this);
00074                 return temp/=num;
00075         }
00076 
00077         // addition
00078         Vector2<T> &operator +=(const Vector2<T> &v)
00079         {
00080                 x+=v.x; y+=v.y;
00081                 return *this;
00082         }
00083 
00084         // subtraction
00085         Vector2<T> &operator -=(const Vector2<T> &v)
00086         {
00087                 x-=v.x; y-=v.y;
00088                 return *this;
00089         }
00090 
00091         // uniform scaling
00092         Vector2<T> &operator *=(const T num)
00093         {
00094                 x*=num; y*=num;
00095                 return *this;
00096         }
00097 
00098         // uniform scaling
00099         Vector2<T> &operator /=(const T num)
00100         {
00101                 x/=num; y/=num;
00102                 return *this;
00103         }
00104 
00105         // dot product
00106         T operator *(const Vector2<T> &v) const
00107         {       return x*v.x + y*v.y;   }
00108 
00109         // gets the length of this vector squared
00110         T length_squared() const
00111         {       return (T)(*this * *this);   }
00112 
00113         // gets the length of this vector
00114         T length() const
00115         {       return (T)sqrt(*this * *this);   }
00116 
00117         // normalizes this vector
00118         void normalize()
00119         {       *this/=length();        }
00120 
00121         // returns the normalized vector
00122         Vector2<T> normalized() const
00123         {   return  *this/length();  }
00124 
00125         // reflects this vector about n
00126         void reflect(const Vector2<T> &n)
00127         {
00128                 Vector2<T> orig(*this);
00129                 project(n);
00130                 *this= *this*2 - orig;
00131         }
00132 
00133         // projects this vector onto v
00134         void project(const Vector2<T> &v)
00135         {       *this= v * (*this * v)/(v*v);   }
00136 
00137         // returns this vector projected onto v
00138         Vector2<T> projected(const Vector2<T> &v)
00139         {   return v * (*this * v)/(v*v);       }
00140 
00141         // computes the angle between 2 arbitrary vectors
00142         T angle(const Vector2<T> &v1, const Vector2<T> &v2)
00143         {   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }
00144 
00145         // computes the angle between 2 normalized arbitrary vectors
00146         T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
00147         {   return (T)acosf(v1*v2);  }
00148 
00149 };
00150 
00151 typedef Vector2<int>                    Vector2i;
00152 typedef Vector2<unsigned int>   Vector2ui;
00153 typedef Vector2<long>                   Vector2l;
00154 typedef Vector2<unsigned long>  Vector2ul;
00155 typedef Vector2<float>                  Vector2f;
00156 
00157 #endif // VECTOR2_H

Generated for ArduPilot Libraries by doxygen