00001
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
00030 template<> PlannerObstacle2D* loadXML(xmlNode* node);
00031 template<> PlannerObstacle3D* loadXML(xmlNode* node);
00032
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
00048
00049
00050 template <size_t N>
00051 class PlannerObstacle : public plist::Dictionary {
00052 public:
00053
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
00067 explicit PlannerObstacle(ObstacleGeometry geom, const std::string& t) :
00068 plist::Dictionary(), name(), type(t), geometry(geom), shapeId(-1), bodyObstacle(false) { init(); }
00069
00070
00071 virtual ~PlannerObstacle() {}
00072
00073
00074 template <typename T>
00075 static int sgn(T x) { return x < 0 ? -1 : 1; }
00076
00077
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
00085
00086
00087
00088
00089
00090
00091 virtual void updatePosition(const fmat::SubVector<N,const fmat::fmatReal>& newPos) = 0;
00092
00093
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
00098 void rotate(const fmat::SubMatrix<N,N,const fmat::fmatReal>& rot) { rotate(getCenter(), rot); }
00099
00100
00101 virtual fmat::Column<N> getCenter() const = 0;
00102
00103
00104 virtual BoundingBox<N> getBoundingBox() const = 0;
00105
00106
00107 bool collides(const PlannerObstacle<N>& other) const;
00108
00109
00110 virtual bool collides(const fmat::SubVector<N,const fmat::fmatReal>& point) const = 0;
00111
00112
00113
00114 virtual fmat::Column<N> getSupport(const fmat::SubVector<N,const fmat::fmatReal>& direction) const = 0;
00115
00116
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
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
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;
00160 bool bodyObstacle;
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
00171 class RectangularObstacle: public PlannerObstacle2D {
00172 protected:
00173 friend class ConvexPolyObstacle;
00174
00175
00176 static const std::string autoRegisterName;
00177
00178
00179 fmat::Column<2> center;
00180
00181
00182 fmat::Column<2> minEx;
00183
00184 fmat::Column<2> maxEx;
00185
00186
00187 BoundingBox2D bBox;
00188
00189
00190 fmat::Matrix<2,2> unrot;
00191
00192
00193
00194
00195
00196
00197 fmat::Matrix<4,2> points;
00198
00199 public:
00200
00201 RectangularObstacle(): PlannerObstacle2D(RECTANGULAR_OBS,autoRegisterName),
00202 center(), minEx(), maxEx(), bBox(),
00203 unrot(fmat::Matrix<2,2>::identity()), points() {}
00204
00205
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
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
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
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
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
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
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
00269 virtual fmat::Column<2> getCenter() const { return center; }
00270
00271
00272 float getWidth() const { return maxEx[0] - minEx[0]; }
00273
00274 float getHeight() const { return maxEx[1] - minEx[1]; }
00275
00276 fmat::Column<2> getExtents() const { return (maxEx-minEx)/2; }
00277
00278 float getOrientation() const;
00279
00280 enum CornerOrder {
00281 TOP_RIGHT,
00282 TOP_LEFT,
00283 BOTTOM_LEFT,
00284 BOTTOM_RIGHT,
00285 NUM_CORNERS
00286 };
00287
00288
00289 fmat::Column<2> getCorner(size_t i) const { return fmat::pack(points(i,0),points(i,1)); }
00290
00291
00292 virtual void bloat(float amount);
00293
00294
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
00302 PLIST_CLONE_DEF(RectangularObstacle,new RectangularObstacle(*this));
00303 };
00304
00305
00306 class CircularObstacle : public PlannerObstacle2D {
00307 protected:
00308
00309 static const std::string autoRegisterName;
00310
00311
00312 fmat::Column<2> center;
00313
00314
00315 fmat::fmatReal radius;
00316
00317 public:
00318
00319 CircularObstacle() : PlannerObstacle2D(CIRCULAR_OBS,autoRegisterName),
00320 center(), radius(0) {}
00321
00322
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
00327 CircularObstacle(const fmat::Column<2>& c, fmat::fmatReal r) : PlannerObstacle2D(CIRCULAR_OBS,autoRegisterName),
00328 center(c), radius(r) {}
00329
00330
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
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
00370 virtual void bloat(float amount) { radius += amount; }
00371
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
00379 PLIST_CLONE_DEF(CircularObstacle,new CircularObstacle(*this));
00380 };
00381
00382
00383 class EllipticalObstacle : public PlannerObstacle2D {
00384 protected:
00385
00386 static const std::string autoRegisterName;
00387
00388 public:
00389
00390 fmat::Column<2> focus1;
00391
00392
00393 fmat::Column<2> focus2;
00394
00395
00396 fmat::Column<2> center;
00397
00398
00399 fmat::fmatReal semimajor;
00400
00401
00402 fmat::fmatReal semiminor;
00403
00404
00405 EllipticalObstacle() : PlannerObstacle2D(ELLIPTICAL_OBS,autoRegisterName),
00406 focus1(), focus2(), center(), semimajor(), semiminor() {}
00407
00408
00409
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
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
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
00431
00432 void reset(const fmat::Column<2>& f1, const fmat::Column<2>& f2, fmat::fmatReal s);
00433
00434
00435
00436 void reset(const fmat::Column<2>& c, fmat::fmatReal smajor, fmat::fmatReal sminor, fmat::fmatReal ori);
00437
00438
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
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
00462 float getAngle() const { return std::atan2(focus1[1] - focus2[1], focus1[0] - focus2[0]); }
00463
00464
00465 fmat::Matrix<2,2> getOrientation() const;
00466
00467
00468
00469
00470 fmat::Column<2> getPointOnEdge(fmat::fmatReal theta) const {
00471 return getPointOnEdge(fmat::pack(std::cos(theta),std::sin(theta)));
00472 }
00473
00474
00475
00476 fmat::Column<2> getPointOnEdge(const fmat::Column<2>& direction) const;
00477
00478 virtual BoundingBox2D getBoundingBox() const;
00479
00480
00481
00482 virtual fmat::Column<2> gradient(const fmat::SubVector<2,const fmat::fmatReal>& pt) const;
00483
00484 virtual std::string toString() const;
00485
00486
00487 virtual void bloat(float amount) { reset(center, semimajor+amount, semiminor+amount, getAngle()); }
00488
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
00496 PLIST_CLONE_DEF(EllipticalObstacle,new EllipticalObstacle(*this));
00497 };
00498
00499
00500
00501 class ConvexPolyObstacle : public PlannerObstacle2D {
00502 protected:
00503
00504 static const std::string autoRegisterName;
00505
00506 std::vector<fmat::Column<2> > points;
00507 std::vector<fmat::Column<2> > normals;
00508
00509 public:
00510
00511 ConvexPolyObstacle() : PlannerObstacle2D(CONVEX_POLY_OBS,autoRegisterName), points(), normals() {}
00512
00513
00514 ConvexPolyObstacle& operator=(const ConvexPolyObstacle& o) {
00515 PlannerObstacle2D::operator=(o);
00516 points = o.points;
00517 normals = o.normals;
00518 return *this;
00519 }
00520
00521
00522 const std::vector<fmat::Column<2> >& getPoints() const { return points; }
00523
00524
00525 const std::vector<fmat::Column<2> >& getNormals() const { return normals; }
00526
00527
00528
00529
00530
00531
00532
00533 void hull(const std::set<fmat::Column<2> >& p);
00534
00535
00536 void clear() { points.clear(); normals.clear(); }
00537
00538
00539 void addPoint(const fmat::Column<2>& p);
00540
00541
00542 void close();
00543
00544 using PlannerObstacle2D::collides;
00545
00546 virtual bool collides(const fmat::SubVector<2,const fmat::fmatReal>& point) const;
00547
00548 bool collides(const RectangularObstacle& other) const;
00549
00550 bool collides(const CircularObstacle& other) const;
00551
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
00574 PLIST_CLONE_DEF(ConvexPolyObstacle,new ConvexPolyObstacle(*this));
00575 };
00576
00577
00578 class HierarchicalObstacle : public PlannerObstacle2D {
00579 protected:
00580
00581 static const std::string autoRegisterName;
00582
00583
00584 void recalculateBoundingBox();
00585
00586
00587 void expandBoundingBox(PlannerObstacle2D& other);
00588
00589 std::vector<PlannerObstacle2D*> components;
00590
00591 BoundingBox2D aabb;
00592
00593 fmat::Column<2> center;
00594 fmat::Matrix<2,2> rotation;
00595
00596 public:
00597
00598 HierarchicalObstacle()
00599 : PlannerObstacle2D(HIERARCHICAL_OBS,autoRegisterName), components(), aabb(), center(fmat::ZERO2), rotation(fmat::Matrix<2,2>::identity()) {}
00600
00601
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()));
00608 }
00609 }
00610
00611
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()));
00618 }
00619 aabb=o.aabb;
00620 center=o.center;
00621 rotation=o.rotation;
00622 return *this;
00623 }
00624
00625
00626 virtual ~HierarchicalObstacle() { clear(); }
00627
00628
00629 template <class T>
00630 void getComponents(std::vector<T> &obs) const;
00631
00632 const std::vector<PlannerObstacle2D*>& getComponents() const { return components; }
00633
00634
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
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
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
00662 virtual std::string componentsToString() const;
00663
00664
00665 void clear() {
00666 for (unsigned int i = 0; i < components.size(); i++)
00667 delete components[i];
00668 components.clear(); recalculateBoundingBox();
00669 }
00670
00671
00672
00673 void add(PlannerObstacle2D* o) { components.push_back(o); expandBoundingBox(*o); }
00674
00675
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
00691 static const std::string autoRegisterName;
00692
00693
00694 fmat::Column<3> center;
00695
00696
00697 fmat::Column<3> minEx;
00698
00699 fmat::Column<3> maxEx;
00700
00701
00702 BoundingBox3D bBox;
00703
00704
00705 fmat::Matrix<3,3> unrot;
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718 fmat::Matrix<8,3> points;
00719
00720 public:
00721
00722 BoxObstacle() : PlannerObstacle3D(BOX_OBS,autoRegisterName),
00723 center(), minEx(), maxEx(), bBox(), unrot(fmat::Matrix<3,3>::identity()), points() {}
00724
00725
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
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
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
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
00782 float getLength() const { return maxEx[0] - minEx[0]; }
00783
00784 float getWidth() const { return maxEx[1] - minEx[1]; }
00785
00786 float getHeight() const { return maxEx[2] - minEx[2]; }
00787
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
00800 };
00801
00802
00803 fmat::Column<3> getCorner(size_t i) const { return fmat::pack(points(i,0),points(i,1),points(i,2)); }
00804
00805
00806 virtual void bloat(float amount);
00807
00808
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
00816 PLIST_CLONE_DEF(BoxObstacle,new BoxObstacle(*this));
00817 };
00818
00819
00820 class CylindricalObstacle : public PlannerObstacle3D {
00821 protected:
00822
00823 static const std::string autoRegisterName;
00824
00825
00826 fmat::Column<3> center;
00827
00828
00829 fmat::Matrix<3,3> orientation;
00830
00831
00832 fmat::fmatReal radius;
00833
00834
00835 fmat::fmatReal halfHeight;
00836
00837 public:
00838
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
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
00882 PLIST_CLONE_DEF(CylindricalObstacle,new CylindricalObstacle(*this));
00883 };
00884
00885
00886 class SphericalObstacle : public PlannerObstacle3D {
00887 protected:
00888
00889 static const std::string autoRegisterName;
00890
00891
00892 fmat::Column<3> center;
00893
00894
00895 fmat::fmatReal radius;
00896
00897 public:
00898
00899 SphericalObstacle() : PlannerObstacle3D(SPHERICAL_OBS,autoRegisterName),
00900 center(), radius(0) {}
00901
00902
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
00907 SphericalObstacle(const fmat::Column<3>& c, fmat::fmatReal r) : PlannerObstacle3D(CIRCULAR_OBS,autoRegisterName),
00908 center(c), radius(r) {}
00909
00910
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
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
00946 PLIST_CLONE_DEF(SphericalObstacle,new SphericalObstacle(*this));
00947 };
00948
00949 class EllipsoidObstacle : public PlannerObstacle3D {
00950 protected:
00951
00952 static const std::string autoRegisterName;
00953
00954
00955 fmat::Column<3> center;
00956
00957
00958 fmat::Matrix<3,3> orientation;
00959
00960
00961 fmat::Column<3> extents;
00962
00963 public:
00964
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
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
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
01008 PLIST_CLONE_DEF(EllipsoidObstacle,new EllipsoidObstacle(*this));
01009 };
01010
01011 #endif