00001 #include "PlannerObstacles.h"
00002 #include "GJK.h"
00003 #include "Shared/debuget.h"
00004 #include "Shared/plistSpecialty.h"
00005 #include "Shared/mathutils.h"
00006
00007 const std::string RectangularObstacle::autoRegisterName = PlannerObstacle2D::getRegistry().registerType<RectangularObstacle>("Rectangle");
00008 const std::string CircularObstacle::autoRegisterName = PlannerObstacle2D::getRegistry().registerType<CircularObstacle>("Circle");
00009 const std::string EllipticalObstacle::autoRegisterName = PlannerObstacle2D::getRegistry().registerType<EllipticalObstacle>("Ellipse");
00010 const std::string ConvexPolyObstacle::autoRegisterName = PlannerObstacle2D::getRegistry().registerType<ConvexPolyObstacle>("ConvexPoly");
00011 const std::string HierarchicalObstacle::autoRegisterName = PlannerObstacle2D::getRegistry().registerType<HierarchicalObstacle>("Hierarchy");
00012 const std::string BoxObstacle::autoRegisterName = PlannerObstacle3D::getRegistry().registerType<BoxObstacle>("Box");
00013 const std::string CylindricalObstacle::autoRegisterName = PlannerObstacle3D::getRegistry().registerType<CylindricalObstacle>("Cylinder");
00014 const std::string SphericalObstacle::autoRegisterName = PlannerObstacle3D::getRegistry().registerType<SphericalObstacle>("Sphere");
00015 const std::string EllipsoidObstacle::autoRegisterName = PlannerObstacle3D::getRegistry().registerType<EllipsoidObstacle>("Ellipsoid");
00016
00017 PLIST_CLONE_IMP(RectangularObstacle,new RectangularObstacle(*this));
00018 PLIST_CLONE_IMP(CircularObstacle,new CircularObstacle(*this));
00019 PLIST_CLONE_IMP(EllipticalObstacle,new EllipticalObstacle(*this));
00020 PLIST_CLONE_IMP(ConvexPolyObstacle,new ConvexPolyObstacle(*this));
00021 PLIST_CLONE_IMP(HierarchicalObstacle,new HierarchicalObstacle(*this));
00022 PLIST_CLONE_IMP(BoxObstacle,new BoxObstacle(*this));
00023 PLIST_CLONE_IMP(CylindricalObstacle,new CylindricalObstacle(*this));
00024 PLIST_CLONE_IMP(SphericalObstacle,new SphericalObstacle(*this));
00025 PLIST_CLONE_IMP(EllipsoidObstacle,new EllipsoidObstacle(*this));
00026
00027 namespace plist {
00028 template<> PlannerObstacle2D* loadXML(xmlNode* node) {
00029 plist::Primitive<std::string> type;
00030 Dictionary td(false);
00031 td.setUnusedWarning(false);
00032 td.addEntry(".type",type);
00033 td.loadXML(node);
00034 PlannerObstacle2D* po = PlannerObstacle2D::getRegistry().create(type);
00035 if(po==NULL)
00036 throw XMLLoadSave::bad_format(node,"Unknown PlannerObstacle2D type "+type);
00037 try {
00038 po->loadXML(node);
00039 } catch(...) {
00040 delete po;
00041 throw;
00042 }
00043 return po;
00044 }
00045 template<> PlannerObstacle3D* loadXML(xmlNode* node) {
00046 plist::Primitive<std::string> type;
00047 Dictionary td(false);
00048 td.setUnusedWarning(false);
00049 td.addEntry(".type",type);
00050 td.loadXML(node);
00051 PlannerObstacle3D* po = PlannerObstacle3D::getRegistry().create(type);
00052 if(po==NULL)
00053 throw XMLLoadSave::bad_format(node,"Unknown PlannerObstacle3D type "+type);
00054 try {
00055 po->loadXML(node);
00056 } catch(...) {
00057 delete po;
00058 throw;
00059 }
00060 return po;
00061 }
00062 }
00063
00064
00065 template <>
00066 bool PlannerObstacle2D::collides(const PlannerObstacle2D& other) const {
00067
00068
00069 switch(geometry * geometry * other.geometry) {
00070
00071 case RECTANGULAR_OBS * RECTANGULAR_OBS * RECTANGULAR_OBS:
00072 return static_cast<const RectangularObstacle*>(this)->collides(static_cast<const RectangularObstacle&>(other));
00073 case RECTANGULAR_OBS * RECTANGULAR_OBS * CIRCULAR_OBS:
00074 return static_cast<const RectangularObstacle*>(this)->collides(static_cast<const CircularObstacle&>(other));
00075 case RECTANGULAR_OBS * CIRCULAR_OBS * CIRCULAR_OBS:
00076 return static_cast<const RectangularObstacle*>(&other)->collides(static_cast<const CircularObstacle&>(*this));
00077
00078
00079 case CIRCULAR_OBS * CIRCULAR_OBS * CIRCULAR_OBS:
00080 return static_cast<const CircularObstacle*>(this)->collides(static_cast<const CircularObstacle&>(other));
00081
00082
00083 case CONVEX_POLY_OBS * CONVEX_POLY_OBS * RECTANGULAR_OBS:
00084 return static_cast<const ConvexPolyObstacle*>(this)->collides(static_cast<const RectangularObstacle&>(other));
00085 case CONVEX_POLY_OBS * RECTANGULAR_OBS * RECTANGULAR_OBS:
00086 return static_cast<const ConvexPolyObstacle*>(&other)->collides(static_cast<const RectangularObstacle&>(*this));
00087 case CONVEX_POLY_OBS * CONVEX_POLY_OBS * CIRCULAR_OBS:
00088 return static_cast<const ConvexPolyObstacle*>(this)->collides(static_cast<const CircularObstacle&>(other));
00089 case CONVEX_POLY_OBS * CIRCULAR_OBS * CIRCULAR_OBS:
00090 return static_cast<const ConvexPolyObstacle*>(&other)->collides(static_cast<const CircularObstacle&>(*this));
00091 case CONVEX_POLY_OBS * CONVEX_POLY_OBS * CONVEX_POLY_OBS:
00092 return static_cast<const ConvexPolyObstacle*>(this)->collides(static_cast<const ConvexPolyObstacle&>(other));
00093
00094
00095 case HIERARCHICAL_OBS * HIERARCHICAL_OBS * RECTANGULAR_OBS:
00096 return static_cast<const HierarchicalObstacle*>(this)->collides(static_cast<const RectangularObstacle&>(other));
00097 case HIERARCHICAL_OBS * RECTANGULAR_OBS * RECTANGULAR_OBS:
00098 return static_cast<const HierarchicalObstacle*>(&other)->collides(static_cast<const RectangularObstacle&>(*this));
00099 case HIERARCHICAL_OBS * HIERARCHICAL_OBS * CIRCULAR_OBS:
00100 return static_cast<const HierarchicalObstacle*>(this)->collides(static_cast<const CircularObstacle&>(other));
00101 case HIERARCHICAL_OBS * CIRCULAR_OBS * CIRCULAR_OBS:
00102 return static_cast<const HierarchicalObstacle*>(&other)->collides(static_cast<const CircularObstacle&>(*this));
00103 case HIERARCHICAL_OBS * HIERARCHICAL_OBS * ELLIPTICAL_OBS:
00104 return static_cast<const HierarchicalObstacle*>(this)->collides(static_cast<const EllipticalObstacle&>(other));
00105 case HIERARCHICAL_OBS * ELLIPTICAL_OBS * ELLIPTICAL_OBS:
00106 return static_cast<const HierarchicalObstacle*>(&other)->collides(static_cast<const EllipticalObstacle&>(*this));
00107 case HIERARCHICAL_OBS * HIERARCHICAL_OBS * CONVEX_POLY_OBS:
00108 return static_cast<const HierarchicalObstacle*>(this)->collides(static_cast<const ConvexPolyObstacle&>(other));
00109 case HIERARCHICAL_OBS * CONVEX_POLY_OBS * CONVEX_POLY_OBS:
00110 return static_cast<const HierarchicalObstacle*>(&other)->collides(static_cast<const ConvexPolyObstacle&>(*this));
00111 case HIERARCHICAL_OBS * HIERARCHICAL_OBS * HIERARCHICAL_OBS:
00112 return static_cast<const HierarchicalObstacle*>(this)->collides(static_cast<const HierarchicalObstacle&>(other));
00113 }
00114
00115
00116
00117 return GJK::collides(this, &other);
00118 }
00119
00120 template <>
00121 bool PlannerObstacle3D::collides(const PlannerObstacle3D& other) const {
00122
00123
00124 switch(geometry * geometry * other.geometry) {
00125
00126 case BOX_OBS * BOX_OBS * BOX_OBS:
00127 return static_cast<const BoxObstacle*>(this)->collides(static_cast<const BoxObstacle&>(other));
00128
00129
00130 case SPHERICAL_OBS * SPHERICAL_OBS * SPHERICAL_OBS:
00131 return static_cast<const SphericalObstacle*>(this)->collides(static_cast<const SphericalObstacle&>(other));
00132 }
00133
00134
00135
00136 return GJK::collides(this, &other);
00137 }
00138
00139 void RectangularObstacle::reset(fmat::Column<2> centerPoint,
00140 const fmat::SubVector<2,const fmat::fmatReal>& extents,
00141 const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot) {
00142 center = centerPoint;
00143
00144 fmat::Column<2> off = rot * extents;
00145 points.row(TOP_RIGHT) = &(center + off)[0];
00146 points.row(BOTTOM_LEFT) = &(center - off)[0];
00147 off = rot * fmat::pack(-extents[0],extents[1]);
00148 points.row(TOP_LEFT) = &(center + off)[0];
00149 points.row(BOTTOM_RIGHT) = &(center - off)[0];
00150
00151 bBox.min = &points.minR()[0];
00152 bBox.max = &points.maxR()[0];
00153
00154 unrot = rot.transpose();
00155 centerPoint = unrot * centerPoint;
00156 maxEx = centerPoint + extents;
00157 minEx = centerPoint - extents;
00158 }
00159
00160 void RectangularObstacle::updatePosition(const fmat::SubVector<2,const fmat::fmatReal>& newPos) {
00161 fmat::Column<2> diff = newPos - center;
00162 center = newPos;
00163 bBox.min += diff;
00164 bBox.max += diff;
00165 points.column(0)+=diff[0];
00166 points.column(1)+=diff[1];
00167
00168 diff = unrot * diff;
00169 minEx += diff;
00170 maxEx += diff;
00171 }
00172
00173 bool RectangularObstacle::collides(const RectangularObstacle& other) const {
00174 if( !bBox.collides(other.bBox) )
00175 return false;
00176 if(unrot(0,0)==1 && other.unrot(0,0)==1)
00177 return true;
00178
00179
00180 fmat::Matrix<4,2> p2 = other.points * unrot.transpose();
00181 float minX,maxX,minY,maxY;
00182 minX=p2.column(0).min();
00183 maxX=p2.column(0).max();
00184 minY=p2.column(1).min();
00185 maxY=p2.column(1).max();
00186 if(maxX <= minEx[0] || maxEx[0] <= minX
00187 || maxY <= minEx[1] || maxEx[1] <= minY)
00188 return false;
00189
00190 if(unrot(0,0)!=other.unrot(0,0)) {
00191
00192 p2 = points * other.unrot.transpose();
00193 minX=p2.column(0).min();
00194 maxX=p2.column(0).max();
00195 minY=p2.column(1).min();
00196 maxY=p2.column(1).max();
00197 if(maxX <= other.minEx[0] || other.maxEx[0] <= minX
00198 || maxY <= other.minEx[1] || other.maxEx[1] <= minY)
00199 return false;
00200 }
00201
00202 return true;
00203 }
00204
00205 bool RectangularObstacle::collides(const CircularObstacle& other) const {
00206 const fmat::Column<2> p = unrot * other.getCenter();
00207 const fmat::Column<2> pmin = p-other.getRadius(), pmax = p+other.getRadius();
00208
00209 if(pmax[0]<=minEx[0] || maxEx[0]<=pmin[0] || pmax[1]<=minEx[1] || maxEx[1]<=pmin[1]) {
00210 return false;
00211 } else if(p[0]<minEx[0]) {
00212 if(p[1]<minEx[1]) {
00213 return ((minEx - p).sumSq() < other.getRadius()*other.getRadius());
00214 } else if(maxEx[1]<p[1]) {
00215 return ((fmat::pack(minEx[0],maxEx[1]) - p).sumSq() < other.getRadius()*other.getRadius());
00216 }
00217 return true;
00218 } else if(maxEx[0] < p[0]) {
00219 if(p[1]<minEx[1]) {
00220 return ((fmat::pack(maxEx[0],minEx[1]) - p).sumSq() < other.getRadius()*other.getRadius());
00221 } else if(maxEx[1]<p[1]) {
00222 return ((maxEx - p).sumSq() < other.getRadius()*other.getRadius());
00223 }
00224 return true;
00225 } else {
00226 return true;
00227 }
00228 }
00229
00230 void RectangularObstacle::rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00231 const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot) {
00232 fmat::Column<2> newCenter = rot * (center - origin) + origin;
00233 reset(newCenter, getExtents(), unrot.transpose() * rot);
00234 }
00235
00236 fmat::Column<2> RectangularObstacle::gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const {
00237 const fmat::Column<2> p = unrot * pt;
00238
00239 if(p[0]<minEx[0]) {
00240 if(p[1]<minEx[1]) {
00241 return fmat::Column<2>(points.row(BOTTOM_LEFT).transpose()) - pt;
00242 } else if(maxEx[1]<p[1]) {
00243 return fmat::Column<2>(points.row(TOP_LEFT).transpose()) - pt;
00244 }
00245 return unrot.row(0).transpose() * (minEx[0]-p[0]);
00246 } else if(maxEx[0] < p[0]) {
00247 if(p[1]<minEx[1]) {
00248 return fmat::Column<2>(points.row(BOTTOM_RIGHT).transpose()) - pt;
00249 } else if(maxEx[1]<p[1]) {
00250 return fmat::Column<2>(points.row(TOP_RIGHT).transpose()) - pt;
00251 }
00252 return unrot.row(0).transpose() * (maxEx[0]-p[0]);
00253 } else if(p[1]<minEx[1]) {
00254 return unrot.row(1).transpose() * (minEx[1]-p[1]);
00255 } else if(maxEx[1]<p[1]) {
00256 return unrot.row(1).transpose() * (maxEx[1]-p[1]);
00257 } else {
00258
00259 fmat::fmatReal dist[4];
00260 dist[0] = p[0]-minEx[0];
00261 dist[1] = p[1]-minEx[1];
00262 dist[2] = maxEx[0]-p[0];
00263 dist[3] = maxEx[1]-p[1];
00264 if(dist[0] < dist[1]) {
00265 if(dist[0] < dist[2]) {
00266 if(dist[0] < dist[3]) {
00267 return unrot.row(0).transpose() * (minEx[0]-p[0]);
00268 }
00269 } else {
00270 if(dist[2] < dist[3]) {
00271 return unrot.row(0).transpose() * (maxEx[0]-p[0]);
00272 }
00273 }
00274 } else {
00275 if(dist[1] < dist[2]) {
00276 if(dist[1] < dist[3]) {
00277 return unrot.row(1).transpose() * (minEx[1]-p[1]);
00278 }
00279 } else {
00280 if(dist[2] < dist[3]) {
00281 return unrot.row(0).transpose() * (maxEx[0]-p[0]);
00282 }
00283 }
00284 }
00285 return unrot.row(1).transpose() * (maxEx[1]-p[1]);
00286 }
00287 }
00288
00289 std::string RectangularObstacle::toString() const {
00290 std::ostringstream os;
00291 os << "RectangularObstacle[" << name << ",points=" << points.fmt() << ",unrot=" << unrot.fmt() << "]";
00292 return os.str();
00293 }
00294
00295 bool RectangularObstacle::collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const {
00296 const fmat::Column<2> p2 = unrot * point;
00297 return (minEx[0]<p2[0] && p2[0]<maxEx[0] &&
00298 minEx[1]<p2[1] && p2[1]<maxEx[1]);
00299 }
00300
00301 fmat::Column<2> RectangularObstacle::getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const {
00302 const fmat::Column<2> dir = unrot * direction;
00303
00304 if (dir[0]>0) {
00305 if (dir[1]>0)
00306 return points.row(TOP_RIGHT).transpose();
00307 else
00308 return points.row(BOTTOM_RIGHT).transpose();
00309 }
00310 else {
00311 if (dir[1]>0)
00312 return points.row(TOP_LEFT).transpose();
00313 else
00314 return points.row(BOTTOM_LEFT).transpose();
00315 }
00316 }
00317
00318 void RectangularObstacle::bloat(float amount) {
00319 reset(center,maxEx+amount,unrot.transpose());
00320 }
00321
00322
00323 void RectangularObstacle::contract(float amount) {
00324 bloat(-amount);
00325 }
00326
00327 void RectangularObstacle::loadXML(xmlNode* node) {
00328
00329 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(2,0,false);
00330 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > d(2,0,false);
00331 plist::Angle ang;
00332 addEntry("Center",c);
00333 addEntry("Dimensions",d);
00334 addEntry("Orientation",ang);
00335 PlannerObstacle2D::loadXML(node);
00336 removeEntry("Center");
00337 removeEntry("Dimensions");
00338 removeEntry("Orientation");
00339 reset(fmat::pack(c[0],c[1]), fmat::pack(d[0]/2,d[1]/2), ang);
00340 }
00341 void RectangularObstacle::saveXML(xmlNode * node) const {
00342
00343 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(2,0,false);
00344 center.exportTo(c);
00345 c.setSaveInlineStyle(true);
00346 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > d(2,0,false);
00347 d[0] = getWidth();
00348 d[1] = getHeight();
00349 d.setSaveInlineStyle(true);
00350 plist::Angle ang = getOrientation();
00351 plist::Dictionary tmp(*this);
00352 tmp.addEntry("Center",c,"Center point");
00353 tmp.addEntry("Dimensions",d,"Width and height");
00354 tmp.addEntry("Orientation",ang,"Rotation (radians unless you specify ° suffix)");
00355 tmp.saveXML(node);
00356 }
00357
00358 float RectangularObstacle::getOrientation() const {
00359 return std::atan2(unrot(0,1),unrot(0,0));
00360 }
00361
00362 fmat::Column<2> CircularObstacle::gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const {
00363 fmat::Column<2> v = pt - center;
00364 fmat::fmatReal n = v.norm();
00365 if(n == 0)
00366 return fmat::pack(radius,0);
00367 return v * ((radius - n)/n);
00368 }
00369
00370 std::string CircularObstacle::toString() const {
00371 std::ostringstream os;
00372 os << "CircularObstacle[" << name << ",center=" << center << ",radius=" << radius << "]";
00373 return os.str();
00374 }
00375
00376 bool CircularObstacle::collides(const CircularObstacle& other) const {
00377 const fmat::Column<2> diff = center - other.center;
00378 const float r = radius + other.radius;
00379 return diff.sumSq() < r*r;
00380 }
00381
00382 fmat::Column<2> CircularObstacle::getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const {
00383 return center + (direction / direction.norm() * radius);
00384 }
00385
00386 void CircularObstacle::loadXML(xmlNode* node) {
00387
00388 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(2,0,false);
00389 plist::Primitive<fmat::fmatReal> r;
00390 addEntry("Center",c);
00391 addEntry("Radius",r);
00392 PlannerObstacle2D::loadXML(node);
00393 removeEntry("Center");
00394 removeEntry("Radius");
00395 center.importFrom(c);
00396 radius = r;
00397 }
00398 void CircularObstacle::saveXML(xmlNode * node) const {
00399
00400 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(2,0,false);
00401 c.setSaveInlineStyle(true);
00402 center.exportTo(c);
00403 plist::Primitive<fmat::fmatReal> r = radius;
00404 plist::Dictionary tmp(*this);
00405 tmp.addEntry("Center",c);
00406 tmp.addEntry("Radius",r);
00407 tmp.saveXML(node);
00408 }
00409
00410 void EllipticalObstacle::reset(const fmat::Column<2>& f1, const fmat::Column<2>& f2, fmat::fmatReal s) {
00411 center = ( (focus1 = f1) + (focus2 = f2) ) / 2;
00412 semimajor = std::abs(s);
00413 fmat::fmatReal d = s*s - (f1-center).sumSq();
00414 if(d<0)
00415 throw std::invalid_argument("EllipticalObstacle::reset with two foci and too-short semimajor");
00416 semiminor = std::sqrt(d);
00417 }
00418
00419 void EllipticalObstacle::reset(const fmat::Column<2>& c, fmat::fmatReal _semimajor, fmat::fmatReal _semiminor,
00420 fmat::fmatReal orientation) {
00421 center = c;
00422 semimajor = _semimajor;
00423 semiminor = _semiminor;
00424 if(semimajor < semiminor) {
00425 std::swap(semimajor, semiminor);
00426 orientation+=static_cast<fmat::fmatReal>(M_PI_2);
00427 }
00428 fmat::fmatReal focusDist = std::sqrt(semimajor*semimajor - semiminor*semiminor);
00429 fmat::Column<2> ax = fmat::pack(focusDist*std::cos(orientation),focusDist*std::sin(orientation));
00430 focus1 = center + ax;
00431 focus2 = center - ax;
00432 }
00433
00434 fmat::Column<2> EllipticalObstacle::getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const {
00435
00436 if (semimajor == semiminor)
00437 return center + (direction / direction.norm() * semimajor);
00438
00439
00440 fmat::Column<2> dir = getOrientation().transpose() * direction;
00441
00442
00443 if (dir[0] == 0) {
00444 return getOrientation() * fmat::pack(0, sgn(dir[1]) * semiminor) + center;
00445 }
00446 else if (dir[1] == 0) {
00447 return getOrientation() * fmat::pack(sgn(dir[0]) * semimajor, 0) + center;
00448 }
00449
00450
00451 fmat::fmatReal a2 = semimajor*semimajor, b2 = semiminor*semiminor;
00452
00453
00454 fmat::fmatReal k = (dir[1]*b2)/(dir[0]*a2);
00455 fmat::fmatReal x = sgn(dir[0]) * std::sqrt(1/((1/(a2)) + (k*k/(b2))));
00456
00457 return getOrientation() * fmat::pack(x, k*x) + center;
00458 }
00459
00460 void EllipticalObstacle::updatePosition(const fmat::SubVector<2,const fmat::fmatReal>& newPos) {
00461 fmat::Column<2> diff = newPos - center;
00462 center = newPos;
00463 focus1 += diff;
00464 focus2 += diff;
00465 }
00466
00467 void EllipticalObstacle::rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00468 const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot) {
00469 focus1 = rot * (focus1 - origin) + origin;
00470 focus2 = rot * (focus2 - origin) + origin;
00471 center = (focus1+focus2)/2;
00472 }
00473
00474 fmat::Matrix<2,2> EllipticalObstacle::getOrientation() const {
00475 if(semimajor==semiminor)
00476 return fmat::Matrix<2,2>::identity();
00477 fmat::fmatReal x[4] = { focus1[0]-center[0], focus1[1]-center[1] };
00478 fmat::fmatReal n = fmat::SubVector<2>(x).norm();
00479 x[0]/=n; x[1]/=n; x[2]=-x[1]; x[3]=x[0];
00480 return fmat::Matrix<2,2>(x);
00481 }
00482
00483 fmat::Column<2> EllipticalObstacle::getPointOnEdge(const fmat::Column<2>& direction) const {
00484 fmat::Matrix<2,2> rot = getOrientation();
00485 fmat::Column<2> x = rot.transpose() * direction;
00486 fmat::fmatReal ratio = semimajor/semiminor;
00487 x[1]*=ratio;
00488 x *= semimajor/x.norm();
00489 x[1]/=ratio;
00490 return rot * x + center;
00491 }
00492
00493 BoundingBox2D EllipticalObstacle::getBoundingBox() const {
00494 float orientation = getAngle();
00495 float t_x = std::atan(-semiminor * tan(orientation) / semimajor);
00496 float t_y = std::atan( semiminor / tan(orientation) / semimajor);
00497 BoundingBox2D b;
00498
00499 float o_sin = std::sin(orientation), t_x_sin = std::sin(t_x), t_y_sin = std::sin(t_y);
00500 float o_cos = std::cos(orientation), t_x_cos = std::cos(t_x), t_y_cos = std::cos(t_y);
00501 b.expand(fmat::pack(center[0] + semimajor*t_x_cos*o_cos - semiminor*t_x_sin*o_sin,
00502 center[1] + semiminor*t_y_sin*o_cos + semimajor*t_y_cos*o_sin));
00503
00504 b.expand(fmat::pack(center[0] - semimajor*t_x_cos*o_cos + semiminor*t_x_sin*o_sin,
00505 center[1] - semiminor*t_y_sin*o_cos - semimajor*t_y_cos*o_sin));
00506 return b;
00507 }
00508
00509 fmat::Column<2> EllipticalObstacle::gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const {
00510
00511 if (semimajor == semiminor)
00512 return getPointOnEdge(pt - center) - pt;
00513
00514 fmat::Column<2> newPt = getOrientation().transpose() * (pt - center);
00515
00516 float a = (semimajor*semimajor - semiminor*semiminor);
00517 float b = semimajor * newPt[0];
00518 float c = semiminor * newPt[1];
00519
00520
00521
00522 float aa = a*a, bb = b*b;
00523
00524 float a3 = -2*b/a;
00525 float a2 = (bb-aa+c*c)/aa;
00526 float a1 = -a3;
00527 float a0 = -bb/aa;
00528
00529 float a2a2 = a2*a2, a3a3 = a3*a3;
00530
00531 float t1 = -a3/4;
00532 float t2 = a2a2 - 3*a3*a1 + 12*a0;
00533 float t3 = (2*a2a2*a2 - 9*a3*a2*a1 + 27*a1*a1 + 27*a3a3*a0 - 72*a2*a0)/2;
00534 float t4 = (-a3a3*a3 + 4*a3*a2 - 8*a1)/32;
00535 float t5 = (3*a3a3 - 8*a2)/48;
00536
00537 float s1 = std::sqrt(t3*t3 - std::pow(t2,3));
00538 float s2 = std::pow(t3 + s1, 1.0f/3.0f);
00539 float s3 = (1.0f/12.0f)*(t2/s2 + s2);
00540 float s4 = std::sqrt(t5 + s3);
00541 float s5 = 2*t5 - s3;
00542 float s6 = t4/s4;
00543
00544 float sq1 = std::sqrt(s5 - s6);
00545 float sq2 = std::sqrt(s5 + s6);
00546
00547 float sols[8];
00548
00549 sols[0] = std::acos(t1 - s4 - sq1);
00550 sols[1] = std::acos(t1 - s4 + sq1);
00551 sols[2] = std::acos(t1 + s4 - sq2);
00552 sols[3] = std::acos(t1 + s4 + sq2);
00553
00554 if (std::isnan(sols[0]) && std::isnan(sols[1]) && std::isnan(sols[2]) && std::isnan(sols[3])) {
00555
00556 return fmat::pack(0,0);
00557 }
00558
00559
00560 if (newPt[1] < 0) {
00561 for (int i = 0; i < 4; i++)
00562 sols[i] = -sols[i];
00563 }
00564
00565 float ori = getAngle(), so = std::sin(ori), co = std::cos(ori);
00566
00567 fmat::Column<2> grads[4];
00568 float minMag = std::numeric_limits<float>::infinity();
00569 int minMagIndex = 0;
00570 for (int i = 0; i < 4; i++) {
00571 if (std::isnan(sols[i])) continue;
00572 float st = std::sin(sols[i]), ct = std::cos(sols[i]);
00573 grads[i] = fmat::pack(semimajor*ct*co - semiminor*st*so,
00574 semimajor*ct*so + semiminor*st*co) + center - pt;
00575 float sumSq = grads[i].sumSq();
00576 if (sumSq < minMag) {
00577 minMag = sumSq;
00578 minMagIndex = i;
00579 }
00580 }
00581
00582 return grads[minMagIndex];
00583 }
00584
00585 std::string EllipticalObstacle::toString() const {
00586 std::ostringstream os;
00587 os << "EllipticalObstacle[" << name << ",focus1=" << focus1 << ",focus2=" << focus2 << "," << std::endl
00588 << " center=" << center << ",semimajor=" << semimajor << ",semiminor=" << semiminor << std::endl
00589 << " orientation=" << getAngle() << "]";
00590 return os.str();
00591 }
00592
00593 void EllipticalObstacle::loadXML(xmlNode* node) {
00594
00595 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(2,0,false);
00596 plist::Primitive<fmat::fmatReal> smajor;
00597 plist::Primitive<fmat::fmatReal> sminor;
00598 plist::Angle ori;
00599 addEntry("Center",c);
00600 addEntry("Orientation",ori);
00601 addEntry("Semimajor",smajor);
00602 addEntry("Semiminor",sminor);
00603 PlannerObstacle2D::loadXML(node);
00604 removeEntry("Center");
00605 removeEntry("Orientation");
00606 removeEntry("Semimajor");
00607 removeEntry("Semiminor");
00608 center.importFrom(c);
00609 reset(center,smajor,sminor,ori);
00610 }
00611
00612 void EllipticalObstacle::saveXML(xmlNode * node) const {
00613
00614 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(2,0,false);
00615 c.setSaveInlineStyle(true);
00616 center.exportTo(c);
00617 plist::Primitive<fmat::fmatReal> smajor = semimajor;
00618 plist::Primitive<fmat::fmatReal> sminor = semiminor;
00619 plist::Angle ori = getAngle();
00620 plist::Dictionary tmp(*this);
00621 tmp.addEntry("Center",c);
00622 tmp.addEntry("Orientation",ori);
00623 tmp.addEntry("Semimajor",smajor);
00624 tmp.addEntry("Semiminor",sminor);
00625 tmp.saveXML(node);
00626 }
00627
00628 void ConvexPolyObstacle::hull(const std::set<fmat::Column<2> >& p) {
00629 size_t k = 0;
00630 points.resize(p.size()+1);
00631
00632
00633 for(std::set<fmat::Column<2> >::const_iterator it=p.begin(); it!=p.end(); ++it) {
00634 while(k>=2 && fmat::crossProduct(points[k-1]-points[k-2], *it-points[k-2])[2] <= 0)
00635 --k;
00636 points[k++] = *it;
00637 }
00638
00639
00640 const size_t t = k+1;
00641 std::set<fmat::Column<2> >::const_reverse_iterator it=p.rbegin();
00642 for(++it; it!=p.rend(); ++it) {
00643 while(k >= t && fmat::crossProduct(points[k-1]-points[k-2], *it-points[k-2])[2] <= 0)
00644 --k;
00645 points[k++] = *it;
00646 }
00647
00648
00649 normals.resize(k-1);
00650 for(size_t i=0; i<k-1; ++i) {
00651 fmat::Column<2> d = points[i+1] - points[i];
00652
00653 normals[i] = (fmat::pack(d[1],-d[0]) / d.norm());
00654 }
00655 points.resize(k-1);
00656 }
00657
00658 void ConvexPolyObstacle::addPoint(const fmat::Column<2>& p) {
00659 if(points.size()>0) {
00660 fmat::Column<2> d = p - points.back();
00661
00662 d = fmat::pack(d[1],-d[0]) / d.norm();
00663 if(normals.size()==points.size())
00664 normals.back() = d;
00665 else
00666 normals.push_back(d);
00667 }
00668 points.push_back(p);
00669 }
00670
00671 void ConvexPolyObstacle::close() {
00672 ASSERTRET(points.size()==normals.size()+1,"ConvexPolyObstacle already closed: " << points.size() << " points " << normals.size() << " normals");
00673 fmat::Column<2> d = points.front() - points.back();
00674
00675 d = fmat::pack(d[1],-d[0]) / d.norm();
00676 normals.push_back(d);
00677 }
00678
00679 bool ConvexPolyObstacle::collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const {
00680 ASSERTRETVAL(points.size()>=3,"ConvexPolyObstacle: not enough points (" << points.size() << ", need at least 3)",false);
00681 ASSERTRETVAL(points.size()==normals.size(),"ConvexPolyObstacle not closed",false);
00682 for(size_t i=0; i<points.size(); ++i) {
00683 if(fmat::dotProduct(point-points[i],normals[i]) >= 0)
00684 return false;
00685 }
00686 return true;
00687 }
00688
00689 bool ConvexPolyObstacle::collides(const RectangularObstacle& other) const {
00690 ASSERTRETVAL(points.size()>=3,"ConvexPolyObstacle: not enough points (" << points.size() << ", need at least 3)",false);
00691 ASSERTRETVAL(points.size()==normals.size(),"ConvexPolyObstacle not closed",false);
00692 for(size_t i=0; i<points.size(); ++i) {
00693 fmat::Matrix<4,2> c(other.points);
00694 c.column(0)-=points[i][0];
00695 c.column(1)-=points[i][1];
00696 fmat::Column<4> d = c * normals[i];
00697 if(d.min() >= 0)
00698 return false;
00699 }
00700 fmat::Column<2> p = other.unrot * points[0];
00701 fmat::fmatReal minX=p[0],maxX=p[0],minY=p[1],maxY=p[1];
00702 for(size_t i=1; i<points.size(); ++i) {
00703 p = other.unrot * points[i];
00704 if(p[0]<minX)
00705 minX=p[0];
00706 else if(maxX<p[0])
00707 maxX=p[0];
00708 if(p[1]<minY)
00709 minY=p[1];
00710 else if(maxY<p[1])
00711 maxY=p[1];
00712 }
00713 return (other.minEx[0]<maxX && minX<other.maxEx[0] && other.minEx[1]<maxY && minY<other.maxEx[1]);
00714 }
00715
00716 bool ConvexPolyObstacle::collides(const CircularObstacle& other) const {
00717 ASSERTRETVAL(points.size()>=3,"ConvexPolyObstacle: not enough points (" << points.size() << ", need at least 3)",false);
00718 ASSERTRETVAL(points.size()==normals.size(),"ConvexPolyObstacle not closed",false);
00719 const fmat::fmatReal r2 = other.getRadius() * other.getRadius();
00720 bool outside = false;
00721 bool testedLast = false;
00722 for(size_t i=0; i<points.size(); ++i) {
00723 fmat::Column<2> v = other.getCenter()-points[i];
00724 fmat::fmatReal d = fmat::dotProduct(v,normals[i]);
00725 if(d >= other.getRadius())
00726 return false;
00727 if(d >= 0) {
00728 outside = true;
00729
00730 d = v[0]*-normals[i][1] + v[1]*normals[i][0];
00731
00732 if(d >= 0) {
00733 const fmat::Column<2>& next = (i+1==points.size() ? points[0] : points[i+1]);
00734 fmat::fmatReal len = (points[i] - next).norm();
00735 if(d <= len)
00736 return true;
00737 if(testedLast) {
00738 testedLast=false;
00739 } else if(d < len+other.getRadius()) {
00740
00741 if((other.getCenter() - next).sumSq() < r2)
00742 return true;
00743 testedLast=true;
00744 }
00745 } else {
00746 if(d > -other.getRadius()) {
00747
00748 if(v.sumSq() < r2)
00749 return true;
00750 }
00751 testedLast=false;
00752 }
00753 }
00754 }
00755 return !outside;
00756 }
00757
00758 bool ConvexPolyObstacle::collides(const ConvexPolyObstacle& other) const {
00759 ASSERTRETVAL(points.size()>=3,"ConvexPolyObstacle: not enough points (" << points.size() << ", need at least 3)",false);
00760 ASSERTRETVAL(points.size()==normals.size(),"ConvexPolyObstacle not closed",false);
00761 ASSERTRETVAL(other.points.size()==other.normals.size(),"ConvexPolyObstacle not closed",false);
00762 for(size_t i=0; i<points.size(); ++i) {
00763 bool inter=false;
00764 for(size_t j=0; j<other.points.size(); ++j) {
00765 if(fmat::dotProduct(other.points[j]-points[i],normals[i]) < 0) {
00766 inter=true;
00767 break;
00768 }
00769 }
00770 if(!inter)
00771 return false;
00772 }
00773 for(size_t i=0; i<other.points.size(); ++i) {
00774 bool inter=false;
00775 for(size_t j=0; j<points.size(); ++j) {
00776 if(fmat::dotProduct(points[j]-other.points[i],other.normals[i]) < 0) {
00777 inter=true;
00778 break;
00779 }
00780 }
00781 if(!inter)
00782 return false;
00783 }
00784 return true;
00785 }
00786
00787 fmat::Column<2> ConvexPolyObstacle::getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const {
00788 ASSERTRETVAL(points.size()>0,"ConvexPolyObstacle: no points",fmat::pack(0,0));
00789 fmat::fmatReal max = fmat::dotProduct(direction, points[0]);
00790 int maxIndex = 0;
00791 for (unsigned int i = 1; i < points.size(); i++) {
00792 fmat::fmatReal newMax = fmat::dotProduct(direction, points[i]);
00793 if (newMax > max) {
00794 max = newMax;
00795 maxIndex = i;
00796 }
00797 }
00798 return points[maxIndex];
00799 }
00800
00801 fmat::Column<2> ConvexPolyObstacle::getCenter() const {
00802 ASSERTRETVAL(points.size()>0,"ConvexPolyObstacle empty (" << points.size() << ", need at least 1 for getCenter())",fmat::Column<2>());
00803 fmat::Column<2> ans=points.front();
00804 for(size_t i=1; i<points.size(); ++i) {
00805 ans+=points[i];
00806 }
00807 return ans/points.size();
00808 }
00809
00810 void ConvexPolyObstacle::rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00811 const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot) {
00812 std::vector<fmat::Column<2> > tmpPoints = points;
00813 clear();
00814 for(size_t i=0; i<tmpPoints.size(); ++i) {
00815 addPoint(rot*(tmpPoints[i] - origin) + origin);
00816 }
00817 close();
00818 }
00819
00820 BoundingBox2D ConvexPolyObstacle::getBoundingBox() const {
00821 BoundingBox2D bb;
00822 for(size_t i=0; i<points.size(); ++i)
00823 bb.expand(points[i]);
00824 return bb;
00825 }
00826
00827 void ConvexPolyObstacle::offset(const fmat::Column<2>& off) {
00828 for(size_t i=0; i<points.size(); ++i) {
00829 points[i]+=off;
00830 }
00831 }
00832
00833 fmat::Column<2> ConvexPolyObstacle::gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const {
00834 ASSERTRETVAL(points.size()>=3,"ConvexPolyObstacle: not enough points (" << points.size() << ", need at least 3)",fmat::Column<2>());
00835 ASSERTRETVAL(points.size()==normals.size(),"ConvexPolyObstacle not closed",fmat::Column<2>());
00836 fmat::fmatReal closest2=std::numeric_limits<fmat::fmatReal>::infinity();
00837 fmat::Column<2> ans;
00838 bool testedLast = false;
00839 for(size_t i=0; i<points.size(); ++i) {
00840 fmat::Column<2> v = pt-points[i];
00841
00842 const fmat::fmatReal d = v[0]*-normals[i][1] + v[1]*normals[i][0];
00843
00844 if(d >= 0) {
00845 const fmat::Column<2>& next = (i+1==points.size() ? points[0] : points[i+1]);
00846 fmat::fmatReal len = (points[i] - next).norm();
00847 if(d <= len) {
00848 fmat::fmatReal n = fmat::dotProduct(v,normals[i]);
00849 fmat::fmatReal n2 = n*n;
00850 if(n2<closest2) {
00851 closest2 = n2;
00852 ans = normals[i]*-n;
00853 }
00854 }
00855 if(testedLast) {
00856 testedLast=false;
00857 } else if(d < len) {
00858
00859 fmat::fmatReal n2 = (next - pt).sumSq();
00860 if(n2 < closest2) {
00861 closest2 = n2;
00862 ans = next - pt;
00863 }
00864 testedLast=true;
00865 }
00866 } else {
00867
00868 fmat::fmatReal n2 = v.sumSq();
00869 if(n2 < closest2) {
00870 closest2 = n2;
00871 ans = -v;
00872 }
00873 testedLast=false;
00874 }
00875 }
00876 return ans;
00877 }
00878
00879 std::string ConvexPolyObstacle::toString() const {
00880 std::ostringstream os;
00881 os << "ConvexPolyObstacle[" << name << ",#points=" << points.size() << ",#normals=" << normals.size() << "]";
00882 return os.str();
00883 }
00884
00885
00886 void ConvexPolyObstacle::loadXML(xmlNode* node) {
00887
00888 plist::ArrayOf< plist::Point > ps;
00889 addEntry("Points",ps);
00890 PlannerObstacle2D::loadXML(node);
00891 removeEntry("Points");
00892 points.resize(ps.size());
00893 for(size_t i=0; i<ps.size(); ++i)
00894 points[i].importFrom(ps[i]);
00895 }
00896
00897 void ConvexPolyObstacle::saveXML(xmlNode * node) const {
00898
00899 plist::ArrayOf< plist::Point > ps;
00900 for(size_t i=0; i<ps.size(); ++i) {
00901 points[i].exportTo(ps[i]);
00902 ps.removeEntry(2);
00903 }
00904 plist::Dictionary tmp(*this);
00905 tmp.addEntry("Points",ps);
00906 tmp.saveXML(node);
00907 }
00908
00909 void HierarchicalObstacle::recalculateBoundingBox() {
00910 aabb = BoundingBox2D();
00911 for (std::vector<PlannerObstacle2D*>::iterator it = components.begin(); it != components.end(); ++it)
00912 expandBoundingBox(**it);
00913 }
00914
00915 void HierarchicalObstacle::expandBoundingBox(PlannerObstacle2D& other) {
00916 other.rotate(fmat::ZERO2, rotation);
00917 fmat::Column<2> oldCenter = other.getCenter();
00918 other.updatePosition(oldCenter+center);
00919
00920 aabb.expand(other.getBoundingBox());
00921
00922 other.updatePosition(oldCenter);
00923 other.rotate(fmat::ZERO2, rotation.transpose());
00924 }
00925
00926 bool HierarchicalObstacle::collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const {
00927
00928 if( !aabb.collides(point) )
00929 return false;
00930
00931
00932 const fmat::Column<2> newPt = rotation.transpose() * (point - center);
00933
00934
00935 for (unsigned int i = 0; i < components.size(); i++)
00936 if (components[i]->collides(newPt))
00937 return true;
00938
00939 return false;
00940 }
00941
00942 bool HierarchicalObstacle::collides(const PlannerObstacle2D& other) const {
00943
00944
00945
00946
00947
00948 PlannerObstacle2D* transformed = dynamic_cast<PlannerObstacle2D*>(other.clone());
00949 transformed->updatePosition(transformed->getCenter()-center);
00950 transformed->rotate(fmat::ZERO2, rotation.transpose());
00951
00952
00953 bool collision = false;
00954 for (unsigned int i = 0; i < components.size(); i++)
00955 if (components[i]->collides(*transformed)) {
00956 collision = true;
00957 break;
00958 }
00959 delete transformed;
00960
00961
00962
00963
00964
00965 return collision;
00966 }
00967
00968 void HierarchicalObstacle::updatePosition(const fmat::SubVector<2,const fmat::fmatReal>& newPos) {
00969 fmat::Column<2> diff = newPos - center;
00970 aabb.min += diff;
00971 aabb.max += diff;
00972 center = newPos;
00973 }
00974
00975 void HierarchicalObstacle::rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00976 const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot) {
00977 rotation = rot * rotation;
00978 center = rot * (center - origin) + origin;
00979 recalculateBoundingBox();
00980 }
00981
00982 fmat::Column<2> HierarchicalObstacle::gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const {
00983 fmat::Column<2> transformedPt = rotation.transpose() * (pt - center);
00984 ASSERTRETVAL(components.size()>0,"HierarchicalObstacle: no components, can't determine gradient",fmat::Column<2>());
00985 fmat::Column<2> minGradient;
00986 bool minSet = false;
00987 for (unsigned int i = 0; i < components.size(); ++i) {
00988 fmat::Column<2> newGradient = components[i]->gradient(transformedPt);
00989 bool collision = false;
00990 for (unsigned int j = 0; j < components.size(); ++j) {
00991 if (i == j) continue;
00992 if (components[j]->collides(transformedPt+newGradient)) {
00993 collision = true;
00994 break;
00995 }
00996 }
00997
00998 if (collision)
00999 continue;
01000
01001 if (!minSet) {
01002 minGradient = newGradient;
01003 minSet = true;
01004 }
01005 else if (newGradient.sumSq() < minGradient.sumSq())
01006 minGradient = newGradient;
01007 }
01008
01009 return rotation * minGradient;
01010 }
01011
01012 std::string HierarchicalObstacle::toString() const {
01013 std::ostringstream os;
01014 os << "HierarchicalObstacle[" << name << ",#components=" << components.size() << "]";
01015 return os.str();
01016 }
01017
01018 std::string HierarchicalObstacle::componentsToString() const {
01019 std::ostringstream os;
01020 os << toString() << std::endl;
01021 for (size_t i = 0; i < components.size(); i++) {
01022 HierarchicalObstacle* ho = dynamic_cast<HierarchicalObstacle*>(components[i]);
01023 if (ho)
01024 os << ho->componentsToString() << std::endl;
01025 else
01026 os << components[i]->toString() << std::endl;
01027 }
01028 return os.str();
01029 }
01030
01031 void BoxObstacle::reset(fmat::Column<3> centerPoint,
01032 const fmat::SubVector<3,const fmat::fmatReal>& extents,
01033 const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot) {
01034 center = centerPoint;
01035 fmat::Column<3> off = rot * extents;
01036 points.row(TOP_UPPER_RIGHT) = &(center + off)[0];
01037 points.row(BOTTOM_LOWER_LEFT) = &(center - off)[0];
01038 off = rot * fmat::pack(extents[0],-extents[1],extents[2]);
01039 points.row(TOP_LOWER_RIGHT) = &(center + off)[0];
01040 points.row(BOTTOM_UPPER_LEFT) = &(center - off)[0];
01041 off = rot * fmat::pack(-extents[0],-extents[1],extents[2]);
01042 points.row(TOP_LOWER_LEFT) = &(center + off)[0];
01043 points.row(BOTTOM_UPPER_RIGHT) = &(center - off)[0];
01044 off = rot * fmat::pack(-extents[0],extents[1],extents[2]);
01045 points.row(TOP_UPPER_LEFT) = &(center + off)[0];
01046 points.row(BOTTOM_LOWER_RIGHT) = &(center - off)[0];
01047
01048 bBox.min = &points.minR()[0];
01049 bBox.max = &points.maxR()[0];
01050
01051 unrot = rot.transpose();
01052 centerPoint = unrot * centerPoint;
01053 maxEx = centerPoint + extents;
01054 minEx = centerPoint - extents;
01055 }
01056
01057 void BoxObstacle::updatePosition(const fmat::SubVector<3, const fmat::fmatReal>& newPos) {
01058 fmat::Column<3> diff = newPos - center;
01059 center = newPos;
01060 bBox.min += diff;
01061 bBox.max += diff;
01062 points.column(0)+=diff[0];
01063 points.column(1)+=diff[1];
01064 points.column(2)+=diff[2];
01065
01066 diff = unrot * diff;
01067 minEx += diff;
01068 maxEx += diff;
01069 }
01070
01071 void BoxObstacle::rotate(const fmat::SubVector<3,const fmat::fmatReal>& origin,
01072 const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot) {
01073 fmat::Column<3> newCenter = rot * (center - origin) + origin;
01074 reset(newCenter, getExtents(), unrot.transpose() * rot);
01075 }
01076
01077 std::string BoxObstacle::toString() const {
01078 std::ostringstream os;
01079 os << "BoxObstacle[" << name << ",points=" << points.fmt() << ",unrot=" << unrot.fmt() << "]";
01080 return os.str();
01081 }
01082
01083 bool BoxObstacle::collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const {
01084 const fmat::Column<3> p2 = unrot * point;
01085 return (minEx[0]<p2[0] && p2[0]<maxEx[0] &&
01086 minEx[1]<p2[1] && p2[1]<maxEx[1] &&
01087 minEx[2]<p2[2] && p2[2]<maxEx[2]);
01088 }
01089
01090 bool BoxObstacle::collides(const BoxObstacle& other) const {
01091 float ra, rb;
01092 fmat::Matrix<3,3> r, absR;
01093
01094
01095 r = other.getOrientation() * unrot;
01096
01097
01098 fmat::Column<3> diff = unrot * (other.getCenter() - center);
01099
01100
01101 fmat::Column<3> extents = getExtents();
01102 fmat::Column<3> otherExtents = other.getExtents();
01103
01104
01105
01106
01107 for (int i = 0; i < 3; i++)
01108 for (int j = 0; j < 3; j++)
01109 absR(i,j) = std::abs(r(i,j)) + 1e-10;
01110
01111
01112 for (int i = 0; i < 3; i++) {
01113 ra = extents[i];
01114 rb = fmat::dotProduct(otherExtents, fmat::Row<3>(absR.row(i)));
01115 if (std::abs(diff[i]) > ra + rb) return false;
01116 }
01117
01118
01119 for (int i = 0; i < 3; i++) {
01120 ra = fmat::dotProduct(extents, absR.column(i));
01121 rb = otherExtents[i];
01122 if (std::abs(fmat::dotProduct(diff, r.column(i))) > ra + rb) return false;
01123 }
01124
01125
01126 ra = extents[1] * absR(2,0) + extents[2] * absR(1,0);
01127 rb = otherExtents[1] * absR(0,2) + otherExtents[2] * absR(0,1);
01128 if (std::abs(diff[2] * r(1,0) - diff[1] * r(2,0)) > ra + rb) return false;
01129
01130
01131 ra = extents[1] * absR(2,1) + extents[2] * absR(1,1);
01132 rb = otherExtents[0] * absR(0,2) + otherExtents[2] * absR(0,0);
01133 if (std::abs(diff[2] * r(1,1) - diff[1] * r(2,1)) > ra + rb) return false;
01134
01135
01136 ra = extents[1] * absR(2,2) + extents[2] * absR(1,2);
01137 rb = otherExtents[0] * absR(0,1) + otherExtents[1] * absR(0,0);
01138 if (std::abs(diff[2] * r(1,2) - diff[1] * r(2,2)) > ra + rb) return false;
01139
01140
01141 ra = extents[0] * absR(2,0) + extents[2] * absR(0,0);
01142 rb = otherExtents[1] * absR(1,2) + otherExtents[2] * absR(1,1);
01143 if (std::abs(diff[0] * r(2,0) - diff[2] * r(0,0)) > ra + rb) return false;
01144
01145
01146 ra = extents[0] * absR(2,1) + extents[2] * absR(0,1);
01147 rb = otherExtents[0] * absR(1,2) + otherExtents[2] * absR(1,0);
01148 if (std::abs(diff[0] * r(2,1) - diff[2] * r(0,1)) > ra + rb) return false;
01149
01150
01151 ra = extents[0] * absR(2,2) + extents[2] * absR(0,2);
01152 rb = otherExtents[0] * absR(1,1) + otherExtents[1] * absR(1,0);
01153 if (std::abs(diff[0] * r(2,2) - diff[2] * r(0,2)) > ra + rb) return false;
01154
01155
01156 ra = extents[0] * absR(1,0) + extents[1] * absR(0,0);
01157 rb = otherExtents[1] * absR(2,2) + otherExtents[2] * absR(2,1);
01158 if (std::abs(diff[1] * r(0,0) - diff[0] * r(1,0)) > ra + rb) return false;
01159
01160
01161 ra = extents[0] * absR(1,1) + extents[1] * absR(0,1);
01162 rb = otherExtents[0] * absR(2,2) + otherExtents[2] * absR(2,0);
01163 if (std::abs(diff[1] * r(0,1) - diff[0] * r(1,1)) > ra + rb) return false;
01164
01165
01166 ra = extents[0] * absR(1,2) + extents[1] * absR(0,2);
01167 rb = otherExtents[0] * absR(2,1) + otherExtents[1] * absR(2,0);
01168 if (std::abs(diff[1] * r(0,2) - diff[0] * r(1,2)) > ra + rb) return false;
01169
01170
01171 return true;
01172 }
01173
01174 fmat::Column<3> BoxObstacle::getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const {
01175 const fmat::Column<3> dir = unrot * direction;
01176
01177 if (dir[0]>0) {
01178 if (dir[1]>0) {
01179 if (dir[2]>0) {
01180 return points.row(TOP_UPPER_RIGHT).transpose();
01181 } else {
01182 return points.row(BOTTOM_UPPER_RIGHT).transpose();
01183 }
01184 } else {
01185 if (dir[2]>0) {
01186 return points.row(TOP_LOWER_RIGHT).transpose();
01187 } else {
01188 return points.row(BOTTOM_LOWER_RIGHT).transpose();
01189 }
01190 }
01191 } else {
01192 if (dir[1]>0) {
01193 if (dir[2]>0) {
01194 return points.row(TOP_UPPER_LEFT).transpose();
01195 } else {
01196 return points.row(BOTTOM_UPPER_LEFT).transpose();
01197 }
01198 } else {
01199 if (dir[2]>0) {
01200 return points.row(TOP_LOWER_LEFT).transpose();
01201 } else {
01202 return points.row(BOTTOM_LOWER_LEFT).transpose();
01203 }
01204 }
01205 }
01206 }
01207
01208 fmat::Column<3> BoxObstacle::gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const {
01209 fmat::Column<3> d = pt - center;
01210
01211 fmat::Column<3> q = center;
01212
01213 for (int i = 0; i < 3; i++) {
01214
01215
01216 float dist = fmat::dotProduct(d, fmat::Row<3>(unrot.row(i)));
01217
01218 if (dist > getExtents()[i]) dist = getExtents()[i];
01219 if (dist < -getExtents()[i]) dist = -getExtents()[i];
01220
01221 q += dist * getOrientation().column(i);
01222 }
01223 return q;
01224 }
01225
01226 void BoxObstacle::bloat(float amount) {
01227 reset(center,maxEx+amount,unrot.transpose());
01228 }
01229
01230 void BoxObstacle::loadXML(xmlNode* node) {
01231
01232 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01233 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > d(3,0,false);
01234 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > r(9,0,false);
01235 addEntry("Center",c);
01236 addEntry("Dimensions",d);
01237 addEntry("Orientation",r);
01238 PlannerObstacle3D::loadXML(node);
01239 removeEntry("Center");
01240 removeEntry("Dimensions");
01241 removeEntry("Orientation");
01242 fmat::Matrix<3,3> rot;
01243 reset(fmat::pack(c[0],c[1],c[2]), fmat::pack(d[0]/2,d[1]/2,d[2]/2), rot.importFromCMajor(r));
01244 }
01245
01246 void BoxObstacle::saveXML(xmlNode * node) const {
01247
01248 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01249 center.exportTo(c);
01250 c.setSaveInlineStyle(true);
01251 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > d(3,0,false);
01252 d[0] = getLength();
01253 d[1] = getWidth();
01254 d[2] = getHeight();
01255 d.setSaveInlineStyle(true);
01256 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > r(9,0,false);
01257 unrot.transpose().exportToCMajor(r);
01258 plist::Dictionary tmp(*this);
01259 tmp.addEntry("Center",c,"Center point");
01260 tmp.addEntry("Dimensions",d,"Width and height");
01261 tmp.addEntry("Orientation",r,"Rotation (3x3 Matrix condensed to float[9])");
01262 tmp.saveXML(node);
01263 }
01264
01265 BoundingBox3D CylindricalObstacle::getBoundingBox() const { return BoundingBox3D(); }
01266
01267 bool CylindricalObstacle::collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const {
01268 fmat::Column<3> newPt = orientation.transpose() * point;
01269 if (std::abs(newPt[2]) < halfHeight && std::sqrt(newPt[0]*newPt[0] + newPt[1]*newPt[1]) < radius)
01270 return true;
01271 else
01272 return false;
01273 }
01274
01275 fmat::Column<3> CylindricalObstacle::getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const {
01276 fmat::Column<3> dir = orientation.transpose() * direction;
01277 fmat::fmatReal sigma = std::sqrt(dir[0]*dir[0] + dir[1]*dir[1]);
01278 if (sigma > 0) {
01279 return orientation.transpose() *
01280 fmat::pack(radius / sigma * dir[0],
01281 radius / sigma * dir[1],
01282 sgn(dir[2]) * halfHeight);
01283 }
01284 else {
01285 return orientation.transpose() *
01286 fmat::pack(0, 0, sgn(dir[2]) * halfHeight);
01287 }
01288 }
01289
01290 fmat::Column<3> CylindricalObstacle::gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const { return fmat::Column<3>(); }
01291
01292 std::string CylindricalObstacle::toString() const {
01293 std::ostringstream os;
01294 os << "CylindricalObstacle[" << name << ",center=" << center << std::endl
01295 << ",orientation=" << orientation << std::endl << ",half-height=" << halfHeight << ",radius=" << radius << std::endl;
01296 return os.str();
01297 }
01298
01299 void CylindricalObstacle::loadXML(xmlNode* node) {
01300
01301 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01302 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > o(9,0,false);
01303 plist::Primitive<fmat::fmatReal> r;
01304 plist::Primitive<fmat::fmatReal> hh;
01305 addEntry("Center",c);
01306 addEntry("Orientation",o);
01307 addEntry("Radius",r);
01308 addEntry("HalfHeight",hh);
01309 PlannerObstacle3D::loadXML(node);
01310 removeEntry("Center");
01311 removeEntry("Orientation");
01312 removeEntry("Radius");
01313 removeEntry("HalfHeight");
01314 center.importFrom(c);
01315 radius = r;
01316 orientation.importFromCMajor(o);
01317 halfHeight = hh;
01318 }
01319
01320 void CylindricalObstacle::saveXML(xmlNode * node) const {
01321
01322 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01323 c.setSaveInlineStyle(true);
01324 center.exportTo(c);
01325 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > o(9,0,false);
01326 o.setSaveInlineStyle(true);
01327 orientation.exportToCMajor(o);
01328 plist::Primitive<fmat::fmatReal> r = radius;
01329 plist::Primitive<fmat::fmatReal> hh = halfHeight;
01330 plist::Dictionary tmp(*this);
01331 tmp.addEntry("Center",c);
01332 tmp.addEntry("Radius",r);
01333 tmp.addEntry("Orientation",o);
01334 tmp.addEntry("HalfHeight",hh);
01335 tmp.saveXML(node);
01336 }
01337
01338 bool SphericalObstacle::collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const {
01339
01340 return (point - center).sumSq() <= radius * radius;
01341 }
01342
01343 bool SphericalObstacle::collides(const SphericalObstacle& other) const {
01344 const fmat::Column<3> diff = center - other.center;
01345 const float r = radius + other.radius;
01346 return diff.sumSq() < r*r;
01347 }
01348
01349 fmat::Column<3> SphericalObstacle::getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const {
01350 return center + (direction / direction.norm() * radius);
01351 }
01352
01353 fmat::Column<3> SphericalObstacle::gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const {
01354 fmat::Column<3> v = pt - center;
01355 fmat::fmatReal n = v.norm();
01356 if(n == 0)
01357 return fmat::pack(radius,0,0);
01358 return v * ((radius - n)/n);
01359 }
01360
01361 std::string SphericalObstacle::toString() const {
01362 std::ostringstream os;
01363 os << "SphericalObstacle[" << name << ",center=" << center << ",radius=" << radius << "]" << std::endl;
01364 return os.str();
01365 }
01366
01367 void SphericalObstacle::loadXML(xmlNode* node) {
01368
01369 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01370 plist::Primitive<fmat::fmatReal> r;
01371 addEntry("Center",c);
01372 addEntry("Radius",r);
01373 PlannerObstacle3D::loadXML(node);
01374 removeEntry("Center");
01375 removeEntry("Radius");
01376 center.importFrom(c);
01377 radius = r;
01378 }
01379
01380 void SphericalObstacle::saveXML(xmlNode * node) const {
01381
01382 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01383 c.setSaveInlineStyle(true);
01384 center.exportTo(c);
01385 plist::Primitive<fmat::fmatReal> r = radius;
01386 plist::Dictionary tmp(*this);
01387 tmp.addEntry("Center",c);
01388 tmp.addEntry("Radius",r);
01389 tmp.saveXML(node);
01390 }
01391
01392 BoundingBox3D EllipsoidObstacle::getBoundingBox() const { return BoundingBox3D(); }
01393
01394 bool EllipsoidObstacle::collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const {
01395 fmat::Column<3> newPt = orientation.transpose() * point;
01396
01397
01398 newPt[0] /= extents[0];
01399 newPt[1] /= extents[1];
01400 newPt[2] /= extents[2];
01401 if (newPt.sumSq() < 1)
01402 return true;
01403 else
01404 return false;
01405 }
01406
01407 fmat::Column<3> EllipsoidObstacle::getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const {
01408 fmat::Column<3> dir = orientation.transpose() * direction;
01409
01410 if (dir[0] == 0) {
01411 fmat::Column<2> s = get2Support(fmat::pack(dir[1],dir[2]));
01412 return orientation * fmat::pack(0, s[0], s[1]) + center;
01413 }
01414 else if (dir[1] == 0) {
01415 fmat::Column<2> s = get2Support(fmat::pack(dir[0],dir[2]));
01416 return orientation * fmat::pack(s[0], 0, s[1]) + center;
01417 }
01418 else if (dir[2] == 0) {
01419 fmat::Column<2> s = get2Support(fmat::pack(dir[0],dir[1]));
01420 return orientation * fmat::pack(s[0], s[1], 0) + center;
01421 }
01422
01423
01424 fmat::fmatReal a2 = extents[0]*extents[0], b2 = extents[1]*extents[1], c2 = extents[2]*extents[2];
01425
01426
01427 fmat::fmatReal k1 = (dir[1]*b2)/(dir[0]*a2);
01428 fmat::fmatReal k2 = (dir[2]*c2)/(dir[0]*a2);
01429 fmat::fmatReal x = sgn(dir[0]) * std::sqrt(1/((1/(a2)) + (k1*k1/(b2)) + (k2*k2/(c2))));
01430
01431 return orientation * fmat::pack(x, k1*x, k2*x) + center;
01432 }
01433
01434 fmat::Column<2> EllipsoidObstacle::get2Support(const fmat::SubVector<2,const fmat::fmatReal>& direction) const {
01435
01436 if (direction[0] == 0) {
01437 return fmat::pack(0, sgn(direction[1]) * extents[1]);
01438 }
01439 else if (direction[1] == 0) {
01440 return fmat::pack(sgn(direction[0]) * extents[0], 0);
01441 }
01442
01443
01444 fmat::fmatReal a2 = extents[0]*extents[0], b2 = extents[1]*extents[1];
01445
01446
01447 fmat::fmatReal k = (direction[1]*b2)/(direction[0]*a2);
01448 fmat::fmatReal x = sgn(direction[0]) * std::sqrt(1/((1/(a2)) + (k*k/(b2))));
01449
01450 return fmat::pack(x, k*x);
01451 }
01452
01453 fmat::Column<3> EllipsoidObstacle::gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const { return fmat::Column<3>(); }
01454
01455 std::string EllipsoidObstacle::toString() const {
01456 std::ostringstream os;
01457 os << "EllipsoidObstacle[" << name << ",center=" << center << std::endl
01458 << ",orientation=" << orientation << std::endl << ",extents=" << extents << std::endl;
01459 return os.str();
01460 }
01461
01462 void EllipsoidObstacle::loadXML(xmlNode* node) {
01463
01464 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01465 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > o(9,0,false);
01466 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > e(3,0,false);
01467 addEntry("Center",c);
01468 addEntry("Orientation",o);
01469 addEntry("Extents",e);
01470 PlannerObstacle3D::loadXML(node);
01471 removeEntry("Center");
01472 removeEntry("Orientation");
01473 removeEntry("Extents");
01474 center.importFrom(c);
01475 orientation.importFromCMajor(o);
01476 extents.importFrom(e);
01477 }
01478
01479 void EllipsoidObstacle::saveXML(xmlNode * node) const {
01480
01481 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > c(3,0,false);
01482 c.setSaveInlineStyle(true);
01483 center.exportTo(c);
01484 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > o(9,0,false);
01485 o.setSaveInlineStyle(true);
01486 orientation.exportToCMajor(o);
01487 plist::ArrayOf< plist::Primitive<fmat::fmatReal> > e(3,0,false);
01488 e.setSaveInlineStyle(true);
01489 extents.exportTo(e);
01490 plist::Dictionary tmp(*this);
01491 tmp.addEntry("Center",c);
01492 tmp.addEntry("Orietation",o);
01493 tmp.addEntry("Extents",e);
01494 tmp.saveXML(node);
01495 }