point.h
Go to the documentation of this file.
00001 #ifndef _POINT_H_
00002 #define _POINT_H_
00003 #include <assert.h>
00004 #include <math.h>
00005 #include <iostream>
00006 #include "gvalues.h"
00007 
00008 #define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME
00009 
00010 namespace GMapping {
00011 
00012 template <class T>
00013 struct point{
00014         inline point():x(0),y(0) {}
00015         inline point(T _x, T _y):x(_x),y(_y){}
00016         T x, y;
00017 };
00018 
00019 template <class T>
00020 inline point<T> operator+(const point<T>& p1, const point<T>& p2){
00021         return point<T>(p1.x+p2.x, p1.y+p2.y);
00022 }
00023 
00024 template <class T>
00025 inline point<T> operator - (const point<T> & p1, const point<T> & p2){
00026         return point<T>(p1.x-p2.x, p1.y-p2.y);
00027 }
00028 
00029 template <class T>
00030 inline point<T> operator * (const point<T>& p, const T& v){
00031         return point<T>(p.x*v, p.y*v);
00032 }
00033 
00034 template <class T>
00035 inline point<T> operator * (const T& v, const point<T>& p){
00036         return point<T>(p.x*v, p.y*v);
00037 }
00038 
00039 template <class T>
00040 inline T operator * (const point<T>& p1, const point<T>& p2){
00041         return p1.x*p2.x+p1.y*p2.y;
00042 }
00043 
00044 
00045 template <class T, class A>
00046 struct orientedpoint: public point<T>{
00047   inline orientedpoint() : point<T>(0,0), theta(0) {};
00048         inline orientedpoint(const point<T>& p);
00049         inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
00050         inline void normalize();
00051         inline orientedpoint<T,A> rotate(A alpha){
00052                 T s=sin(alpha), c=cos(alpha);
00053                 A a=alpha+theta;
00054                 a=atan2(sin(a),cos(a));
00055                 return orientedpoint(
00056                         c*this->x-s*this->y,
00057                         s*this->x+c*this->y, 
00058                         a);
00059         }
00060         A theta;
00061 };
00062 
00063 
00064 template <class T, class A>
00065 void orientedpoint<T,A>::normalize() {
00066   if (theta >= -M_PI && theta < M_PI)
00067     return;
00068   
00069   int multiplier = (int)(theta / (2*M_PI));
00070   theta = theta - multiplier*2*M_PI;
00071   if (theta >= M_PI)
00072     theta -= 2*M_PI;
00073   if (theta < -M_PI)
00074     theta += 2*M_PI;
00075 }
00076 
00077 
00078 template <class T, class A>
00079 orientedpoint<T,A>::orientedpoint(const point<T>& p){
00080         this->x=p.x;
00081         this->y=p.y;
00082         this->theta=0.;
00083 }
00084 
00085 
00086 template <class T, class A>
00087 orientedpoint<T,A> operator+(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
00088         return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
00089 }
00090 
00091 template <class T, class A>
00092 orientedpoint<T,A> operator - (const orientedpoint<T,A> & p1, const orientedpoint<T,A> & p2){
00093         return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
00094 }
00095 
00096 template <class T, class A>
00097 orientedpoint<T,A> operator * (const orientedpoint<T,A>& p, const T& v){
00098         return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
00099 }
00100 
00101 template <class T, class A>
00102 orientedpoint<T,A> operator * (const T& v, const orientedpoint<T,A>& p){
00103         return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
00104 }
00105 
00106 template <class T, class A>
00107 orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
00108         orientedpoint<T,A> delta=p1-p2;
00109         delta.theta=atan2(sin(delta.theta), cos(delta.theta));
00110         double s=sin(p2.theta), c=cos(p2.theta);
00111         return orientedpoint<T,A>(c*delta.x+s*delta.y, 
00112                                  -s*delta.x+c*delta.y, delta.theta);
00113 }
00114 
00115 template <class T, class A>
00116 orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
00117         double s=sin(p1.theta), c=cos(p1.theta);
00118         return orientedpoint<T,A>(c*p2.x-s*p2.y,
00119                                   s*p2.x+c*p2.y, p2.theta) + p1;
00120 }
00121 
00122 template <class T, class A>
00123 point<T> absoluteSum(const orientedpoint<T,A>& p1,const point<T>& p2){
00124         double s=sin(p1.theta), c=cos(p1.theta);
00125         return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
00126 }
00127 
00128 template <class T>
00129 struct pointcomparator{
00130         bool operator ()(const point<T>& a, const point<T>& b) const {
00131                 return a.x<b.x || (a.x==b.x && a.y<b.y);
00132         }       
00133 };
00134 
00135 template <class T>
00136 struct pointradialcomparator{
00137         point<T> origin;
00138         bool operator ()(const point<T>& a, const point<T>& b) const {
00139                 point<T> delta1=a-origin;
00140                 point<T> delta2=b-origin;
00141                 return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
00142         }       
00143 };
00144 
00145 template <class T>
00146 inline point<T> max(const point<T>& p1, const point<T>& p2){
00147         point<T> p=p1;
00148         p.x=p.x>p2.x?p.x:p2.x;
00149         p.y=p.y>p2.y?p.y:p2.y;
00150         return p;
00151 }
00152 
00153 template <class T>
00154 inline point<T> min(const point<T>& p1, const point<T>& p2){
00155         point<T> p=p1;
00156         p.x=p.x<p2.x?p.x:p2.x;
00157         p.y=p.y<p2.y?p.y:p2.y;
00158         return p;
00159 }
00160 
00161 template <class T, class F>
00162 inline point<T> interpolate(const point<T>& p1,  const F& t1, const point<T>& p2, const F& t2, const F& t3){
00163         F gain=(t3-t1)/(t2-t1);
00164         point<T> p=p1+(p2-p1)*gain;
00165         return p;
00166 }
00167 
00168 template <class T, class A, class F>
00169 inline orientedpoint<T,A> 
00170 interpolate(const orientedpoint<T,A>& p1,  const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
00171         F gain=(t3-t1)/(t2-t1);
00172         orientedpoint<T,A> p;
00173         p.x=p1.x+(p2.x-p1.x)*gain;
00174         p.y=p1.y+(p2.y-p1.y)*gain;
00175         double  s=sin(p1.theta)+sin(p2.theta)*gain,
00176                 c=cos(p1.theta)+cos(p2.theta)*gain;
00177         p.theta=atan2(s,c);
00178         return p;
00179 }
00180 
00181 
00182 template <class T>
00183 inline double euclidianDist(const point<T>& p1, const point<T>& p2){
00184   return hypot(p1.x-p2.x, p1.y-p2.y);
00185 }
00186 template <class T, class A>
00187 inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
00188   return hypot(p1.x-p2.x, p1.y-p2.y);
00189 }
00190 template <class T, class A>
00191 inline double euclidianDist(const orientedpoint<T,A>& p1, const point<T>& p2){
00192   return hypot(p1.x-p2.x, p1.y-p2.y);
00193 }
00194 template <class T, class A>
00195 inline double euclidianDist(const point<T>& p1, const orientedpoint<T,A>& p2 ){
00196   return hypot(p1.x-p2.x, p1.y-p2.y);
00197 }
00198 
00199 
00200 
00201 typedef point<int> IntPoint;
00202 typedef point<double> Point;
00203 typedef orientedpoint<double, double> OrientedPoint;
00204 
00205 }; //end namespace
00206 
00207 #endif


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13