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>