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

geom.C

Go to the documentation of this file.
00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve 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 // Iowa State University 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 #include <fstream.h>
00020 #include <math.h>
00021 
00022 #include "geom.h"
00023 #include "defs.h"
00024 
00025 #include <LEDA/REDEFINE_NAMES.h>
00026 
00027 Geom::Geom(string path = "") {
00028 
00029   if ((path.length() > 0)&&(path[path.length()-1] != '/'))
00030     path += "/";
00031   FilePath = path;
00032 
00033   GeomDim = 3;
00034   if (is_file(FilePath + "GeomDim")) {
00035     file_istream fin(FilePath + "GeomDim");
00036     fin >> GeomDim;
00037   }
00038   else {
00039     cout << "Assuming default geometry dimension: " << GeomDim << "\n";
00040   }
00041 }
00042 
00043 
00044 
00045 vector Geom::ConfigurationDifference(const vector &q1, 
00046                                      const vector &q2)
00047 {
00048   return (q2 - q1);  // By default, assume no topological problems
00049 }
00050 
00051 
00052 // *********************************************************************
00053 // *********************************************************************
00054 // CLASS:     GeomLedaPolygonal
00055 // 
00056 // *********************************************************************
00057 // *********************************************************************
00058 
00059 // Constructor
00060 GeomLedaPolygonal::GeomLedaPolygonal(string path = ""):Geom(path) {
00061 
00062   NumBodies = 1;
00063   
00064   if (is_file(FilePath + "Obst")) {
00065     file_istream fin(FilePath + "Obst");
00066     fin >> Obst;
00067   }
00068 
00069   MaxDeviates = vector(1.0,1.0);  // Translation only
00070 }
00071 
00072 
00073 
00074 // This simple collision checker uses LEDA's p.inside method.
00075 // Certainly more efficient methods exist (incremental).
00076 bool GeomLedaPolygonal::CollisionFree(const vector &x) {
00077   polygon p;
00078 
00079   forall(p,Obst) {
00080     if (p.inside(point(x))) {
00081       return false;
00082     }
00083   }
00084 
00085   return true;
00086 }
00087 
00088 
00089 double GeomLedaPolygonal::DistanceComp(const vector &x) {
00090   polygon p;
00091   double d_min,d;
00092 
00093   d_min = INFINITY;
00094 
00095   if (CollisionFree(x)) {
00096     forall(p,Obst) {
00097       d = p.distance(point(x));
00098       if (d < d_min)
00099         d_min = d;
00100     }
00101   }
00102   else
00103     d_min = -1.0;
00104   
00105   return d_min;
00106 }
00107 
00108 
00109 
00110 list<polygon> GeomLedaPolygonal::EnvironmentToLedaPolygons() {
00111   return Obst;
00112 }
00113 
00114 
00115 
00116 list<polygon> GeomLedaPolygonal::RobotToLedaPolygons(const vector &x) {
00117   list<polygon> trobot;
00118 
00119   point p1(0.5,0.5);
00120   point p2(-0.5,0.5);
00121   point p3(-0.5,-0.5);
00122   point p4(0.5,-0.5);
00123   list<point> pl;
00124   pl.push(p4); pl.push(p3); pl.push(p2); pl.push(p1);
00125   polygon p(pl);
00126   trobot.push(p.translate(x));
00127   
00128   return trobot;
00129 }
00130 
00131  
00132 
00133 // *********************************************************************
00134 // *********************************************************************
00135 // CLASS:     GeomLedaPolygonalRigid
00136 // 
00137 // *********************************************************************
00138 // *********************************************************************
00139 
00140 // Constructor
00141 GeomLedaPolygonalRigid::GeomLedaPolygonalRigid(string path = ""):GeomLedaPolygonal(path) {
00142 
00143   if ((path.length() > 0)&&(path[path.length()-1] != '/'))
00144     path += "/";
00145 
00146   FilePath = path;
00147   
00148   if (is_file(FilePath + "Robot")) {
00149     file_istream fin(FilePath + "Robot");
00150     fin >> Robot;
00151   }
00152 
00153   // Compute the maximum deviates
00154   double dmax = 0.0;
00155   point pt;
00156   polygon p;
00157   forall(p,Robot) {
00158     forall_vertices(pt,p) {
00159       if (pt.distance() > dmax)
00160         dmax = pt.distance();
00161     }
00162   }
00163   MaxDeviates = vector(1.0,1.0,dmax);
00164 
00165 }
00166 
00167 
00168 // This is a very naive collision checker
00169 // This should certainly be replaced some day with a 
00170 // a more efficient algorithm.  It would be best to add
00171 // a 2D incremental distance algorithm, based on the
00172 // ideas of Lin and Canny, and Mirtich.  One could also
00173 // use bitmaps since this is only 2D.
00174 
00175 bool GeomLedaPolygonalRigid::CollisionFree(const vector &x) {
00176   polygon op,rp;
00177   segment seg,seg2;
00178 
00179   list<polygon> trobot = RobotToLedaPolygons(x);
00180 
00181   forall(rp,trobot) {
00182     forall(op,Obst) {
00183       forall_segments(seg,rp) {
00184         forall_segments(seg2,op) {
00185           if (seg.intersection(seg2)) {
00186             return false;
00187           }
00188         }
00189       }
00190     }
00191   }
00192 
00193   // Check for containment
00194   forall(rp,trobot) {
00195     forall(op,Obst) {
00196       if (op.contains(rp.vertices().head()))
00197         return false;
00198     }
00199   }
00200 
00201   return true;
00202 }
00203 
00204 
00205 // This should certainly be replaced some day with a 
00206 // a more efficient algorithm.  It would be best to add
00207 // a 2D incremental distance algorithm, based on the
00208 // ideas of Lin and Canny, and Mirtich.
00209 
00210 double GeomLedaPolygonalRigid::DistanceComp(const vector &x) {
00211   polygon rp,op;
00212   double d_min,d;
00213   point rpt;
00214 
00215   d_min = INFINITY;
00216 
00217   list<polygon> trobot = RobotToLedaPolygons(x);
00218 
00219   // This is obviously inefficient
00220   if (CollisionFree(x)) {
00221     forall(rp,trobot) {
00222       forall_vertices(rpt,rp) {
00223         forall(op,Obst) {
00224           d = op.distance(rpt);
00225           if (d < d_min)
00226             d_min = d;
00227         }
00228       }
00229     }
00230   }
00231   else
00232     d_min = -1.0;
00233   
00234   return d_min;
00235 }
00236 
00237 
00238 
00239 list<polygon> GeomLedaPolygonalRigid::RobotToLedaPolygons(const vector &q) {
00240   list<polygon> trobot;
00241   polygon p,tp;
00242 
00243   forall(p,Robot) {
00244     tp = p.rotate(q[2]).translate(q[0],q[1]);
00245     trobot.push_front(tp);
00246   }
00247 
00248   return trobot;
00249 }
00250 
00251 
00252 
00253 vector GeomLedaPolygonalRigid::ConfigurationDifference(const vector &q1, 
00254                                            const vector &q2)
00255 {
00256   vector dq(3);
00257 
00258   dq[0] = q2[0] - q1[0];
00259   dq[1] = q2[1] - q1[1];
00260 
00261   if (fabs(q1[2]-q2[2]) < PI)
00262     dq[2] = q2[2] - q1[2];
00263   else {
00264     if (q1[2] < q2[2])
00265       dq[2] = -(2.0*PI - fabs(q1[2]-q2[2]));
00266     else
00267       dq[2] = (2.0*PI - fabs(q1[2]-q2[2]));
00268   }
00269 
00270   return dq;
00271 }
00272 
00273 #include <LEDA/UNDEFINE_NAMES.h>
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.