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

point3d.C

Go to the documentation of this file.
00001 #include <math.h>
00002 #include <ctype.h>
00003 #include "point3d.h"
00004 
00005 ostream& operator<<(ostream& out, const list<MSLPoint3d>& L)
00006 {
00007   list<MSLPoint3d>::iterator x; 
00008   list<MSLPoint3d> vl;
00009   vl = L;
00010   for (x = vl.begin(); x != vl.end(); x++) 
00011     out << " " << *x;
00012   return out;
00013 }
00014 
00015 
00016 istream& operator>>(istream& in, list<MSLPoint3d>& L)
00017 { 
00018   L.clear();
00019   MSLPoint3d x;
00020   for(;;)
00021     { 
00022       char c;
00023       while (in.get(c) && isspace(c));
00024       if (!in) break;
00025       in.putback(c);
00026       x = MSLPoint3d(); in >> x; L.push_back(x);
00027     }
00028   return in;
00029 }
00030 
00031 
00032 static void error_handler(int i, const char* s) {
00033   cerr << s << "\n";
00034   exit(i);
00035 }
00036 
00037 
00038 MSLPoint3d::MSLPoint3d() { xrep = 0; yrep = 0; zrep=0; }
00039 
00040 MSLPoint3d::MSLPoint3d(double x, double y, double z) 
00041 { xrep = x; yrep = y; zrep=z; }
00042 
00043 
00044 // Translations
00045 
00046 MSLPoint3d MSLPoint3d::translate(double dx, double dy, double dz) const
00047 { return MSLPoint3d(xcoord()+dx,ycoord()+dy,zcoord()+dz); }
00048 
00049 
00050 
00051 // Distances
00052 
00053 double MSLPoint3d::sqr_dist(const MSLPoint3d& p)  const
00054 { double dx = p.xcoord() - xrep; 
00055   double dy = p.ycoord() - yrep;
00056   double dz = p.zcoord() - zrep;
00057   return dx*dx + dy*dy + dz*dz;
00058  }
00059 
00060 double MSLPoint3d::xdist(const MSLPoint3d& q) const 
00061 { return fabs(xrep - q.xcoord()); }
00062 
00063 double MSLPoint3d::ydist(const MSLPoint3d& q) const 
00064 { return fabs(yrep - q.ycoord()); }
00065 
00066 double MSLPoint3d::zdist(const MSLPoint3d& q) const 
00067 { return fabs(zrep - q.zcoord()); }
00068 
00069 
00070 MSLPoint3d MSLPoint3d::reflect(const MSLPoint3d& q) const
00071 { // reflect point across point q
00072   return MSLPoint3d(2*q.xcoord()-xcoord(), 2*q.ycoord()-ycoord(),
00073                                          2*q.zcoord()-zcoord());
00074  }
00075 
00076 
00077 MSLPoint3d  MSLPoint3d::reflect(const MSLPoint3d& a, const MSLPoint3d& b,
00078                                                const MSLPoint3d& c) const
00079 {  
00080   // reflect point across plane through a, b, and c
00081 
00082   double x1 = b.xcoord() - a.xcoord();
00083   double y1 = b.ycoord() - a.ycoord();
00084   double z1 = b.zcoord() - a.zcoord();
00085 
00086   double x2 = c.xcoord() - a.xcoord();
00087   double y2 = c.ycoord() - a.ycoord();
00088   double z2 = c.zcoord() - a.zcoord();
00089 
00090   double x3 = xcoord() - a.xcoord();
00091   double y3 = ycoord() - a.ycoord();
00092   double z3 = zcoord() - a.zcoord();
00093 
00094   double x = (z1*y2-y1*z2);
00095   double y = (x1*z2-z1*x2);
00096   double z = (y1*x2-x1*y2);
00097 
00098   if (x == 0 && y == 0 && z == 0)
00099       error_handler(1,"MSLPoint3d::reflect(a,b,c): a,b,c are coplanar");
00100 
00101 
00102   double f = -2*(x*x3+y*y3+z*z3)/(x*x+y*y+z*z);
00103 
00104   return translate(f*x,f*y,f*z);
00105 }
00106 
00107   
00108 
00109 
00110 double  MSLPoint3d::distance(const MSLPoint3d& q) const 
00111 { return sqrt(sqr_dist(q)); }
00112 
00113 
00114 bool MSLPoint3d::operator==(const MSLPoint3d& p) const 
00115 { return xrep == p.xrep && 
00116          yrep == p.yrep && 
00117          zrep == p.zrep; 
00118 }
00119 
00120 
00121 
00122 ostream& operator<<(ostream& out, const MSLPoint3d& p)
00123 { out << "(" << p.xcoord() << "," << p.ycoord() << "," << p.zcoord() << ")";
00124   return out;
00125  } 
00126 
00127 istream& operator>>(istream& in, MSLPoint3d& p) 
00128 { // syntax: {(} x {,} y {,} z{)}
00129 
00130   double x,y,z; 
00131   char c;
00132 
00133   do in.get(c); while (in && isspace(c));
00134 
00135   if (!in) return in;
00136 
00137   if (c != '(') in.putback(c);
00138 
00139   in >> x;
00140 
00141   do in.get(c); while (isspace(c));
00142   if (c != ',') in.putback(c);
00143 
00144   in >> y; 
00145 
00146   do in.get(c); while (isspace(c));
00147   if (c != ',') in.putback(c);
00148 
00149   in >> z; 
00150 
00151   do in.get(c); while (c == ' ');
00152   if (c != ')') in.putback(c);
00153 
00154   p = MSLPoint3d(x,y,z); 
00155   return in; 
00156 
00157  } 
00158 
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.