KrisLibrary  1.0.0
WorkspaceBound.h
1 #ifndef ROBOTICS_WORKSPACE_BOUND_H
2 #define ROBOTICS_WORKSPACE_BOUND_H
3 
4 #include <KrisLibrary/math3d/Line3D.h>
5 #include <KrisLibrary/math3d/Circle3D.h>
6 #include <KrisLibrary/math3d/Sphere3D.h>
8 #include <vector>
9 using namespace Math3D;
10 using namespace std;
11 
12 namespace Math {
13  struct AngleSet;
14  struct ClosedIntervalSet;
15 } //namespace Math
16 
17 
26 {
27  Vector3 center() const;
28  Real radius() const;
29  void getCircle(Circle3D&) const;
30  Vector3 eval(Real theta) const;
31 
32  Line3D axis;
33  Vector3 p;
34 };
35 
39 struct Arc3D : public AxisSweptPoint
40 {
41  AngleInterval interval;
42 };
43 
47 struct HollowBall
48 {
49  inline void setFull()
50  { center.setZero(); outerRadius = Inf; innerRadius = -Inf; }
51  inline void setEmpty()
52  { center.setZero(); outerRadius = -Inf; innerRadius = Inf; }
53  inline void setPoint(const Vector3& pt)
54  { center = pt; outerRadius = 0; innerRadius = -Inf; }
55  inline void setSphere(const Sphere3D& s)
56  { center = s.center; outerRadius = innerRadius = s.radius; }
57  inline void setBall(const Sphere3D& s)
58  { center = s.center; outerRadius = s.radius; innerRadius = -Inf; }
59  inline void setBallComplement(const Sphere3D& s)
60  { center = s.center; innerRadius = s.radius; outerRadius = Inf; }
61  inline bool isEmpty() const { return outerRadius < 0; }
62  inline bool isFull() const { return IsInf(outerRadius)==1 && innerRadius<0; }
63  inline bool isBall() const { return innerRadius < 0; }
64  inline bool isSphere() const { return innerRadius == outerRadius; }
65  inline void getOuterSphere(Sphere3D& s) const
66  { s.center=center; s.radius=outerRadius; }
67  inline void getInnerSphere(Sphere3D& s) const
68  { s.center=center; s.radius=innerRadius; }
69  bool contains(const Vector3& pt) const;
70  bool contains(const HollowBall& b) const;
71  bool containsBall(const Sphere3D& s) const;
72  bool containsSphere(const Sphere3D& s) const;
73  bool intersects(const Line3D& line, ClosedIntervalSet& intervals) const;
74  bool intersects(const AxisSweptPoint& circle, AngleSet& arcs) const;
75  bool intersects(const Arc3D& arc, AngleSet& arcs) const;
76  //returns the parameter of the line that is the closest point
77  Real closestPoint(const Line3D& line) const;
78  //returns the parameter of the swept point that is the closest point
79  Real closestPoint(const AxisSweptPoint& circle) const;
80 
81  Vector3 center;
82  Real outerRadius;
83  Real innerRadius;
84 };
85 
86 ostream& operator <<(ostream& out,const AngleInterval& i);
87 ostream& operator <<(ostream& out,const AngleSet& i);
88 
96 {
98  void SetEmpty();
99  void SetFull();
100  void SetPoint(const Vector3&);
101  void SetArc(const Arc3D&);
102  void SetCircle(const Circle3D&);
103  void SetSphere(const Sphere3D&);
104  void SetTransformed(const WorkspaceBound&, RigidTransform& T);
105  //sets the bound as a intersect b
106  bool SetIntersection(const WorkspaceBound& a, const WorkspaceBound& b);
107  bool InplaceIntersection(const WorkspaceBound& b);
108  //sets the bound as a+b where + is the minkowski sum
109  void SetMinkowskiSum(const WorkspaceBound& a, const WorkspaceBound& b);
110  void InplaceMinkowskiSum(const WorkspaceBound& b);
111  void RemoveRedundancies();
112  bool IsEmpty() const;
113  bool IsFull() const;
114  bool IsBall() const;
115  Real GetWidth() const;
116  void GetBounds(AABB3D&) const;
117  void GetBounds(Sphere3D&) const;
118  void GetBounds(HollowBall&) const;
119  bool Contains(const Vector3& pt) const;
120  bool Intersects(const Line3D& line, ClosedIntervalSet& intervals) const;
121  bool Intersects(const AxisSweptPoint& circle, AngleSet& arcs) const;
122  bool Intersects(const Arc3D& arc, AngleSet& arcs) const;
123  Real ClosestPoint(const Line3D& line) const;
124  Real ClosestPoint(const AxisSweptPoint& circle) const;
125 
126  std::vector<HollowBall> balls;
127  Real maxAngle;
128 };
129 
130 #if 0
131 struct WorkspaceBound
132 {
133  WorkspaceBound();
134  void SetEmpty();
135  void SetFull();
136  void SetPoint(const Vector3&);
137  void SetArc(const Arc&);
138  void SetCircle(const Circle3D&);
139  void SetSphere(const Sphere3D&);
140  void SetTransformed(const WorkspaceBound&, RigidTransform& T);
141  //sets the bound as a intersect b
142  bool SetIntersection(const WorkspaceBound& a, const WorkspaceBound& b);
143  bool InplaceIntersection(const WorkspaceBound& b);
144  //sets the bound as a+b where + is the minkowski sum
145  void SetMinkowskiSum(const WorkspaceBound& a, const WorkspaceBound& b);
146  void InplaceMinkowskiSum(const WorkspaceBound& b);
147  bool IsEmpty() const;
148  bool IsSphere() const;
149  Real GetWidth() const;
150  void GetBounds(AABB3D&) const;
151  void GetBounds(Sphere3D&) const;
152  bool Intersects(const Line3D& line, ClosedIntervalSet& intervals) const;
153  bool Intersects(const AxisSweptPoint& circle, AngleSet& arcs) const;
154  bool Intersects(const Arc& arc, AngleSet& arcs) const;
155  Real ClosestPoint(const Line3D& line) const;
156  Real ClosestPoint(const AxisSweptPoint& circle) const;
157 
158  Vector3 center;
159  Real outerRadius;
160  Real innerRadius;
161  Real maxAngle;
162 };
163 #endif
164 
165 #endif
int IsInf(double x)
Returns +1 if x is +inf, -1 if x is -inf, and 0 otherwise.
Definition: infnan.cpp:40
A 3D vector class.
Definition: math3d/primitives.h:136
Definition: rayprimitives.h:132
A 3D axis-aligned bounding box.
Definition: AABB3D.h:13
A 3-D annulus innerRadius <= |x-center| <= outerRadius.
Definition: WorkspaceBound.h:47
Bounds the workspace of a robot linkage with a simple bound.
Definition: WorkspaceBound.h:95
A rigid-body transformation.
Definition: math3d/primitives.h:820
A 3D sphere class.
Definition: Sphere3D.h:21
Definition: IntervalSet.h:24
A contiguous range of angles.
Definition: angle.h:33
Contains all the definitions in the Math3D package.
Definition: AnyGeometry.h:12
Planar (R2) rotation utilities.
A 2D circle in 3D space class.
Definition: Circle3D.h:21
A 3D line class.
Definition: Line3D.h:18
Contains all definitions in the Math package.
Definition: WorkspaceBound.h:12
An arc segment of a circle in space.
Definition: WorkspaceBound.h:39
Parameterizes a circle in space by sweeping a point around a line.
Definition: WorkspaceBound.h:25
A set of AngleIntervals.
Definition: AngleSet.h:12