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