Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PlannerObstacles.cc

Go to the documentation of this file.
00001 #include "PlannerObstacles.h"
00002 #include "GJK.h"
00003 #include "Shared/debuget.h"
00004 #include "Shared/plistSpecialty.h"
00005 #include "Shared/mathutils.h" // for isnan fix
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 // Collision dispatching
00065 template <>
00066 bool PlannerObstacle2D::collides(const PlannerObstacle2D& other) const {
00067   /* Although we have the generalized GJK algorithm for collision detection,
00068    we use analytic solutions that are fully tested and are faster than GJK. */
00069   switch(geometry * geometry * other.geometry) {
00070     // handled by rect: rect-rect, rect-circ
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     // handled by circ: circ-circ
00079     case CIRCULAR_OBS * CIRCULAR_OBS * CIRCULAR_OBS:
00080       return static_cast<const CircularObstacle*>(this)->collides(static_cast<const CircularObstacle&>(other));
00081       
00082     // handled by convex: convex-rect, convex-circ, convex-convex
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     // handled by hierarchical: hierarchical-rect, hierarchical-circ, hierarchical-ellipse, hierarchical-convex
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   /* For the shapes that don't have efficient or accurate analytic solutions
00116    we default to GJK.*/
00117   return GJK::collides(this, &other);
00118 }
00119 
00120 template <>
00121 bool PlannerObstacle3D::collides(const PlannerObstacle3D& other) const {
00122   /* Although we have the generalized GJK algorithm for collision detection,
00123    we use analytic solutions that are fully tested and are faster than GJK. */
00124   switch(geometry * geometry * other.geometry) {
00125     // handled by box: box-box
00126     case BOX_OBS * BOX_OBS * BOX_OBS:
00127       return static_cast<const BoxObstacle*>(this)->collides(static_cast<const BoxObstacle&>(other));
00128       
00129     // handled by sphere: sphere-sphere
00130     case SPHERICAL_OBS * SPHERICAL_OBS * SPHERICAL_OBS:
00131       return static_cast<const SphericalObstacle*>(this)->collides(static_cast<const SphericalObstacle&>(other));
00132   }
00133   
00134   /* For the shapes that don't have efficient or accurate analytic solutions
00135    we default to GJK.*/
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]; // 0 top right
00146   points.row(BOTTOM_LEFT) = &(center - off)[0]; // 2 bottom left
00147   off = rot * fmat::pack(-extents[0],extents[1]);
00148   points.row(TOP_LEFT) = &(center + off)[0]; // 1 top left
00149   points.row(BOTTOM_RIGHT) = &(center - off)[0]; // 3 bottom right
00150   
00151   bBox.min = &points.minR()[0];
00152   bBox.max = &points.maxR()[0];
00153   
00154   unrot = rot.transpose(); // now switch to inverse
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) ) // test cheap aabb first
00175     return false;
00176   if(unrot(0,0)==1 && other.unrot(0,0)==1)
00177     return true; // rectangle is axis aligned, bb hit is all we needed
00178   
00179   // first test of other against current axes
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     // test current points against other's axes
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]) { // left margin
00212     if(p[1]<minEx[1]) { // bottom left corner
00213       return ((minEx - p).sumSq() < other.getRadius()*other.getRadius());
00214     } else if(maxEx[1]<p[1]) { // top left corner
00215       return ((fmat::pack(minEx[0],maxEx[1]) - p).sumSq() < other.getRadius()*other.getRadius());
00216     }
00217     return true; // center left
00218   } else if(maxEx[0] < p[0]) { // right margin
00219     if(p[1]<minEx[1]) { // bottom right corner
00220       return ((fmat::pack(maxEx[0],minEx[1]) - p).sumSq() < other.getRadius()*other.getRadius());
00221     } else if(maxEx[1]<p[1]) { // top right corner
00222       return ((maxEx - p).sumSq() < other.getRadius()*other.getRadius());
00223     }
00224     return true; // center right
00225   } else {
00226     return true; // center top or bottom margin
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]) { // left margin
00240     if(p[1]<minEx[1]) { // bottom left corner
00241       return fmat::Column<2>(points.row(BOTTOM_LEFT).transpose()) - pt;
00242     } else if(maxEx[1]<p[1]) { // top left corner
00243       return fmat::Column<2>(points.row(TOP_LEFT).transpose()) - pt;
00244     }
00245     return unrot.row(0).transpose() * (minEx[0]-p[0]); // center left
00246   } else if(maxEx[0] < p[0]) { // right margin
00247     if(p[1]<minEx[1]) { // bottom right corner
00248       return fmat::Column<2>(points.row(BOTTOM_RIGHT).transpose()) - pt;
00249     } else if(maxEx[1]<p[1]) { // top right corner
00250       return fmat::Column<2>(points.row(TOP_RIGHT).transpose()) - pt;
00251     }
00252     return unrot.row(0).transpose() * (maxEx[0]-p[0]); // center right
00253   } else if(p[1]<minEx[1]) {
00254     return unrot.row(1).transpose() * (minEx[1]-p[1]); // bottom margin
00255   } else if(maxEx[1]<p[1]) {
00256     return unrot.row(1).transpose() * (maxEx[1]-p[1]); // top margin
00257   } else {
00258     // inside
00259     fmat::fmatReal dist[4];
00260     dist[0] = p[0]-minEx[0]; // left
00261     dist[1] = p[1]-minEx[1]; // bottom
00262     dist[2] = maxEx[0]-p[0]; // right
00263     dist[3] = maxEx[1]-p[1]; // top
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]); // left
00268         }
00269       } else {
00270         if(dist[2] < dist[3]) {
00271           return unrot.row(0).transpose() * (maxEx[0]-p[0]); // right
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]); // bottom
00278         }
00279       } else {
00280         if(dist[2] < dist[3]) {
00281           return unrot.row(0).transpose() * (maxEx[0]-p[0]); // right
00282         }
00283       }
00284     }
00285     return unrot.row(1).transpose() * (maxEx[1]-p[1]); // top
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   // choose the closest corner
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 //! Decreases the size of the obstacle in all directions by at least @a amount
00323 void RectangularObstacle::contract(float amount) {
00324   bloat(-amount);
00325 }
00326 
00327 void RectangularObstacle::loadXML(xmlNode* node) {
00328   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // temporarily add plist entries for serialization
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // handle circle case (semimajor == semiminor)
00436   if (semimajor == semiminor)
00437     return center + (direction / direction.norm() * semimajor); // or semiminor...
00438   
00439   // otherwise, we use a Lagrange Multiplier.
00440   fmat::Column<2> dir = getOrientation().transpose() * direction;
00441   
00442   // if we're zero on any axis, it's easy.
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   // for efficiency
00451   fmat::fmatReal a2 = semimajor*semimajor, b2 = semiminor*semiminor;
00452   
00453   // solve for x
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   // derived from parametric equation of the ellipse
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   // shift t by PI for both x and y, which just flips the signs of the sin/cos
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   // TODO: completely re-do this. the circle approximation might be fine.
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   // http://planetmath.org/encyclopedia/QuarticFormula.html -- explanation
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     //std::cout << a << ' ' << b << ' ' << c << std::endl;
00556     return fmat::pack(0,0);
00557   }
00558   
00559   // if we're below the x-axis, negative angles
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   // temporarily add plist entries for serialization
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // Build lower hull
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   // Build upper hull
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   // update normals
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     // this is why we specify counter-clockwise... right normal is outside
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     // this is why we specify counter-clockwise... right normal is outside
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   // this is why we specify counter-clockwise... right normal is outside
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; // once we see we are outside, must 'hit' corner or edge (see final return)
00721   bool testedLast = false; // prevents duplicate corner tests on consecutive edges (not a big issue though)
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       // close... now project onto the edge
00730       d = v[0]*-normals[i][1] + v[1]*normals[i][0]; // reuse the normal, just flip it
00731       // see if it hits either endpoint or the edge itself
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) // within edge itself
00736           return true;
00737         if(testedLast) {
00738           testedLast=false;
00739         } else if(d < len+other.getRadius()) {
00740           // may hit next corner point
00741           if((other.getCenter() - next).sumSq() < r2)
00742             return true;
00743           testedLast=true;
00744         }
00745       } else {
00746         if(d > -other.getRadius()) {
00747           // may hit this corner point
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; // prevents duplicate corner tests on consecutive edges (not a big issue though)
00839   for(size_t i=0; i<points.size(); ++i) {
00840     fmat::Column<2> v = pt-points[i];
00841     // project onto the edge
00842     const fmat::fmatReal d = v[0]*-normals[i][1] + v[1]*normals[i][0]; // reuse the normal, just flip it
00843     // is it closer to an endpoint or the edge itself
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         // test corner point
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       // may hit this corner point
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   // temporarily add plist entries for serialization
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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); // only two dimensions
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   // if we're not inside the box, no collision
00928   if( !aabb.collides(point) )
00929     return false;
00930   
00931   // rotate point into Hierarchical Obstacle frame
00932   const fmat::Column<2> newPt = rotation.transpose() * (point - center);
00933   
00934   // if we are, test further
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   /*RectangularObstacle bounds(aabb.getCenter(), aabb.getDimensions()/2, 0);
00944   // if no collision with the box, done.
00945   if (!bounds.collides(other))
00946     return false;*/
00947   
00948   PlannerObstacle2D* transformed = dynamic_cast<PlannerObstacle2D*>(other.clone());
00949   transformed->updatePosition(transformed->getCenter()-center);
00950   transformed->rotate(fmat::ZERO2, rotation.transpose());
00951   
00952   // if there is, test further
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   std::cout << (collision ? "" : "no ") 
00962             << "collides: center=" << center << ",rot=" << atan2(rotation(1,0),rotation(0,0))/M_PI*180 <<" deg. with "
00963             << other << " = " << collision << std::endl;
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]; // 0
01037   points.row(BOTTOM_LOWER_LEFT) = &(center - off)[0]; // 6
01038   off = rot * fmat::pack(extents[0],-extents[1],extents[2]);
01039   points.row(TOP_LOWER_RIGHT) = &(center + off)[0]; // 1
01040   points.row(BOTTOM_UPPER_LEFT) = &(center - off)[0]; // 7
01041   off = rot * fmat::pack(-extents[0],-extents[1],extents[2]);
01042   points.row(TOP_LOWER_LEFT) = &(center + off)[0]; // 2
01043   points.row(BOTTOM_UPPER_RIGHT) = &(center - off)[0]; // 4
01044   off = rot * fmat::pack(-extents[0],extents[1],extents[2]);
01045   points.row(TOP_UPPER_LEFT) = &(center + off)[0]; // 3
01046   points.row(BOTTOM_LOWER_RIGHT) = &(center - off)[0]; // 5
01047   
01048   bBox.min = &points.minR()[0];
01049   bBox.max = &points.maxR()[0];
01050   
01051   unrot = rot.transpose(); // now switch to inverse
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   // Compute rotation matrix expressing the other in this coordinate frame
01095   r = other.getOrientation() * unrot;
01096   
01097   // Compute translation, in terms of a's coordinate frame
01098   fmat::Column<3> diff = unrot * (other.getCenter() - center);
01099   
01100   // Extents
01101   fmat::Column<3> extents = getExtents();
01102   fmat::Column<3> otherExtents = other.getExtents();
01103   
01104   // Compute common subexpressions. Add in an epsilon term to
01105   // counteract arithmetic errors when two edges are parallel and
01106   // their cross product is (near) null (see text for details)
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   // Test axes L = A0, L = A1, L = A2
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   // Test axes L = B0, L = B1, L = B2
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   // Test axis L = A0 x B0
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   // Test axis L = A0 x B1
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   // Test axis L = A0 x B2
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   // Test axis L = A1 x B0
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   // Test axis L = A1 x B1
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   // Test axis L = A1 x B2
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   // Test axis L = A2 x B0
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   // Test axis L = A2 x B1
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   // Test axis L = A2 x B2
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   // Since no separating axis found, the OBBs must be intersecting
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   // choose the closest corner
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   // Start result at center of box; make steps from there
01211   fmat::Column<3> q = center;
01212   // For each OBB axis...
01213   for (int i = 0; i < 3; i++) {
01214     // ...project d onto that axis to get the distance
01215     // along the axis of d from the box center
01216     float dist = fmat::dotProduct(d, fmat::Row<3>(unrot.row(i)));
01217     // If distance farther than the box extents, clamp to the box
01218     if (dist > getExtents()[i]) dist = getExtents()[i];
01219     if (dist < -getExtents()[i]) dist = -getExtents()[i];
01220     // Step that distance along the axis to get world coordinate
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // temporarily add plist entries for serialization
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // point intersects if its distance from the center is less than the radius
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   // temporarily add plist entries for serialization
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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   // shrink ourselves to circle world, then test
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   // convenience
01424   fmat::fmatReal a2 = extents[0]*extents[0], b2 = extents[1]*extents[1], c2 = extents[2]*extents[2];
01425   
01426   // solve for x (lagrange multiplier)
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   // if we're zero on either axis, we just use the extents of the other.
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   // convenience
01444   fmat::fmatReal a2 = extents[0]*extents[0], b2 = extents[1]*extents[1];
01445   
01446   // solve for x (lagrange multiplier)
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   // temporarily add plist entries for serialization
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   // temporarily add plist entries to get 'normalized' coordinates, then convert
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 }

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:47 2016 by Doxygen 1.6.3