Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PlannerObstacles.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_PlannerObstacles_H
00003 #define INCLUDED_PlannerObstacles_H
00004 
00005 #include <ostream>
00006 
00007 #include "Shared/fmat.h"
00008 #include "Shared/plist.h"
00009 #include "Shared/FamilyFactory.h"
00010 #include "Shared/BoundingBox.h"
00011 #include <vector>
00012 
00013 template<size_t N>
00014 class PlannerObstacle;
00015 typedef PlannerObstacle<2> PlannerObstacle2D;
00016 typedef PlannerObstacle<3> PlannerObstacle3D;
00017 
00018 class RectangularObstacle;
00019 class CircularObstacle;
00020 class EllipticalObstacle;
00021 class ConvexPolyObstacle;
00022 class HierarchicalObstacle;
00023 class BoxObstacle;
00024 class CylindricalObstacle;
00025 class SphericalObstacle;
00026 class EllipsoidObstacle;
00027 
00028 namespace plist {
00029   //! supports use of plist::ArrayOf<PlannerObstacle> for polymorphic load/save
00030   template<> PlannerObstacle2D* loadXML(xmlNode* node);
00031   template<> PlannerObstacle3D* loadXML(xmlNode* node);
00032   //! not supported
00033   template<> inline PlannerObstacle2D* allocate() {
00034     throw std::runtime_error("cannot plist::allocate generic instances (PlannerObstacle2D)");
00035   }
00036   template<> inline PlannerObstacle3D* allocate() {
00037     throw std::runtime_error("cannot plist::allocate generic instances (PlannerObstacle3D)");
00038   }
00039   template<> void inline assign<PlannerObstacle2D>(PlannerObstacle2D&, PlannerObstacle2D const&) { throw std::runtime_error("cannot assign"); }
00040   template<> void inline assign<PlannerObstacle3D>(PlannerObstacle3D&, PlannerObstacle3D const&) { throw std::runtime_error("cannot assign"); }
00041 }
00042 
00043 namespace DualCoding {
00044   class ShapeRoot;
00045 }
00046 
00047 //! Base class for all obstacles
00048 /*! Subclasses should remember to override operator= to avoid using the plist::Dictionary version
00049  *  (plist::Dictionary::operator= will only copy dictionary entries, skips other state */
00050 template <size_t N>
00051 class PlannerObstacle : public plist::Dictionary {
00052 public:
00053   //! each collision shape is assigned a prime number, these provide fast dispatch in collides()
00054   enum ObstacleGeometry {
00055     RECTANGULAR_OBS=2,
00056     CIRCULAR_OBS=3,
00057     ELLIPTICAL_OBS=5,
00058     CONVEX_POLY_OBS=7,
00059     HIERARCHICAL_OBS=11,
00060     BOX_OBS=13,
00061     CYLINDRICAL_OBS=17,
00062     SPHERICAL_OBS=19,
00063     ELLIPSOID_OBS=23
00064   };
00065   
00066   //! Default constructor
00067   explicit PlannerObstacle(ObstacleGeometry geom, const std::string& t) : 
00068     plist::Dictionary(), name(), type(t), geometry(geom), shapeId(-1), bodyObstacle(false) { init(); }
00069   
00070   //! Destructor
00071   virtual ~PlannerObstacle() {}
00072   
00073   //! sgn(x): returns sign of input
00074   template <typename T>
00075   static int sgn(T x) { return x < 0 ? -1 : 1; }
00076   
00077   //! Sub-components of an obstacle
00078   plist::Primitive<std::string> name;
00079   
00080   static void convertShapeToPlannerObstacle(const DualCoding::ShapeRoot& shape, 
00081               float inflation,
00082               std::vector<PlannerObstacle*> &obstacles);
00083   
00084   //! Increases the size of the obstacle in all directions by at least @a amount
00085   //virtual void bloat(float amount) = 0;
00086   
00087   //! Decreases the size of the obstacle in all directions by at least @a amount
00088   //virtual void contract(float amount) = 0;
00089   
00090   //! moves the obstacle
00091   virtual void updatePosition(const fmat::SubVector<N,const fmat::fmatReal>& newPos) = 0;
00092   
00093   //! rotate the obstacle about a specified origin and a 2d rotation matrix
00094   virtual void rotate(const fmat::SubVector<N,const fmat::fmatReal>& origin,
00095             const fmat::SubMatrix<N,N,const fmat::fmatReal>& rot) = 0;
00096   
00097   //! rotate the obstacle about its center by a 2d rotation matrix
00098   void rotate(const fmat::SubMatrix<N,N,const fmat::fmatReal>& rot) { rotate(getCenter(), rot); }
00099   
00100   //! get center point of obstacle
00101   virtual fmat::Column<N> getCenter() const = 0;
00102   
00103   //! get boundaries of the current obstacle
00104   virtual BoundingBox<N> getBoundingBox() const = 0;
00105   
00106   //! checks for collision with other PlannerObstacles
00107   bool collides(const PlannerObstacle<N>& other) const;
00108   
00109   //! All collision tests that need to be handled by subclasses for specific points
00110   virtual bool collides(const fmat::SubVector<N,const fmat::fmatReal>& point) const = 0;
00111   
00112   //! Support function for GJK collision detection algorithm.
00113   /*! Gets the point on the convex polygon farthest along the specified direction. */
00114   virtual fmat::Column<N> getSupport(const fmat::SubVector<N,const fmat::fmatReal>& direction) const = 0;
00115   
00116   //! Calculate the vector to closest point on the obstacle boundary from @a pt
00117   virtual fmat::Column<N> gradient(const fmat::SubVector<N,const fmat::fmatReal>& pt) const = 0;
00118   
00119   virtual std::string toString() const;
00120   friend std::ostream& operator<<(std::ostream& os, const PlannerObstacle& po) { return os << po.toString(); }
00121   
00122   ObstacleGeometry getObstacleGeometry() const { return geometry; }
00123   
00124   int getShapeId() const { return shapeId; }
00125   void setShapeId(int id) { shapeId = id; }
00126   
00127   bool isBodyObstacle() { return bodyObstacle; }
00128   void setBodyObstacle() { bodyObstacle = true; }
00129   
00130   const std::string& getTypeName() const { return type; }
00131   
00132   typedef FamilyFactory<PlannerObstacle,std::string> registry_t;
00133   static registry_t& getRegistry() { static registry_t registry; return registry; }
00134   
00135   PLIST_CLONE_FWD(PlannerObstacle);
00136   
00137 protected:
00138   //! Copy constructor, be careful not to cross-copy different types
00139   PlannerObstacle(const PlannerObstacle& o) : 
00140     plist::Dictionary(), name(o.name), 
00141     type(o.type), geometry(o.geometry), shapeId(o.shapeId), bodyObstacle(false) { init(); }
00142   //! Don't use this assignment...
00143   PlannerObstacle& operator=(const PlannerObstacle& o) {
00144     name = o.name;
00145     type = o.type;
00146     geometry = o.geometry;
00147     init();
00148     return *this;
00149   }
00150   
00151   void init() {
00152     addEntry("Name",name,"Human readable name for debugging, display");
00153     addEntry(".type",type,"Allows polymorphic load/save, must match known class name");
00154     setLoadSavePolicy(FIXED,SYNC);
00155   }
00156   
00157   plist::Primitive<std::string> type;
00158   ObstacleGeometry geometry;
00159   int shapeId; //!< For obstacles defined by DualCoding shapes
00160   bool bodyObstacle; //!< For 3D path planning, to avoid arm-body collisions
00161 };
00162 
00163 template<size_t N>
00164 std::string PlannerObstacle<N>::toString() const {
00165   std::ostringstream os;
00166   os << "PlannerObstacle[type=" << type << ",name=" << name << "]";
00167   return os.str();
00168 }
00169 
00170 //! Rectangular defined obstacle designated by 4 corners and its rotation matrix from axis alignment
00171 class RectangularObstacle: public PlannerObstacle2D {
00172 protected:
00173   friend class ConvexPolyObstacle;
00174   
00175   //! stores the class name used for polymorphic load/save
00176   static const std::string autoRegisterName;
00177   
00178   //! Center of rectangle
00179   fmat::Column<2> center;
00180   
00181   //! minimum extents (along rectangle axes)
00182   fmat::Column<2> minEx;
00183   //! maximum extents (along rectangle axes)
00184   fmat::Column<2> maxEx;
00185   
00186   //! axis aligned bounding box
00187   BoundingBox2D bBox;
00188   
00189   //! The rotation matrix to rotate from 'normal' coordinates to the obstacle orientation
00190   fmat::Matrix<2,2> unrot;
00191   //! corner points for fast collision checking
00192   /*! points are stored in clockwise order from upper right:
00193    *  - top right
00194    *  - bottom right
00195    *  - bottom left
00196    *  - top left */
00197   fmat::Matrix<4,2> points;
00198   
00199 public:
00200   //! Default constructor
00201   RectangularObstacle(): PlannerObstacle2D(RECTANGULAR_OBS,autoRegisterName),
00202                          center(), minEx(), maxEx(), bBox(), 
00203                          unrot(fmat::Matrix<2,2>::identity()), points() {}
00204   
00205   //! Pass the center, the extents from center (half-width and half-height), and an angular (radian) rotation
00206   RectangularObstacle(const fmat::SubVector<2,const fmat::fmatReal>& centerPoint,
00207             const fmat::SubVector<2,const fmat::fmatReal>& extents,
00208             fmat::fmatReal orient)
00209   : PlannerObstacle2D(RECTANGULAR_OBS,autoRegisterName), center(), minEx(), maxEx(), bBox(), unrot(), points()
00210   {
00211     reset(centerPoint,extents,orient); 
00212   }
00213   
00214   //! Pass a bounding box and a transform... the transformation is applied relative to origin, e.g. the @a bb center will rotate about the origin
00215   RectangularObstacle(const BoundingBox2D& bb, const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot, const fmat::SubVector<2,const fmat::fmatReal>& off)
00216   : PlannerObstacle2D(RECTANGULAR_OBS,autoRegisterName), center(), minEx(), maxEx(), bBox(), unrot(), points()
00217   {
00218     fmat::Column<2> c(bb.getCenter()), d(bb.getDimensions());
00219     reset(rot*c+off, d/2, rot);
00220   }
00221   
00222   //! Assignment, should not use plist::Dictionary version
00223   RectangularObstacle& operator=(const RectangularObstacle& o) {
00224     PlannerObstacle2D::operator=(o);
00225     center=o.center;
00226     minEx=o.minEx;
00227     maxEx=o.maxEx;
00228     bBox=o.bBox;
00229     unrot=o.unrot;
00230     points=o.points;
00231     return *this;
00232   }
00233   
00234   using PlannerObstacle2D::collides;
00235   
00236   //! Test if the rectangle includes a specific point
00237   virtual bool collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const;
00238   
00239   bool collides(const RectangularObstacle& other) const;
00240   bool collides(const CircularObstacle& other) const;
00241   bool collides(const EllipticalObstacle& other) const;
00242   
00243   virtual fmat::Column<2> getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const;
00244   
00245   //! do a complete reset of parameters
00246   virtual void reset(const fmat::Column<2>& centerPoint, const fmat::Column<2>& extents, fmat::fmatReal orient) {
00247     reset(centerPoint,extents,fmat::rotation2D(orient));
00248   }
00249   
00250   //! do a complete reset of parameters (so if you already have the rotation matrix, avoids regeneration from the angular value)
00251   virtual void reset(fmat::Column<2> centerPoint,
00252              const fmat::SubVector<2,const fmat::fmatReal>& extents,
00253              const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot);
00254   
00255   virtual void updatePosition(const fmat::SubVector<2, const fmat::fmatReal>& newPos);
00256   
00257   using PlannerObstacle2D::rotate;
00258   virtual void rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00259             const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot);
00260   
00261   //! Minimum and maximum X and Y extents (axis aligned bounding box)
00262   virtual BoundingBox2D getBoundingBox() const { return bBox; }
00263   
00264   virtual fmat::Column<2> gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const;
00265   
00266   virtual std::string toString() const;
00267   
00268   //! Gets the center (X,Y)
00269   virtual fmat::Column<2> getCenter() const { return center; }
00270   
00271   //! returns the width along 'local' axes
00272   float getWidth() const { return maxEx[0] - minEx[0]; }
00273   //! returns the height along 'local' axes
00274   float getHeight() const { return maxEx[1] - minEx[1]; }
00275   //! returns the half-width and half-height, suitable for easy use with reset()
00276   fmat::Column<2> getExtents() const { return (maxEx-minEx)/2; }
00277   //! returns the orienation of the rectangle in radians
00278   float getOrientation() const;
00279   
00280   enum CornerOrder {
00281     TOP_RIGHT,
00282     TOP_LEFT,
00283     BOTTOM_LEFT,
00284     BOTTOM_RIGHT,
00285     NUM_CORNERS // for looping
00286   };
00287   
00288   //! returns the specified corner point
00289   fmat::Column<2> getCorner(size_t i) const { return fmat::pack(points(i,0),points(i,1)); }
00290   
00291   //! Increases the size of the obstacle in all directions by at least @a amount
00292   virtual void bloat(float amount);
00293   
00294   //! Decreases the size of the obstacle in all directions by at least @a amount
00295   virtual void contract(float amount);
00296   
00297   virtual void loadXML(xmlNode* node);
00298   virtual void saveXML(xmlNode * node) const;
00299   using plist::DictionaryBase::saveXML;
00300   
00301   //! clone definition for RectangularObstacle
00302   PLIST_CLONE_DEF(RectangularObstacle,new RectangularObstacle(*this));
00303 };
00304 
00305 //! Circle/Sphere defined obstacle designated by a center point and a radius
00306 class CircularObstacle : public PlannerObstacle2D {
00307 protected:
00308   //! stores the class name used for polymorphic load/save
00309   static const std::string autoRegisterName;
00310   
00311   //! Center of the obstacle
00312   fmat::Column<2> center;
00313   
00314   //! Radius of the obstacle
00315   fmat::fmatReal radius;
00316   
00317 public:
00318   //! Default constructor
00319   CircularObstacle() : PlannerObstacle2D(CIRCULAR_OBS,autoRegisterName),
00320   center(), radius(0) {}
00321   
00322   //! point and radius
00323   CircularObstacle(fmat::fmatReal x, fmat::fmatReal y, fmat::fmatReal r) : PlannerObstacle2D(CIRCULAR_OBS,autoRegisterName),
00324   center(fmat::pack(x,y)), radius(r) {}
00325   
00326   //! point and radius
00327   CircularObstacle(const fmat::Column<2>& c, fmat::fmatReal r) : PlannerObstacle2D(CIRCULAR_OBS,autoRegisterName),
00328   center(c), radius(r) {}
00329   
00330   //! Assignment, should not use plist::Dictionary version
00331   CircularObstacle& operator=(const CircularObstacle& o) {
00332     PlannerObstacle2D::operator=(o);
00333     center=o.center;
00334     radius=o.radius;
00335     return *this;
00336   }
00337   
00338   using PlannerObstacle2D::collides;
00339   
00340   //! Test if the circle includes a specific point
00341   virtual bool collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const {
00342     return (point - center).sumSq() < radius*radius;
00343   }
00344   
00345   bool collides(const CircularObstacle& other) const;
00346   
00347   virtual fmat::Column<2> getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const;
00348   
00349   virtual fmat::Column<2> getCenter() const { return center; }
00350   virtual fmat::fmatReal getRadius() const { return radius; }
00351   virtual void setRadius(fmat::fmatReal r) { radius = r; }
00352   
00353   virtual void updatePosition(const fmat::SubVector<2, const fmat::fmatReal>& newPos) { center = newPos; }
00354   
00355   using PlannerObstacle2D::rotate;
00356   virtual void rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00357             const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot) {
00358     updatePosition(rot * (getCenter()-origin) + origin);
00359   }
00360   
00361   virtual BoundingBox2D getBoundingBox() const {
00362     return BoundingBox2D(center - radius, center + radius);
00363   }
00364   
00365   virtual fmat::Column<2> gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const;
00366   
00367   virtual std::string toString() const;
00368   
00369   //! Increases the size of the obstacle in all directions by at least @a amount
00370   virtual void bloat(float amount) { radius += amount; }
00371   //! Decreases the size of the obstacle in all directions by at least @a amount
00372   virtual void contract(float amount) { radius += amount; }
00373   
00374   virtual void loadXML(xmlNode* node);
00375   virtual void saveXML(xmlNode * node) const;
00376   using plist::DictionaryBase::saveXML;
00377   
00378   //! clone definition for CircularObstacle
00379   PLIST_CLONE_DEF(CircularObstacle,new CircularObstacle(*this));
00380 };
00381 
00382 //! Elliptically defined obstacle
00383 class EllipticalObstacle : public PlannerObstacle2D {
00384 protected:
00385   //! stores the class name used for polymorphic load/save
00386   static const std::string autoRegisterName;
00387   
00388 public:
00389   //! Focus of the ellipse
00390   fmat::Column<2> focus1;
00391   
00392   //! Focus of the ellipse  
00393   fmat::Column<2> focus2;
00394   
00395   //! Center of the ellipse
00396   fmat::Column<2> center;
00397   
00398   //! Half-length of the major axis
00399   fmat::fmatReal semimajor;
00400   
00401   //! Half-length of the minor axis
00402   fmat::fmatReal semiminor;
00403   
00404   //! Default constructor
00405   EllipticalObstacle() : PlannerObstacle2D(ELLIPTICAL_OBS,autoRegisterName),
00406   focus1(), focus2(), center(), semimajor(), semiminor() {}
00407   
00408   //! Location, semimajor, semiminor, orientation
00409   /*! Swaps the major/minor and adds 90° to the orientation if major<minor */
00410   EllipticalObstacle(const fmat::Column<2>& c, fmat::fmatReal _semimajor, fmat::fmatReal _semiminor, 
00411          fmat::fmatReal orientation) : PlannerObstacle2D(ELLIPTICAL_OBS,autoRegisterName),
00412   focus1(), focus2(), center(c), semimajor(_semimajor), semiminor(_semiminor)
00413     { reset(c, _semimajor, _semiminor, orientation); }
00414   
00415   //! Circle with point and radius
00416   EllipticalObstacle(const fmat::Column<2>& c, fmat::fmatReal r) : PlannerObstacle2D(ELLIPTICAL_OBS,autoRegisterName),
00417   focus1(c), focus2(c), center(c), semimajor(r), semiminor(r) {}
00418   
00419   //! Assignment, should not use plist::Dictionary version
00420   EllipticalObstacle& operator=(const EllipticalObstacle& o) {
00421     PlannerObstacle2D::operator=(o);
00422     focus1=o.focus1;
00423     focus2=o.focus2;
00424     center=o.center;
00425     semimajor=o.semimajor;
00426     semiminor=o.semiminor;
00427     return *this;
00428   }
00429   
00430   //! Initialize from two foci and semimajor @a s
00431   /*! Will throw an exception if @a s is too short (must be at least half the distance between the foci) */
00432   void reset(const fmat::Column<2>& f1, const fmat::Column<2>& f2, fmat::fmatReal s);
00433   
00434   //! Initialize from center, extents, and orientation
00435   /*! Swaps the major/minor and adds 90° to the orientation if major<minor */
00436   void reset(const fmat::Column<2>& c, fmat::fmatReal smajor, fmat::fmatReal sminor, fmat::fmatReal ori);
00437   
00438   //! Initialize from circle: center and radius
00439   void reset(const fmat::Column<2>& c, fmat::fmatReal radius) {
00440     focus1 = focus2 = center = c;
00441     semimajor = semiminor = std::abs(radius);
00442   }
00443   
00444   using PlannerObstacle2D::collides;
00445   
00446   //! Test if the circle includes a specific point
00447   virtual bool collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const {
00448     return (point - focus1).norm() + (point - focus2).norm() < 2*semimajor;
00449   }
00450   
00451   virtual fmat::Column<2> getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const;
00452   
00453   virtual fmat::Column<2> getCenter() const { return center; }
00454   
00455   virtual void updatePosition(const fmat::SubVector<2, const fmat::fmatReal>& newPos);
00456   
00457   using PlannerObstacle2D::rotate;
00458   virtual void rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00459             const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot);
00460   
00461   //! Returns the angle of the major axis
00462   float getAngle() const { return std::atan2(focus1[1] - focus2[1], focus1[0] - focus2[0]); }
00463   
00464   //! If rotating into the ellipse's frame, construct rotation matrix directly
00465   fmat::Matrix<2,2> getOrientation() const;
00466   
00467   //! Returns the point on the edge of the ellipse at the specified angle
00468   /*! @a theta should be relative to the reference frame of the space that the ellipse resides in
00469    *  (and returns results in the external frame as well) */
00470   fmat::Column<2> getPointOnEdge(fmat::fmatReal theta) const {
00471     return getPointOnEdge(fmat::pack(std::cos(theta),std::sin(theta)));
00472   }
00473   
00474   //! Returns the point on the edge of the ellipse which intersects vector @a v through the center.
00475   /*! @a v does not need to be normalized */
00476   fmat::Column<2> getPointOnEdge(const fmat::Column<2>& direction) const;
00477   
00478   virtual BoundingBox2D getBoundingBox() const;
00479   
00480   //! Approximates the closest point on the ellipse to @a pt
00481   /*! Basically scales the vector from the center to @a pt */
00482   virtual fmat::Column<2> gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const;
00483   
00484   virtual std::string toString() const;
00485   
00486   //! Increases the size of the obstacle in all directions by at least @a amount
00487   virtual void bloat(float amount) { reset(center, semimajor+amount, semiminor+amount, getAngle()); }
00488   //! Decreases the size of the obstacle in all directions by at least @a amount
00489   virtual void contract(float amount) { reset(center, semimajor-amount, semiminor-amount, getAngle()); }
00490   
00491   virtual void loadXML(xmlNode* node);
00492   virtual void saveXML(xmlNode* node) const;
00493   using plist::DictionaryBase::saveXML;
00494   
00495   //! clone definition for EllipticalObstacle
00496   PLIST_CLONE_DEF(EllipticalObstacle,new EllipticalObstacle(*this));
00497 };
00498 
00499 //! Manages collision detection with convex polygons
00500 /*! Basically the same algorithm used for rectangles, just generalized for more sides */
00501 class ConvexPolyObstacle : public PlannerObstacle2D {
00502 protected:
00503   //! stores the class name used for polymorphic load/save
00504   static const std::string autoRegisterName;
00505   
00506   std::vector<fmat::Column<2> > points; //! the points of the polygon, in counter-clockwise order
00507   std::vector<fmat::Column<2> > normals; //! the 'outside' normal of segment following corresponding entry in #points
00508   
00509 public:
00510   //! default constructor
00511   ConvexPolyObstacle() : PlannerObstacle2D(CONVEX_POLY_OBS,autoRegisterName), points(), normals() {}
00512   
00513   //! Assignment, should not use plist::Dictionary version
00514   ConvexPolyObstacle& operator=(const ConvexPolyObstacle& o) {
00515     PlannerObstacle2D::operator=(o);
00516     points = o.points;
00517     normals = o.normals;
00518     return *this;
00519   }
00520   
00521   //! returns the points, in counter-clockwise order
00522   const std::vector<fmat::Column<2> >& getPoints() const { return points; }
00523   
00524   //! returns the normals, in counter-clockwise order
00525   const std::vector<fmat::Column<2> >& getNormals() const { return normals; }
00526   
00527   //! Selects points which form a convex hull, clears current points
00528   /*! Passing a set because the hulling algorithm (Andrew's Monotone Chain)
00529    *  begins with sorting the list of points anyway.  If you have a different STL container 'c'
00530    *  just pass std::set<fmat::Column<2>(c.begin(),c.end()) to convert to a set.
00531    *  This implementation adapted from:
00532    *  http://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain */
00533   void hull(const std::set<fmat::Column<2> >& p);
00534   
00535   //! remove all points and normals
00536   void clear() { points.clear(); normals.clear(); }
00537   
00538   //! Add a point to the polygon, must maintain convexity and in counter-clockwise order, call close() when done
00539   void addPoint(const fmat::Column<2>& p);
00540   
00541   //! Calculates values for final segment of polygon, call after a series of addPoint()
00542   void close();
00543   
00544   using PlannerObstacle2D::collides;
00545   //! Test if the polygon includes a specific point
00546   virtual bool collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const;
00547   //! For polygon on rectangle collision
00548   bool collides(const RectangularObstacle& other) const;
00549   //! For polygon on circle collision
00550   bool collides(const CircularObstacle& other) const;
00551   //! For polygon on polygon collision
00552   bool collides(const ConvexPolyObstacle& other) const;
00553   
00554   virtual fmat::Column<2> getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const;
00555   
00556   virtual fmat::Column<2> getCenter() const;
00557   virtual void updatePosition(const fmat::SubVector<2, const fmat::fmatReal>& newPos) { offset(newPos-getCenter()); }
00558   
00559   using PlannerObstacle2D::rotate;
00560   virtual void rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00561             const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot);
00562   virtual BoundingBox2D getBoundingBox() const;
00563   
00564   virtual void offset(const fmat::Column<2>& off);
00565   virtual fmat::Column<2> gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const;
00566   
00567   virtual std::string toString() const;
00568   
00569   virtual void loadXML(xmlNode* node);
00570   virtual void saveXML(xmlNode * node) const;
00571   using plist::DictionaryBase::saveXML;
00572   
00573   //! clone definition for ConvexPolyObstacle
00574   PLIST_CLONE_DEF(ConvexPolyObstacle,new ConvexPolyObstacle(*this));
00575 };
00576 
00577 //! Hierarchically defined obstacle containing multiple obstacles
00578 class HierarchicalObstacle : public PlannerObstacle2D {
00579 protected:
00580   //! stores the class name used for polymorphic load/save
00581   static const std::string autoRegisterName;
00582   
00583   //! recalculate rectangular bounding box based on contained obstacles
00584   void recalculateBoundingBox();
00585   
00586   //! grow the bounding box to include that of @a other
00587   void expandBoundingBox(PlannerObstacle2D& other);
00588   
00589   std::vector<PlannerObstacle2D*> components; //!< the obstacles contained in the hierarchy
00590   
00591   BoundingBox2D aabb; //!< axis-aligned bounding box of all obstacles
00592   
00593   fmat::Column<2> center; //!< origin of the obstacle, all sub-obstacles will be relative to this point
00594   fmat::Matrix<2,2> rotation; //!< rotation about the origin
00595   
00596 public:
00597   //! default constructor
00598   HierarchicalObstacle()
00599   : PlannerObstacle2D(HIERARCHICAL_OBS,autoRegisterName), components(), aabb(), center(fmat::ZERO2), rotation(fmat::Matrix<2,2>::identity()) {}
00600   
00601   //! copy constructor (deep copy #components)
00602   HierarchicalObstacle(const HierarchicalObstacle& ho)
00603   : PlannerObstacle2D(ho), components(), aabb(ho.aabb), center(ho.center), rotation(ho.rotation)
00604   {
00605     components.reserve(ho.components.size());
00606     for(std::vector<PlannerObstacle2D*>::const_iterator it=ho.components.begin(); it!=ho.components.end(); ++it) {
00607       components.push_back(dynamic_cast<PlannerObstacle2D*>((*it)->clone())); // need explicit cast for old compilers, should be no-op
00608     }
00609   }
00610   
00611   //! Assignment, should not use plist::Dictionary version
00612   HierarchicalObstacle& operator=(const HierarchicalObstacle& o) {
00613     PlannerObstacle2D::operator=(o);
00614     clear();
00615     components.reserve(o.components.size());
00616     for(std::vector<PlannerObstacle2D*>::const_iterator it=o.components.begin(); it!=o.components.end(); ++it) {
00617       components.push_back(dynamic_cast<PlannerObstacle2D*>((*it)->clone())); // need explicit cast for old compilers, should be no-op
00618     }
00619     aabb=o.aabb;
00620     center=o.center;
00621     rotation=o.rotation;
00622     return *this;
00623   }
00624   
00625   //! destructor
00626   virtual ~HierarchicalObstacle() { clear(); }
00627   
00628   //! returns components of specified type
00629   template <class T>
00630   void getComponents(std::vector<T> &obs) const;
00631   
00632   const std::vector<PlannerObstacle2D*>& getComponents() const { return components; }
00633   
00634   //! For obstacle on point
00635   virtual bool collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const;
00636   
00637   using PlannerObstacle2D::collides;
00638   virtual bool collides(const PlannerObstacle2D& other) const;
00639   
00640   //! Never called: since HO's aren't convex, must test collision with each obstacle
00641   virtual fmat::Column<2> getSupport(const fmat::SubVector<2,const fmat::fmatReal>& direction) const { return fmat::Column<2>(); }
00642   
00643   virtual BoundingBox2D getBoundingBox() const { return aabb; }
00644   
00645   virtual fmat::Column<2> getCenter() const { return center; }
00646   virtual fmat::Matrix<2,2> getOrientation() const { return rotation; }
00647   
00648   virtual void updatePosition(const fmat::SubVector<2, const fmat::fmatReal>& newPos);
00649   
00650   //! update to specified orientation about the center
00651   virtual void updateRotation(const fmat::Matrix<2,2>& rot) { rotation = rot; }
00652   
00653   using PlannerObstacle2D::rotate;
00654   virtual void rotate(const fmat::SubVector<2,const fmat::fmatReal>& origin,
00655             const fmat::SubMatrix<2,2,const fmat::fmatReal>& rot);
00656   
00657   virtual fmat::Column<2> gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const;
00658   
00659   virtual std::string toString() const;
00660   
00661   //! Print the string representation for all components contained in the Hierarchy.
00662   virtual std::string componentsToString() const;
00663   
00664   //! clear all components
00665   void clear() {
00666     for (unsigned int i = 0; i < components.size(); i++)
00667       delete components[i];
00668     components.clear(); recalculateBoundingBox();
00669   }
00670   
00671   //! add component to end of component list; components should be transformed in terms of Hierarchical Obstacle
00672   /*! Memory allocation of @a o will be claimed by the hierarchical obstacle. */
00673   void add(PlannerObstacle2D* o) { components.push_back(o); expandBoundingBox(*o); }
00674   
00675   //! clone definition for HierarchicalObstacle
00676   PLIST_CLONE_DEF(HierarchicalObstacle,new HierarchicalObstacle(*this));
00677 };
00678 
00679 template <class T>
00680 void HierarchicalObstacle::getComponents(std::vector<T> &comps) const {
00681   for (unsigned int i = 0; i < components.size(); i++) {
00682     T t = dynamic_cast<T>(components[i]);
00683     if (t)
00684       comps.push_back(t);
00685   }
00686 }
00687 
00688 class BoxObstacle : public PlannerObstacle3D {
00689 protected:
00690   //! stores the class name used for polymorphic load/save
00691   static const std::string autoRegisterName;
00692   
00693   //! Center of box
00694   fmat::Column<3> center;
00695   
00696   //! minimum extents (along box axes)
00697   fmat::Column<3> minEx;
00698   //! maximum extents (along box axes)
00699   fmat::Column<3> maxEx;
00700   
00701   //! axis aligned bounding box
00702   BoundingBox3D bBox;
00703   
00704   //! The rotation matrix to rotate from 'normal' coordinates to the obstacle orientation
00705   fmat::Matrix<3,3> unrot;
00706   
00707   //! corner points for fast collision checking
00708   /*! points are stored first top then bottom,
00709    both in clockwise order from upper right:
00710    *  - top upper right
00711    *  - top lower right
00712    *  - top lower left
00713    *  - top upper left
00714    *  - bottom upper right
00715    *  - bottom lower right
00716    *  - bottom lower left
00717    *  - bottom upper left */
00718   fmat::Matrix<8,3> points;
00719   
00720 public:
00721   //! Default constructor
00722   BoxObstacle() : PlannerObstacle3D(BOX_OBS,autoRegisterName),
00723                   center(), minEx(), maxEx(), bBox(), unrot(fmat::Matrix<3,3>::identity()), points() {}
00724   
00725   //! Pass the center, the extents from center (half-length, half-width and half-height), and a rotation
00726   BoxObstacle(const fmat::SubVector<3,const fmat::fmatReal>& centerPoint,
00727               const fmat::SubVector<3,const fmat::fmatReal>& extents,
00728               const fmat::SubMatrix<3,3,const fmat::fmatReal>& orient)
00729     : PlannerObstacle3D(BOX_OBS,autoRegisterName), center(), minEx(), maxEx(), bBox(), unrot(), points()
00730   {
00731     reset(centerPoint,extents,orient); 
00732   }
00733   
00734   //! Pass a bounding box and a transform... the transformation is applied relative to origin, e.g. the @a bb center will rotate about the origin
00735   BoxObstacle(const BoundingBox3D& bb,
00736               const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot,
00737               const fmat::SubVector<3,const fmat::fmatReal>& off)
00738     : PlannerObstacle3D(BOX_OBS,autoRegisterName), center(), minEx(), maxEx(), bBox(), unrot(), points()
00739   {
00740     fmat::Column<3> c(bb.getCenter()), d(bb.getDimensions());
00741     reset(rot*c+off, d/2, rot);
00742   }
00743   
00744   //! Assignment, should not use plist::Dictionary version
00745   BoxObstacle& operator=(const BoxObstacle& o) {
00746     PlannerObstacle3D::operator=(o);
00747     center=o.center;
00748     minEx=o.minEx;
00749     maxEx=o.maxEx;
00750     bBox=o.bBox;
00751     unrot=o.unrot;
00752     points=o.points;
00753     return *this;
00754   }
00755   
00756   //! do a complete reset of parameters
00757   virtual void reset(fmat::Column<3> centerPoint,
00758              const fmat::SubVector<3,const fmat::fmatReal>& extents,
00759              const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot);
00760   
00761   virtual void updatePosition(const fmat::SubVector<3, const fmat::fmatReal>& newPos);
00762   
00763   using PlannerObstacle3D::rotate;
00764   virtual void rotate(const fmat::SubVector<3,const fmat::fmatReal>& origin,
00765             const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot);
00766   
00767   virtual std::string toString() const;
00768   virtual fmat::Column<3> getCenter() const { return center; }
00769   virtual BoundingBox3D getBoundingBox() const { return bBox; }
00770   virtual fmat::Matrix<3,3> getOrientation() const { return unrot.transpose(); }
00771   
00772   using PlannerObstacle3D::collides;
00773   virtual bool collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const;
00774   
00775   bool collides(const BoxObstacle& other) const;
00776   
00777   virtual fmat::Column<3> getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const;
00778   
00779   virtual fmat::Column<3> gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const;
00780   
00781   //! returns the length along 'local' axes
00782   float getLength() const { return maxEx[0] - minEx[0]; }
00783   //! returns the width along 'local' axes
00784   float getWidth() const { return maxEx[1] - minEx[1]; }
00785   //! returns the height along 'local' axes
00786   float getHeight() const { return maxEx[2] - minEx[2]; }
00787   //! returns the half-width and half-height, suitable for easy use with reset()
00788   fmat::Column<3> getExtents() const { return (maxEx-minEx)/2; }
00789   
00790   enum CornerOrder {
00791     TOP_UPPER_RIGHT,
00792     TOP_LOWER_RIGHT,
00793     TOP_LOWER_LEFT,
00794     TOP_UPPER_LEFT,
00795     BOTTOM_UPPER_RIGHT,
00796     BOTTOM_LOWER_RIGHT,
00797     BOTTOM_LOWER_LEFT,
00798     BOTTOM_UPPER_LEFT,
00799     NUM_CORNERS // for looping
00800   };
00801   
00802   //! returns the specified corner point
00803   fmat::Column<3> getCorner(size_t i) const { return fmat::pack(points(i,0),points(i,1),points(i,2)); }
00804   
00805   //! Increases the size of the obstacle in all directions by at least @a amount
00806   virtual void bloat(float amount);
00807   
00808   //! Decreases the size of the obstacle in all directions by at least @a amount
00809   virtual void contract(float amount) { bloat(-amount); }
00810   
00811   virtual void loadXML(xmlNode* node);
00812   virtual void saveXML(xmlNode * node) const;
00813   using plist::DictionaryBase::saveXML;
00814   
00815   //! clone definition for BoxObstacle
00816   PLIST_CLONE_DEF(BoxObstacle,new BoxObstacle(*this));
00817 };
00818 
00819 //! Cylinder with central axis in alignment with z-axis by default.
00820 class CylindricalObstacle : public PlannerObstacle3D {
00821 protected:
00822   //! stores the class name used for polymorphic load/save
00823   static const std::string autoRegisterName;
00824   
00825   //! center of the cylinder
00826   fmat::Column<3> center;
00827   
00828   //! 3D orientation of cylinder
00829   fmat::Matrix<3,3> orientation;
00830   
00831   //! radius of cylinder
00832   fmat::fmatReal radius;
00833   
00834   //! half-height
00835   fmat::fmatReal halfHeight;
00836   
00837 public:
00838   //! Default constructor
00839   CylindricalObstacle() : PlannerObstacle3D(CYLINDRICAL_OBS,autoRegisterName),
00840   center(), orientation(), radius(0), halfHeight(0) {}
00841   
00842   CylindricalObstacle(const fmat::SubVector<3,const fmat::fmatReal>& c,
00843             const fmat::SubMatrix<3,3,const fmat::fmatReal>& o,
00844             fmat::fmatReal r,
00845             fmat::fmatReal hh) :
00846   PlannerObstacle3D(CYLINDRICAL_OBS,autoRegisterName), center(c), orientation(o), radius(r), halfHeight(hh) {}
00847   
00848   //! Assignment, should not use plist::Dictionary version
00849   CylindricalObstacle& operator=(const CylindricalObstacle& o) {
00850     PlannerObstacle3D::operator=(o);
00851     center=o.center;
00852     orientation=o.orientation;
00853     radius=o.radius;
00854     halfHeight=o.halfHeight;
00855     return *this;
00856   }
00857   
00858   virtual void updatePosition(const fmat::SubVector<3, const fmat::fmatReal>& newPos) { center = newPos; }
00859   virtual void rotate(const fmat::SubVector<3,const fmat::fmatReal>& origin,
00860             const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot) {
00861     center = rot * (center - origin) + origin;
00862     orientation = rot * orientation;
00863   }
00864   virtual fmat::Column<3> getCenter() const { return center; }
00865   virtual float getRadius() const { return radius; }
00866   virtual BoundingBox3D getBoundingBox() const;
00867   
00868   using PlannerObstacle3D::collides;
00869   virtual bool collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const;
00870   
00871   virtual fmat::Column<3> getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const;
00872   
00873   virtual fmat::Column<3> gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const;
00874   
00875   virtual std::string toString() const;
00876   
00877   virtual void loadXML(xmlNode* node);
00878   virtual void saveXML(xmlNode* node) const;
00879   using plist::DictionaryBase::saveXML;
00880   
00881   //! clone definition for CylindricalObstacle
00882   PLIST_CLONE_DEF(CylindricalObstacle,new CylindricalObstacle(*this));
00883 };
00884 
00885 //! Spherically defined obstacle
00886 class SphericalObstacle : public PlannerObstacle3D {
00887 protected:
00888   //! stores the class name used for polymorphic load/save
00889   static const std::string autoRegisterName;
00890   
00891   //! center of the sphere
00892   fmat::Column<3> center;
00893   
00894   //! radius of the sphere
00895   fmat::fmatReal radius;
00896   
00897 public:
00898   //! Default constructor
00899   SphericalObstacle() : PlannerObstacle3D(SPHERICAL_OBS,autoRegisterName),
00900   center(), radius(0) {}
00901   
00902   //! point and radius
00903   SphericalObstacle(fmat::fmatReal x, fmat::fmatReal y, fmat::fmatReal z, fmat::fmatReal r) : PlannerObstacle3D(CIRCULAR_OBS,autoRegisterName),
00904   center(fmat::pack(x,y,z)), radius(r) {}
00905   
00906   //! point and radius
00907   SphericalObstacle(const fmat::Column<3>& c, fmat::fmatReal r) : PlannerObstacle3D(CIRCULAR_OBS,autoRegisterName),
00908   center(c), radius(r) {}
00909   
00910   //! Assignment, should not use plist::Dictionary version
00911   SphericalObstacle& operator=(const SphericalObstacle& o) {
00912     PlannerObstacle3D::operator=(o);
00913     center=o.center;
00914     radius=o.radius;
00915     return *this;
00916   }
00917   
00918   virtual void updatePosition(const fmat::SubVector<3, const fmat::fmatReal>& newPos) { center = newPos; }
00919   
00920   virtual void rotate(const fmat::SubVector<3,const fmat::fmatReal>& origin,
00921             const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot) { center = rot * (center - origin) + origin; }
00922   
00923   virtual fmat::Column<3> getCenter() const { return center; }
00924   virtual fmat::fmatReal getRadius() const { return radius; }
00925   virtual BoundingBox3D getBoundingBox() const {
00926     return BoundingBox3D(center - radius, center + radius);
00927   }
00928   
00929   using PlannerObstacle3D::collides;
00930   //! For obstacle on point
00931   virtual bool collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const;
00932   
00933   bool collides(const SphericalObstacle& other) const;
00934   
00935   virtual fmat::Column<3> getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const;
00936   
00937   virtual fmat::Column<3> gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const;
00938   
00939   virtual std::string toString() const;
00940   
00941   virtual void loadXML(xmlNode* node);
00942   virtual void saveXML(xmlNode* node) const;
00943   using plist::DictionaryBase::saveXML;
00944   
00945   //! clone definition for SphericalObstacle
00946   PLIST_CLONE_DEF(SphericalObstacle,new SphericalObstacle(*this));
00947 };
00948 
00949 class EllipsoidObstacle : public PlannerObstacle3D {
00950 protected:
00951   //! stores the class name used for polymorphic load/save
00952   static const std::string autoRegisterName;
00953   
00954   //! center of the ellipsoid
00955   fmat::Column<3> center;
00956   
00957   //! 3D orientation of the ellipse
00958   fmat::Matrix<3,3> orientation;
00959   
00960   //! positive half-width extents along each axis
00961   fmat::Column<3> extents;
00962   
00963 public:
00964   //! Default constructor
00965   EllipsoidObstacle() :
00966   PlannerObstacle3D(ELLIPSOID_OBS,autoRegisterName), center(), orientation(), extents() {}
00967   
00968   EllipsoidObstacle(const fmat::SubVector<3, const fmat::fmatReal>& c,
00969             const fmat::SubMatrix<3,3, const fmat::fmatReal>& o,
00970             const fmat::SubVector<3, const fmat::fmatReal>& e) :
00971   PlannerObstacle3D(ELLIPSOID_OBS,autoRegisterName), center(c), orientation(o), extents(e) {}
00972   
00973   //! Assignment, should not use plist::Dictionary version
00974   EllipsoidObstacle& operator=(const EllipsoidObstacle& o) {
00975     PlannerObstacle3D::operator=(o);
00976     center=o.center;
00977     orientation=o.orientation;
00978     extents=o.extents;
00979     return *this;
00980   }
00981   
00982   virtual void updatePosition(const fmat::SubVector<3, const fmat::fmatReal>& newPos) { center = newPos; }
00983   virtual void rotate(const fmat::SubVector<3,const fmat::fmatReal>& origin,
00984             const fmat::SubMatrix<3,3,const fmat::fmatReal>& rot) {
00985     center = rot * (center - origin) + origin;
00986     orientation = rot * orientation;
00987   }
00988   virtual fmat::Column<3> getCenter() const { return center; }
00989   virtual BoundingBox3D getBoundingBox() const;
00990   
00991   using PlannerObstacle3D::collides;
00992   virtual bool collides(const fmat::SubVector<3,const fmat::fmatReal>& point) const;
00993   
00994   virtual fmat::Column<3> getSupport(const fmat::SubVector<3,const fmat::fmatReal>& direction) const;
00995   
00996   //! helper function, in the case of any of the components being zero.
00997   virtual fmat::Column<2> get2Support(const fmat::SubVector<2,const fmat::fmatReal>& direction) const;
00998   
00999   virtual fmat::Column<3> gradient(const fmat::SubVector<3,const fmat::fmatReal>& pt) const;
01000   
01001   virtual std::string toString() const;
01002   
01003   virtual void loadXML(xmlNode* node);
01004   virtual void saveXML(xmlNode* node) const;
01005   using plist::DictionaryBase::saveXML;
01006   
01007   //! clone definition for EllipsoidObstacle
01008   PLIST_CLONE_DEF(EllipsoidObstacle,new EllipsoidObstacle(*this));
01009 };
01010 
01011 #endif

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