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 
00023 #include <iostream>
00024 #include <list>
00025 using namespace std;
00026 
00027 #include "mslio.h"
00028 
00029 class MSLPoint
00030 {
00031   double xrep;
00032   double yrep;
00033 
00034 public:
00035  
00036   MSLPoint();
00037   MSLPoint(double x, double y);
00038   ~MSLPoint() {}
00039   double xcoord()  const   { return xrep; }
00040   double ycoord()  const   { return yrep; }
00041   void normalize() const {}
00042   int dim() const { return 2; }
00043   double sqr_dist(const MSLPoint& q) const;
00044   double xdist(const MSLPoint& q) const;
00045   double ydist(const MSLPoint& q) const;
00046   double distance(const MSLPoint& q) const;
00047   double distance() const { return distance(MSLPoint(0,0)); }
00048   double angle(const MSLPoint& q, const MSLPoint& r) const;
00049   MSLPoint translate_by_angle(double alpha, double d) const;
00050   MSLPoint translate(double dx, double dy) const;
00051   MSLPoint rotate(const MSLPoint& q, double a) const;
00052   MSLPoint rotate(double a) const;
00053   MSLPoint rotate90(const MSLPoint& q) const;
00054   MSLPoint rotate90() const;
00055   MSLPoint reflect(const MSLPoint& q, const MSLPoint& r) const;
00056   MSLPoint reflect(const MSLPoint& q) const;
00057   bool operator==(const MSLPoint& q) const;
00058   bool operator!=(const MSLPoint& q)  const { return !operator==(q);}
00059 
00060   friend ostream& operator<<(ostream& O, const MSLPoint& p) ;
00061   friend istream& operator>>(istream& I, MSLPoint& p) ;
00062   //friend istream& operator>>(istream& is, list<MSLPoint>& vl);
00063   //friend ostream& operator<<(ostream& os, const list<MSLPoint>& vl);
00064   //friend istream& operator>>(istream& is, list<list<MSLPoint> >& vl);
00065   //friend ostream& operator<<(ostream& os, const list<list<MSLPoint> >& vl);
00066 };
00067 
00068 ostream& operator<<(ostream& O, const MSLPoint& p) ;
00069 istream& operator>>(istream& I, MSLPoint& p) ;
00070 
00071 
00072 inline MSLPoint center(const MSLPoint& a, const MSLPoint& b) { 
00073   return MSLPoint((a.xcoord()+b.xcoord())/2,(a.ycoord()+b.ycoord())/2); 
00074 } 
00075 
00076 
00077 inline MSLPoint midMSLPoint(const MSLPoint& a, const MSLPoint& b) { 
00078   return center(a,b); 
00079 }
00080 
00081 
00082 inline int orientation(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00083 { double d1 = (a.xcoord() - b.xcoord()) * (a.ycoord() - c.ycoord());
00084   double d2 = (a.ycoord() - b.ycoord()) * (a.xcoord() - c.xcoord()); 
00085   if (d1 == d2) return 0; else return (d1 > d2) ? +1 : -1;
00086 }
00087 
00088 inline int cmp_signed_dist(const MSLPoint& a, const MSLPoint& b, 
00089                            const MSLPoint& c, const MSLPoint& d)
00090 { double d1 = (a.xcoord() - b.xcoord()) * (d.ycoord() - c.ycoord());
00091   double d2 = (a.ycoord() - b.ycoord()) * (d.xcoord() - c.xcoord()); 
00092   if (d1 == d2) return 0; else return (d1 > d2) ? +1 : -1;
00093 }
00094 
00095 inline double area(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00096 { return ((a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()) -
00097           (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()))/2; }
00098 
00099 inline bool collinear(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00100 { return (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()) ==
00101          (a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()); }
00102 
00103 inline bool right_turn(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00104 { return (a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()) <
00105          (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()); }
00106 
00107 inline bool left_turn(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c)
00108 { return (a.xcoord()-b.xcoord()) * (a.ycoord()-c.ycoord()) >
00109          (a.ycoord()-b.ycoord()) * (a.xcoord()-c.xcoord()); }
00110 
00111 extern int side_of_circle(const MSLPoint& a, const MSLPoint& b, 
00112                                     const MSLPoint& c, const MSLPoint& d);
00113 
00114 inline bool incircle(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c,
00115                                                      const MSLPoint& d)
00116 { return (orientation(a,b,c) * side_of_circle(a,b,c,d)) > 0; }
00117 
00118 inline bool outcircle(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c,
00119                                                       const MSLPoint& d)
00120 { return (orientation(a,b,c) * side_of_circle(a,b,c,d)) < 0; }
00121 
00122 inline bool cocircular(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c,
00123                                                        const MSLPoint& d)
00124 { return side_of_circle(a,b,c,d) == 0; }
00125 
00126 
00127 // Add a fake polygon class which is a list of points
00128 // typedef list<MSLPoint> MSLPolygon;
00129 
00130 #endif
00131
point.h
