visilibity 0.0.1
Sign up to get free protection for your applications and to get access to all the features.
- data/Rakefile +63 -0
- data/ext/VisiLibity/Makefile +150 -0
- data/ext/VisiLibity/README.visilibity +167 -0
- data/ext/VisiLibity/VisiLibity.i +49 -0
- data/ext/VisiLibity/VisiLibity_wrap.cxx +25164 -0
- data/ext/VisiLibity/extconf.rb +19 -0
- data/ext/VisiLibity/visilibity.cpp +3659 -0
- data/ext/VisiLibity/visilibity.hpp +2158 -0
- data/lib/VisiLibity_native.bundle +0 -0
- data/lib/visilibity.rb +131 -0
- metadata +64 -0
@@ -0,0 +1,2158 @@
|
|
1
|
+
/**
|
2
|
+
* \file visilibity.hpppp
|
3
|
+
* \authors Karl J. Obermeyer
|
4
|
+
* \date March 20, 2008
|
5
|
+
*
|
6
|
+
VisiLibity: A Floating-Point Visibility Algorithms Library,
|
7
|
+
Copyright (C) 2008 Karl J. Obermeyer (karl.obermeyer [ at ] gmail.com)
|
8
|
+
|
9
|
+
This file is part of VisiLibity.
|
10
|
+
|
11
|
+
VisiLibity is free software: you can redistribute it and/or modify it under
|
12
|
+
the terms of the GNU Lesser General Public License as published by the
|
13
|
+
Free Software Foundation, either version 3 of the License, or (at your
|
14
|
+
option) any later version.
|
15
|
+
|
16
|
+
VisiLibity is distributed in the hope that it will be useful, but WITHOUT
|
17
|
+
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
18
|
+
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
|
19
|
+
License for more details.
|
20
|
+
|
21
|
+
You should have received a copy of the GNU Lesser General Public
|
22
|
+
License along with VisiLibity. If not, see <http://www.gnu.org/licenses/>.
|
23
|
+
*/
|
24
|
+
|
25
|
+
/**
|
26
|
+
* \mainpage
|
27
|
+
* <center>
|
28
|
+
* see also the <a href="../index.html">VisiLibity Project Page</a>
|
29
|
+
* </center>
|
30
|
+
* <b>Authors</b>: Karl J. Obermeyer
|
31
|
+
* <hr>
|
32
|
+
* \section developers For Developers
|
33
|
+
* <a href="../VisiLibity.coding_standards.html">Coding Standards</a>
|
34
|
+
* <hr>
|
35
|
+
* \section release_notes Release Notes
|
36
|
+
* <b>Current Functionality</b>
|
37
|
+
* <ul>
|
38
|
+
* <li>visibility polygons in polygonal environments with holes</li>
|
39
|
+
* <li>visibility graphs</li>
|
40
|
+
* <li>shortest path planning for a point</li>
|
41
|
+
* </ul>
|
42
|
+
*/
|
43
|
+
|
44
|
+
#ifndef VISILIBITY_H
|
45
|
+
#define VISILIBITY_H
|
46
|
+
|
47
|
+
|
48
|
+
#include <cmath> //math functions in std namespace
|
49
|
+
#include <vector>
|
50
|
+
#include <queue> //queue and priority_queue.
|
51
|
+
#include <set> //priority queues with iteration,
|
52
|
+
//integrated keys
|
53
|
+
#include <list>
|
54
|
+
#include <algorithm> //sorting, min, max, reverse
|
55
|
+
#include <cstdlib> //rand and srand
|
56
|
+
#include <ctime> //Unix time
|
57
|
+
#include <fstream> //file I/O
|
58
|
+
#include <iostream>
|
59
|
+
#include <cstring> //C-string manipulation
|
60
|
+
#include <string> //string class
|
61
|
+
#include <cassert> //assertions
|
62
|
+
|
63
|
+
|
64
|
+
/// VisiLibity's sole namespace
|
65
|
+
namespace VisiLibity
|
66
|
+
{
|
67
|
+
|
68
|
+
//Fwd declaration of all classes and structs serves as index.
|
69
|
+
struct Bounding_Box;
|
70
|
+
class Point;
|
71
|
+
class Line_Segment;
|
72
|
+
class Angle;
|
73
|
+
class Ray;
|
74
|
+
class Polar_Point;
|
75
|
+
class Polyline;
|
76
|
+
class Polygon;
|
77
|
+
class Environment;
|
78
|
+
class Guards;
|
79
|
+
class Visibility_Polygon;
|
80
|
+
class Visibility_Graph;
|
81
|
+
|
82
|
+
|
83
|
+
/** \brief floating-point display precision.
|
84
|
+
*
|
85
|
+
* This is the default precision with which floating point
|
86
|
+
* numbers are displayed or written to files for classes with a
|
87
|
+
* write_to_file() method.
|
88
|
+
*/
|
89
|
+
const int FIOS_PRECISION = 10;
|
90
|
+
|
91
|
+
|
92
|
+
/** \brief get a uniform random sample from an (inclusive) interval
|
93
|
+
* on the real line
|
94
|
+
*
|
95
|
+
* \author Karl J. Obermeyer
|
96
|
+
* \param lower_bound lower bound of the real interval
|
97
|
+
* \param upper_bound upper bound of the real interval
|
98
|
+
* \pre \a lower_bound <= \a upper_bound
|
99
|
+
* \return a random sample from a uniform probability distribution
|
100
|
+
* on the real interval [\a lower_bound, \a upper_bound]
|
101
|
+
* \remarks Uses the Standard Library's rand() function. rand()
|
102
|
+
* should be seeded (only necessary once at the beginning of the
|
103
|
+
* program) using the command
|
104
|
+
* std::srand( std::time( NULL ) ); rand();
|
105
|
+
* \warning performance degrades as upper_bound - lower_bound
|
106
|
+
* approaches RAND_MAX.
|
107
|
+
*/
|
108
|
+
double uniform_random_sample(double lower_bound, double upper_bound);
|
109
|
+
|
110
|
+
|
111
|
+
/** \brief rectangle with sides parallel to the x- and y-axes
|
112
|
+
*
|
113
|
+
* \author Karl J. Obermeyer
|
114
|
+
* Useful for enclosing other geometric objects.
|
115
|
+
*/
|
116
|
+
struct Bounding_Box { double x_min, x_max, y_min, y_max; };
|
117
|
+
|
118
|
+
|
119
|
+
/// Point in the plane represented by Cartesian coordinates
|
120
|
+
class Point
|
121
|
+
{
|
122
|
+
public:
|
123
|
+
//Constructors
|
124
|
+
/** \brief default
|
125
|
+
*
|
126
|
+
* \remarks Data defaults to NAN so that checking whether the
|
127
|
+
* data are numbers can be used as a precondition in functions.
|
128
|
+
*/
|
129
|
+
Point() : x_(NAN) , y_(NAN) { }
|
130
|
+
/// costruct from raw coordinates
|
131
|
+
Point(double x_temp, double y_temp)
|
132
|
+
{ x_=x_temp; y_=y_temp; }
|
133
|
+
//Accessors
|
134
|
+
/// get x coordinate
|
135
|
+
double x () const { return x_; }
|
136
|
+
/// get y coordinate
|
137
|
+
double y () const { return y_; }
|
138
|
+
/** \brief closest Point on \a line_segment_temp
|
139
|
+
*
|
140
|
+
* \author Karl J. Obermeyer
|
141
|
+
* \pre the calling Point data are numbers
|
142
|
+
* and \a line_segment_temp is nonempty
|
143
|
+
* \return the Point on \a line_segment_temp which is the smallest
|
144
|
+
* Euclidean distance from the calling Point
|
145
|
+
*/
|
146
|
+
Point projection_onto(const Line_Segment& line_segment_temp) const;
|
147
|
+
/** \brief closest Point on \a ray_temp
|
148
|
+
*
|
149
|
+
* \author Karl J. Obermeyer
|
150
|
+
* \pre the calling Point and \a ray_temp data are numbers
|
151
|
+
* \return the Point on \a ray_temp which is the smallest
|
152
|
+
* Euclidean distance from the calling Point
|
153
|
+
*/
|
154
|
+
Point projection_onto(const Ray& ray_temp) const;
|
155
|
+
/** \brief closest Point on \a polyline_temp
|
156
|
+
*
|
157
|
+
* \pre the calling Point data are numbers and \a polyline_temp
|
158
|
+
* is nonempty
|
159
|
+
* \return the Point on \a polyline_temp which is the smallest
|
160
|
+
* Euclidean distance from the calling Point
|
161
|
+
*/
|
162
|
+
Point projection_onto(const Polyline& polyline_temp) const;
|
163
|
+
/** \brief closest vertex of \a polygon_temp
|
164
|
+
*
|
165
|
+
* \author Karl J. Obermeyer
|
166
|
+
* \pre the calling Point data are numbers and \a polygon_temp
|
167
|
+
* is nonempty
|
168
|
+
* \return the vertex of \a polygon_temp which is the
|
169
|
+
* smallest Euclidean distance from the calling Point
|
170
|
+
*/
|
171
|
+
Point projection_onto_vertices_of(const Polygon& polygon_temp) const;
|
172
|
+
/** \brief closest vertex of \a environment_temp
|
173
|
+
*
|
174
|
+
* \author Karl J. Obermeyer
|
175
|
+
* \pre the calling Point data are numbers and \a environment_temp
|
176
|
+
* is nonempty
|
177
|
+
* \return the vertex of \a environment_temp which is
|
178
|
+
* the smallest Euclidean distance from the calling Point
|
179
|
+
*/
|
180
|
+
Point projection_onto_vertices_of(const Environment&
|
181
|
+
enviroment_temp) const;
|
182
|
+
/** \brief closest Point on boundary of \a polygon_temp
|
183
|
+
*
|
184
|
+
* \author Karl J. Obermeyer
|
185
|
+
* \pre the calling Point data are numbers and \a polygon_temp
|
186
|
+
* is nonempty
|
187
|
+
* \return the Point on the boundary of \a polygon_temp which is the
|
188
|
+
* smallest Euclidean distance from the calling Point
|
189
|
+
*/
|
190
|
+
Point projection_onto_boundary_of(const Polygon& polygon_temp) const;
|
191
|
+
/** \brief closest Point on boundary of \a environment_temp
|
192
|
+
*
|
193
|
+
* \author Karl J. Obermeyer
|
194
|
+
* \pre the calling Point data are numbers and \a environment_temp
|
195
|
+
* is nonempty
|
196
|
+
* \return the Point on the boundary of \a environment_temp which is
|
197
|
+
* the smalles Euclidean distance from the calling Point
|
198
|
+
*/
|
199
|
+
Point projection_onto_boundary_of(const Environment&
|
200
|
+
enviroment_temp) const;
|
201
|
+
/** \brief true iff w/in \a epsilon of boundary of \a polygon_temp
|
202
|
+
*
|
203
|
+
* \author Karl J. Obermeyer
|
204
|
+
* \pre the calling Point data are numbers and \a polygon_temp
|
205
|
+
* is nonempty
|
206
|
+
* \return true iff the calling Point is within Euclidean distance
|
207
|
+
* \a epsilon of \a polygon_temp 's boundary
|
208
|
+
* \remarks O(n) time complexity, where n is the number
|
209
|
+
* of vertices of \a polygon_temp
|
210
|
+
*/
|
211
|
+
bool on_boundary_of(const Polygon& polygon_temp,
|
212
|
+
double epsilon=0.0) const;
|
213
|
+
/** \brief true iff w/in \a epsilon of boundary of \a environment_temp
|
214
|
+
*
|
215
|
+
* \author Karl J. Obermeyer
|
216
|
+
* \pre the calling Point data are numbers and \a environment_temp
|
217
|
+
* is nonempty
|
218
|
+
* \return true iff the calling Point is within Euclidean distance
|
219
|
+
* \a epsilon of \a environment_temp 's boundary
|
220
|
+
* \remarks O(n) time complexity, where n is the number
|
221
|
+
* of vertices of \a environment_temp
|
222
|
+
*/
|
223
|
+
bool on_boundary_of(const Environment& environment_temp,
|
224
|
+
double epsilon=0.0) const;
|
225
|
+
/** \brief true iff w/in \a epsilon of \a line_segment_temp
|
226
|
+
*
|
227
|
+
* \author Karl J. Obermeyer
|
228
|
+
* \pre the calling Point data are numbers and \a line_segment_temp
|
229
|
+
* is nonempty
|
230
|
+
* \return true iff the calling Point is within distance
|
231
|
+
* \a epsilon of the (closed) Line_Segment \a line_segment_temp
|
232
|
+
*/
|
233
|
+
bool in(const Line_Segment& line_segment_temp,
|
234
|
+
double epsilon=0.0) const;
|
235
|
+
/** \brief true iff w/in \a epsilon of interior but greater than
|
236
|
+
* \a espilon away from endpoints of \a line_segment_temp
|
237
|
+
*
|
238
|
+
* \author Karl J. Obermeyer
|
239
|
+
* \pre the calling Point data are numbers and \a line_segment_temp
|
240
|
+
* is nonempty
|
241
|
+
* \return true iff the calling Point is within distance \a
|
242
|
+
* epsilon of \line_segment_temp, but distance (strictly) greater
|
243
|
+
* than epsilon from \a line_segment_temp 's endpoints.
|
244
|
+
*/
|
245
|
+
bool in_relative_interior_of(const Line_Segment& line_segment_temp,
|
246
|
+
double epsilon=0.0) const;
|
247
|
+
/** \brief true iff w/in \a epsilon of \a polygon_temp
|
248
|
+
*
|
249
|
+
* \author Karl J. Obermeyer
|
250
|
+
*
|
251
|
+
* \pre the calling Point data are numbers and \a polygon_temp is
|
252
|
+
* \a epsilon -simple. Test simplicity with
|
253
|
+
* Polygon::is_simple(epsilon)
|
254
|
+
*
|
255
|
+
* \return true iff the calling Point is a Euclidean distance no greater
|
256
|
+
* than \a epsilon from the (closed) Polygon (with vertices listed
|
257
|
+
* either cw or ccw) \a polygon_temp.
|
258
|
+
* \remarks O(n) time complexity, where n is the number of vertices
|
259
|
+
* in \a polygon_temp
|
260
|
+
*/
|
261
|
+
bool in(const Polygon& polygon_temp,
|
262
|
+
double epsilon=0.0) const;
|
263
|
+
/** \brief true iff w/in \a epsilon of \a environment_temp
|
264
|
+
*
|
265
|
+
* \author Karl J. Obermeyer
|
266
|
+
*
|
267
|
+
* \pre the calling Point data are numbers and \a environment_temp
|
268
|
+
* is nonempty and \a epsilon -valid. Test validity with
|
269
|
+
* Enviroment::is_valid(epsilon)
|
270
|
+
*
|
271
|
+
* \return true iff the calling Point is a Euclidean distance no greater
|
272
|
+
* than \a epsilon from the in the (closed) Environment \a environment_temp
|
273
|
+
* \remarks O(n) time complexity, where n is the number of
|
274
|
+
* vertices in \a environment_temp
|
275
|
+
*/
|
276
|
+
bool in(const Environment& environment_temp,
|
277
|
+
double epsilon=0.0) const;
|
278
|
+
/** \brief true iff w/in \a epsilon of some endpoint
|
279
|
+
* of \a line_segment_temp
|
280
|
+
*
|
281
|
+
* \pre the calling Point data are numbers and \a line_segment_temp
|
282
|
+
* is nonempty
|
283
|
+
* \return true iff calling Point is a Euclidean distance no greater
|
284
|
+
* than \a epsilon from some endpoint of \a line_segment_temp
|
285
|
+
*/
|
286
|
+
bool is_endpoint_of(const Line_Segment& line_segment_temp,
|
287
|
+
double epsilon=0.0) const;
|
288
|
+
//Mutators
|
289
|
+
/// change x coordinate
|
290
|
+
void set_x(double x_temp) { x_ = x_temp;}
|
291
|
+
/// change y coordinate
|
292
|
+
void set_y(double y_temp) { y_ = y_temp;}
|
293
|
+
/** \brief relocate to closest vertex if w/in \a epsilon of some
|
294
|
+
* vertex (of \a polygon_temp)
|
295
|
+
*
|
296
|
+
* \author Karl J. Obermeyer
|
297
|
+
* \pre the calling Point data are numbers and \a polygon_temp
|
298
|
+
* is nonempty
|
299
|
+
* \post If the calling Point was a Euclidean distance no greater
|
300
|
+
* than \a epsilon from any vertex of \a polygon_temp, then it
|
301
|
+
* will be repositioned to coincide with the closest such vertex
|
302
|
+
* \remarks O(n) time complexity, where n is the number of
|
303
|
+
* vertices in \a polygon_temp.
|
304
|
+
*/
|
305
|
+
void snap_to_vertices_of(const Polygon& polygon_temp,
|
306
|
+
double epsilon=0.0);
|
307
|
+
/** \brief relocate to closest vertex if w/in \a epsilon of some
|
308
|
+
* vertex (of \a environment_temp)
|
309
|
+
*
|
310
|
+
* \author Karl J. Obermeyer
|
311
|
+
* \pre the calling Point data are numbers and \a environment_temp
|
312
|
+
* is nonempty
|
313
|
+
* \post If the calling Point was a Euclidean distance no greater
|
314
|
+
* than \a epsilon from any vertex of \a environment_temp, then it
|
315
|
+
* will be repositioned to coincide with the closest such vertex
|
316
|
+
* \remarks O(n) time complexity, where n is the number of
|
317
|
+
* vertices in \a environment_temp.
|
318
|
+
*/
|
319
|
+
void snap_to_vertices_of(const Environment& environment_temp,
|
320
|
+
double epsilon=0.0);
|
321
|
+
/** \brief relocate to closest Point on boundary if w/in \a epsilon
|
322
|
+
* of the boundary (of \a polygon_temp)
|
323
|
+
*
|
324
|
+
* \author Karl J. Obermeyer
|
325
|
+
* \pre the calling Point data are numbers and \a polygon_temp
|
326
|
+
* is nonempty
|
327
|
+
* \post if the calling Point was a Euclidean distance no greater
|
328
|
+
* than \a epsilon from the boundary of \a polygon_temp, then it
|
329
|
+
* will be repositioned to it's projection onto that boundary
|
330
|
+
* \remarks O(n) time complexity, where n is the number of
|
331
|
+
* vertices in \a polygon_temp.
|
332
|
+
*/
|
333
|
+
void snap_to_boundary_of(const Polygon& polygon_temp,
|
334
|
+
double epsilon=0.0);
|
335
|
+
/** \brief relocate to closest Point on boundary if w/in \a epsilon
|
336
|
+
* of the boundary (of \a environment_temp)
|
337
|
+
*
|
338
|
+
* \author Karl J. Obermeyer
|
339
|
+
* \pre the calling Point data are numbers and \a environment_temp
|
340
|
+
* is nonempty
|
341
|
+
* \post if the calling Point was a Euclidean distance no greater
|
342
|
+
* than \a epsilon from the boundary of \a environment_temp, then it
|
343
|
+
* will be repositioned to it's projection onto that boundary
|
344
|
+
* \remarks O(n) time complexity, where n is the number of
|
345
|
+
* vertices in \a environment_temp.
|
346
|
+
*/
|
347
|
+
void snap_to_boundary_of(const Environment& environment_temp,
|
348
|
+
double epsilon=0.0);
|
349
|
+
protected:
|
350
|
+
double x_;
|
351
|
+
double y_;
|
352
|
+
};
|
353
|
+
|
354
|
+
|
355
|
+
/** \brief True iff Points' coordinates are identical.
|
356
|
+
*
|
357
|
+
* \remarks NAN==NAN returns false, so if either point has
|
358
|
+
* not been assigned real number coordinates, they will not be ==
|
359
|
+
*/
|
360
|
+
bool operator == (const Point& point1, const Point& point2);
|
361
|
+
/// True iff Points' coordinates are not identical.
|
362
|
+
bool operator != (const Point& point1, const Point& point2);
|
363
|
+
|
364
|
+
|
365
|
+
/** \brief compare lexicographic order of points
|
366
|
+
*
|
367
|
+
* For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or
|
368
|
+
* p1.x()==p2.x() and p1.y()<p2.y(). False if any member data have
|
369
|
+
* not been assigned (numbers).
|
370
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
371
|
+
* two Points nearly define a line parallel to one of the axes
|
372
|
+
*/
|
373
|
+
bool operator < (const Point& point1, const Point& point2);
|
374
|
+
/** \brief compare lexicographic order of points
|
375
|
+
*
|
376
|
+
* For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or
|
377
|
+
* p1.x()==p2.x() and p1.y()<p2.y(). False if any member data have
|
378
|
+
* not been assigned (numbers).
|
379
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
380
|
+
* two Points nearly define a line parallel to one of the axes
|
381
|
+
*/
|
382
|
+
bool operator > (const Point& point1, const Point& point2);
|
383
|
+
/** \brief compare lexicographic order of points
|
384
|
+
*
|
385
|
+
* For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or
|
386
|
+
* p1.x()==p2.x() and p1.y()<p2.y(). False if any member data have
|
387
|
+
* not been assigned (numbers).
|
388
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
389
|
+
* two Points nearly define a line parallel to one of the axes
|
390
|
+
*/
|
391
|
+
bool operator >= (const Point& point1, const Point& point2);
|
392
|
+
/** \brief compare lexicographic order of points
|
393
|
+
*
|
394
|
+
* For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or
|
395
|
+
* p1.x()==p2.x() and p1.y()<p2.y(). False if any member data have
|
396
|
+
* not been assigned (numbers).
|
397
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
398
|
+
* two Points nearly define a line parallel to one of the axes
|
399
|
+
*/
|
400
|
+
bool operator <= (const Point& point1, const Point& point2);
|
401
|
+
|
402
|
+
|
403
|
+
/// vector addition of Points
|
404
|
+
Point operator + (const Point& point1, const Point& point2);
|
405
|
+
/// vector subtraction of Points
|
406
|
+
Point operator - (const Point& point1, const Point& point2);
|
407
|
+
|
408
|
+
|
409
|
+
//// dot (scalar) product treats the Points as vectors
|
410
|
+
Point operator * (const Point& point1, const Point& point2);
|
411
|
+
|
412
|
+
|
413
|
+
/// simple scaling treats the Point as a vector
|
414
|
+
Point operator * (double scalar, const Point& point2);
|
415
|
+
/// simple scaling treats the Point as a vector
|
416
|
+
Point operator * (const Point& point1, double scalar);
|
417
|
+
|
418
|
+
|
419
|
+
/** \brief cross product (signed) magnitude treats the Points as vectors
|
420
|
+
*
|
421
|
+
* \author Karl J. Obermeyer
|
422
|
+
* \pre Points' data are numbers
|
423
|
+
* \remarks This is equal to the (signed) area of the parallelogram created
|
424
|
+
* by the Points viewed as vectors.
|
425
|
+
*/
|
426
|
+
double cross(const Point& point1, const Point& point2);
|
427
|
+
|
428
|
+
|
429
|
+
/** \brief Euclidean distance between Points
|
430
|
+
*
|
431
|
+
* \pre Points' data are numbers
|
432
|
+
*/
|
433
|
+
double distance(const Point& point1, const Point& point2);
|
434
|
+
|
435
|
+
|
436
|
+
/** \brief Euclidean distance between a Point and a Line_Segment
|
437
|
+
*
|
438
|
+
* \author Karl J. Obermeyer
|
439
|
+
* \pre Point's data are numbers and the Line_Segment is nonempty
|
440
|
+
*/
|
441
|
+
double distance(const Point& point_temp,
|
442
|
+
const Line_Segment& line_segment_temp);
|
443
|
+
/** \brief Euclidean distance between a Point and a Line_Segment
|
444
|
+
*
|
445
|
+
* \author Karl J. Obermeyer
|
446
|
+
* \pre Point's data are numbers and the Line_Segment is nonempty
|
447
|
+
*/
|
448
|
+
double distance(const Line_Segment& line_segment_temp,
|
449
|
+
const Point& point_temp);
|
450
|
+
|
451
|
+
|
452
|
+
/** \brief Euclidean distance between a Point and a Ray
|
453
|
+
*
|
454
|
+
* \author Karl J. Obermeyer
|
455
|
+
* \pre Point's and Ray's data are numbers
|
456
|
+
*/
|
457
|
+
double distance(const Point& point_temp,
|
458
|
+
const Ray& ray_temp);
|
459
|
+
/** \brief Euclidean distance between a Point and a Ray
|
460
|
+
*
|
461
|
+
* \author Karl J. Obermeyer
|
462
|
+
* \pre Point's and Ray's data are numbers
|
463
|
+
*/
|
464
|
+
double distance(const Ray& ray_temp,
|
465
|
+
const Point& point_temp);
|
466
|
+
|
467
|
+
|
468
|
+
/** \brief Euclidean distance between a Point and a Polyline
|
469
|
+
*
|
470
|
+
* \author Karl J. Obermeyer
|
471
|
+
* \pre Point's data are numbers and the Polyline is nonempty
|
472
|
+
*/
|
473
|
+
double distance(const Point& point_temp,
|
474
|
+
const Polyline& polyline_temp);
|
475
|
+
/** \brief Euclidean distance between a Point and a Polyline
|
476
|
+
*
|
477
|
+
* \author Karl J. Obermeyer
|
478
|
+
* \pre Point's data are numbers and the Polyline is nonempty
|
479
|
+
*/
|
480
|
+
double distance(const Polyline& polyline_temp,
|
481
|
+
const Point& point_temp);
|
482
|
+
|
483
|
+
|
484
|
+
/** \brief Euclidean distance between a Point and a Polygon's boundary
|
485
|
+
*
|
486
|
+
* \author Karl J. Obermeyer
|
487
|
+
* \pre Point's data are numbers and the Polygon is nonempty
|
488
|
+
*/
|
489
|
+
double boundary_distance(const Point& point_temp,
|
490
|
+
const Polygon& polygon_temp);
|
491
|
+
/** \brief Euclidean distance between a Point and a Polygon's boundary
|
492
|
+
*
|
493
|
+
* \author Karl J. Obermeyer
|
494
|
+
* \pre Point's data are numbers and the Polygon is nonempty
|
495
|
+
*/
|
496
|
+
double boundary_distance(const Polygon& polygon_temp,
|
497
|
+
const Point& point_temp);
|
498
|
+
|
499
|
+
|
500
|
+
/** \brief Euclidean distance between a Point and a Environment's boundary
|
501
|
+
*
|
502
|
+
* \author Karl J. Obermeyer
|
503
|
+
* \pre Point's data are numbers and the Environment is nonempty
|
504
|
+
*/
|
505
|
+
double boundary_distance(const Point& point_temp,
|
506
|
+
const Environment& environment_temp);
|
507
|
+
/** \brief Euclidean distance between a Point and a Environment's boundary
|
508
|
+
*
|
509
|
+
* \author Karl J. Obermeyer
|
510
|
+
* \pre Point's data are numbers and the Environment is nonempty
|
511
|
+
*/
|
512
|
+
double boundary_distance(const Environment& environment_temp,
|
513
|
+
const Point& point_temp);
|
514
|
+
|
515
|
+
|
516
|
+
/// print a Point
|
517
|
+
std::ostream& operator << (std::ostream& outs, const Point& point_temp);
|
518
|
+
|
519
|
+
|
520
|
+
/** \brief line segment in the plane represented by its endpoints
|
521
|
+
*
|
522
|
+
* Closed and oriented line segment in the plane represented by its endpoints.
|
523
|
+
* \remarks may be degenerate (colocated endpoints) or empty
|
524
|
+
*/
|
525
|
+
class Line_Segment
|
526
|
+
{
|
527
|
+
public:
|
528
|
+
//Constructors
|
529
|
+
/// default to empty
|
530
|
+
Line_Segment();
|
531
|
+
/// copy constructor
|
532
|
+
Line_Segment(const Line_Segment& line_segment_temp);
|
533
|
+
/// construct degenerate segment from a single Point
|
534
|
+
Line_Segment(const Point& point_temp);
|
535
|
+
/// Line_Segment pointing from first_point_temp to second_point_temp
|
536
|
+
Line_Segment(const Point& first_point_temp,
|
537
|
+
const Point& second_point_temp, double epsilon=0);
|
538
|
+
//Accessors
|
539
|
+
/** \brief first endpoint
|
540
|
+
*
|
541
|
+
* \pre size() > 0
|
542
|
+
* \return the first Point of the Line_Segment
|
543
|
+
* \remarks If size() == 1, then both first() and second() are valid
|
544
|
+
* and will return the same Point
|
545
|
+
*/
|
546
|
+
Point first() const;
|
547
|
+
/** \brief second endpoint
|
548
|
+
*
|
549
|
+
* \pre size() > 0
|
550
|
+
* \return the second Point of the Line_Segment
|
551
|
+
* \remarks If size() == 1, then both first() and second() are valid
|
552
|
+
* and will return the same Point
|
553
|
+
*/
|
554
|
+
Point second() const;
|
555
|
+
/** \brief number of distinct endpoints
|
556
|
+
*
|
557
|
+
* \remarks
|
558
|
+
* size 0 => empty line segment;
|
559
|
+
* size 1 => degenerate (single point) line segment;
|
560
|
+
* size 2 => full-fledged (bona fide) line segment
|
561
|
+
*/
|
562
|
+
unsigned size() const { return size_; }
|
563
|
+
/** \brief midpoint
|
564
|
+
*
|
565
|
+
* \pre size() > 0
|
566
|
+
*/
|
567
|
+
Point midpoint() const;
|
568
|
+
/** \brief Euclidean length
|
569
|
+
*
|
570
|
+
* \pre size() > 0
|
571
|
+
*/
|
572
|
+
double length() const;
|
573
|
+
/** \brief true iff vertices in lex. order
|
574
|
+
*
|
575
|
+
* \pre size() > 0
|
576
|
+
* \return true iff vertices are listed beginning with the vertex
|
577
|
+
* which is lexicographically smallest (lowest x, then lowest y)
|
578
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
579
|
+
* two Points nearly define a line parallel to one of the axes
|
580
|
+
*/
|
581
|
+
bool is_in_standard_form() const;
|
582
|
+
//Mutators
|
583
|
+
/// assignment operator
|
584
|
+
Line_Segment& operator = (const Line_Segment& line_segment_temp);
|
585
|
+
/** \brief set first endpoint
|
586
|
+
*
|
587
|
+
* \remarks if \a point_temp is w/in a distance \a epsilon of an existing
|
588
|
+
* endpoint, the coordinates of \a point_temp are used and size is set to
|
589
|
+
* 1 as appropriate
|
590
|
+
*/
|
591
|
+
void set_first(const Point& point_temp, double epsilon=0.0);
|
592
|
+
/** \brief set second endpoint
|
593
|
+
*
|
594
|
+
* \remarks if \a point_temp is w/in a distance \a epsilon of an existing
|
595
|
+
* endpoint, the coordinates of \a point_temp are used and size is set to
|
596
|
+
* 1 as appropriate
|
597
|
+
*/
|
598
|
+
void set_second(const Point& point_temp, double epsilon=0.0);
|
599
|
+
/** \brief reverse order of endpoints
|
600
|
+
*
|
601
|
+
* \post order of endpoints is reversed.
|
602
|
+
*/
|
603
|
+
void reverse();
|
604
|
+
/** \brief enforce that lex. smallest endpoint first
|
605
|
+
*
|
606
|
+
* \post the lexicographically smallest endpoint (lowest x, then lowest y)
|
607
|
+
* is first
|
608
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
609
|
+
* two Points nearly define a line parallel to one of the axes
|
610
|
+
*/
|
611
|
+
void enforce_standard_form();
|
612
|
+
/// erase both endpoints and set line segment empty (size 0)
|
613
|
+
void clear();
|
614
|
+
/// destructor
|
615
|
+
virtual ~Line_Segment();
|
616
|
+
protected:
|
617
|
+
//Pointer to dynamic array of endpoints.
|
618
|
+
Point *endpoints_;
|
619
|
+
//See size() comments.
|
620
|
+
unsigned size_;
|
621
|
+
};
|
622
|
+
|
623
|
+
|
624
|
+
/** \brief true iff endpoint coordinates are exactly equal, but
|
625
|
+
* false if either Line_Segment has size 0
|
626
|
+
*
|
627
|
+
* \remarks respects ordering of vertices, i.e., even if the line segments
|
628
|
+
* overlap exactly, they are not considered == unless the orientations are
|
629
|
+
* the same
|
630
|
+
*/
|
631
|
+
bool operator == (const Line_Segment& line_segment1,
|
632
|
+
const Line_Segment& line_segment2);
|
633
|
+
/// true iff endpoint coordinates are not ==
|
634
|
+
bool operator != (const Line_Segment& line_segment1,
|
635
|
+
const Line_Segment& line_segment2);
|
636
|
+
|
637
|
+
|
638
|
+
/** \brief true iff line segments' endpoints match up w/in a (closed)
|
639
|
+
* \a epsilon ball of each other, but false if either
|
640
|
+
* Line_Segment has size 0
|
641
|
+
*
|
642
|
+
* \author Karl J. Obermeyer
|
643
|
+
* \remarks this function will return true even if it has to flip
|
644
|
+
* the orientation of one of the segments to get the vertices to
|
645
|
+
* match up
|
646
|
+
*/
|
647
|
+
bool equivalent(Line_Segment line_segment1,
|
648
|
+
Line_Segment line_segment2, double epsilon=0);
|
649
|
+
|
650
|
+
|
651
|
+
/** \brief Euclidean distance between Line_Segments
|
652
|
+
*
|
653
|
+
* \author Karl J. Obermeyer
|
654
|
+
* \pre \a line_segment1.size() > 0 and \a line_segment2.size() > 0
|
655
|
+
*/
|
656
|
+
double distance(const Line_Segment& line_segment1,
|
657
|
+
const Line_Segment& line_segment2);
|
658
|
+
|
659
|
+
|
660
|
+
/** \brief Euclidean distance between a Line_Segment and the
|
661
|
+
* boundary of a Polygon
|
662
|
+
*
|
663
|
+
* \author Karl J. Obermeyer
|
664
|
+
* \pre \a line_segment.size() > 0 and \a polygon.n() > 0
|
665
|
+
*/
|
666
|
+
double boundary_distance(const Line_Segment& line_segment,
|
667
|
+
const Polygon& polygon);
|
668
|
+
|
669
|
+
|
670
|
+
/** \brief Euclidean distance between a Line_Segment and the
|
671
|
+
* boundary of a Polygon
|
672
|
+
*
|
673
|
+
* \author Karl J. Obermeyer
|
674
|
+
* \pre \a line_segment.size() > 0 and \a polygon.n() > 0
|
675
|
+
*/
|
676
|
+
double boundary_distance(const Polygon& polygon,
|
677
|
+
const Line_Segment& line_segment);
|
678
|
+
|
679
|
+
|
680
|
+
/** \brief true iff the Euclidean distance between Line_Segments is
|
681
|
+
* no greater than \a epsilon, false if either line segment
|
682
|
+
* has size 0
|
683
|
+
*
|
684
|
+
* \author Karl J. Obermeyer
|
685
|
+
*/
|
686
|
+
bool intersect(const Line_Segment& line_segment1,
|
687
|
+
const Line_Segment& line_segment2,
|
688
|
+
double epsilon=0.0);
|
689
|
+
|
690
|
+
|
691
|
+
/** \brief true iff line segments intersect properly w/in epsilon,
|
692
|
+
* false if either line segment has size 0
|
693
|
+
*
|
694
|
+
* \author Karl J. Obermeyer
|
695
|
+
* \return true iff Line_Segments intersect exactly at a single
|
696
|
+
* point in their relative interiors. For robustness, here the
|
697
|
+
* relative interior of a Line_Segment is consider to be any Point
|
698
|
+
* in the Line_Segment which is a distance greater than \a epsilon
|
699
|
+
* from both endpoints.
|
700
|
+
*/
|
701
|
+
bool intersect_proper(const Line_Segment& line_segment1,
|
702
|
+
const Line_Segment& line_segment2,
|
703
|
+
double epsilon=0.0);
|
704
|
+
|
705
|
+
|
706
|
+
/** \brief intersection of Line_Segments
|
707
|
+
*
|
708
|
+
* \author Karl J. Obermeyer
|
709
|
+
* \return a Line_Segment of size 0, 1, or 2
|
710
|
+
* \remarks size 0 results if the distance (or at least the
|
711
|
+
* floating-point computed distance) between line_segment1 and
|
712
|
+
* line_segment2 is (strictly) greater than epsilon. size 1 results
|
713
|
+
* if the segments intersect poperly, form a T intersection, or --
|
714
|
+
* intersection. size 2 results when two or more endpoints are a
|
715
|
+
* Euclidean distance no greater than \a epsilon from the opposite
|
716
|
+
* segment, and the overlap of the segments has a length greater
|
717
|
+
* than \a epsilon.
|
718
|
+
*/
|
719
|
+
Line_Segment intersection(const Line_Segment& line_segment1,
|
720
|
+
const Line_Segment& line_segment2,
|
721
|
+
double epsilon=0.0);
|
722
|
+
|
723
|
+
|
724
|
+
/// print a Line_Segment
|
725
|
+
std::ostream& operator << (std::ostream& outs,
|
726
|
+
const Line_Segment& line_segment_temp);
|
727
|
+
|
728
|
+
|
729
|
+
/** \brief angle in radians represented by a value in
|
730
|
+
* the interval [0,2*M_PI]
|
731
|
+
*
|
732
|
+
* \remarks the intended interpretation is that angles 0 and 2*M_PI
|
733
|
+
* correspond to the positive x-axis of the coordinate system
|
734
|
+
*/
|
735
|
+
class Angle
|
736
|
+
{
|
737
|
+
public:
|
738
|
+
//Constructors
|
739
|
+
/** \brief default
|
740
|
+
*
|
741
|
+
* \remarks data defaults to NAN so that checking whether the
|
742
|
+
* data are numbers can be used as a precondition in functions
|
743
|
+
*/
|
744
|
+
Angle() : angle_radians_(NAN) { }
|
745
|
+
/// construct from real value, mod into interval [0, 2*M_PI)
|
746
|
+
Angle(double data_temp);
|
747
|
+
/** \brief construct using 4 quadrant inverse tangent into [0, 2*M_PI),
|
748
|
+
* where 0 points along the x-axis
|
749
|
+
*/
|
750
|
+
Angle(double rise_temp, double run_temp);
|
751
|
+
//Accessors
|
752
|
+
/// get radians
|
753
|
+
double get() const { return angle_radians_; }
|
754
|
+
//Mutators
|
755
|
+
/// set angle, mod into interval [0, 2*PI)
|
756
|
+
void set(double data_temp);
|
757
|
+
/** \brief set angle data to 2*M_PI
|
758
|
+
*
|
759
|
+
* \remarks sometimes it is necessary to set the angle value to
|
760
|
+
* 2*M_PI instead of 0, so that the lex. inequalities behave
|
761
|
+
* appropriately during a radial line sweep
|
762
|
+
*/
|
763
|
+
void set_to_2pi() { angle_radians_=2*M_PI; }
|
764
|
+
/// set to new random angle in [0, 2*M_PI)
|
765
|
+
void randomize();
|
766
|
+
private:
|
767
|
+
double angle_radians_;
|
768
|
+
};
|
769
|
+
|
770
|
+
|
771
|
+
/// compare angle radians
|
772
|
+
bool operator == (const Angle& angle1, const Angle& angle2);
|
773
|
+
/// compare angle radians
|
774
|
+
bool operator != (const Angle& angle1, const Angle& angle2);
|
775
|
+
|
776
|
+
|
777
|
+
/// compare angle radians
|
778
|
+
bool operator > (const Angle& angle1, const Angle& angle2);
|
779
|
+
/// compare angle radians
|
780
|
+
bool operator < (const Angle& angle1, const Angle& angle2);
|
781
|
+
/// compare angle radians
|
782
|
+
bool operator >= (const Angle& angle1, const Angle& angle2);
|
783
|
+
/// compare angle radians
|
784
|
+
bool operator <= (const Angle& angle1, const Angle& angle2);
|
785
|
+
|
786
|
+
|
787
|
+
/// add angles' radians and mod into [0, 2*M_PI)
|
788
|
+
Angle operator + (const Angle& angle1, const Angle& angle2);
|
789
|
+
/// subtract angles' radians and mod into [0, 2*M_PI)
|
790
|
+
Angle operator - (const Angle& angle1, const Angle& angle2);
|
791
|
+
|
792
|
+
|
793
|
+
/** \brief geodesic distance in radians between Angles
|
794
|
+
*
|
795
|
+
* \author Karl J. Obermeyer
|
796
|
+
* \pre \a angle1 and \a angle2 data are numbers
|
797
|
+
*/
|
798
|
+
double geodesic_distance(const Angle& angle1, const Angle& angle2);
|
799
|
+
|
800
|
+
|
801
|
+
/** \brief 1.0 => geodesic path from angle1 to angle2
|
802
|
+
* is couterclockwise, -1.0 => clockwise
|
803
|
+
*
|
804
|
+
* \author Karl J. Obermeyer
|
805
|
+
* \pre \a angle1 and \a angle2 data are numbers
|
806
|
+
*/
|
807
|
+
double geodesic_direction(const Angle& angle1, const Angle& angle2);
|
808
|
+
|
809
|
+
|
810
|
+
/// print Angle
|
811
|
+
std::ostream& operator << (std::ostream& outs, const Angle& angle_temp);
|
812
|
+
|
813
|
+
|
814
|
+
/** \brief Point in the plane packaged together with polar
|
815
|
+
* coordinates w.r.t. specified origin
|
816
|
+
*
|
817
|
+
* The origin of the polar coordinate system is stored with the
|
818
|
+
* Polar_Point (in \a polar_origin_) and bearing is measured ccw from the
|
819
|
+
* positive x-axis.
|
820
|
+
* \remarks used, e.g., for radial line sweeps
|
821
|
+
*/
|
822
|
+
class Polar_Point : public Point
|
823
|
+
{
|
824
|
+
public:
|
825
|
+
//Constructors
|
826
|
+
/** \brief default
|
827
|
+
*
|
828
|
+
* \remarks Data defaults to NAN so that checking whether the
|
829
|
+
* data are numbers can be used as a precondition in functions.
|
830
|
+
*/
|
831
|
+
Polar_Point() : Point(), range_(NAN), bearing_(NAN) { }
|
832
|
+
/** \brief construct from (Cartesian) Points
|
833
|
+
*
|
834
|
+
* \pre member data of \a polar_origin_temp and \a point_temp have
|
835
|
+
* been assigned (numbers)
|
836
|
+
* \param polar_origin_temp the origin of the polar coordinate system
|
837
|
+
* \param point_temp the point to be represented
|
838
|
+
* \remarks if polar_origin_temp == point_temp, the default
|
839
|
+
* bearing is Angle(0.0)
|
840
|
+
*/
|
841
|
+
Polar_Point(const Point& polar_origin_temp,
|
842
|
+
const Point& point_temp,
|
843
|
+
double epsilon=0.0);
|
844
|
+
//Accessors
|
845
|
+
/** \brief origin of the polar coordinate system in which the point is
|
846
|
+
* represented
|
847
|
+
*/
|
848
|
+
Point polar_origin() const { return polar_origin_; }
|
849
|
+
/// Euclidean distance from the point represented to the origin of
|
850
|
+
/// the polar coordinate system
|
851
|
+
double range() const { return range_; }
|
852
|
+
/// bearing from polar origin w.r.t. direction parallel to x-axis
|
853
|
+
Angle bearing() const { return bearing_; }
|
854
|
+
//Mutators
|
855
|
+
/** \brief set the origin of the polar coordinate system
|
856
|
+
*
|
857
|
+
* \remarks x and y held constant, bearing and range modified
|
858
|
+
* accordingly
|
859
|
+
*/
|
860
|
+
void set_polar_origin(const Point& polar_origin_temp);
|
861
|
+
/** \brief set x
|
862
|
+
*
|
863
|
+
* \remarks polar_origin held constant, bearing and range modified
|
864
|
+
* accordingly
|
865
|
+
*/
|
866
|
+
void set_x(double x_temp);
|
867
|
+
/** \brief set y
|
868
|
+
*
|
869
|
+
* \remarks polar_origin held constant, bearing and range modified
|
870
|
+
* accordingly
|
871
|
+
*/
|
872
|
+
void set_y(double y_temp);
|
873
|
+
/** \brief set range
|
874
|
+
*
|
875
|
+
* \remarks polar_origin held constant, x and y modified
|
876
|
+
* accordingly
|
877
|
+
*/
|
878
|
+
void set_range(double range_temp);
|
879
|
+
/** \brief set bearing
|
880
|
+
*
|
881
|
+
* \remarks polar_origin and range held constant, x and y modified
|
882
|
+
* accordingly
|
883
|
+
*/
|
884
|
+
void set_bearing(const Angle& bearing_temp);
|
885
|
+
/** \brief set bearing Angle data to 2*M_PI
|
886
|
+
*
|
887
|
+
* \remarks Special function for use in computations involving a
|
888
|
+
* radial line sweep; sometimes it is necessary to set the angle
|
889
|
+
* value to 2*PI instead of 0, so that the lex. inequalities
|
890
|
+
* behave appropriately
|
891
|
+
*/
|
892
|
+
void set_bearing_to_2pi() { bearing_.set_to_2pi(); }
|
893
|
+
protected:
|
894
|
+
//Origin of the polar coordinate system in world coordinates.
|
895
|
+
Point polar_origin_;
|
896
|
+
//Polar coordinates where radius always positive, and angle
|
897
|
+
//measured ccw from the world coordinate system's x-axis.
|
898
|
+
double range_;
|
899
|
+
Angle bearing_;
|
900
|
+
};
|
901
|
+
|
902
|
+
|
903
|
+
/** \brief compare member data
|
904
|
+
*
|
905
|
+
* \remarks returns false if any member data are NaN
|
906
|
+
*/
|
907
|
+
bool operator == (const Polar_Point& polar_point1,
|
908
|
+
const Polar_Point& polar_point2);
|
909
|
+
bool operator != (const Polar_Point& polar_point1,
|
910
|
+
const Polar_Point& polar_point2);
|
911
|
+
|
912
|
+
|
913
|
+
/** \brief compare according to polar lexicographic order
|
914
|
+
* (smaller bearing, then smaller range)
|
915
|
+
*
|
916
|
+
* false if any member data have not been assigned (numbers)
|
917
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
918
|
+
* two Points nearly define a radial line
|
919
|
+
*/
|
920
|
+
bool operator > (const Polar_Point& polar_point1,
|
921
|
+
const Polar_Point& polar_point2);
|
922
|
+
/** \brief compare according to polar lexicographic order
|
923
|
+
* (smaller bearing, then smaller range)
|
924
|
+
*
|
925
|
+
* false if any member data have not been assigned (numbers)
|
926
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
927
|
+
* two Points nearly define a radial line
|
928
|
+
*/
|
929
|
+
bool operator < (const Polar_Point& polar_point1,
|
930
|
+
const Polar_Point& polar_point2);
|
931
|
+
/** \brief compare according to polar lexicographic order
|
932
|
+
* (smaller bearing, then smaller range)
|
933
|
+
*
|
934
|
+
* false if any member data have not been assigned (numbers)
|
935
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
936
|
+
* two Points nearly define a radial line
|
937
|
+
*/
|
938
|
+
bool operator >= (const Polar_Point& polar_point1,
|
939
|
+
const Polar_Point& polar_point2);
|
940
|
+
/** \brief compare according to polar lexicographic order
|
941
|
+
* (smaller bearing, then smaller range)
|
942
|
+
*
|
943
|
+
* false if any member data have not been assigned (numbers)
|
944
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
945
|
+
* two Points nearly define a radial line
|
946
|
+
*/
|
947
|
+
bool operator <= (const Polar_Point& polar_point1,
|
948
|
+
const Polar_Point& polar_point2);
|
949
|
+
|
950
|
+
|
951
|
+
/// print Polar_Point
|
952
|
+
std::ostream& operator << (std::ostream& outs,
|
953
|
+
const Polar_Point& polar_point_temp);
|
954
|
+
|
955
|
+
|
956
|
+
/// ray in the plane represented by base Point and bearing Angle
|
957
|
+
class Ray
|
958
|
+
{
|
959
|
+
public:
|
960
|
+
//Constructors
|
961
|
+
/** \brief default
|
962
|
+
*
|
963
|
+
* \remarks data defaults to NAN so that checking whether the data
|
964
|
+
* are numbers can be used as a precondition in functions
|
965
|
+
*/
|
966
|
+
Ray() { }
|
967
|
+
/// construct ray emanating from \a base_point_temp in the direction
|
968
|
+
/// \a bearing_temp
|
969
|
+
Ray(Point base_point_temp, Angle bearing_temp) :
|
970
|
+
base_point_(base_point_temp) , bearing_(bearing_temp) {}
|
971
|
+
/// construct ray emanating from \a base_point_temp towards
|
972
|
+
/// \a bearing_point
|
973
|
+
Ray(Point base_point_temp, Point bearing_point);
|
974
|
+
//Accessors
|
975
|
+
/// get base point
|
976
|
+
Point base_point() const { return base_point_; }
|
977
|
+
/// get bearing
|
978
|
+
Angle bearing() const { return bearing_; }
|
979
|
+
//Mutators
|
980
|
+
/// set base point
|
981
|
+
void set_base_point(const Point& point_temp)
|
982
|
+
{ base_point_ = point_temp; }
|
983
|
+
/// set bearing
|
984
|
+
void set_bearing(const Angle& angle_temp)
|
985
|
+
{ bearing_ = angle_temp; }
|
986
|
+
private:
|
987
|
+
Point base_point_;
|
988
|
+
Angle bearing_;
|
989
|
+
};
|
990
|
+
|
991
|
+
|
992
|
+
/** \brief compare member data
|
993
|
+
*
|
994
|
+
* \remarks returns false if any member data are NaN
|
995
|
+
*/
|
996
|
+
bool operator == (const Ray& ray1,
|
997
|
+
const Ray& ray2);
|
998
|
+
/** \brief compare member data
|
999
|
+
*
|
1000
|
+
* \remarks negation of ==
|
1001
|
+
*/
|
1002
|
+
bool operator != (const Ray& ray1,
|
1003
|
+
const Ray& ray2);
|
1004
|
+
|
1005
|
+
|
1006
|
+
/** \brief compute the intersection of a Line_Segment with a Ray
|
1007
|
+
*
|
1008
|
+
* \author Karl J. Obermeyer
|
1009
|
+
* \pre member data of \a ray_temp has been assigned (numbers) and
|
1010
|
+
* \a line_segment_temp has size greater than 0
|
1011
|
+
* \remarks as a convention, if the intersection has positive
|
1012
|
+
* length, the Line_Segment returned has the first point closest to
|
1013
|
+
* the Ray's base point
|
1014
|
+
*/
|
1015
|
+
Line_Segment intersection(const Ray ray_temp,
|
1016
|
+
const Line_Segment& line_segment_temp,
|
1017
|
+
double epsilon=0.0);
|
1018
|
+
/** \brief compute the intersection of a Line_Segment with a Ray
|
1019
|
+
*
|
1020
|
+
* \author Karl J. Obermeyer
|
1021
|
+
* \pre member data of \a ray_temp has been assigned (numbers) and
|
1022
|
+
* \a line_segment_temp has size greater than 0
|
1023
|
+
* \remarks as a convention, if the intersection has positive
|
1024
|
+
* length, the Line_Segment returned has the first point closest to
|
1025
|
+
* the Ray's base point
|
1026
|
+
*/
|
1027
|
+
Line_Segment intersection(const Line_Segment& line_segment_temp,
|
1028
|
+
const Ray& ray_temp,
|
1029
|
+
double epsilon=0.0);
|
1030
|
+
|
1031
|
+
|
1032
|
+
///oriented polyline in the plane represented by list of vertices
|
1033
|
+
class Polyline
|
1034
|
+
{
|
1035
|
+
public:
|
1036
|
+
friend class Point;
|
1037
|
+
//Constructors
|
1038
|
+
/// default to empty
|
1039
|
+
Polyline() { }
|
1040
|
+
/// construct from vector of vertices
|
1041
|
+
Polyline(const std::vector<Point>& vertices_temp)
|
1042
|
+
{ vertices_ = vertices_temp; }
|
1043
|
+
//Accessors
|
1044
|
+
/** \brief raw access
|
1045
|
+
*
|
1046
|
+
* \remarks for efficiency, no bounds checks; usually trying to
|
1047
|
+
* access out of bounds causes a bus error
|
1048
|
+
*/
|
1049
|
+
Point operator [] (unsigned i) const
|
1050
|
+
{ return vertices_[i]; }
|
1051
|
+
/// vertex count
|
1052
|
+
unsigned size() const
|
1053
|
+
{ return vertices_.size(); }
|
1054
|
+
/// Euclidean length of the Polyline
|
1055
|
+
double length() const;
|
1056
|
+
/** \brief Euclidean diameter
|
1057
|
+
*
|
1058
|
+
* \pre Polyline has greater than 0 vertices
|
1059
|
+
* \return the maximum Euclidean distance between all pairs of
|
1060
|
+
* vertices
|
1061
|
+
* \remarks time complexity O(n^2), where n is the number of
|
1062
|
+
* vertices representing the Polyline
|
1063
|
+
*/
|
1064
|
+
double diameter() const;
|
1065
|
+
//a box which fits snugly around the Polyline
|
1066
|
+
Bounding_Box bbox() const;
|
1067
|
+
//Mutators
|
1068
|
+
/** \brief raw access
|
1069
|
+
*
|
1070
|
+
* \remarks for efficiency, no bounds checks; usually trying to
|
1071
|
+
* access out of bounds causes a bus error
|
1072
|
+
*/
|
1073
|
+
Point& operator [] (unsigned i)
|
1074
|
+
{ return vertices_[i]; }
|
1075
|
+
/// erase all points
|
1076
|
+
void clear()
|
1077
|
+
{ vertices_.clear(); }
|
1078
|
+
/// add a vertex to the back (end) of the list
|
1079
|
+
void push_back(const Point& point_temp)
|
1080
|
+
{ vertices_.push_back(point_temp); }
|
1081
|
+
/// delete a vertex to the back (end) of the list
|
1082
|
+
void pop_back()
|
1083
|
+
{ vertices_.pop_back(); }
|
1084
|
+
/// reset the whole list of vertices at once
|
1085
|
+
void set_vertices(const std::vector<Point>& vertices_temp)
|
1086
|
+
{ vertices_ = vertices_temp; }
|
1087
|
+
/** \brief eliminates vertices which are (\a epsilon) - colinear
|
1088
|
+
* with their respective neighbors
|
1089
|
+
*
|
1090
|
+
* \author Karl J. Obermeyer
|
1091
|
+
* \post the Euclidean distance between each vertex and the line
|
1092
|
+
* segment connecting its neighbors is at least \a epsilon
|
1093
|
+
* \remarks time complexity O(n), where n is the number of
|
1094
|
+
* vertices representing the Polyline.
|
1095
|
+
*/
|
1096
|
+
void eliminate_redundant_vertices(double epsilon=0.0);
|
1097
|
+
//Reduce number of vertices in representation...
|
1098
|
+
//void smooth(double epsilon);
|
1099
|
+
/// reverse order of vertices
|
1100
|
+
void reverse();
|
1101
|
+
/// append the points from another polyline
|
1102
|
+
void append( const Polyline& polyline );
|
1103
|
+
private:
|
1104
|
+
std::vector<Point> vertices_;
|
1105
|
+
};
|
1106
|
+
|
1107
|
+
|
1108
|
+
//print Polyline
|
1109
|
+
std::ostream& operator << (std::ostream& outs,
|
1110
|
+
const Polyline& polyline_temp);
|
1111
|
+
|
1112
|
+
|
1113
|
+
/** \brief simple polygon in the plane represented by list of vertices
|
1114
|
+
*
|
1115
|
+
* Simple here means non-self-intersecting. More precisely, edges
|
1116
|
+
* should not (i) intersect with an edge not adjacent to it, nor
|
1117
|
+
* (ii) intersect at more than one Point with an adjacent edge.
|
1118
|
+
* \remarks vertices may be listed cw or ccw
|
1119
|
+
*/
|
1120
|
+
class Polygon
|
1121
|
+
{
|
1122
|
+
public:
|
1123
|
+
friend class Point;
|
1124
|
+
//Constructors
|
1125
|
+
///default to empty
|
1126
|
+
Polygon() { }
|
1127
|
+
/** \brief construct from *.polygon file
|
1128
|
+
*
|
1129
|
+
* \author Karl J. Obermeyer
|
1130
|
+
* \remarks for efficiency, simplicity check not called here
|
1131
|
+
*/
|
1132
|
+
Polygon(const std::string& filename);
|
1133
|
+
/** \brief construct from vector of vertices
|
1134
|
+
*
|
1135
|
+
* \remarks for efficiency, simplicity check not called here
|
1136
|
+
*/
|
1137
|
+
Polygon(const std::vector<Point>& vertices_temp);
|
1138
|
+
/** \brief construct triangle from 3 Points
|
1139
|
+
*
|
1140
|
+
* \remarks for efficiency, simplicity check not called here
|
1141
|
+
*/
|
1142
|
+
Polygon(const Point& point0, const Point& point1, const Point& point2);
|
1143
|
+
//Accessors
|
1144
|
+
/** \brief access with automatic wrap-around in forward direction
|
1145
|
+
*
|
1146
|
+
* \remarks For efficiency, no bounds check; usually trying to
|
1147
|
+
* access out of bounds causes a bus error
|
1148
|
+
*/
|
1149
|
+
const Point& operator [] (unsigned i) const
|
1150
|
+
{ return vertices_[i % vertices_.size()]; }
|
1151
|
+
/** \brief vertex count
|
1152
|
+
*
|
1153
|
+
* \remarks O(1) time complexity
|
1154
|
+
*/
|
1155
|
+
unsigned n() const { return vertices_.size(); }
|
1156
|
+
/** \brief reflex vertex count (nonconvex vertices)
|
1157
|
+
*
|
1158
|
+
* \author Karl J. Obermeyer
|
1159
|
+
* \remarks Works regardless of polygon orientation (ccw vs cw),
|
1160
|
+
* but assumes no redundant vertices. Time complexity O(n), where
|
1161
|
+
* n is the number of vertices representing the Polygon
|
1162
|
+
*/
|
1163
|
+
unsigned r() const;
|
1164
|
+
/** \brief true iff Polygon is (\a epsilon) simple
|
1165
|
+
*
|
1166
|
+
* \author Karl J. Obermeyer
|
1167
|
+
*
|
1168
|
+
* \remarks A Polygon is considered \a epsilon -simple iff (i) the
|
1169
|
+
* Euclidean distance between nonadjacent edges is no greater than
|
1170
|
+
* \a epsilon, (ii) adjacent edges intersect only at their single
|
1171
|
+
* shared Point, (iii) and it has at least 3 vertices. One
|
1172
|
+
* consequence of these conditions is that there can be no
|
1173
|
+
* redundant vertices.
|
1174
|
+
*/
|
1175
|
+
bool is_simple(double epsilon=0.0) const;
|
1176
|
+
/** \brief true iff lexicographically smallest vertex is first in
|
1177
|
+
* the list of vertices representing the Polygon
|
1178
|
+
*
|
1179
|
+
* \author Karl J. Obermeyer
|
1180
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
1181
|
+
* two Points nearly define a line parallel to one of the axes
|
1182
|
+
*/
|
1183
|
+
bool is_in_standard_form() const;
|
1184
|
+
/// perimeter length
|
1185
|
+
double boundary_length() const;
|
1186
|
+
/** oriented area of the Polygon
|
1187
|
+
*
|
1188
|
+
* \author Karl J. Obermeyer
|
1189
|
+
* \pre Polygon is simple, but for efficiency simplicity is not asserted.
|
1190
|
+
* area > 0 => vertices listed ccw,
|
1191
|
+
* area < 0 => cw
|
1192
|
+
* \remarks O(n) time complexity, where n is the number
|
1193
|
+
* of vertices representing the Polygon
|
1194
|
+
*/
|
1195
|
+
double area() const;
|
1196
|
+
/** \brief Polygon's centroid (center of mass)
|
1197
|
+
*
|
1198
|
+
* \author Karl J. Obermeyer
|
1199
|
+
* \pre Polygon has greater than 0 vertices and is simple,
|
1200
|
+
* but for efficiency simplicity is not asserted
|
1201
|
+
*/
|
1202
|
+
Point centroid() const;
|
1203
|
+
/** \brief Euclidean diameter
|
1204
|
+
*
|
1205
|
+
* \pre Polygon has greater than 0 vertices
|
1206
|
+
* \return maximum Euclidean distance between all pairs of
|
1207
|
+
* vertices
|
1208
|
+
* \remarks time complexity O(n^2), where n is the number of
|
1209
|
+
* vertices representing the Polygon
|
1210
|
+
*/
|
1211
|
+
double diameter() const;
|
1212
|
+
/** \brief box which fits snugly around the Polygon
|
1213
|
+
*
|
1214
|
+
* \author Karl J. Obermeyer
|
1215
|
+
* \pre Polygon has greater than 0 vertices
|
1216
|
+
*/
|
1217
|
+
Bounding_Box bbox() const;
|
1218
|
+
// Returns a vector of n pts randomly situated in the polygon.
|
1219
|
+
std::vector<Point> random_points(const unsigned& count,
|
1220
|
+
double epsilon=0.0) const;
|
1221
|
+
/** \brief write list of vertices to *.polygon file
|
1222
|
+
*
|
1223
|
+
* \author Karl J. Obermeyer
|
1224
|
+
* Uses intuitive human and computer readable decimal format with
|
1225
|
+
* display precision \a fios_precision_temp.
|
1226
|
+
* \pre \a fios_precision_temp >=1
|
1227
|
+
*/
|
1228
|
+
void write_to_file(const std::string& filename,
|
1229
|
+
int fios_precision_temp=FIOS_PRECISION);
|
1230
|
+
//Mutators
|
1231
|
+
/** \brief access with automatic wrap-around in forward direction
|
1232
|
+
*
|
1233
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
1234
|
+
* access out of bounds causes a bus error
|
1235
|
+
*/
|
1236
|
+
Point& operator [] (unsigned i) { return vertices_[i % vertices_.size()]; }
|
1237
|
+
/// set vertices using STL vector of Points
|
1238
|
+
void set_vertices(const std::vector<Point>& vertices_temp)
|
1239
|
+
{ vertices_ = vertices_temp; }
|
1240
|
+
/// push a Point onto the back of the vertex list
|
1241
|
+
void push_back(const Point& vertex_temp )
|
1242
|
+
{ vertices_.push_back( vertex_temp ); }
|
1243
|
+
/// erase all vertices
|
1244
|
+
void clear()
|
1245
|
+
{ vertices_.clear(); }
|
1246
|
+
/** \brief enforces that the lexicographically smallest vertex is first
|
1247
|
+
* in the list of vertices representing the Polygon
|
1248
|
+
*
|
1249
|
+
* \author Karl J. Obermeyer
|
1250
|
+
* \remarks O(n) time complexity, where n is the number of
|
1251
|
+
* vertices representing the Polygon. Lex. comparison is very
|
1252
|
+
* sensitive to perturbations if two Points nearly define a line
|
1253
|
+
* parallel to one of the axes
|
1254
|
+
*/
|
1255
|
+
void enforce_standard_form();
|
1256
|
+
/** \brief eliminates vertices which are (\a epsilon) - colinear
|
1257
|
+
* with their respective neighbors
|
1258
|
+
*
|
1259
|
+
* \author Karl J. Obermeyer
|
1260
|
+
* \post the Euclidean distance between each vertex and the line
|
1261
|
+
* segment connecting its neighbors is at least \a epsilon, and the
|
1262
|
+
* Polygon is in standard form
|
1263
|
+
* \remarks time complexity O(n), where n is the number of
|
1264
|
+
* vertices representing the the Polygon
|
1265
|
+
*/
|
1266
|
+
void eliminate_redundant_vertices(double epsilon=0.0);
|
1267
|
+
/** \brief reverse (cyclic) order of vertices
|
1268
|
+
*
|
1269
|
+
* \remarks vertex first in list is held first
|
1270
|
+
*/
|
1271
|
+
void reverse();
|
1272
|
+
protected:
|
1273
|
+
std::vector<Point> vertices_;
|
1274
|
+
};
|
1275
|
+
|
1276
|
+
|
1277
|
+
/** \brief true iff vertex lists are identical, but false if either
|
1278
|
+
* Polygon has size 0
|
1279
|
+
*
|
1280
|
+
* \remarks returns false if either Polygon has size 0
|
1281
|
+
* \remarks O(n) time complexity
|
1282
|
+
*/
|
1283
|
+
bool operator == (Polygon polygon1, Polygon polygon2);
|
1284
|
+
bool operator != (Polygon polygon1, Polygon polygon2);
|
1285
|
+
/** \brief true iff the Polygon's vertices match up w/in a (closed)
|
1286
|
+
* epsilon ball of each other, but false if either Polygon
|
1287
|
+
* has size 0
|
1288
|
+
*
|
1289
|
+
* Respects number, ordering, and orientation of vertices, i.e.,
|
1290
|
+
* even if the (conceptual) polygons represented by two Polygons are
|
1291
|
+
* identical, they are not considered \a epsilon - equivalent unless
|
1292
|
+
* the number of vertices is the same, the orientations are the same
|
1293
|
+
* (cw vs. ccw list), and the Points of the vertex lists match up
|
1294
|
+
* within epsilon. This function does attempt to match the polygons
|
1295
|
+
* for all possible cyclic permutations, hence the quadratic time
|
1296
|
+
* complexity.
|
1297
|
+
* \author Karl J. Obermeyer
|
1298
|
+
* \remarks O(n^2) time complexity, where n is the number of
|
1299
|
+
* vertices representing the polygon
|
1300
|
+
*/
|
1301
|
+
bool equivalent(Polygon polygon1, Polygon polygon2,
|
1302
|
+
double epsilon=0.0);
|
1303
|
+
|
1304
|
+
|
1305
|
+
/** \brief Euclidean distance between Polygons' boundaries
|
1306
|
+
*
|
1307
|
+
* \author Karl J. Obermeyer
|
1308
|
+
* \pre \a polygon1 and \a polygon2 each have greater than 0 vertices
|
1309
|
+
*/
|
1310
|
+
double boundary_distance( const Polygon& polygon1,
|
1311
|
+
const Polygon& polygon2 );
|
1312
|
+
|
1313
|
+
|
1314
|
+
//print Polygon
|
1315
|
+
std::ostream& operator << (std::ostream& outs,
|
1316
|
+
const Polygon& polygon_temp);
|
1317
|
+
|
1318
|
+
|
1319
|
+
/** \brief environment represented by simple polygonal outer boundary
|
1320
|
+
* with simple polygonal holes
|
1321
|
+
*
|
1322
|
+
* \remarks For methods to work correctly, the outer boundary vertices must
|
1323
|
+
* be listed ccw and the hole vertices cw
|
1324
|
+
*/
|
1325
|
+
class Environment
|
1326
|
+
{
|
1327
|
+
public:
|
1328
|
+
friend class Point;
|
1329
|
+
//Constructors
|
1330
|
+
/// default to empty
|
1331
|
+
Environment() { }
|
1332
|
+
/** \brief construct Environment without holes
|
1333
|
+
*
|
1334
|
+
* \remarks time complexity O(n), where n is the number of vertices
|
1335
|
+
* representing the Environment
|
1336
|
+
*/
|
1337
|
+
Environment(const Polygon& polygon_temp)
|
1338
|
+
{ outer_boundary_=polygon_temp; update_flattened_index_key(); }
|
1339
|
+
/** \brief construct Environment with holes from STL vector of Polygons
|
1340
|
+
*
|
1341
|
+
* the first Polygon in the vector becomes the outer boundary,
|
1342
|
+
* the rest become the holes
|
1343
|
+
* \remarks time complexity O(n), where n is the number of vertices
|
1344
|
+
* representing the Environment
|
1345
|
+
*/
|
1346
|
+
Environment(const std::vector<Polygon>& polygons);
|
1347
|
+
/** construct from *.environment file.
|
1348
|
+
*
|
1349
|
+
* \author Karl J. Obermeyer
|
1350
|
+
* \remarks time complexity O(n), where n is the number of vertices
|
1351
|
+
* representing the Environment
|
1352
|
+
*/
|
1353
|
+
Environment(const std::string& filename);
|
1354
|
+
//Accessors
|
1355
|
+
/** \brief raw access to Polygons
|
1356
|
+
*
|
1357
|
+
* An argument of 0 accesses the outer boundary, 1 and above
|
1358
|
+
* access the holes.
|
1359
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
1360
|
+
* access out of bounds causes a bus error
|
1361
|
+
*/
|
1362
|
+
const Polygon& operator [] (unsigned i) const
|
1363
|
+
{ if(i==0){return outer_boundary_;} else{return holes_[i-1];} }
|
1364
|
+
/** \brief raw access to vertices via flattened index
|
1365
|
+
*
|
1366
|
+
* By flattened index is intended the label given to a vertex if
|
1367
|
+
* you were to label all the vertices from 0 to n-1 (where n is
|
1368
|
+
* the number of vertices representing the Environment) starting
|
1369
|
+
* with the first vertex of the outer boundary and progressing in
|
1370
|
+
* order through all the remaining vertices of the outer boundary
|
1371
|
+
* and holes.
|
1372
|
+
*
|
1373
|
+
* \remarks Time complexity O(1). For efficiency, no bounds
|
1374
|
+
* check; usually trying to access out of bounds causes a bus
|
1375
|
+
* error.
|
1376
|
+
*/
|
1377
|
+
const Point& operator () (unsigned k) const;
|
1378
|
+
/// hole count
|
1379
|
+
unsigned h() const { return holes_.size(); }
|
1380
|
+
/** \brief vertex count
|
1381
|
+
*
|
1382
|
+
* \remarks time complexity O(h)
|
1383
|
+
*/
|
1384
|
+
unsigned n() const;
|
1385
|
+
/** \brief total reflex vertex count (nonconvex vertices)
|
1386
|
+
*
|
1387
|
+
* \author Karl J. Obermeyer
|
1388
|
+
* \remarks time complexity O(n), where n is the number of
|
1389
|
+
* vertices representing the Environment
|
1390
|
+
*/
|
1391
|
+
unsigned r() const;
|
1392
|
+
/** \brief true iff lexicographically smallest vertex is first in
|
1393
|
+
* each list of vertices representing a Polygon of the
|
1394
|
+
* Environment
|
1395
|
+
*
|
1396
|
+
* \author Karl J. Obermeyer
|
1397
|
+
* \remarks lex. comparison is very sensitive to perturbations if
|
1398
|
+
* two Points nearly define a line parallel to one of the axes
|
1399
|
+
*/
|
1400
|
+
bool is_in_standard_form() const;
|
1401
|
+
/** \brief true iff \a epsilon -valid
|
1402
|
+
*
|
1403
|
+
* \a epsilon -valid means (i) the outer boundary and holes are
|
1404
|
+
* pairwise \a epsilon -disjoint (no two features should come
|
1405
|
+
* within \a epsilon of each other) simple polygons, (ii) outer
|
1406
|
+
* boundary is oriented ccw, and (iii) holes are oriented cw.
|
1407
|
+
*
|
1408
|
+
* \author Karl J. Obermeyer
|
1409
|
+
*
|
1410
|
+
* \pre Environment has greater than 2 vertices
|
1411
|
+
* (otherwise it can't even have nonzero area)
|
1412
|
+
* \remarks time complexity O(h^2*n^2), where h is the number of
|
1413
|
+
* holes and n is the number of vertices representing the
|
1414
|
+
* Environment
|
1415
|
+
*/
|
1416
|
+
bool is_valid(double epsilon=0.0) const;
|
1417
|
+
/** \brief sum of perimeter lengths of outer boundary and holes
|
1418
|
+
*
|
1419
|
+
* \author Karl J. Obermeyer
|
1420
|
+
* \remarks O(n) time complexity, where n is the number of
|
1421
|
+
* vertices representing the Environment
|
1422
|
+
*/
|
1423
|
+
double boundary_length() const;
|
1424
|
+
/** \brief (obstacle/hole free) area of the Environment
|
1425
|
+
*
|
1426
|
+
* \author Karl J. Obermeyer
|
1427
|
+
* \remarks O(n) time complexity, where n is the number of
|
1428
|
+
* vertices representing the Environment
|
1429
|
+
*/
|
1430
|
+
double area() const;
|
1431
|
+
/** \brief Euclidean diameter
|
1432
|
+
*
|
1433
|
+
* \author Karl J. Obermeyer
|
1434
|
+
* \pre Environment has greater than 0 vertices
|
1435
|
+
* \return maximum Euclidean distance between all pairs of
|
1436
|
+
* vertices
|
1437
|
+
* \remarks time complexity O(n^2), where n is the number of
|
1438
|
+
* vertices representing the Environment
|
1439
|
+
*/
|
1440
|
+
double diameter() const { return outer_boundary_.diameter(); }
|
1441
|
+
/** \brief box which fits snugly around the Environment
|
1442
|
+
*
|
1443
|
+
* \author Karl J. Obermeyer
|
1444
|
+
* \pre Environment has greater than 0 vertices
|
1445
|
+
*/
|
1446
|
+
Bounding_Box bbox() const { return outer_boundary_.bbox(); }
|
1447
|
+
/** \brief get STL vector of \a count Points randomly situated
|
1448
|
+
* within \a epsilon of the Environment
|
1449
|
+
*
|
1450
|
+
* \author Karl J. Obermeyer
|
1451
|
+
* \pre the Environment has positive area
|
1452
|
+
*/
|
1453
|
+
std::vector<Point> random_points(const unsigned& count,
|
1454
|
+
double epsilon=0.0) const;
|
1455
|
+
/** \brief compute a shortest path between 2 Points
|
1456
|
+
*
|
1457
|
+
* Uses the classical visibility graph method as described, e.g.,
|
1458
|
+
* in ``Robot Motion Planning" (Ch. 4 Sec. 1) by J.C. Latombe.
|
1459
|
+
* Specifically, an A* search is performed on the visibility graph
|
1460
|
+
* using the Euclidean distance as the heuristic function.
|
1461
|
+
*
|
1462
|
+
* \author Karl J. Obermeyer
|
1463
|
+
*
|
1464
|
+
* \pre \a start and \a finish must be in the environment.
|
1465
|
+
* Environment must be \a epsilon -valid. Test with
|
1466
|
+
* Environment::is_valid(epsilon).
|
1467
|
+
*
|
1468
|
+
* \remarks If multiple shortest path queries are made for the
|
1469
|
+
* same Envrionment, it is better to precompute the
|
1470
|
+
* Visibility_Graph. For a precomputed Visibility_Graph, the time
|
1471
|
+
* complexity of a shortest_path() query is O(n^2), where n is the
|
1472
|
+
* number of vertices representing the Environment.
|
1473
|
+
*
|
1474
|
+
* \todo return not just one, but all shortest paths (w/in
|
1475
|
+
* epsilon), e.g., returning a std::vector<Polyline>)
|
1476
|
+
*/
|
1477
|
+
Polyline shortest_path(const Point& start,
|
1478
|
+
const Point& finish,
|
1479
|
+
const Visibility_Graph& visibility_graph,
|
1480
|
+
double epsilon=0.0);
|
1481
|
+
/** \brief compute shortest path between 2 Points
|
1482
|
+
*
|
1483
|
+
* \author Karl J. Obermeyer
|
1484
|
+
*
|
1485
|
+
* \pre \a start and \a finish must be in the environment.
|
1486
|
+
* Environment must be \a epsilon -valid. Test with
|
1487
|
+
* Environment::is_valid(epsilon).
|
1488
|
+
*
|
1489
|
+
* \remarks For single shortest path query, visibility graph is
|
1490
|
+
* not precomputed. Time complexity O(n^3), where n is the number
|
1491
|
+
* of vertices representing the Environment.
|
1492
|
+
*/
|
1493
|
+
Polyline shortest_path(const Point& start,
|
1494
|
+
const Point& finish,
|
1495
|
+
double epsilon=0.0);
|
1496
|
+
/** \brief compute the faces (partition cells) of an arrangement
|
1497
|
+
* of Line_Segments inside the Environment
|
1498
|
+
*
|
1499
|
+
* \author Karl J. Obermeyer
|
1500
|
+
* \todo finish this
|
1501
|
+
*/
|
1502
|
+
std::vector<Polygon> compute_partition_cells( std::vector<Line_Segment>
|
1503
|
+
partition_inducing_segments,
|
1504
|
+
double epsilon=0.0 )
|
1505
|
+
{
|
1506
|
+
std::vector<Polygon> cells;
|
1507
|
+
return cells;
|
1508
|
+
}
|
1509
|
+
/** \brief write lists of vertices to *.environment file
|
1510
|
+
*
|
1511
|
+
* uses intuitive human and computer readable decimal format with
|
1512
|
+
* display precision \a fios_precision_temp
|
1513
|
+
* \author Karl J. Obermeyer
|
1514
|
+
* \pre \a fios_precision_temp >=1
|
1515
|
+
*/
|
1516
|
+
void write_to_file(const std::string& filename,
|
1517
|
+
int fios_precision_temp=FIOS_PRECISION);
|
1518
|
+
//Mutators
|
1519
|
+
/** \brief raw access to Polygons
|
1520
|
+
*
|
1521
|
+
* An argument of 0 accesses the outer boundary, 1 and above
|
1522
|
+
* access the holes.
|
1523
|
+
* \author Karl J. Obermeyer
|
1524
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
1525
|
+
* access out of bounds causes a bus error
|
1526
|
+
*/
|
1527
|
+
Polygon& operator [] (unsigned i)
|
1528
|
+
{ if(i==0){return outer_boundary_;} else{return holes_[i-1];} }
|
1529
|
+
//Mutators
|
1530
|
+
/** \brief raw access to vertices via flattened index
|
1531
|
+
*
|
1532
|
+
* By flattened index is intended the label given to a vertex if
|
1533
|
+
* you were to label all the vertices from 0 to n-1 (where n is
|
1534
|
+
* the number of vertices representing the Environment) starting
|
1535
|
+
* with the first vertex of the outer boundary and progressing in
|
1536
|
+
* order through all the remaining vertices of the outer boundary
|
1537
|
+
* and holes.
|
1538
|
+
* \author Karl J. Obermeyer
|
1539
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
1540
|
+
* access out of bounds causes a bus error.
|
1541
|
+
*/
|
1542
|
+
Point& operator () (unsigned k);
|
1543
|
+
/// set outer boundary
|
1544
|
+
void set_outer_boundary(const Polygon& polygon_temp)
|
1545
|
+
{ outer_boundary_ = polygon_temp; update_flattened_index_key(); }
|
1546
|
+
/// add hole
|
1547
|
+
void add_hole(const Polygon& polygon_temp)
|
1548
|
+
{ holes_.push_back(polygon_temp); update_flattened_index_key(); }
|
1549
|
+
/** \brief enforces outer boundary vertices are listed ccw and
|
1550
|
+
* holes listed cw, and that these lists begin with respective
|
1551
|
+
* lexicographically smallest vertex
|
1552
|
+
*
|
1553
|
+
* \author Karl J. Obermeyer
|
1554
|
+
* \remarks O(n) time complexity, where n is the number of
|
1555
|
+
* vertices representing the Environment. Lex. comparison is very
|
1556
|
+
* sensitive to perturbations if two Points nearly define a line
|
1557
|
+
* parallel to one of the axes.
|
1558
|
+
*/
|
1559
|
+
void enforce_standard_form();
|
1560
|
+
/** \brief eliminates vertices which are (\a epsilon) - colinear
|
1561
|
+
* with their respective neighbors
|
1562
|
+
*
|
1563
|
+
* \author Karl J. Obermeyer
|
1564
|
+
* \post the Euclidean distance between each vertex and the line
|
1565
|
+
* segment connecting its neighbors is at least \a epsilon
|
1566
|
+
* \remarks time complexity O(n), where n is the number of
|
1567
|
+
* vertices representing the the Environment
|
1568
|
+
*/
|
1569
|
+
void eliminate_redundant_vertices(double epsilon=0.0);
|
1570
|
+
/** \brief reverse (cyclic) order of vertices belonging to holes
|
1571
|
+
* only
|
1572
|
+
*
|
1573
|
+
* \remarks vertex first in each hole's list is held first
|
1574
|
+
*/
|
1575
|
+
void reverse_holes();
|
1576
|
+
private:
|
1577
|
+
Polygon outer_boundary_;
|
1578
|
+
//obstacles
|
1579
|
+
std::vector<Polygon> holes_;
|
1580
|
+
//allows constant time access to vertices via operator () with
|
1581
|
+
//flattened index as argument
|
1582
|
+
std::vector< std::pair<unsigned,unsigned> > flattened_index_key_;
|
1583
|
+
//Must call if size of outer_boundary and/or holes_ changes. Time
|
1584
|
+
//complexity O(n), where n is the number of vertices representing
|
1585
|
+
//the Environment.
|
1586
|
+
void update_flattened_index_key();
|
1587
|
+
//converts flattened index to index pair (hole #, vertex #) in
|
1588
|
+
//time O(n), where n is the number of vertices representing the
|
1589
|
+
//Environment
|
1590
|
+
std::pair<unsigned,unsigned> one_to_two(unsigned k) const;
|
1591
|
+
//node used for search tree of A* search in shortest_path() method
|
1592
|
+
class Shortest_Path_Node
|
1593
|
+
{
|
1594
|
+
public:
|
1595
|
+
//flattened index of corresponding Environment vertex
|
1596
|
+
//convention vertex_index = n() => corresponds to start Point
|
1597
|
+
//vertex_index = n() + 1 => corresponds to finish Point
|
1598
|
+
unsigned vertex_index;
|
1599
|
+
//pointer to self in search tree.
|
1600
|
+
std::list<Shortest_Path_Node>::iterator search_tree_location;
|
1601
|
+
//pointer to parent in search tree.
|
1602
|
+
std::list<Shortest_Path_Node>::iterator parent_search_tree_location;
|
1603
|
+
//Geodesic distance from start Point.
|
1604
|
+
double cost_to_come;
|
1605
|
+
//Euclidean distance to finish Point.
|
1606
|
+
double estimated_cost_to_go;
|
1607
|
+
//std::vector<Shortest_Path_Node> expand();
|
1608
|
+
bool operator < (const Shortest_Path_Node& spn2) const
|
1609
|
+
{
|
1610
|
+
double f1 = this->cost_to_come + this->estimated_cost_to_go;
|
1611
|
+
double f2 = spn2.cost_to_come + spn2.estimated_cost_to_go;
|
1612
|
+
if( f1 < f2 )
|
1613
|
+
return true;
|
1614
|
+
else if( f2 < f1 )
|
1615
|
+
return false;
|
1616
|
+
else if( this->vertex_index < spn2.vertex_index )
|
1617
|
+
return true;
|
1618
|
+
else if( this->vertex_index > spn2.vertex_index )
|
1619
|
+
return false;
|
1620
|
+
else if( &(*(this->parent_search_tree_location))
|
1621
|
+
< &(*(spn2.parent_search_tree_location)) )
|
1622
|
+
return true;
|
1623
|
+
else
|
1624
|
+
return false;
|
1625
|
+
}
|
1626
|
+
// print member data for debugging
|
1627
|
+
void print() const
|
1628
|
+
{
|
1629
|
+
std::cout << " vertex_index = " << vertex_index << std::endl
|
1630
|
+
<< "parent's vertex_index = "
|
1631
|
+
<< parent_search_tree_location->vertex_index
|
1632
|
+
<< std::endl
|
1633
|
+
<< " cost_to_come = " << cost_to_come << std::endl
|
1634
|
+
<< " estimated_cost_to_go = "
|
1635
|
+
<< estimated_cost_to_go << std::endl;
|
1636
|
+
}
|
1637
|
+
};
|
1638
|
+
};
|
1639
|
+
|
1640
|
+
|
1641
|
+
/// printing Environment
|
1642
|
+
std::ostream& operator << (std::ostream& outs,
|
1643
|
+
const Environment& environment_temp);
|
1644
|
+
|
1645
|
+
|
1646
|
+
/** \brief set of Guards represented by a list of Points
|
1647
|
+
*/
|
1648
|
+
class Guards
|
1649
|
+
{
|
1650
|
+
public:
|
1651
|
+
friend class Visibility_Graph;
|
1652
|
+
//Constructors
|
1653
|
+
/// default to empty
|
1654
|
+
Guards() { }
|
1655
|
+
/** \brief construct from *.guards file
|
1656
|
+
*
|
1657
|
+
* \author Karl J. Obermeyer
|
1658
|
+
*/
|
1659
|
+
Guards(const std::string& filename);
|
1660
|
+
/// construct from STL vector of Points
|
1661
|
+
Guards(const std::vector<Point>& positions)
|
1662
|
+
{ positions_ = positions; }
|
1663
|
+
//Accessors
|
1664
|
+
/** \brief raw access to guard position Points
|
1665
|
+
*
|
1666
|
+
* \author Karl J. Obermeyer
|
1667
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
1668
|
+
* access out of bounds causes a bus error
|
1669
|
+
*/
|
1670
|
+
const Point& operator [] (unsigned i) const { return positions_[i]; }
|
1671
|
+
/// guard count
|
1672
|
+
unsigned N() const { return positions_.size(); }
|
1673
|
+
/// true iff positions are lexicographically ordered
|
1674
|
+
bool are_lex_ordered() const;
|
1675
|
+
/// true iff no two guards are w/in epsilon of each other
|
1676
|
+
bool noncolocated(double epsilon=0.0) const;
|
1677
|
+
/// true iff all guards are located in \a polygon_temp
|
1678
|
+
bool in(const Polygon& polygon_temp, double epsilon=0.0) const;
|
1679
|
+
/// true iff all guards are located in \a environment_temp
|
1680
|
+
bool in(const Environment& environment_temp, double epsilon=0.0) const;
|
1681
|
+
/** \brief Euclidean diameter
|
1682
|
+
*
|
1683
|
+
* \author Karl J. Obermeyer
|
1684
|
+
* \pre greater than 0 guards
|
1685
|
+
* \return maximum Euclidean distance between all pairs of
|
1686
|
+
* vertices
|
1687
|
+
* \remarks time complexity O(N^2), where N is the number of
|
1688
|
+
* guards
|
1689
|
+
*/
|
1690
|
+
double diameter() const;
|
1691
|
+
/** \brief box which fits snugly around the Guards
|
1692
|
+
*
|
1693
|
+
* \author Karl J. Obermeyer
|
1694
|
+
* \pre greater than 0 guards
|
1695
|
+
*/
|
1696
|
+
Bounding_Box bbox() const;
|
1697
|
+
/** \brief write list of positions to *.guards file
|
1698
|
+
*
|
1699
|
+
* Uses intuitive human and computer readable decimal format with
|
1700
|
+
* display precision \a fios_precision_temp.
|
1701
|
+
* \author Karl J. Obermeyer
|
1702
|
+
* \pre \a fios_precision_temp >=1
|
1703
|
+
*/
|
1704
|
+
void write_to_file(const std::string& filename,
|
1705
|
+
int fios_precision_temp=FIOS_PRECISION);
|
1706
|
+
//Mutators
|
1707
|
+
/** \brief raw access to guard position Points
|
1708
|
+
*
|
1709
|
+
* \author Karl J. Obermeyer
|
1710
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
1711
|
+
* access out of bounds causes a bus error
|
1712
|
+
*/
|
1713
|
+
Point& operator [] (unsigned i) { return positions_[i]; }
|
1714
|
+
/// add a guard
|
1715
|
+
void push_back(const Point& point_temp)
|
1716
|
+
{ positions_.push_back(point_temp); }
|
1717
|
+
/// set positions with STL vector of Points
|
1718
|
+
void set_positions(const std::vector<Point>& positions_temp)
|
1719
|
+
{ positions_ = positions_temp; }
|
1720
|
+
/** \brief sort positions in lexicographic order
|
1721
|
+
*
|
1722
|
+
* from (lowest x, then lowest y) to (highest x, then highest y)
|
1723
|
+
* \author Karl J. Obermeyer
|
1724
|
+
* \remarks time complexity O(N logN), where N is the guard count.
|
1725
|
+
* Lex. comparison is very sensitive to perturbations if two
|
1726
|
+
* Points nearly define a line parallel to one of the axes.
|
1727
|
+
*/
|
1728
|
+
void enforce_lex_order();
|
1729
|
+
/// reverse order of positions
|
1730
|
+
void reverse();
|
1731
|
+
/** \brief relocate each guard to closest vertex if within
|
1732
|
+
* \a epsilon of some vertex (of \a environment_temp)
|
1733
|
+
*
|
1734
|
+
* \author Karl J. Obermeyer
|
1735
|
+
* \pre the guards' position data are numbers and \a environment_temp
|
1736
|
+
* is nonempty
|
1737
|
+
* \post if a guard was a Euclidean distance no greater
|
1738
|
+
* than \a epsilon from any vertex of \a environment_temp, then it
|
1739
|
+
* will be repositioned to coincide with the closest such vertex
|
1740
|
+
* \remarks O(N*n) time complexity, where N is the guard count
|
1741
|
+
* and n is the number of vertices in \a environment_temp.
|
1742
|
+
*/
|
1743
|
+
void snap_to_vertices_of(const Environment& environment_temp,
|
1744
|
+
double epsilon=0.0);
|
1745
|
+
|
1746
|
+
/** \brief relocate each guard to closest vertex if within
|
1747
|
+
* \a epsilon of some vertex (of \a environment_temp)
|
1748
|
+
*
|
1749
|
+
* \author Karl J. Obermeyer
|
1750
|
+
* \pre the guards' position data are numbers and \a polygon_temp
|
1751
|
+
* is nonempty
|
1752
|
+
* \post if a guard was a Euclidean distance no greater
|
1753
|
+
* than \a epsilon from any vertex of \a polygon_temp, then it
|
1754
|
+
* will be repositioned to coincide with the closest such vertex
|
1755
|
+
* \remarks O(N*n) time complexity, where N is the guard count
|
1756
|
+
* and n is the number of vertices in \a polygon_temp
|
1757
|
+
*/
|
1758
|
+
void snap_to_vertices_of(const Polygon& polygon_temp,
|
1759
|
+
double epsilon=0.0);
|
1760
|
+
/** \brief relocate each guard to closest Point on boundary if
|
1761
|
+
* within \a epsilon of the boundary (of \a environment_temp)
|
1762
|
+
*
|
1763
|
+
* \author Karl J. Obermeyer
|
1764
|
+
* \pre the guards' position data are numbers and \a environment_temp
|
1765
|
+
* is nonempty
|
1766
|
+
* \post If the calling Point was a Euclidean distance no greater
|
1767
|
+
* than \a epsilon from the boundary of \a environment_temp, then it
|
1768
|
+
* will be repositioned to it's projection onto that boundary
|
1769
|
+
* \remarks O(N*n) time complexity, where N is the guard count and
|
1770
|
+
* n is the number of vertices in \a environment_temp
|
1771
|
+
*/
|
1772
|
+
void snap_to_boundary_of(const Environment& environment_temp,
|
1773
|
+
double epsilon=0.0);
|
1774
|
+
/** \brief relocate each guard to closest Point on boundary if
|
1775
|
+
* within \a epsilon of the boundary (of \a polygon_temp)
|
1776
|
+
*
|
1777
|
+
* \author Karl J. Obermeyer
|
1778
|
+
* \pre the guards' position data are numbers and \a polygon_temp
|
1779
|
+
* is nonempty
|
1780
|
+
* \post If the calling Point was a Euclidean distance no greater
|
1781
|
+
* than \a epsilon from the boundary of \a polygon_temp, then it
|
1782
|
+
* will be repositioned to it's projection onto that boundary
|
1783
|
+
* \remarks O(N*n) time complexity, where N is the guard count and
|
1784
|
+
* n is the number of vertices in \a polygon_temp
|
1785
|
+
*/
|
1786
|
+
void snap_to_boundary_of(const Polygon& polygon_temp,
|
1787
|
+
double epsilon=0.0);
|
1788
|
+
private:
|
1789
|
+
std::vector<Point> positions_;
|
1790
|
+
};
|
1791
|
+
|
1792
|
+
|
1793
|
+
/// print Guards
|
1794
|
+
std::ostream& operator << (std::ostream& outs,
|
1795
|
+
const Guards& guards);
|
1796
|
+
|
1797
|
+
|
1798
|
+
/** \brief visibility polygon of a Point in an Environment or Polygon
|
1799
|
+
*
|
1800
|
+
* A Visibility_Polygon represents the closure of the set of all
|
1801
|
+
* points in an environment which are {\it clearly visible} from a
|
1802
|
+
* point (the observer). Two Points p1 and p2 are (mutually) {\it
|
1803
|
+
* clearly visible} in an environment iff the relative interior of
|
1804
|
+
* the line segment connecting p1 and p2 does not intersect the
|
1805
|
+
* boundary of the environment.
|
1806
|
+
*
|
1807
|
+
* \remarks average case time complexity O(n log(n)), where n is the
|
1808
|
+
* number of vertices in the Evironment (resp. Polygon). Note the
|
1809
|
+
* Standard Library's sort() function performs O(n log(n))
|
1810
|
+
* comparisons (both average and worst-case) and the sort() member
|
1811
|
+
* function of an STL list performs "approximately O(n log(n))
|
1812
|
+
* comparisons". For robustness, any Point (observer) should be \a
|
1813
|
+
* epsilon -snapped to the environment boundary and vertices before
|
1814
|
+
* computing its Visibility_Polygon (use the Point methods
|
1815
|
+
* snap_to_vertices_of(...) and snap_to_boundary_of(...) ).
|
1816
|
+
*/
|
1817
|
+
class Visibility_Polygon : public Polygon
|
1818
|
+
{
|
1819
|
+
public:
|
1820
|
+
//Constructors
|
1821
|
+
/// default to empty
|
1822
|
+
Visibility_Polygon() { }
|
1823
|
+
//:TRICKY:
|
1824
|
+
/** \brief visibility set of a Point in an Environment
|
1825
|
+
*
|
1826
|
+
* \author Karl J. Obermeyer
|
1827
|
+
*
|
1828
|
+
* \pre \a observer is in \a environment_temp (w/in \a epsilon )
|
1829
|
+
* and has been epsilon-snapped to the Environment using the
|
1830
|
+
* method Point::snap_to_boundary_of() followed by (order is
|
1831
|
+
* important) Point::snap_to_vertices_of(). \a environment_temp
|
1832
|
+
* must be \a epsilon -valid. Test with
|
1833
|
+
* Environment::is_valid(epsilon).
|
1834
|
+
*
|
1835
|
+
* \remarks O(n log(n)) average case time complexity, where n is the
|
1836
|
+
* number of vertices in the Evironment (resp. Polygon).
|
1837
|
+
*/
|
1838
|
+
Visibility_Polygon(const Point& observer,
|
1839
|
+
const Environment& environment_temp,
|
1840
|
+
double epsilon=0.0);
|
1841
|
+
/** \brief visibility set of a Point in a Polygon
|
1842
|
+
*
|
1843
|
+
* \pre \a observer is in \a polygon_temp (w/in \a epsilon ) and
|
1844
|
+
* has been epsilon-snapped to the Polygon using the methods
|
1845
|
+
* Point::snap_to_vertices_of() and Point::snap_to_boundary_of().
|
1846
|
+
* \a environment_temp must be \a epsilon -valid. Test with
|
1847
|
+
* Environment::is_valid(epsilon).
|
1848
|
+
*
|
1849
|
+
* \remarks less efficient because constructs an Environment from
|
1850
|
+
* a Polygon and then calls the other Visibility_Polygon constructor.
|
1851
|
+
* O(n log(n)) average case time complexity, where n is the
|
1852
|
+
* number of vertices in the Evironment (resp. Polygon).
|
1853
|
+
*/
|
1854
|
+
Visibility_Polygon(const Point& observer,
|
1855
|
+
const Polygon& polygon_temp,
|
1856
|
+
double epsilon=0.0);
|
1857
|
+
//Accessors
|
1858
|
+
//std::vector<bool> get_gap_edges(double epsilon=0.0) { return gap_edges_; }
|
1859
|
+
/// location of observer which induced the visibility polygon
|
1860
|
+
Point observer() const
|
1861
|
+
{ return observer_; }
|
1862
|
+
//Mutators
|
1863
|
+
private:
|
1864
|
+
//ith entry of gap_edges is true iff the edge following ith vertex
|
1865
|
+
//is a gap edge (not solid).
|
1866
|
+
//std::vector<bool> gap_edges_;
|
1867
|
+
Point observer_;
|
1868
|
+
|
1869
|
+
struct Polar_Edge
|
1870
|
+
{
|
1871
|
+
Polar_Point first;
|
1872
|
+
Polar_Point second;
|
1873
|
+
Polar_Edge() { }
|
1874
|
+
Polar_Edge(const Polar_Point& ppoint1,
|
1875
|
+
const Polar_Point& ppoint2) :
|
1876
|
+
first(ppoint1), second(ppoint2) {}
|
1877
|
+
};
|
1878
|
+
|
1879
|
+
class Polar_Point_With_Edge_Info : public Polar_Point
|
1880
|
+
{
|
1881
|
+
public:
|
1882
|
+
std::list<Polar_Edge>::iterator incident_edge;
|
1883
|
+
bool is_first; //True iff polar_point is the first_point of the
|
1884
|
+
//Polar_Edge pointed to by
|
1885
|
+
//incident_edge.
|
1886
|
+
void set_polar_point(const Polar_Point& ppoint_temp)
|
1887
|
+
{
|
1888
|
+
set_polar_origin( ppoint_temp.polar_origin() );
|
1889
|
+
set_x( ppoint_temp.x() );
|
1890
|
+
set_y( ppoint_temp.y() );
|
1891
|
+
set_range( ppoint_temp.range() );
|
1892
|
+
set_bearing( ppoint_temp.bearing() );
|
1893
|
+
}
|
1894
|
+
//The operator < is the same as for Polar_Point with one
|
1895
|
+
//exception. If two vertices have equal coordinates, but one is
|
1896
|
+
//the first point of its respecitve edge and the other is the
|
1897
|
+
//second point of its respective edge, then the vertex which is
|
1898
|
+
//the second point of its respective edge is considered
|
1899
|
+
//lexicographically smaller.
|
1900
|
+
friend bool operator < (const Polar_Point_With_Edge_Info& ppwei1,
|
1901
|
+
const Polar_Point_With_Edge_Info& ppwei2)
|
1902
|
+
{
|
1903
|
+
if( Polar_Point(ppwei1) == Polar_Point(ppwei2)
|
1904
|
+
and !ppwei1.is_first and ppwei2.is_first )
|
1905
|
+
return true;
|
1906
|
+
else
|
1907
|
+
return Polar_Point(ppwei1) < Polar_Point(ppwei2);
|
1908
|
+
}
|
1909
|
+
};
|
1910
|
+
|
1911
|
+
//Strict weak ordering (acts like <) for pointers to Polar_Edges.
|
1912
|
+
//Used to sort the priority_queue q2 used in the radial line sweep
|
1913
|
+
//of Visibility_Polygon constructors. Let p1 be a pointer to
|
1914
|
+
//Polar_Edge e1 and p2 be a pointer to Polar_Edge e2. Then p1 is
|
1915
|
+
//considered greater (higher priority) than p2 if the distance
|
1916
|
+
//from the observer (pointed to by observer_pointer) to e1 along
|
1917
|
+
//the direction to current_vertex is smaller than the distance
|
1918
|
+
//from the observer to e2 along the direction to current_vertex.
|
1919
|
+
class Incident_Edge_Compare
|
1920
|
+
{
|
1921
|
+
const Point *const observer_pointer;
|
1922
|
+
const Polar_Point_With_Edge_Info *const current_vertex_pointer;
|
1923
|
+
double epsilon;
|
1924
|
+
public:
|
1925
|
+
Incident_Edge_Compare(const Point& observer,
|
1926
|
+
const Polar_Point_With_Edge_Info& current_vertex,
|
1927
|
+
double epsilon_temp) :
|
1928
|
+
observer_pointer(&observer),
|
1929
|
+
current_vertex_pointer(¤t_vertex),
|
1930
|
+
epsilon(epsilon_temp) { }
|
1931
|
+
bool operator () (std::list<Polar_Edge>::iterator e1,
|
1932
|
+
std::list<Polar_Edge>::iterator e2) const
|
1933
|
+
{
|
1934
|
+
Polar_Point k1, k2;
|
1935
|
+
Line_Segment xing1 = intersection( Ray(*observer_pointer,
|
1936
|
+
current_vertex_pointer->bearing()),
|
1937
|
+
Line_Segment(e1->first,
|
1938
|
+
e1->second),
|
1939
|
+
epsilon);
|
1940
|
+
Line_Segment xing2 = intersection( Ray(*observer_pointer,
|
1941
|
+
current_vertex_pointer->bearing()),
|
1942
|
+
Line_Segment(e2->first,
|
1943
|
+
e2->second),
|
1944
|
+
epsilon);
|
1945
|
+
if( xing1.size() > 0 and xing2.size() > 0 ){
|
1946
|
+
k1 = Polar_Point( *observer_pointer,
|
1947
|
+
xing1.first() );
|
1948
|
+
k2 = Polar_Point( *observer_pointer,
|
1949
|
+
xing2.first() );
|
1950
|
+
if( k1.range() <= k2.range() )
|
1951
|
+
return false;
|
1952
|
+
return true;
|
1953
|
+
}
|
1954
|
+
//Otherwise infeasible edges are given higher priority, so they
|
1955
|
+
//get pushed out the top of the priority_queue's (q2's)
|
1956
|
+
//heap.
|
1957
|
+
else if( xing1.size() == 0 and xing2.size() > 0 )
|
1958
|
+
return false;
|
1959
|
+
else if( xing1.size() > 0 and xing2.size() == 0 )
|
1960
|
+
return true;
|
1961
|
+
else
|
1962
|
+
return true;
|
1963
|
+
}
|
1964
|
+
};
|
1965
|
+
|
1966
|
+
bool is_spike( const Point& observer,
|
1967
|
+
const Point& point1,
|
1968
|
+
const Point& point2,
|
1969
|
+
const Point& point3,
|
1970
|
+
double epsilon=0.0 ) const;
|
1971
|
+
|
1972
|
+
//For eliminating spikes as they appear. In the
|
1973
|
+
//Visibility_Polygon constructors, these are checked every time a
|
1974
|
+
//Point is added to vertices.
|
1975
|
+
void chop_spikes_at_back(const Point& observer,
|
1976
|
+
double epsilon);
|
1977
|
+
void chop_spikes_at_wrap_around(const Point& observer,
|
1978
|
+
double epsilon);
|
1979
|
+
void chop_spikes(const Point& observer,
|
1980
|
+
double epsilon);
|
1981
|
+
//For debugging Visibility_Polygon constructors.
|
1982
|
+
//Prints current_vertex and active_edge data to screen.
|
1983
|
+
void print_cv_and_ae(const Polar_Point_With_Edge_Info& current_vertex,
|
1984
|
+
const std::list<Polar_Edge>::iterator&
|
1985
|
+
active_edge);
|
1986
|
+
};
|
1987
|
+
|
1988
|
+
|
1989
|
+
/** \brief visibility graph of points in an Environment,
|
1990
|
+
* represented by adjacency matrix
|
1991
|
+
*
|
1992
|
+
* \remarks used for shortest path planning in the
|
1993
|
+
* Environment::shortest_path() method
|
1994
|
+
*
|
1995
|
+
* \todo Add method to prune edges for faster shortest path
|
1996
|
+
* calculation, e.g., exclude concave vertices and only include
|
1997
|
+
* tangent edges as described in ``Robot Motion Planning" (Ch. 4
|
1998
|
+
* Sec. 1) by J.C. Latombe.
|
1999
|
+
*/
|
2000
|
+
class Visibility_Graph
|
2001
|
+
{
|
2002
|
+
public:
|
2003
|
+
//Constructors
|
2004
|
+
/// default to empty
|
2005
|
+
Visibility_Graph() { n_=0; adjacency_matrix_ = NULL; }
|
2006
|
+
/// copy
|
2007
|
+
Visibility_Graph( const Visibility_Graph& vg2 );
|
2008
|
+
/** \brief construct the visibility graph of Environment vertices
|
2009
|
+
*
|
2010
|
+
* \author Karl J. Obermeyer
|
2011
|
+
*
|
2012
|
+
* \pre \a environment must be \a epsilon -valid. Test with
|
2013
|
+
* Environment::is_valid(epsilon).
|
2014
|
+
*
|
2015
|
+
* \remarks Currently this constructor simply computes the
|
2016
|
+
* Visibility_Polygon of each vertex and checks inclusion of the
|
2017
|
+
* other vertices, taking time complexity O(n^3), where n is the
|
2018
|
+
* number of vertices representing the Environment. This time
|
2019
|
+
* complexity is not optimal. As mentioned in ``Robot Motion
|
2020
|
+
* Planning" by J.C. Latombe p.157, there are algorithms able to
|
2021
|
+
* construct a visibility graph for a polygonal environment with
|
2022
|
+
* holes in time O(n^2). The nonoptimal algorithm is being used
|
2023
|
+
* temporarily because of (1) its ease to implement using the
|
2024
|
+
* Visibility_Polygon class, and (2) its apparent robustness.
|
2025
|
+
* Implementing the optimal algorithm robustly is future work.
|
2026
|
+
*/
|
2027
|
+
Visibility_Graph(const Environment& environment, double epsilon=0.0);
|
2028
|
+
//Constructors
|
2029
|
+
/** \brief construct the visibility graph of Points in an Environment
|
2030
|
+
*
|
2031
|
+
* \pre \a environment must be \a epsilon -valid. Test with
|
2032
|
+
* Environment::is_valid(epsilon).
|
2033
|
+
*
|
2034
|
+
* \author Karl J. Obermeyer \remarks Currently this constructor
|
2035
|
+
* simply computes the Visibility_Polygon of each Point and checks
|
2036
|
+
* inclusion of the other Points, taking time complexity
|
2037
|
+
* O(N n log(n) + N^2 n), where N is the number of Points and n is
|
2038
|
+
* the number of vertices representing the Environment. This time
|
2039
|
+
* complexity is not optimal, but has been used for
|
2040
|
+
* simplicity. More efficient algorithms are discussed in ``Robot
|
2041
|
+
* Motion Planning" by J.C. Latombe p.157.
|
2042
|
+
*/
|
2043
|
+
Visibility_Graph(const std::vector<Point> points,
|
2044
|
+
const Environment& environment, double epsilon=0.0);
|
2045
|
+
//Constructors
|
2046
|
+
/** \brief construct the visibility graph of Guards in an Environment
|
2047
|
+
*
|
2048
|
+
* \pre \a start and \a finish must be in the environment.
|
2049
|
+
* Environment must be \a epsilon -valid. Test with
|
2050
|
+
* Environment::is_valid(epsilon).
|
2051
|
+
*
|
2052
|
+
* \author Karl J. Obermeyer
|
2053
|
+
* \remarks Currently this constructor simply computes the
|
2054
|
+
* Visibility_Polygon of each guard and checks inclusion of the
|
2055
|
+
* other guards, taking time complexity O(N n log(n) + N^2 n),
|
2056
|
+
* where N is the number of guards and n is the number of vertices
|
2057
|
+
* representing the Environment. This time complexity is not
|
2058
|
+
* optimal, but has been used for simplicity. More efficient
|
2059
|
+
* algorithms are discussed in ``Robot Motion Planning" by
|
2060
|
+
* J.C. Latombe p.157.
|
2061
|
+
*/
|
2062
|
+
Visibility_Graph(const Guards& guards,
|
2063
|
+
const Environment& environment, double epsilon=0.0);
|
2064
|
+
//Accessors
|
2065
|
+
/** \brief raw access to adjacency matrix data
|
2066
|
+
*
|
2067
|
+
* \author Karl J. Obermeyer
|
2068
|
+
* \param i1 Polygon index of first vertex
|
2069
|
+
* \param j1 index of first vertex within its Polygon
|
2070
|
+
* \param i2 Polygon index of second vertex
|
2071
|
+
* \param j2 index of second vertex within its Polygon
|
2072
|
+
* \return true iff first vertex is visible from second vertex
|
2073
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
2074
|
+
* access out of bounds causes a bus error
|
2075
|
+
*/
|
2076
|
+
bool operator () (unsigned i1,
|
2077
|
+
unsigned j1,
|
2078
|
+
unsigned i2,
|
2079
|
+
unsigned j2) const;
|
2080
|
+
/** \brief raw access to adjacency matrix data via flattened
|
2081
|
+
* indices
|
2082
|
+
*
|
2083
|
+
* By flattened index is intended the label given to a vertex if
|
2084
|
+
* you were to label all the vertices from 0 to n-1 (where n is
|
2085
|
+
* the number of vertices representing the Environment) starting
|
2086
|
+
* with the first vertex of the outer boundary and progressing in
|
2087
|
+
* order through all the remaining vertices of the outer boundary
|
2088
|
+
* and holes.
|
2089
|
+
* \author Karl J. Obermeyer
|
2090
|
+
* \param k1 flattened index of first vertex
|
2091
|
+
* \param k1 flattened index of second vertex
|
2092
|
+
* \return true iff first vertex is visible from second vertex
|
2093
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
2094
|
+
* access out of bounds causes a bus error
|
2095
|
+
*/
|
2096
|
+
bool operator () (unsigned k1,
|
2097
|
+
unsigned k2) const;
|
2098
|
+
/// \brief total number of vertices in corresponding Environment
|
2099
|
+
unsigned n() const { return n_; }
|
2100
|
+
//Mutators
|
2101
|
+
/** \brief raw access to adjacency matrix data
|
2102
|
+
*
|
2103
|
+
* \author Karl J. Obermeyer
|
2104
|
+
* \param i1 Polygon index of first vertex
|
2105
|
+
* \param j1 index of first vertex within its Polygon
|
2106
|
+
* \param i2 Polygon index of second vertex
|
2107
|
+
* \param j2 index of second vertex within its Polygon
|
2108
|
+
* \return true iff first vertex is visible from second vertex
|
2109
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
2110
|
+
* access out of bounds causes a bus error
|
2111
|
+
*/
|
2112
|
+
bool& operator () (unsigned i1,
|
2113
|
+
unsigned j1,
|
2114
|
+
unsigned i2,
|
2115
|
+
unsigned j2);
|
2116
|
+
/** \brief raw access to adjacency matrix data via flattened
|
2117
|
+
* indices
|
2118
|
+
*
|
2119
|
+
* By flattened index is intended the label given to a vertex if
|
2120
|
+
* you were to label all the vertices from 0 to n-1 (where n is
|
2121
|
+
* the number of vertices representing the Environment) starting
|
2122
|
+
* with the first vertex of the outer boundary and progressing in
|
2123
|
+
* order through all the remaining vertices of the outer boundary
|
2124
|
+
* and holes.
|
2125
|
+
* \author Karl J. Obermeyer
|
2126
|
+
* \param k1 flattened index of first vertex
|
2127
|
+
* \param k1 flattened index of second vertex
|
2128
|
+
* \return true iff first vertex is visible from second vertex
|
2129
|
+
* \remarks for efficiency, no bounds check; usually trying to
|
2130
|
+
* access out of bounds causes a bus error
|
2131
|
+
*/
|
2132
|
+
bool& operator () (unsigned k1,
|
2133
|
+
unsigned k2);
|
2134
|
+
/// assignment operator
|
2135
|
+
Visibility_Graph& operator =
|
2136
|
+
(const Visibility_Graph& visibility_graph_temp);
|
2137
|
+
/// destructor
|
2138
|
+
virtual ~Visibility_Graph();
|
2139
|
+
private:
|
2140
|
+
//total number of vertices of corresponding Environment
|
2141
|
+
unsigned n_;
|
2142
|
+
//the number of vertices in each Polygon of corresponding Environment
|
2143
|
+
std::vector<unsigned> vertex_counts_;
|
2144
|
+
// n_-by-n_ adjacency matrix data stored as 2D dynamic array
|
2145
|
+
bool **adjacency_matrix_;
|
2146
|
+
//converts vertex pairs (hole #, vertex #) to flattened index
|
2147
|
+
unsigned two_to_one(unsigned i,
|
2148
|
+
unsigned j) const;
|
2149
|
+
};
|
2150
|
+
|
2151
|
+
|
2152
|
+
/// print Visibility_Graph adjacency matrix
|
2153
|
+
std::ostream& operator << (std::ostream& outs,
|
2154
|
+
const Visibility_Graph& visibility_graph);
|
2155
|
+
|
2156
|
+
}
|
2157
|
+
|
2158
|
+
#endif //VISILIBITY_H
|