00001 #ifndef INCLUDE_NAVLIB_GEOMETRY 00002 #define INCLUDE_NAVLIB_GEOMETRY 00003 00004 #include "wykobi/wykobi.hpp" 00005 00006 namespace navlib 00007 { 00008 00009 const static double PI = static_cast<double>(wykobi::PI); 00010 const static double PI2 = static_cast<double>(wykobi::PI2); 00011 00012 class Circle; 00013 class Polar; 00014 class Segment; 00015 class Sector; 00016 00017 template <class T> 00018 T maximum(const T& lhs, const T& rhs) 00019 { 00020 if (lhs > rhs) 00021 return lhs; 00022 return rhs; 00023 } 00024 00025 template <class T> 00026 T minimum(const T& lhs, const T& rhs) 00027 { 00028 if (lhs < rhs) 00029 return lhs; 00030 return rhs; 00031 } 00032 00034 double pi2pi(const double& angle); 00035 00037 double sign(const double& num); 00038 00040 bool sortSectors(const Sector& lhs, const Sector& rhs, const double& goalHeading); 00041 00043 class Point : public wykobi::point2d<double> 00044 { 00045 public: 00048 Point(double xi=0.0f, double yi=0.0f); 00050 Point(wykobi::point2d<double> point); 00052 bool operator ==(const Point& point) const; 00054 double getDistance(const Point& point) const; 00056 double getAngleTo(const Point& point) const; 00057 }; 00058 00060 class Pose 00061 { 00062 public: 00063 double x; 00064 double y; 00065 double theta; 00066 00070 Pose(double xi=0.0f, double yi=0.0f, double thetai=0.0); 00072 bool operator ==(const Pose& pose) const; 00074 Point globalize(const Point& point) const; 00076 Pose globalize(const Pose& pose) const; 00078 Point localize(const Point& point) const; 00080 Pose localize(const Pose& pose) const; 00082 Pose rotate(const double& dtheta); 00084 Pose translate(const double& dx, const double& dy); 00085 00086 private: 00087 double _ct; 00088 double _st; 00089 }; 00090 00092 class Shape 00093 { 00094 public: 00095 virtual ~Shape() {}; 00097 virtual std::vector<Segment> clip(std::vector<Segment> segments)=0; 00099 virtual bool contains(Point& p) const = 0; 00101 virtual bool collides(std::vector<Segment> segments); 00103 virtual Circle getBoundingCircle(); 00105 virtual double getDistance(std::vector<Point> points); 00107 virtual std::vector<Point> getPoints() const=0; 00109 virtual Pose getPose() const=0; 00111 virtual std::vector<Segment> getSegments() const; 00113 void globalize(const Pose& pose); 00115 void localize(const Pose& pose); 00117 virtual void setPose(const Pose& pose)=0; 00118 }; 00119 00121 class Circle : public wykobi::circle<double>, public Shape 00122 { 00123 public: 00128 Circle(double x, double y, double radiusi, unsigned int ni=16); 00129 virtual ~Circle() {}; 00130 virtual std::vector<Segment> clip(std::vector<Segment> segments); 00131 virtual bool contains(Point& p) const; 00132 virtual Circle getBoundingCircle(); 00133 virtual std::vector<Point> getPoints() const; 00134 virtual Pose getPose() const; 00135 virtual void setPose(const Pose& pose); 00136 00137 protected: 00138 double _theta; 00139 unsigned int _n; 00140 }; 00141 00143 class Gap 00144 { 00145 public: 00146 double lRho; 00147 double rRho; 00148 double theta; 00149 int index; 00150 00155 Gap(double lRhoi, double rRhoi, double thetai, int indexi); 00159 Gap(const Polar& lhs, const Polar& rhs, int indexi); 00161 bool operator ==(const Gap& gap) const; 00162 }; 00163 00165 class Polar 00166 { 00167 public: 00168 double rho; 00169 double theta; 00170 00173 Polar(double rhoi=0.0f, double thetai=0.0f); 00176 Polar(Point p, Pose origin=Pose(0.0f, 0.0f, 0.0f)); 00178 bool operator < (const Polar& polar) const; 00180 bool operator ==(const Polar& polar) const; 00182 Point toPoint() const; 00184 Point toPoint(const Pose& origin) const; 00185 }; 00186 00188 class Polygon : public wykobi::polygon<double, 2>, public Shape 00189 { 00190 public: 00192 Polygon(std::vector<Point> points); 00193 virtual ~Polygon() {}; 00195 virtual std::vector<Segment> clip(std::vector<Segment> segments); 00196 virtual bool contains(Point& p) const; 00197 virtual std::vector<Point> getPoints() const; 00198 virtual Pose getPose() const; 00199 virtual void setPose(const Pose& pose); 00200 00201 protected: 00202 Pose _pose; 00203 }; 00204 00206 class Sector 00207 { 00208 public: 00209 Gap lGap; 00210 Gap rGap; 00211 00214 Sector(const Gap& lGapi, const Gap& rGapi); 00216 bool operator ==(const Sector& sector) const; 00217 }; 00218 00220 class Segment : public wykobi::segment<double, 2> 00221 { 00222 public: 00227 Segment(double x1, double y1, double x2, double y2); 00230 Segment(Point p1, Point p2); 00232 bool operator ==(const Segment& segment) const; 00234 double getDistance(const Point& point) const; 00236 double getLength() const; 00238 Point getPoint1() const { return (*this)[0]; } 00240 Point getPoint2() const { return (*this)[1]; } 00242 void setPoint1(const Point& p) { (*this)[0].x = p.x; (*this)[0].y = p.y; } 00244 void setPoint2(const Point& p) { (*this)[1].x = p.x; (*this)[1].y = p.y; } 00245 }; 00246 00247 } 00248 00249 #endif