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