Motion Strategy Library

00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) University of Illinois and Steven M. LaValle.     
00006 // All Rights Reserved.
00007 //
00008 // Permission to use, copy, and distribute this software and its
00009 // documentation is hereby granted free of charge, provided that
00010 // (1) it is not a component of a commercial product, and
00011 // (2) this notice appears in all copies of the software and
00012 //     related documentation.
00013 //
00014 // The University of Illinois and the author make no representations
00015 // about the suitability or fitness of this software for any purpose.
00016 // It is provided "as is" without express or implied warranty.
00017 //----------------------------------------------------------------------
00018 
00019 #ifndef MSL_POINT_H
00020 #define MSL_POINT_H
00021 
00022 #include <stream.h>
00023 #include <list.h>
00024 
00025 class MSLPoint
00026 {
00027   double xrep;
00028   double yrep;
00029 
00030 public:
00031  
00032   MSLPoint();
00033   MSLPoint(double x, double y);
00034   ~MSLPoint() {}
00035   double xcoord()  const   { return xrep; }
00036   double ycoord()  const   { return yrep; }
00037   void normalize() const {}
00038   int dim() const { return 2; }
00039   double sqr_dist(const MSLPoint& q) const;
00040   double xdist(const MSLPoint& q) const;
00041   double ydist(const MSLPoint& q) const;
00042   double distance(const MSLPoint& q) const;
00043   double distance() const { return distance(MSLPoint(0,0)); }
00044   double angle(const MSLPoint& q, const MSLPoint& r) const;
00045   MSLPoint translate_by_angle(double alpha, double d) const;
00046   MSLPoint translate(double dx, double dy) const;
00047   MSLPoint rotate(const MSLPoint& q, double a) const;
00048   MSLPoint rotate(double a) const;
00049   MSLPoint rotate90(const MSLPoint& q) const;
00050   MSLPoint rotate90() const;
00051   MSLPoint reflect(const MSLPoint& q, const MSLPoint& r) const;
00052   MSLPoint reflect(const MSLPoint& q) const;
00053   bool operator==(const MSLPoint& q) const;
00054   bool operator!=(const MSLPoint& q)  const { return !operator==(q);}
00055 
00056   friend ostream& operator<<(ostream& O, const MSLPoint& p) ;
00057   friend istream& operator>>(istream& I, MSLPoint& p) ;
00058   friend istream& operator>>(istream& is, list<MSLPoint> & vl);
00059   friend ostream& operator<<(ostream& os, const list<MSLPoint> & vl);
00060   friend istream& operator>>(istream& is, list<list<MSLPoint> > & vl);
00061   friend ostream& operator<<(ostream& os, const list<list<MSLPoint> > & vl);
00062 };
00063 
00064 
00065 inline MSLPoint center(const MSLPoint& a, const MSLPoint& b) { 
00066   return MSLPoint((a.xcoord()+b.xcoord())/2,(a.ycoord()+b.ycoord())/2); 
00067 } 
00068 
00069 
00070 inline MSLPoint midMSLPoint(const MSLPoint& a, const MSLPoint& b) { 
00071   return center(a,b); 
00072 }
00073 
00074 
00075 inline int orientation(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00076 { double d1 = (a.xcoord() - b.xcoord()) * (a.ycoord() - c.ycoord());
00077   double d2 = (a.ycoord() - b.ycoord()) * (a.xcoord() - c.xcoord()); 
00078   if (d1 == d2) return 0; else return (d1 > d2) ? +1 : -1;
00079 }
00080 
00081 inline int cmp_signed_dist(const MSLPoint& a, const MSLPoint& b, 
00082                            const MSLPoint& c, const MSLPoint& d)
00083 { double d1 = (a.xcoord() - b.xcoord()) * (d.ycoord() - c.ycoord());
00084   double d2 = (a.ycoord() - b.ycoord()) * (d.xcoord() - c.xcoord()); 
00085   if (d1 == d2) return 0; else return (d1 > d2) ? +1 : -1;
00086 }
00087 
00088 inline double area(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00089 { return ((a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()) -
00090           (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()))/2; }
00091 
00092 inline bool collinear(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00093 { return (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()) ==
00094          (a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()); }
00095 
00096 inline bool right_turn(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00097 { return (a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()) <
00098          (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()); }
00099 
00100 inline bool left_turn(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00101 { return (a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()) >
00102          (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()); }
00103 
00104 extern int side_of_circle(const MSLPoint& a, const MSLPoint& b, 
00105                                     const MSLPoint& c, const MSLPoint& d);
00106 
00107 inline bool incircle(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c,
00108                                                      const MSLPoint& d)
00109 { return (orientation(a,b,c) * side_of_circle(a,b,c,d)) > 0; }
00110 
00111 inline bool outcircle(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c,
00112                                                       const MSLPoint& d)
00113 { return (orientation(a,b,c) * side_of_circle(a,b,c,d)) < 0; }
00114 
00115 inline bool cocircular(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c,
00116                                                        const MSLPoint& d)
00117 { return side_of_circle(a,b,c,d) == 0; }
00118 
00119 
00120 // Add a fake polygon class which is a list of points
00121 typedef list<MSLPoint> MSLPolygon;
00122 
00123 #endif
00124
point.h
