KrisLibrary  1.0.0
Geometric2DCSpace.h
1 #ifndef GEOMETRIC_2D_CSPACE_H
2 #define GEOMETRIC_2D_CSPACE_H
3 
4 #include "CSpace.h"
5 #include <KrisLibrary/math3d/geometry2d.h>
6 #include <vector>
7 using namespace Math3D;
8 using namespace std;
9 
10 class Geometric2DCollection
11 {
12  public:
13  enum Type { AABB, Triangle, Circle, Box };
14 
15  int NumObstacles() const;
16  Type ObstacleType(int obstacle) const;
17  int ObstacleIndex(int obstacle) const;
18  GeometricPrimitive2D Obstacle(int obstacle) const;
19  const char* ObstacleTypeName(int obstacle) const;
20 
21  void Add(const Triangle2D& tri);
22  void Add(const AABB2D& bbox);
23  void Add(const Box2D& box);
24  //void Add(const Polygon2D& poly);
25  void Add(const Circle2D& sphere);
26  void Add(const Geometric2DCollection& geom);
27  void Add(const GeometricPrimitive2D& geom);
28  void Clear();
29  void DrawGL() const;
30  void DrawOutlinesGL() const;
31  void ToPolygons(vector<vector<Vector2> >& polys) const;
32 
33  //distance query
34  Real Distance(const Vector2& x) const;
35  Real Distance(const Circle2D& circle) const;
36  //collision query
37  bool Collides(const Vector2& x) const;
38  bool Collides(const Segment2D& s) const;
39  bool Collides(const Circle2D& circle) const;
40  bool Collides(const Box2D& box) const;
41  bool Collides(const Triangle2D& tri) const;
42  bool Collides(const GeometricPrimitive2D& geom) const;
43  bool Collides(const Geometric2DCollection& geom) const;
44  //distance query
45  Real Distance(const Vector2& x,int obstacle) const;
46  Real Distance(const Circle2D& circle,int obstacle) const;
47  //collision query
48  bool Collides(const Vector2& x,int obstacle) const;
49  bool Collides(const Segment2D& s,int obstacle) const;
50  bool Collides(const Circle2D& circle,int obstacle) const;
51  bool Collides(const Box2D& box,int obstacle) const;
52  bool Collides(const Triangle2D& tri,int obstacle) const;
53  bool Collides(const GeometricPrimitive2D& geom,int obstacle) const;
54  bool Collides(const Geometric2DCollection& geom,int obstacle) const;
55 
56  void Transform(const RigidTransform2D& T);
57 
58  vector<AABB2D> aabbs;
59  vector<Box2D> boxes;
60  vector<Circle2D> circles;
61  vector<Triangle2D> triangles;
62 };
63 
69 class Geometric2DCSpace : public CSpace, public Geometric2DCollection
70 {
71 public:
72  Geometric2DCSpace();
73  void InitConstraints();
74  void DrawGL() const;
75 
76  Real ObstacleDistance(const Vector2& x) const;
77  Real ObstacleDistance(const Circle2D& circle) const;
78  bool ObstacleOverlap(const Segment2D& s) const;
79  bool ObstacleOverlap(const Circle2D& circle) const;
80  bool ObstacleOverlap(const Box2D& box) const;
81  bool ObstacleOverlap(const Triangle2D& tri) const;
82  bool ObstacleOverlap(const Segment2D& s,int obstacle) const;
83 
84  virtual int NumDimensions() const { return 2; }
85  virtual void Sample(Config& x);
86  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
87  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
88  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);
89  virtual Real Distance(const Config& x, const Config& y);
90  virtual Real ObstacleDistance(const Config& x) { return ObstacleDistance(Vector2(x(0),x(1))); }
91  virtual void Properties(PropertyMap&) const;
92 
93  bool euclideanSpace;
94  Real visibilityEpsilon;
95  AABB2D domain;
96 };
97 
98 class Geometric2DObstacleFreeSet : public CSet
99 {
100 public:
101  Geometric2DObstacleFreeSet(const GeometricPrimitive2D& obstacle);
102  Geometric2DObstacleFreeSet(const GeometricPrimitive2D& obstacle,const Geometric2DCollection& robot,bool translationOnly);
103  virtual ~Geometric2DObstacleFreeSet() {}
104  virtual int NumDimensions() { return translationOnly ? 2:3; }
105  virtual bool Contains(const Config& x);
106  virtual Real ObstacleDistance(const Config& x);
107 
108  GeometricPrimitive2D obstacle;
109  const Geometric2DCollection* robot;
110  bool translationOnly;
111 };
112 
113 
114 #endif
Math::Sample
Real Sample(const Interval &s)
Uniformly samples the given intervals.
Definition: sample.cpp:116
Math3D::Circle2D
A 2D circle class.
Definition: Circle2D.h:16
Math3D::Triangle2D
A 2D triangle class.
Definition: Triangle2D.h:25
Geometric2DCollection
Definition: Geometric2DCSpace.h:10
Geometric2DCSpace
a 2D cspace whose obstacles are geometric primitives.
Definition: Geometric2DCSpace.h:69
Config
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
CSpace
Motion planning configuration space base class. The configuration space implements an interpolation s...
Definition: CSpace.h:39
Geometric2DObstacleFreeSet
Definition: Geometric2DCSpace.h:98
std
Definition: rayprimitives.h:132
Math3D::Box2D
A 2D boxThe box is the unit square [0,1]^2 set in the scaled local coordinate system. That is, one corner is at the origin, and it has dimensions [dims.x,dims.y] in the coordinates given by {xbasis,ybasis}.
Definition: Box2D.h:20
Math3D::GeometricPrimitive2D
A generic 2D geometric primitive class.
Definition: geometry2d.h:24
Math3D::Segment2D
A 2D segment class.
Definition: Segment2D.h:17
Math3D
Contains all the definitions in the Math3D package.
Definition: AnyGeometry.h:13
Math3D::Vector2
A 2D vector class.
Definition: math3d/primitives.h:41
Math3D::AABB2D
A 2D axis-aligned bounding box.
Definition: AABB2D.h:13
PropertyMap
A simple map from keys to values.
Definition: PropertyMap.h:27
Math3D::RigidTransform2D
Same as above, but in 2D.
Definition: math3d/primitives.h:902
CSet
A subset of a CSpace, which establishes a constraint for a configuration must meet. Mathematically, this is a set S which imposes the constraint [q in S].
Definition: CSet.h:20
Geometric2DCSpace::ObstacleDistance
virtual Real ObstacleDistance(const Config &x)
for local planners using obstacle distance
Definition: Geometric2DCSpace.h:90
Generated by   doxygen 1.8.11