geometry2D.h
1/*
2Copyright (c) 2004, Tim Bailey
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without
6modification, 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
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
21ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
24ANY 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
26SOFTWARE, 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
44namespace Geom2D {
45
46//
47// Basic Structures
48//
49
50struct Point
51{
52 double x;
53 double y;
54 short int laser_index;
55};
56
57struct Pose
58{
59 Point p;
60 double phi;
61};
62
63struct Line {
64 Point first;
65 Point second;
66};
67
68//
69// Utility functions
70//
71
72const double PI = 3.14159265358979;
73
74inline
75double sqr(double x) { return x*x; }
76
77inline
78double abs(double x) { return (x<0.) ? -x : x; }
79
80inline
81double round(double x) {
82 return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
83}
84/*
85template<class T>
86inline
87void swap(T& a, T& b)
88{
89 T tmp(a);
90 a = b;
91 b = tmp;
92}
93*/
94inline
95double 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
107inline
108double 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
112double dist(const Point& p, const Point& q);
113Pose compute_relative_pose(const std::vector<Point>& a, const std::vector<Point>& b);
114
115//
116// Line algorithms
117//
118
119bool intersection_line_line (Point& p, const Line& l, const Line& m);
120double distance_line_point (const Line& lne, const Point& p);
121void 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
128public:
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
136private:
137 const Pose base;
138 double c;
139 double s;
140};
141
142inline
143void 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
151inline
152void 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
158inline
159void Transform2D::transform_to_relative(Pose &p) {
160 transform_to_relative(p.p);
161 p.phi= pi_to_pi(p.phi-base.phi);
162}
163
164inline
165void 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 geometry2D.h:127
Definition types.hh:43
Definition geometry2D.h:63
Definition geometry2D.h:51
Definition geometry2D.h:58