00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef GEOMETRY_2D_H_
00040 #define GEOMETRY_2D_H_
00041
00042 #include <vector>
00043
00044 namespace Geom2D {
00045
00046
00047
00048
00049
00050 struct Point
00051 {
00052 double x;
00053 double y;
00054 short int laser_index;
00055 };
00056
00057 struct Pose
00058 {
00059 Point p;
00060 double phi;
00061 };
00062
00063 struct Line {
00064 Point first;
00065 Point second;
00066 };
00067
00068
00069
00070
00071
00072 const double PI = 3.14159265358979;
00073
00074 inline
00075 double sqr(double x) { return x*x; }
00076
00077 inline
00078 double abs(double x) { return (x<0.) ? -x : x; }
00079
00080 inline
00081 double round(double x) {
00082 return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
00083 }
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094 inline
00095 double pi_to_pi(double angle) {
00096 while (angle < -PI)
00097 angle += 2.*PI;
00098 while (angle > PI)
00099 angle -= 2.*PI;
00100 return angle;
00101 }
00102
00103
00104
00105
00106
00107 inline
00108 double dist_sqr(const Point& p, const Point& q) {
00109 return (sqr(p.x-q.x) + sqr(p.y-q.y));
00110 }
00111
00112 double dist(const Point& p, const Point& q);
00113 Pose compute_relative_pose(const std::vector<Point>& a, const std::vector<Point>& b);
00114
00115
00116
00117
00118
00119 bool intersection_line_line (Point& p, const Line& l, const Line& m);
00120 double distance_line_point (const Line& lne, const Point& p);
00121 void intersection_line_point(Point& p, const Line& l, const Point& q);
00122
00123
00124
00125
00126
00127 class Transform2D {
00128 public:
00129 Transform2D(const Pose& ref);
00130
00131 void transform_to_relative(Point &p);
00132 void transform_to_relative(Pose &p);
00133 void transform_to_global(Point &p);
00134 void transform_to_global(Pose &p);
00135
00136 private:
00137 const Pose base;
00138 double c;
00139 double s;
00140 };
00141
00142 inline
00143 void Transform2D::transform_to_relative(Point &p) {
00144 p.x -= base.p.x;
00145 p.y -= base.p.y;
00146 double t(p.x);
00147 p.x = p.x*c + p.y*s;
00148 p.y = p.y*c - t*s;
00149 }
00150
00151 inline
00152 void Transform2D::transform_to_global(Point &p) {
00153 double t(p.x);
00154 p.x = base.p.x + c*p.x - s*p.y;
00155 p.y = base.p.y + s*t + c*p.y;
00156 }
00157
00158 inline
00159 void Transform2D::transform_to_relative(Pose &p) {
00160 transform_to_relative(p.p);
00161 p.phi= pi_to_pi(p.phi-base.phi);
00162 }
00163
00164 inline
00165 void Transform2D::transform_to_global(Pose &p) {
00166 transform_to_global(p.p);
00167 p.phi= pi_to_pi(p.phi+base.phi);
00168 }
00169
00170 }
00171
00172 #endif