Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

point.C

Go to the documentation of this file.
00001 #ifndef PI
00002 #define PI 3.1415926535897932385
00003 #endif 
00004 
00005 #include <math.h>
00006 #include <ctype.h>
00007 
00008 #include "point.h"
00009 
00010 
00011 MSLPoint::MSLPoint() { 
00012   xrep = 0; yrep = 0; 
00013 }
00014 
00015 
00016 MSLPoint::MSLPoint(double x, double y) { 
00017   xrep = x; yrep = y; 
00018 }
00019 
00020 
00021 double MSLPoint::angle(const MSLPoint& q, const MSLPoint& r) const
00022 {
00023   double dx1 = q.xcoord() - xrep; 
00024   double dy1 = q.ycoord() - yrep; 
00025   double dx2 = r.xcoord() - xrep; 
00026   double dy2 = r.ycoord() - yrep; 
00027 
00028   if ((dx1 == 0 && dy1 == 0) || (dx2 == 0 && dy2 == 0)) {
00029     cerr << "MSLPoint::angle:  zero vector input.\n";
00030     exit(-1);
00031   }
00032   
00033   double norm  = (dx1*dx1+dy1*dy1)*(dx2*dx2+dy2*dy2);
00034 
00035   double cosfi = (dx1*dx2+dy1*dy2) / sqrt(norm);
00036 
00037   if (cosfi >=  1.0 ) return 0;
00038   if (cosfi <= -1.0 ) return PI;
00039   
00040   double fi = acos(cosfi);
00041 
00042   if (dx1*dy2 < dy1*dx2) fi = -fi;
00043 
00044   if (fi < 0) fi += 2*PI;
00045 
00046   return fi;
00047 }
00048   
00049 
00050 
00051 // Rotations 
00052 
00053 MSLPoint MSLPoint::rotate90(const MSLPoint& p) const
00054 { double px = p.xcoord();
00055   double py = p.ycoord();
00056   double dx = xrep - px;
00057   double dy = yrep - py;
00058   return MSLPoint(px-dy,py+dx);
00059 }
00060 
00061 MSLPoint MSLPoint::rotate90() const
00062 { return MSLPoint(-yrep,xrep); }
00063 
00064 
00065 MSLPoint MSLPoint::rotate(const MSLPoint& origin, double fi) const
00066 { double cx = origin.xcoord();
00067   double cy = origin.ycoord();
00068   double sinfi = sin(fi);
00069   double cosfi = cos(fi);
00070   double dx = xrep - cx;
00071   double dy = yrep - cy;
00072   return MSLPoint(cx+dx*cosfi-dy*sinfi,cy+dx*sinfi+dy*cosfi);
00073 }
00074 
00075 
00076 MSLPoint MSLPoint::rotate(double fi) const
00077 { double sinfi = sin(fi);
00078   double cosfi = cos(fi);
00079   double x = xrep;
00080   double y = yrep;
00081   return MSLPoint(x*cosfi-y*sinfi,x*sinfi+y*cosfi);
00082 }
00083 
00084 
00085 MSLPoint MSLPoint::reflect(const MSLPoint& p, const MSLPoint& q) const
00086 { // reflect MSLPoint across line through p and q
00087 
00088   double px = p.xrep;
00089   double py = p.yrep;
00090 
00091   double x1 = xrep   - px;
00092   double y1 = yrep   - py;
00093   double x2 = q.xcoord() - px;
00094   double y2 = q.ycoord() - py;
00095 
00096   double L = (x1*x1 + y1*y1) * (x2*x2 + y2*y2);
00097 
00098   double cosfi = (x1*x2 + y1*y2);
00099   double sinfi = (x1*y2 - x2*y1); 
00100   double cos2 = (cosfi*cosfi - sinfi*sinfi)/L;
00101   double sin2 = 2*cosfi*sinfi/L;
00102 
00103   return MSLPoint(px + x1*cos2-y1*sin2, py + x1*sin2+y1*cos2);
00104 }
00105 
00106 
00107 MSLPoint MSLPoint::reflect(const MSLPoint& q) const
00108 { // reflect MSLPoint across MSLPoint q
00109   return MSLPoint(2*q.xcoord()-xrep, 2*q.ycoord()-yrep);
00110  }
00111   
00112 
00113 
00114 // Translations
00115 
00116 MSLPoint MSLPoint::translate(double dx, double dy) const
00117 { return MSLPoint(xrep+dx,yrep+dy); }
00118 
00119 
00120 MSLPoint MSLPoint::translate_by_angle(double phi, double d) const
00121 { double dx = cos(phi) * d;
00122   double dy = sin(phi) * d;
00123   if (fabs(dx) < 1e-12) dx = 0; 
00124   if (fabs(dy) < 1e-12) dy = 0; 
00125   return MSLPoint(xrep+dx,yrep+dy);
00126  }
00127 
00128 
00129 // Distances
00130 
00131 double MSLPoint::sqr_dist(const MSLPoint& p)  const
00132 { double dx = p.xcoord() - xrep; 
00133   double dy = p.ycoord() - yrep;
00134   return dx*dx + dy*dy;
00135  }
00136 
00137 
00138 
00139 double MSLPoint::xdist(const MSLPoint& q) const 
00140 { return fabs(xrep - q.xcoord()); }
00141 
00142 double MSLPoint::ydist(const MSLPoint& q) const 
00143 { return fabs(yrep - q.ycoord()); }
00144 
00145 double  MSLPoint::distance(const MSLPoint& q) const 
00146 { return sqrt(sqr_dist(q)); }
00147 
00148 
00149 bool MSLPoint::operator==(const MSLPoint& p) const 
00150 { return (xrep == p.xcoord()) && (yrep == p.ycoord()); }
00151 
00152    
00153 
00154 int side_of_circle(const MSLPoint& a, const MSLPoint& b, const MSLPoint& c, 
00155                                                    const MSLPoint& d)
00156 {  // comments indicate bit lengths of values if coordinates have 
00157    // at most L bits. 
00158    double ax = a.xcoord();    // L bits
00159    double ay = a.ycoord();
00160 
00161    double bx = b.xcoord() - ax;  // L + 1 bits
00162    double by = b.ycoord() - ay;
00163    double bw = bx*bx + by*by;    // 2L + 3 bits
00164 
00165    double cx = c.xcoord() - ax;  // L + 1 bits
00166    double cy = c.ycoord() - ay;
00167    double cw = cx*cx + cy*cy;    // 2L + 3 bits
00168 
00169    double D1 = cy*bw - by*cw;  // 2L + 3 + L + 1 + 1 = 3L + 5 bits
00170    double D2 = bx*cw - cx*bw;  // 3L + 5 bits
00171    double D3 = by*cx - bx*cy;  // 2L + 3
00172 
00173    double dx = d.xcoord() - ax;  // L + 1 bits
00174    double dy = d.ycoord() - ay;
00175 
00176    double D  = D1*dx  + D2*dy + D3*(dx*dx + dy*dy);
00177                                // 3L + 5 + L + 1 + 2 = 4L + 8 bits
00178 
00179    if (D != 0) 
00180       return (D > 0) ? 1 : -1;
00181    else
00182       return 0;
00183 }
00184 
00185 
00186 
00187 ostream& operator<<(ostream& out, const MSLPoint& p)
00188 { out << "(" << p.xcoord() << "," << p.ycoord() << ")";
00189   return out;
00190 } 
00191 
00192 istream& operator>>(istream& in, MSLPoint& p) 
00193 { // syntax: {(} x {,} y {)}
00194 
00195   double x,y; 
00196   char c;
00197 
00198   do in.get(c); while (in && isspace(c));
00199 
00200   if (!in) return in;
00201 
00202   if (c != '(') in.putback(c);
00203 
00204   in >> x;
00205 
00206   do in.get(c); while (isspace(c));
00207   if (c != ',') in.putback(c);
00208 
00209   in >> y; 
00210 
00211   do in.get(c); while (c == ' ');
00212   if (c != ')') in.putback(c);
00213 
00214   p = MSLPoint(x,y); 
00215   return in; 
00216 
00217 } 
00218 
00219 
00220 ostream& operator<<(ostream& out, const list<MSLPoint>& L)
00221 {
00222   list<MSLPoint>::iterator x; 
00223   list<MSLPoint> vl;
00224   vl = L;
00225   for (x = vl.begin(); x != vl.end(); x++) 
00226     out << " " << *x;
00227   return out;
00228 }
00229 
00230 
00231 istream& operator>>(istream& in, list<MSLPoint>& L)
00232 { 
00233   L.clear();
00234   MSLPoint x;
00235   char c;
00236   for(;;)
00237     { 
00238       while (in.get(c) && c==' ');
00239       if ((!in) || isspace(c)) break;      
00240       in.putback(c);
00241       x = MSLPoint(); in >> x; L.push_back(x);
00242     }
00243   return in;
00244 }
00245 
00246 
00247 
00248 ostream& operator<<(ostream& out, const list<list<MSLPoint> >& L)
00249 {
00250   list<list<MSLPoint> >::iterator x; 
00251   list<list<MSLPoint> > vl;
00252   vl = L;
00253   for (x = vl.begin(); x != vl.end(); x++) 
00254     out << *x << "\n";
00255   return out;
00256 }
00257 
00258 
00259 istream& operator>>(istream& in, list<list<MSLPoint> >& L)
00260 { 
00261   L.clear();
00262   list<MSLPoint> x;
00263   for(;;)
00264     { 
00265       char c;
00266       while (in.get(c) && isspace(c));
00267       if (!in) break;
00268       in.putback(c);
00269       x = MSLPolygon(); in >> x; L.push_back(x);
00270     }
00271   return in;
00272 }
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.