geometry2D.h

00001 /* Simple 2D geometric operations with points, poses, and lines.
00002  *
00003  * These algorithms were worked out by me from first principles several years
00004  * ago. They work ok, but are not particularly good implementations. I recommend
00005  * the geometric source-code and resources by David Eberly on the "Magic Software"
00006  * website: www.magic-software.com.
00007  *
00008  * Tim Bailey 2004.
00009  */
00010 #ifndef GEOMETRY_2D_H_
00011 #define GEOMETRY_2D_H_
00012 
00013 #include <vector>
00014 
00015 namespace Geom2D {
00016 
00017 //
00018 // Basic Structures
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 // Utility functions
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 template<class T>
00057 inline
00058 void swap(T& a, T& b) 
00059 {
00060         T tmp(a);
00061         a = b;
00062         b = tmp;
00063 }
00064 */
00065 inline
00066 double pi_to_pi(double angle) { // normalise an angle to within +/- PI
00067         while (angle < -PI)
00068                 angle += 2.*PI;
00069         while (angle > PI)
00070                 angle -= 2.*PI;
00071         return angle;
00072 }
00073 
00074 //
00075 // Point and Pose algorithms
00076 //
00077 
00078 inline 
00079 double dist_sqr(const Point& p, const Point& q) { // squared distance between two Points
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 // Line algorithms
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 // Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).
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 } // namespace Geom2D
00142 
00143 #endif

Last updated 12 September 2005 21:38:45