point.h
Go to the documentation of this file.
1 #ifndef _POINT_H_
2 #define _POINT_H_
3 #include <assert.h>
4 #include <math.h>
5 #include <iostream>
7 
8 #define DEBUG_STREAM cerr << __func__ << ":" //FIXME
9 
10 namespace GMapping {
11 
12 template <class T>
13 struct point{
14  inline point():x(0),y(0) {}
15  inline point(T _x, T _y):x(_x),y(_y){}
16  T x, y;
17 };
18 
19 template <class T>
20 inline point<T> operator+(const point<T>& p1, const point<T>& p2){
21  return point<T>(p1.x+p2.x, p1.y+p2.y);
22 }
23 
24 template <class T>
25 inline point<T> operator - (const point<T> & p1, const point<T> & p2){
26  return point<T>(p1.x-p2.x, p1.y-p2.y);
27 }
28 
29 template <class T>
30 inline point<T> operator * (const point<T>& p, const T& v){
31  return point<T>(p.x*v, p.y*v);
32 }
33 
34 template <class T>
35 inline point<T> operator * (const T& v, const point<T>& p){
36  return point<T>(p.x*v, p.y*v);
37 }
38 
39 template <class T>
40 inline T operator * (const point<T>& p1, const point<T>& p2){
41  return p1.x*p2.x+p1.y*p2.y;
42 }
43 
44 
45 template <class T, class A>
46 struct orientedpoint: public point<T>{
47  inline orientedpoint() : point<T>(0,0), theta(0) {};
48  inline orientedpoint(const point<T>& p);
49  inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
50  inline void normalize();
51  inline orientedpoint<T,A> rotate(A alpha){
52  T s=sin(alpha), c=cos(alpha);
53  A a=alpha+theta;
54  a=atan2(sin(a),cos(a));
55  return orientedpoint(
56  c*this->x-s*this->y,
57  s*this->x+c*this->y,
58  a);
59  }
60  A theta;
61 };
62 
63 
64 template <class T, class A>
66  if (theta >= -M_PI && theta < M_PI)
67  return;
68 
69  int multiplier = (int)(theta / (2*M_PI));
70  theta = theta - multiplier*2*M_PI;
71  if (theta >= M_PI)
72  theta -= 2*M_PI;
73  if (theta < -M_PI)
74  theta += 2*M_PI;
75 }
76 
77 
78 template <class T, class A>
80  this->x=p.x;
81  this->y=p.y;
82  this->theta=0.;
83 }
84 
85 
86 template <class T, class A>
88  return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
89 }
90 
91 template <class T, class A>
93  return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
94 }
95 
96 template <class T, class A>
98  return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
99 }
100 
101 template <class T, class A>
103  return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
104 }
105 
106 template <class T, class A>
109  delta.theta=atan2(sin(delta.theta), cos(delta.theta));
110  double s=sin(p2.theta), c=cos(p2.theta);
111  return orientedpoint<T,A>(c*delta.x+s*delta.y,
112  -s*delta.x+c*delta.y, delta.theta);
113 }
114 
115 template <class T, class A>
117  double s=sin(p1.theta), c=cos(p1.theta);
118  return orientedpoint<T,A>(c*p2.x-s*p2.y,
119  s*p2.x+c*p2.y, p2.theta) + p1;
120 }
121 
122 template <class T, class A>
124  double s=sin(p1.theta), c=cos(p1.theta);
125  return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
126 }
127 
128 template <class T>
130  bool operator ()(const point<T>& a, const point<T>& b) const {
131  return a.x<b.x || (a.x==b.x && a.y<b.y);
132  }
133 };
134 
135 template <class T>
138  bool operator ()(const point<T>& a, const point<T>& b) const {
139  point<T> delta1=a-origin;
140  point<T> delta2=b-origin;
141  return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
142  }
143 };
144 
145 template <class T>
146 inline point<T> max(const point<T>& p1, const point<T>& p2){
147  point<T> p=p1;
148  p.x=p.x>p2.x?p.x:p2.x;
149  p.y=p.y>p2.y?p.y:p2.y;
150  return p;
151 }
152 
153 template <class T>
154 inline point<T> min(const point<T>& p1, const point<T>& p2){
155  point<T> p=p1;
156  p.x=p.x<p2.x?p.x:p2.x;
157  p.y=p.y<p2.y?p.y:p2.y;
158  return p;
159 }
160 
161 template <class T, class F>
162 inline point<T> interpolate(const point<T>& p1, const F& t1, const point<T>& p2, const F& t2, const F& t3){
163  F gain=(t3-t1)/(t2-t1);
164  point<T> p=p1+(p2-p1)*gain;
165  return p;
166 }
167 
168 template <class T, class A, class F>
169 inline orientedpoint<T,A>
170 interpolate(const orientedpoint<T,A>& p1, const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
171  F gain=(t3-t1)/(t2-t1);
173  p.x=p1.x+(p2.x-p1.x)*gain;
174  p.y=p1.y+(p2.y-p1.y)*gain;
175  double s=sin(p1.theta)+sin(p2.theta)*gain,
176  c=cos(p1.theta)+cos(p2.theta)*gain;
177  p.theta=atan2(s,c);
178  return p;
179 }
180 
181 
182 template <class T>
183 inline double euclidianDist(const point<T>& p1, const point<T>& p2){
184  return hypot(p1.x-p2.x, p1.y-p2.y);
185 }
186 template <class T, class A>
187 inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
188  return hypot(p1.x-p2.x, p1.y-p2.y);
189 }
190 template <class T, class A>
191 inline double euclidianDist(const orientedpoint<T,A>& p1, const point<T>& p2){
192  return hypot(p1.x-p2.x, p1.y-p2.y);
193 }
194 template <class T, class A>
195 inline double euclidianDist(const point<T>& p1, const orientedpoint<T,A>& p2 ){
196  return hypot(p1.x-p2.x, p1.y-p2.y);
197 }
198 
199 
200 
204 
205 }; //end namespace
206 
207 #endif
const char *const *argv double delta
Definition: gfs2stream.cpp:19
void normalize(const Iterator &begin, const Iterator &end)
point< T > min(const point< T > &p1, const point< T > &p2)
Definition: point.h:154
point< double > Point
Definition: point.h:202
orientedpoint< T, A > rotate(A alpha)
Definition: point.h:51
point< T > operator-(const point< T > &p1, const point< T > &p2)
Definition: point.h:25
point< int > IntPoint
Definition: point.h:201
double euclidianDist(const point< T > &p1, const point< T > &p2)
Definition: point.h:183
orientedpoint< T, A > absoluteSum(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:116
point< T > max(const point< T > &p1, const point< T > &p2)
Definition: point.h:146
orientedpoint< T, A > absoluteDifference(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:107
point< T > operator*(const point< T > &p, const T &v)
Definition: point.h:30
orientedpoint(T x, T y, A _theta)
Definition: point.h:49
point< T > operator+(const point< T > &p1, const point< T > &p2)
Definition: point.h:20
point(T _x, T _y)
Definition: point.h:15
unsigned int c
Definition: gfs2stream.cpp:41
point< T > interpolate(const point< T > &p1, const F &t1, const point< T > &p2, const F &t2, const F &t3)
Definition: point.h:162
orientedpoint< double, double > OrientedPoint
Definition: point.h:203


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20