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

vector.C

Go to the documentation of this file.
00001 //------------------------------------------------------------------------------
00002 // MSLVectors of real numbers
00003 //
00004 //------------------------------------------------------------------------------
00005 
00006 #include <math.h>
00007 #include <ctype.h>
00008 
00009 #include "vector.h"
00010 #define nil 0
00011 
00012 ostream& operator<<(ostream& out, const list<MSLVector>& L)
00013 {
00014   list<MSLVector>::iterator x; 
00015   list<MSLVector> vl;
00016   vl = L;
00017   for (x = vl.begin(); x != vl.end(); x++) 
00018     out << " " << *x;
00019   return out;
00020 }
00021 
00022 
00023 istream& operator>>(istream& in, list<MSLVector>& L)
00024 { 
00025   L.clear();
00026   MSLVector x;
00027   for(;;)
00028     { 
00029       char c;
00030       while (in.get(c) && isspace(c));
00031       if (!in) break;
00032       in.putback(c);
00033       x = MSLVector(); in >> x; L.push_back(x);
00034    }
00035   return in;
00036 }
00037 
00038 
00039 ostream& operator<<(ostream& out, const vector<MSLVector>& L)
00040 {
00041   vector<MSLVector>::iterator x; 
00042   vector<MSLVector> vl;
00043   vl = L;
00044   for (x = vl.begin(); x != vl.end(); x++) 
00045     out << " " << *x;
00046   return out;
00047 }
00048 
00049 
00050 istream& operator>>(istream& in, vector<MSLVector>& L)
00051 { 
00052   L.clear();
00053   MSLVector x;
00054   for(;;)
00055     { 
00056       char c;
00057       while (in.get(c) && isspace(c));
00058       if (!in) break;
00059       in.putback(c);
00060       x = MSLVector(); in >> x; L.push_back(x);
00061    }
00062   return in;
00063 }
00064 
00065 
00066 //istream& operator>> (istream& is, list<MSLVector> & vl) 
00067 //  { is >> tr.p1 >> tr.p2 >> tr.p3; 
00068 //  return is; 
00069 //}
00070 
00071 //ostream& operator<< (ostream& os, const list<MSLVector> & vl)
00072 //{ 
00073 //  list<MSLVector>::iterator lit;
00074 //  list<MSLVector> vl2;
00075 //  vl2 = vl;
00076 //  for (lit = vl2.begin(); lit != vl2.end(); lit++)
00077 //    os << *lit << "  ";
00078 //  return os;
00079 //} 
00080 
00081 
00082 void error_handler(int i, const char* s) {
00083   cerr << s << "\n";
00084   exit(i);
00085 }
00086 
00087 
00088 void MSLVector::check_dimensions(const MSLVector& vec) const
00089 { if (d!=vec.d)
00090    error_handler(1,"MSLVector arguments have different dimensions.");
00091  }
00092 
00093 
00094 MSLVector::MSLVector() 
00095 { d = 0; 
00096   v = nil;
00097 }
00098 
00099 
00100 MSLVector::MSLVector(int n) 
00101 { 
00102  if (n<0) error_handler(1,"MSLVector: negative dimension."); 
00103  d = n; 
00104  v = nil;
00105  if (d > 0)
00106    { v = new double[d]; 
00107    for(int i=0; i<d; i++) 
00108      v[i] = 0.0;
00109   }
00110 }
00111 
00112 
00113 MSLVector::~MSLVector() 
00114 {   
00115   if (v) {
00116     delete[] v; 
00117     v = nil;
00118   }
00119 }
00120 
00121 
00122 MSLVector::MSLVector(const MSLVector& p) 
00123 { d = p.d; 
00124   v = nil;
00125   if (d > 0)
00126     { v = new double[d];
00127     for(int i=0; i<d; i++) v[i] = p.v[i];
00128    }
00129 }
00130 
00131 
00132 
00133 MSLVector::MSLVector(double x, double y) 
00134 { v = new double[2];
00135   d = 2;
00136   v[0] = x;
00137   v[1] = y;
00138  }
00139 
00140 MSLVector::MSLVector(double x, double y, double z) 
00141 { v = new double[3];
00142   d = 3;
00143   v[0] = x;
00144   v[1] = y;
00145   v[2] = z;
00146  }
00147 
00148 
00149 MSLVector MSLVector::rotate90() const
00150 { if (d !=2)  error_handler(1,"MSLVector::rotate90: dimension must be two. ");
00151   return MSLVector(-v[1],v[0]);
00152 }
00153 
00154 MSLVector MSLVector::rotate(double fi ) const
00155 { if (d !=2)  error_handler(1,"MSLVector::rotate: dimension must be two. ");
00156   double sinfi = sin(fi);
00157   double cosfi = cos(fi);
00158   return MSLVector(v[0]*cosfi-v[1]*sinfi,v[0]*sinfi+v[1]*cosfi);
00159 }
00160 
00161 
00162 double  MSLVector::operator[](int i) const
00163 { if (i<0 || i>=d)  error_handler(1,"MSLVector: index out of range ");
00164   return v[i]; 
00165 }
00166 
00167 double& MSLVector::operator[](int i)
00168 { if (i<0 || i>=d)  error_handler(1,"MSLVector: index out of range ");
00169   return v[i]; 
00170 }
00171 
00172 
00173 MSLVector& MSLVector::operator+=(const MSLVector& vec)
00174 { check_dimensions(vec);
00175   int n = d;
00176   while (n--) v[n] += vec.v[n];
00177   return *this;
00178 }
00179 
00180 MSLVector& MSLVector::operator-=(const MSLVector& vec)
00181 { check_dimensions(vec);
00182   int n = d;
00183   while (n--) v[n] -= vec.v[n];
00184   return *this;
00185 }
00186 
00187 MSLVector MSLVector::operator+(const MSLVector& vec) const
00188 { check_dimensions(vec);
00189   int n = d;
00190   MSLVector result(n);
00191   while (n--) result.v[n] = v[n]+vec.v[n];
00192   return result;
00193 }
00194 
00195 MSLVector MSLVector::operator-(const MSLVector& vec) const
00196 { check_dimensions(vec);
00197   int n = d;
00198   MSLVector result(n);
00199   while (n--) result.v[n] = v[n]-vec.v[n];
00200   return result;
00201 }
00202 
00203 MSLVector MSLVector::operator-() const
00204 { int n = d;
00205   MSLVector result(n);
00206   while (n--) result.v[n] = -v[n];
00207   return result;
00208 }
00209 
00210 
00211 MSLVector MSLVector::operator*(double x) const
00212 { int n = d;
00213   MSLVector result(n);
00214   while (n--) result.v[n] = v[n] * x;
00215   return result;
00216 }
00217 
00218 MSLVector MSLVector::operator/(double x) const
00219 { int n = d;
00220   MSLVector result(n);
00221   while (n--) result.v[n] = v[n] / x;
00222   return result;
00223 }
00224 
00225 double MSLVector::operator*(const MSLVector& vec) const
00226 { check_dimensions(vec);
00227   double result=0;
00228   int n = d;
00229   while (n--) result = result+v[n]*vec.v[n];
00230   return result;
00231 }
00232 
00233 MSLVector& MSLVector::operator=(const MSLVector& vec)
00234 { 
00235   if (d != vec.d)
00236   { if (v)
00237       delete(v);
00238 
00239     d = vec.d;
00240 
00241     if (d > 0)
00242       v = new double[d];
00243     else
00244       v = 0;
00245    }
00246 
00247   for(int i=0; i<d; i++) v[i] = vec.v[i];
00248 
00249   return *this;
00250 }
00251 
00252 
00253 bool MSLVector::operator==(const MSLVector& vec)  const
00254 { if (vec.d != d) return false;
00255   int i = 0;
00256   while ((i<d) && (v[i]==vec.v[i])) i++;
00257   return (i==d) ? true : false;
00258 }
00259 
00260 
00261 void MSLVector::read(istream& is) 
00262 { for(int i=0;i<d;i++) is  >> v[i]; }
00263 
00264 void MSLVector::print(ostream& os) 
00265 { os << "(";
00266   for(int i=0;i<d;i++) os << v[i];
00267   os << " )";
00268 }
00269 
00270 
00271 
00272 ostream& operator<<(ostream& os, const MSLVector& v)
00273 { os << v.d << " ";
00274   for (int i=0;i<v.d;i++) os << " " << v[i];
00275   return os;
00276 }
00277 
00278 
00279 istream& operator>>(istream& is, MSLVector& v) 
00280 { int d;
00281   is >> d;
00282   MSLVector w(d);
00283   for (int i=0;i<d;i++) is >> w[i];
00284   v = w;
00285   return is; 
00286 } 
00287 
00288 
00289 double MSLVector::sqr_length() const { return *this * *this; }
00290 
00291 double MSLVector::length() const { return sqrt(sqr_length()); }
00292 
00293 
00294 double MSLVector::angle(const MSLVector& y) const
00295 { 
00296   const MSLVector& x = *this;
00297 
00298   double L = x.length() * y.length();
00299 
00300   if (L == 0)
00301     error_handler(1,"angle: zero argument\n");
00302 
00303   double cosin = (x*y)/L;
00304 
00305   if (cosin < -1) cosin = -1;
00306   if (cosin >  1) cosin =  1;
00307 
00308   return acos(cosin);
00309 }
00310 
00311 
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.