geometry2D.h
1 /*
2 Copyright (c) 2004, Tim Bailey
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7 
8  * Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10  * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13  * Neither the name of the Player Project nor the names of its contributors
14  may be used to endorse or promote products derived from this software
15  without specific prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
21 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
24 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28 
29 
30 /* Simple 2D geometric operations with points, poses, and lines.
31  *
32  * These algorithms were worked out by me from first principles several years
33  * ago. They work ok, but are not particularly good implementations. I recommend
34  * the geometric source-code and resources by David Eberly on the "Magic Software"
35  * website: www.magic-software.com.
36  *
37  * Tim Bailey 2004.
38  */
39 #ifndef GEOMETRY_2D_H_
40 #define GEOMETRY_2D_H_
41 
42 #include <vector>
43 
44 namespace Geom2D {
45 
46 //
47 // Basic Structures
48 //
49 
50 struct Point
51 {
52  double x;
53  double y;
54  short int laser_index;
55 };
56 
57 struct Pose
58 {
59  Point p;
60  double phi;
61 };
62 
63 struct Line {
64  Point first;
65  Point second;
66 };
67 
68 //
69 // Utility functions
70 //
71 
72 const double PI = 3.14159265358979;
73 
74 inline
75 double sqr(double x) { return x*x; }
76 
77 inline
78 double abs(double x) { return (x<0.) ? -x : x; }
79 
80 inline
81 double round(double x) {
82  return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
83 }
84 /*
85 template<class T>
86 inline
87 void swap(T& a, T& b)
88 {
89  T tmp(a);
90  a = b;
91  b = tmp;
92 }
93 */
94 inline
95 double pi_to_pi(double angle) { // normalise an angle to within +/- PI
96  while (angle < -PI)
97  angle += 2.*PI;
98  while (angle > PI)
99  angle -= 2.*PI;
100  return angle;
101 }
102 
103 //
104 // Point and Pose algorithms
105 //
106 
107 inline
108 double dist_sqr(const Point& p, const Point& q) { // squared distance between two Points
109  return (sqr(p.x-q.x) + sqr(p.y-q.y));
110 }
111 
112 double dist(const Point& p, const Point& q);
113 Pose compute_relative_pose(const std::vector<Point>& a, const std::vector<Point>& b);
114 
115 //
116 // Line algorithms
117 //
118 
119 bool intersection_line_line (Point& p, const Line& l, const Line& m);
120 double distance_line_point (const Line& lne, const Point& p);
121 void intersection_line_point(Point& p, const Line& l, const Point& q);
122 
123 //
124 // Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).
125 //
126 
127 class Transform2D {
128 public:
129  Transform2D(const Pose& ref);
130 
131  void transform_to_relative(Point &p);
132  void transform_to_relative(Pose &p);
133  void transform_to_global(Point &p);
134  void transform_to_global(Pose &p);
135 
136 private:
137  const Pose base;
138  double c;
139  double s;
140 };
141 
142 inline
143 void Transform2D::transform_to_relative(Point &p) {
144  p.x -= base.p.x;
145  p.y -= base.p.y;
146  double t(p.x);
147  p.x = p.x*c + p.y*s;
148  p.y = p.y*c - t*s;
149 }
150 
151 inline
152 void Transform2D::transform_to_global(Point &p) {
153  double t(p.x);
154  p.x = base.p.x + c*p.x - s*p.y;
155  p.y = base.p.y + s*t + c*p.y;
156 }
157 
158 inline
159 void Transform2D::transform_to_relative(Pose &p) {
160  transform_to_relative(p.p);
161  p.phi= pi_to_pi(p.phi-base.phi);
162 }
163 
164 inline
165 void Transform2D::transform_to_global(Pose &p) {
166  transform_to_global(p.p);
167  p.phi= pi_to_pi(p.phi+base.phi);
168 }
169 
170 } // namespace Geom2D
171 
172 #endif
Definition: types.hh:42
Definition: geometry2D.h:57
Definition: geometry2D.h:63
Definition: geometry2D.h:50
Definition: geometry2D.h:127