visilibity 0.0.1
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- 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,19 @@
|
|
1
|
+
require 'mkmf'
|
2
|
+
|
3
|
+
# need C++ support for visilibity
|
4
|
+
$libs = append_library($libs, "stdc++")
|
5
|
+
|
6
|
+
swig_dir = with_config('swig-dir', File.dirname(__FILE__))
|
7
|
+
|
8
|
+
create_makefile('VisiLibity_native')
|
9
|
+
|
10
|
+
# eww eww eww
|
11
|
+
original_makefile = File.read('Makefile')
|
12
|
+
File.open('Makefile', 'w') do |make|
|
13
|
+
make.print %(
|
14
|
+
#{original_makefile}
|
15
|
+
|
16
|
+
#{swig_dir}/VisiLibity_wrap.cxx: #{swig_dir}/VisiLibity.i
|
17
|
+
\tswig -c++ -ruby #{swig_dir}/VisiLibity.i
|
18
|
+
)
|
19
|
+
end
|
@@ -0,0 +1,3659 @@
|
|
1
|
+
/**
|
2
|
+
* \file visilibity.cpp
|
3
|
+
* \author Karl J. Obermeyer
|
4
|
+
* \date March 20, 2008
|
5
|
+
*
|
6
|
+
\remarks
|
7
|
+
VisiLibity: A Floating-Point Visibility Algorithms Library,
|
8
|
+
Copyright (C) 2008 Karl J. Obermeyer (karl.obermeyer [ at ] gmail.com)
|
9
|
+
|
10
|
+
This file is part of VisiLibity.
|
11
|
+
|
12
|
+
VisiLibity is free software: you can redistribute it and/or modify it under
|
13
|
+
the terms of the GNU Lesser General Public License as published by the
|
14
|
+
Free Software Foundation, either version 3 of the License, or (at your
|
15
|
+
option) any later version.
|
16
|
+
|
17
|
+
VisiLibity is distributed in the hope that it will be useful, but WITHOUT
|
18
|
+
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
19
|
+
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
|
20
|
+
License for more details.
|
21
|
+
|
22
|
+
You should have received a copy of the GNU Lesser General Public
|
23
|
+
License along with VisiLibity. If not, see <http://www.gnu.org/licenses/>.
|
24
|
+
*/
|
25
|
+
|
26
|
+
|
27
|
+
#include "visilibity.hpp" //VisiLibity header
|
28
|
+
#include <cmath> //math functions in std namespace
|
29
|
+
#include <vector>
|
30
|
+
#include <queue> //queue and priority_queue
|
31
|
+
#include <set> //priority queues with iteration,
|
32
|
+
//integrated keys
|
33
|
+
#include <list>
|
34
|
+
#include <algorithm> //sorting, min, max, reverse
|
35
|
+
#include <cstdlib> //rand and srand
|
36
|
+
#include <ctime> //Unix time
|
37
|
+
#include <fstream> //file I/O
|
38
|
+
#include <iostream>
|
39
|
+
#include <cstring> //gives C-string manipulation
|
40
|
+
#include <string> //string class
|
41
|
+
#include <cassert> //assertions
|
42
|
+
|
43
|
+
|
44
|
+
///Hide helping functions in unnamed namespace (local to .C file).
|
45
|
+
namespace
|
46
|
+
{
|
47
|
+
|
48
|
+
}
|
49
|
+
|
50
|
+
|
51
|
+
/// VisiLibity's sole namespace
|
52
|
+
namespace VisiLibity
|
53
|
+
{
|
54
|
+
|
55
|
+
double uniform_random_sample(double lower_bound, double upper_bound)
|
56
|
+
{
|
57
|
+
assert( lower_bound <= upper_bound );
|
58
|
+
if( lower_bound == upper_bound )
|
59
|
+
return lower_bound;
|
60
|
+
double sample_point;
|
61
|
+
double span = upper_bound - lower_bound;
|
62
|
+
sample_point = lower_bound
|
63
|
+
+ span * static_cast<double>( std::rand() )
|
64
|
+
/ static_cast<double>( RAND_MAX );
|
65
|
+
return sample_point;
|
66
|
+
}
|
67
|
+
|
68
|
+
|
69
|
+
//Point
|
70
|
+
|
71
|
+
|
72
|
+
Point Point::projection_onto(const Line_Segment& line_segment_temp) const
|
73
|
+
{
|
74
|
+
assert( *this == *this
|
75
|
+
and line_segment_temp.size() > 0 );
|
76
|
+
|
77
|
+
if(line_segment_temp.size() == 1)
|
78
|
+
return line_segment_temp.first();
|
79
|
+
//The projection of point_temp onto the line determined by
|
80
|
+
//line_segment_temp can be represented as an affine combination
|
81
|
+
//expressed in the form projection of Point =
|
82
|
+
//theta*line_segment_temp.first +
|
83
|
+
//(1.0-theta)*line_segment_temp.second. if theta is outside
|
84
|
+
//the interval [0,1], then one of the Line_Segment's endpoints
|
85
|
+
//must be closest to calling Point.
|
86
|
+
double theta =
|
87
|
+
( (line_segment_temp.second().x()-x())
|
88
|
+
*(line_segment_temp.second().x()
|
89
|
+
-line_segment_temp.first().x())
|
90
|
+
+ (line_segment_temp.second().y()-y())
|
91
|
+
*(line_segment_temp.second().y()
|
92
|
+
-line_segment_temp.first().y()) )
|
93
|
+
/ ( pow(line_segment_temp.second().x()
|
94
|
+
-line_segment_temp.first().x(),2)
|
95
|
+
+ pow(line_segment_temp.second().y()
|
96
|
+
-line_segment_temp.first().y(),2) );
|
97
|
+
//std::cout << "\E[1;37;40m" << "Theta is: " << theta << "\x1b[0m"
|
98
|
+
//<< std::endl;
|
99
|
+
if( (0.0<=theta) and (theta<=1.0) )
|
100
|
+
return theta*line_segment_temp.first()
|
101
|
+
+ (1.0-theta)*line_segment_temp.second();
|
102
|
+
//Else pick closest endpoint.
|
103
|
+
if( distance(*this, line_segment_temp.first())
|
104
|
+
< distance(*this, line_segment_temp.second()) )
|
105
|
+
return line_segment_temp.first();
|
106
|
+
return line_segment_temp.second();
|
107
|
+
}
|
108
|
+
|
109
|
+
|
110
|
+
Point Point::projection_onto(const Ray& ray_temp) const
|
111
|
+
{
|
112
|
+
assert( *this == *this
|
113
|
+
and ray_temp == ray_temp );
|
114
|
+
|
115
|
+
//Construct a Line_Segment parallel with the Ray which is so long,
|
116
|
+
//that the projection of the the calling Point onto that
|
117
|
+
//Line_Segment must be the same as the projection of the calling
|
118
|
+
//Point onto the Ray.
|
119
|
+
double R = distance( *this , ray_temp.base_point() );
|
120
|
+
Line_Segment seg_approx =
|
121
|
+
Line_Segment( ray_temp.base_point(), ray_temp.base_point() +
|
122
|
+
Point( R*std::cos(ray_temp.bearing().get()),
|
123
|
+
R*std::sin(ray_temp.bearing().get()) ) );
|
124
|
+
return projection_onto( seg_approx );
|
125
|
+
}
|
126
|
+
|
127
|
+
|
128
|
+
Point Point::projection_onto(const Polyline& polyline_temp) const
|
129
|
+
{
|
130
|
+
assert( *this == *this
|
131
|
+
and polyline_temp.size() > 0 );
|
132
|
+
|
133
|
+
Point running_projection = polyline_temp[0];
|
134
|
+
double running_min = distance(*this, running_projection);
|
135
|
+
Point point_temp;
|
136
|
+
for(unsigned i=0; i<=polyline_temp.size()-1; i++){
|
137
|
+
point_temp = projection_onto( Line_Segment(polyline_temp[i],
|
138
|
+
polyline_temp[i+1]) );
|
139
|
+
if( distance(*this, point_temp) < running_min ){
|
140
|
+
running_projection = point_temp;
|
141
|
+
running_min = distance(*this, running_projection);
|
142
|
+
}
|
143
|
+
}
|
144
|
+
return running_projection;
|
145
|
+
}
|
146
|
+
|
147
|
+
|
148
|
+
Point Point::projection_onto_vertices_of(const Polygon& polygon_temp) const
|
149
|
+
{
|
150
|
+
assert(*this == *this
|
151
|
+
and polygon_temp.vertices_.size() > 0 );
|
152
|
+
|
153
|
+
Point running_projection = polygon_temp[0];
|
154
|
+
double running_min = distance(*this, running_projection);
|
155
|
+
for(unsigned i=1; i<=polygon_temp.n()-1; i++){
|
156
|
+
if( distance(*this, polygon_temp[i]) < running_min ){
|
157
|
+
running_projection = polygon_temp[i];
|
158
|
+
running_min = distance(*this, running_projection);
|
159
|
+
}
|
160
|
+
}
|
161
|
+
return running_projection;
|
162
|
+
}
|
163
|
+
|
164
|
+
|
165
|
+
Point Point::projection_onto_vertices_of(const Environment&
|
166
|
+
environment_temp) const
|
167
|
+
{
|
168
|
+
assert(*this == *this
|
169
|
+
and environment_temp.n() > 0 );
|
170
|
+
|
171
|
+
Point running_projection
|
172
|
+
= projection_onto_vertices_of(environment_temp.outer_boundary_);
|
173
|
+
double running_min = distance(*this, running_projection);
|
174
|
+
Point point_temp;
|
175
|
+
for(unsigned i=0; i<environment_temp.h(); i++){
|
176
|
+
point_temp = projection_onto_vertices_of(environment_temp.holes_[i]);
|
177
|
+
if( distance(*this, point_temp) < running_min ){
|
178
|
+
running_projection = point_temp;
|
179
|
+
running_min = distance(*this, running_projection);
|
180
|
+
}
|
181
|
+
}
|
182
|
+
return running_projection;
|
183
|
+
}
|
184
|
+
|
185
|
+
|
186
|
+
Point Point::projection_onto_boundary_of(const Polygon& polygon_temp) const
|
187
|
+
{
|
188
|
+
assert( *this == *this
|
189
|
+
and polygon_temp.n() > 0 );
|
190
|
+
|
191
|
+
Point running_projection = polygon_temp[0];
|
192
|
+
double running_min = distance(*this, running_projection);
|
193
|
+
Point point_temp;
|
194
|
+
for(unsigned i=0; i<=polygon_temp.n()-1; i++){
|
195
|
+
point_temp = projection_onto( Line_Segment(polygon_temp[i],
|
196
|
+
polygon_temp[i+1]) );
|
197
|
+
if( distance(*this, point_temp) < running_min ){
|
198
|
+
running_projection = point_temp;
|
199
|
+
running_min = distance(*this, running_projection);
|
200
|
+
}
|
201
|
+
}
|
202
|
+
return running_projection;
|
203
|
+
}
|
204
|
+
|
205
|
+
|
206
|
+
Point Point::projection_onto_boundary_of(const Environment&
|
207
|
+
environment_temp) const
|
208
|
+
{
|
209
|
+
assert( *this == *this
|
210
|
+
and environment_temp.n() > 0 );
|
211
|
+
|
212
|
+
Point running_projection
|
213
|
+
= projection_onto_boundary_of(environment_temp.outer_boundary_);
|
214
|
+
double running_min = distance(*this, running_projection);
|
215
|
+
Point point_temp;
|
216
|
+
for(unsigned i=0; i<environment_temp.h(); i++){
|
217
|
+
point_temp = projection_onto_boundary_of(environment_temp.holes_[i]);
|
218
|
+
if( distance(*this, point_temp) < running_min ){
|
219
|
+
running_projection = point_temp;
|
220
|
+
running_min = distance(*this, running_projection);
|
221
|
+
}
|
222
|
+
}
|
223
|
+
return running_projection;
|
224
|
+
}
|
225
|
+
|
226
|
+
|
227
|
+
bool Point::on_boundary_of(const Polygon& polygon_temp,
|
228
|
+
double epsilon) const
|
229
|
+
{
|
230
|
+
assert( *this == *this
|
231
|
+
and polygon_temp.vertices_.size() > 0 );
|
232
|
+
|
233
|
+
if( distance(*this, projection_onto_boundary_of(polygon_temp) )
|
234
|
+
<= epsilon ){
|
235
|
+
return true;
|
236
|
+
}
|
237
|
+
return false;
|
238
|
+
}
|
239
|
+
|
240
|
+
|
241
|
+
bool Point::on_boundary_of(const Environment& environment_temp,
|
242
|
+
double epsilon) const
|
243
|
+
{
|
244
|
+
assert( *this == *this
|
245
|
+
and environment_temp.outer_boundary_.n() > 0 );
|
246
|
+
|
247
|
+
if( distance(*this, projection_onto_boundary_of(environment_temp) )
|
248
|
+
<= epsilon ){
|
249
|
+
return true;
|
250
|
+
}
|
251
|
+
return false;
|
252
|
+
}
|
253
|
+
|
254
|
+
|
255
|
+
bool Point::in(const Line_Segment& line_segment_temp,
|
256
|
+
double epsilon) const
|
257
|
+
{
|
258
|
+
assert( *this == *this
|
259
|
+
and line_segment_temp.size() > 0 );
|
260
|
+
|
261
|
+
if( distance(*this, line_segment_temp) < epsilon )
|
262
|
+
return true;
|
263
|
+
return false;
|
264
|
+
}
|
265
|
+
|
266
|
+
|
267
|
+
bool Point::in_relative_interior_of(const Line_Segment& line_segment_temp,
|
268
|
+
double epsilon) const
|
269
|
+
{
|
270
|
+
assert( *this == *this
|
271
|
+
and line_segment_temp.size() > 0 );
|
272
|
+
|
273
|
+
return in(line_segment_temp, epsilon)
|
274
|
+
and distance(*this, line_segment_temp.first()) > epsilon
|
275
|
+
and distance(*this, line_segment_temp.second()) > epsilon;
|
276
|
+
}
|
277
|
+
|
278
|
+
|
279
|
+
bool Point::in(const Polygon& polygon_temp,
|
280
|
+
double epsilon) const
|
281
|
+
{
|
282
|
+
assert( *this == *this
|
283
|
+
and polygon_temp.vertices_.size() > 0 );
|
284
|
+
|
285
|
+
int n = polygon_temp.vertices_.size();
|
286
|
+
if( on_boundary_of(polygon_temp, epsilon) )
|
287
|
+
return true;
|
288
|
+
// Then check the number of times a ray emanating from the Point
|
289
|
+
// crosses the boundary of the Polygon. An odd number of
|
290
|
+
// crossings indicates the Point is in the interior of the
|
291
|
+
// Polygon. Based on
|
292
|
+
// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
|
293
|
+
int i, j; bool c = false;
|
294
|
+
for (i = 0, j = n-1; i < n; j = i++){
|
295
|
+
if ( (((polygon_temp[i].y() <= y())
|
296
|
+
and (y() < polygon_temp[j].y()))
|
297
|
+
or ((polygon_temp[j].y() <= y())
|
298
|
+
and (y() < polygon_temp[i].y())))
|
299
|
+
and ( x() < (polygon_temp[j].x()
|
300
|
+
- polygon_temp[i].x())
|
301
|
+
* (y() - polygon_temp[i].y())
|
302
|
+
/ (polygon_temp[j].y()
|
303
|
+
- polygon_temp[i].y())
|
304
|
+
+ polygon_temp[i].x()) )
|
305
|
+
c = !c;
|
306
|
+
}
|
307
|
+
return c;
|
308
|
+
}
|
309
|
+
|
310
|
+
|
311
|
+
bool Point::in(const Environment& environment_temp, double epsilon) const
|
312
|
+
{
|
313
|
+
assert( *this == *this
|
314
|
+
and environment_temp.outer_boundary_.n() > 0 );
|
315
|
+
|
316
|
+
//On outer boundary?
|
317
|
+
if( on_boundary_of(environment_temp, epsilon) )
|
318
|
+
return true;
|
319
|
+
//Not in outer boundary?
|
320
|
+
if( !in(environment_temp.outer_boundary_, epsilon) )
|
321
|
+
return false;
|
322
|
+
//In hole?
|
323
|
+
for(unsigned i=0; i<environment_temp.h(); i++)
|
324
|
+
if( in(environment_temp.holes_[i]) )
|
325
|
+
return false;
|
326
|
+
//Must be in interior.
|
327
|
+
return true;
|
328
|
+
}
|
329
|
+
|
330
|
+
|
331
|
+
bool Point::is_endpoint_of(const Line_Segment& line_segment_temp,
|
332
|
+
double epsilon) const
|
333
|
+
{
|
334
|
+
assert( *this == *this
|
335
|
+
and line_segment_temp.size() > 0 );
|
336
|
+
|
337
|
+
if( distance(line_segment_temp.first(), *this)<=epsilon
|
338
|
+
or distance(line_segment_temp.second(), *this)<=epsilon )
|
339
|
+
return true;
|
340
|
+
return false;
|
341
|
+
}
|
342
|
+
|
343
|
+
|
344
|
+
void Point::snap_to_vertices_of(const Polygon& polygon_temp,
|
345
|
+
double epsilon)
|
346
|
+
{
|
347
|
+
assert( *this == *this
|
348
|
+
and polygon_temp.n() > 0 );
|
349
|
+
|
350
|
+
Point point_temp( this->projection_onto_vertices_of(polygon_temp) );
|
351
|
+
if( distance( *this , point_temp ) <= epsilon )
|
352
|
+
*this = point_temp;
|
353
|
+
}
|
354
|
+
void Point::snap_to_vertices_of(const Environment& environment_temp,
|
355
|
+
double epsilon)
|
356
|
+
{
|
357
|
+
assert( *this == *this
|
358
|
+
and environment_temp.n() > 0 );
|
359
|
+
|
360
|
+
Point point_temp( this->projection_onto_vertices_of(environment_temp) );
|
361
|
+
if( distance( *this , point_temp ) <= epsilon )
|
362
|
+
*this = point_temp;
|
363
|
+
}
|
364
|
+
|
365
|
+
|
366
|
+
void Point::snap_to_boundary_of(const Polygon& polygon_temp,
|
367
|
+
double epsilon)
|
368
|
+
{
|
369
|
+
assert( *this == *this
|
370
|
+
and polygon_temp.n() > 0 );
|
371
|
+
|
372
|
+
Point point_temp( this->projection_onto_boundary_of(polygon_temp) );
|
373
|
+
if( distance( *this , point_temp ) <= epsilon )
|
374
|
+
*this = point_temp;
|
375
|
+
}
|
376
|
+
void Point::snap_to_boundary_of(const Environment& environment_temp,
|
377
|
+
double epsilon)
|
378
|
+
{
|
379
|
+
assert( *this == *this
|
380
|
+
and environment_temp.n() > 0 );
|
381
|
+
|
382
|
+
Point point_temp( this->projection_onto_boundary_of(environment_temp) );
|
383
|
+
if( distance( *this , point_temp ) <= epsilon )
|
384
|
+
*this = point_temp;
|
385
|
+
}
|
386
|
+
|
387
|
+
|
388
|
+
bool operator == (const Point& point1, const Point& point2)
|
389
|
+
{ return ( ( point1.x() == point2.x() )
|
390
|
+
and ( point1.y() == point2.y() ) ); }
|
391
|
+
bool operator != (const Point& point1, const Point& point2)
|
392
|
+
{ return !( point1 == point2 ); }
|
393
|
+
|
394
|
+
|
395
|
+
bool operator < (const Point& point1, const Point& point2)
|
396
|
+
{
|
397
|
+
if( point1 != point1 or point2 != point2 )
|
398
|
+
return false;
|
399
|
+
if(point1.x() < point2.x())
|
400
|
+
return true;
|
401
|
+
else if( ( point1.x() == point2.x() )
|
402
|
+
and ( point1.y() < point2.y() ) )
|
403
|
+
return true;
|
404
|
+
return false;
|
405
|
+
}
|
406
|
+
bool operator > (const Point& point1, const Point& point2)
|
407
|
+
{
|
408
|
+
if( point1 != point1 or point2 != point2 )
|
409
|
+
return false;
|
410
|
+
if( point1.x() > point2.x() )
|
411
|
+
return true;
|
412
|
+
else if( ( point1.x() == point2.x() )
|
413
|
+
and ( point1.y() > point2.y() ) )
|
414
|
+
return true;
|
415
|
+
return false;
|
416
|
+
}
|
417
|
+
bool operator >= (const Point& point1, const Point& point2)
|
418
|
+
{
|
419
|
+
if( point1 != point1 or point2 != point2 )
|
420
|
+
return false;
|
421
|
+
return !( point1 < point2 );
|
422
|
+
}
|
423
|
+
bool operator <= (const Point& point1, const Point& point2)
|
424
|
+
{
|
425
|
+
if( point1 != point1 or point2 != point2 )
|
426
|
+
return false;
|
427
|
+
return !( point1 > point2 );
|
428
|
+
}
|
429
|
+
|
430
|
+
|
431
|
+
Point operator + (const Point& point1, const Point& point2)
|
432
|
+
{
|
433
|
+
return Point( point1.x() + point2.x(),
|
434
|
+
point1.y() + point2.y() );
|
435
|
+
}
|
436
|
+
Point operator - (const Point& point1, const Point& point2)
|
437
|
+
{
|
438
|
+
return Point( point1.x() - point2.x(),
|
439
|
+
point1.y() - point2.y() );
|
440
|
+
}
|
441
|
+
|
442
|
+
|
443
|
+
Point operator * (const Point& point1, const Point& point2)
|
444
|
+
{
|
445
|
+
return Point( point1.x()*point2.x(),
|
446
|
+
point1.y()*point2.y() );
|
447
|
+
}
|
448
|
+
|
449
|
+
|
450
|
+
Point operator * (double scalar, const Point& point2)
|
451
|
+
{
|
452
|
+
return Point( scalar*point2.x(),
|
453
|
+
scalar*point2.y());
|
454
|
+
}
|
455
|
+
Point operator * (const Point& point1, double scalar)
|
456
|
+
{
|
457
|
+
return Point( scalar*point1.x(),
|
458
|
+
scalar*point1.y());
|
459
|
+
}
|
460
|
+
|
461
|
+
|
462
|
+
double cross(const Point& point1, const Point& point2)
|
463
|
+
{
|
464
|
+
assert( point1 == point1
|
465
|
+
and point2 == point2 );
|
466
|
+
|
467
|
+
//The area of the parallelogram created by the Points viewed as vectors.
|
468
|
+
return point1.x()*point2.y() - point2.x()*point1.y();
|
469
|
+
}
|
470
|
+
|
471
|
+
|
472
|
+
double distance(const Point& point1, const Point& point2)
|
473
|
+
{
|
474
|
+
assert( point1 == point1
|
475
|
+
and point2 == point2 );
|
476
|
+
|
477
|
+
return sqrt( pow( point1.x() - point2.x() , 2 )
|
478
|
+
+ pow( point1.y() - point2.y() , 2 ) );
|
479
|
+
}
|
480
|
+
|
481
|
+
|
482
|
+
double distance(const Point& point_temp,
|
483
|
+
const Line_Segment& line_segment_temp)
|
484
|
+
{
|
485
|
+
assert( point_temp == point_temp
|
486
|
+
and line_segment_temp.size() > 0 );
|
487
|
+
|
488
|
+
return distance( point_temp,
|
489
|
+
point_temp.projection_onto(line_segment_temp) );
|
490
|
+
}
|
491
|
+
double distance(const Line_Segment& line_segment_temp,
|
492
|
+
const Point& point_temp)
|
493
|
+
{
|
494
|
+
return distance( point_temp,
|
495
|
+
line_segment_temp );
|
496
|
+
}
|
497
|
+
|
498
|
+
|
499
|
+
double distance(const Point& point_temp,
|
500
|
+
const Ray& ray_temp)
|
501
|
+
{
|
502
|
+
assert( point_temp == point_temp
|
503
|
+
and ray_temp == ray_temp );
|
504
|
+
return distance( point_temp,
|
505
|
+
point_temp.projection_onto(ray_temp) );
|
506
|
+
}
|
507
|
+
double distance(const Ray& ray_temp,
|
508
|
+
const Point& point_temp)
|
509
|
+
{
|
510
|
+
return distance( point_temp,
|
511
|
+
point_temp.projection_onto(ray_temp) );
|
512
|
+
}
|
513
|
+
|
514
|
+
|
515
|
+
double distance(const Point& point_temp,
|
516
|
+
const Polyline& polyline_temp)
|
517
|
+
{
|
518
|
+
assert( point_temp == point_temp
|
519
|
+
and polyline_temp.size() > 0 );
|
520
|
+
|
521
|
+
double running_min = distance(point_temp, polyline_temp[0]);
|
522
|
+
double distance_temp;
|
523
|
+
for(unsigned i=0; i<polyline_temp.size()-1; i++){
|
524
|
+
distance_temp = distance(point_temp, Line_Segment(polyline_temp[i],
|
525
|
+
polyline_temp[i+1]) );
|
526
|
+
if(distance_temp < running_min)
|
527
|
+
running_min = distance_temp;
|
528
|
+
}
|
529
|
+
return running_min;
|
530
|
+
}
|
531
|
+
double distance(const Polyline& polyline_temp,
|
532
|
+
const Point& point_temp)
|
533
|
+
{
|
534
|
+
return distance(point_temp, polyline_temp);
|
535
|
+
}
|
536
|
+
|
537
|
+
|
538
|
+
double boundary_distance(const Point& point_temp,
|
539
|
+
const Polygon& polygon_temp)
|
540
|
+
{
|
541
|
+
assert( point_temp == point_temp
|
542
|
+
and polygon_temp.n() > 0);
|
543
|
+
|
544
|
+
double running_min = distance(point_temp, polygon_temp[0]);
|
545
|
+
double distance_temp;
|
546
|
+
for(unsigned i=0; i<=polygon_temp.n(); i++){
|
547
|
+
distance_temp = distance(point_temp, Line_Segment(polygon_temp[i],
|
548
|
+
polygon_temp[i+1]) );
|
549
|
+
if(distance_temp < running_min)
|
550
|
+
running_min = distance_temp;
|
551
|
+
}
|
552
|
+
return running_min;
|
553
|
+
}
|
554
|
+
double boundary_distance(const Polygon& polygon_temp, const Point& point_temp)
|
555
|
+
{
|
556
|
+
return boundary_distance(point_temp, polygon_temp);
|
557
|
+
}
|
558
|
+
|
559
|
+
|
560
|
+
double boundary_distance(const Point& point_temp,
|
561
|
+
const Environment& environment_temp)
|
562
|
+
{
|
563
|
+
assert( point_temp == point_temp
|
564
|
+
and environment_temp.n() > 0 );
|
565
|
+
|
566
|
+
double running_min = distance(point_temp, environment_temp[0][0]);
|
567
|
+
double distance_temp;
|
568
|
+
for(unsigned i=0; i <= environment_temp.h(); i++){
|
569
|
+
distance_temp = boundary_distance(point_temp, environment_temp[i]);
|
570
|
+
if(distance_temp < running_min)
|
571
|
+
running_min = distance_temp;
|
572
|
+
}
|
573
|
+
return running_min;
|
574
|
+
}
|
575
|
+
double boundary_distance(const Environment& environment_temp,
|
576
|
+
const Point& point_temp)
|
577
|
+
{
|
578
|
+
return boundary_distance(point_temp, environment_temp);
|
579
|
+
}
|
580
|
+
|
581
|
+
|
582
|
+
std::ostream& operator << (std::ostream& outs, const Point& point_temp)
|
583
|
+
{
|
584
|
+
outs << point_temp.x() << " " << point_temp.y();
|
585
|
+
return outs;
|
586
|
+
}
|
587
|
+
|
588
|
+
|
589
|
+
//Line_Segment
|
590
|
+
|
591
|
+
|
592
|
+
Line_Segment::Line_Segment()
|
593
|
+
{
|
594
|
+
endpoints_ = NULL;
|
595
|
+
size_ = 0;
|
596
|
+
}
|
597
|
+
|
598
|
+
|
599
|
+
Line_Segment::Line_Segment(const Line_Segment& line_segment_temp)
|
600
|
+
{
|
601
|
+
switch(line_segment_temp.size_){
|
602
|
+
case 0:
|
603
|
+
endpoints_ = NULL;
|
604
|
+
size_ = 0;
|
605
|
+
break;
|
606
|
+
case 1:
|
607
|
+
endpoints_ = new Point[1];
|
608
|
+
endpoints_[0] = line_segment_temp.endpoints_[0];
|
609
|
+
size_ = 1;
|
610
|
+
break;
|
611
|
+
case 2:
|
612
|
+
endpoints_ = new Point[2];
|
613
|
+
endpoints_[0] = line_segment_temp.endpoints_[0];
|
614
|
+
endpoints_[1] = line_segment_temp.endpoints_[1];
|
615
|
+
size_ = 2;
|
616
|
+
}
|
617
|
+
}
|
618
|
+
|
619
|
+
|
620
|
+
Line_Segment::Line_Segment(const Point& point_temp)
|
621
|
+
{
|
622
|
+
endpoints_ = new Point[1];
|
623
|
+
endpoints_[0] = point_temp;
|
624
|
+
size_ = 1;
|
625
|
+
}
|
626
|
+
|
627
|
+
|
628
|
+
Line_Segment::Line_Segment(const Point& first_point_temp,
|
629
|
+
const Point& second_point_temp, double epsilon)
|
630
|
+
{
|
631
|
+
if( distance(first_point_temp, second_point_temp) <= epsilon ){
|
632
|
+
endpoints_ = new Point[1];
|
633
|
+
endpoints_[0] = first_point_temp;
|
634
|
+
size_ = 1;
|
635
|
+
}
|
636
|
+
else{
|
637
|
+
endpoints_ = new Point[2];
|
638
|
+
endpoints_[0] = first_point_temp;
|
639
|
+
endpoints_[1] = second_point_temp;
|
640
|
+
size_ = 2;
|
641
|
+
}
|
642
|
+
}
|
643
|
+
|
644
|
+
|
645
|
+
Point Line_Segment::first() const
|
646
|
+
{
|
647
|
+
assert( size() > 0 );
|
648
|
+
|
649
|
+
return endpoints_[0];
|
650
|
+
}
|
651
|
+
|
652
|
+
|
653
|
+
Point Line_Segment::second() const
|
654
|
+
{
|
655
|
+
assert( size() > 0 );
|
656
|
+
|
657
|
+
if(size_==2)
|
658
|
+
return endpoints_[1];
|
659
|
+
else
|
660
|
+
return endpoints_[0];
|
661
|
+
}
|
662
|
+
|
663
|
+
|
664
|
+
Point Line_Segment::midpoint() const
|
665
|
+
{
|
666
|
+
assert( size_ > 0 );
|
667
|
+
|
668
|
+
return 0.5*( first() + second() );
|
669
|
+
}
|
670
|
+
|
671
|
+
|
672
|
+
double Line_Segment::length() const
|
673
|
+
{
|
674
|
+
assert( size_ > 0 );
|
675
|
+
|
676
|
+
return distance(first(), second());
|
677
|
+
}
|
678
|
+
|
679
|
+
|
680
|
+
bool Line_Segment::is_in_standard_form() const
|
681
|
+
{
|
682
|
+
assert( size_ > 0);
|
683
|
+
|
684
|
+
if(size_<2)
|
685
|
+
return true;
|
686
|
+
return first() <= second();
|
687
|
+
}
|
688
|
+
|
689
|
+
|
690
|
+
Line_Segment& Line_Segment::operator = (const Line_Segment& line_segment_temp)
|
691
|
+
{
|
692
|
+
//Makes sure not to delete dynamic vars before they're copied.
|
693
|
+
if(this==&line_segment_temp)
|
694
|
+
return *this;
|
695
|
+
delete [] endpoints_;
|
696
|
+
switch(line_segment_temp.size_){
|
697
|
+
case 0:
|
698
|
+
endpoints_ = NULL;
|
699
|
+
size_ = 0;
|
700
|
+
break;
|
701
|
+
case 1:
|
702
|
+
endpoints_ = new Point[1];
|
703
|
+
endpoints_[0] = line_segment_temp.endpoints_[0];
|
704
|
+
size_ = 1;
|
705
|
+
break;
|
706
|
+
case 2:
|
707
|
+
endpoints_ = new Point[2];
|
708
|
+
endpoints_[0] = line_segment_temp.endpoints_[0];
|
709
|
+
endpoints_[1] = line_segment_temp.endpoints_[1];
|
710
|
+
size_ = 2;
|
711
|
+
}
|
712
|
+
return *this;
|
713
|
+
}
|
714
|
+
|
715
|
+
|
716
|
+
void Line_Segment::set_first(const Point& point_temp, double epsilon)
|
717
|
+
{
|
718
|
+
Point second_point_temp;
|
719
|
+
switch(size_){
|
720
|
+
case 0:
|
721
|
+
endpoints_ = new Point[1];
|
722
|
+
endpoints_[0] = point_temp;
|
723
|
+
size_ = 1;
|
724
|
+
break;
|
725
|
+
case 1:
|
726
|
+
if( distance(endpoints_[0], point_temp) <= epsilon )
|
727
|
+
{ endpoints_[0] = point_temp; return; }
|
728
|
+
second_point_temp = endpoints_[0];
|
729
|
+
delete [] endpoints_;
|
730
|
+
endpoints_ = new Point[2];
|
731
|
+
endpoints_[0] = point_temp;
|
732
|
+
endpoints_[1] = second_point_temp;
|
733
|
+
size_ = 2;
|
734
|
+
break;
|
735
|
+
case 2:
|
736
|
+
if( distance(point_temp, endpoints_[1]) > epsilon )
|
737
|
+
{ endpoints_[0] = point_temp; return; }
|
738
|
+
delete [] endpoints_;
|
739
|
+
endpoints_ = new Point[1];
|
740
|
+
endpoints_[0] = point_temp;
|
741
|
+
size_ = 1;
|
742
|
+
}
|
743
|
+
}
|
744
|
+
|
745
|
+
|
746
|
+
void Line_Segment::set_second(const Point& point_temp, double epsilon)
|
747
|
+
{
|
748
|
+
Point first_point_temp;
|
749
|
+
switch(size_){
|
750
|
+
case 0:
|
751
|
+
endpoints_ = new Point[1];
|
752
|
+
endpoints_[0] = point_temp;
|
753
|
+
size_ = 1;
|
754
|
+
break;
|
755
|
+
case 1:
|
756
|
+
if( distance(endpoints_[0], point_temp) <= epsilon )
|
757
|
+
{ endpoints_[0] = point_temp; return; }
|
758
|
+
first_point_temp = endpoints_[0];
|
759
|
+
delete [] endpoints_;
|
760
|
+
endpoints_ = new Point[2];
|
761
|
+
endpoints_[0] = first_point_temp;
|
762
|
+
endpoints_[1] = point_temp;
|
763
|
+
size_ = 2;
|
764
|
+
break;
|
765
|
+
case 2:
|
766
|
+
if( distance(endpoints_[0], point_temp) > epsilon )
|
767
|
+
{ endpoints_[1] = point_temp; return; }
|
768
|
+
delete [] endpoints_;
|
769
|
+
endpoints_ = new Point[1];
|
770
|
+
endpoints_[0] = point_temp;
|
771
|
+
size_ = 1;
|
772
|
+
}
|
773
|
+
}
|
774
|
+
|
775
|
+
|
776
|
+
void Line_Segment::reverse()
|
777
|
+
{
|
778
|
+
if(size_<2)
|
779
|
+
return;
|
780
|
+
Point point_temp(first());
|
781
|
+
endpoints_[0] = second();
|
782
|
+
endpoints_[1] = point_temp;
|
783
|
+
}
|
784
|
+
|
785
|
+
|
786
|
+
void Line_Segment::enforce_standard_form()
|
787
|
+
{
|
788
|
+
if(first() > second())
|
789
|
+
reverse();
|
790
|
+
}
|
791
|
+
|
792
|
+
|
793
|
+
void Line_Segment::clear()
|
794
|
+
{
|
795
|
+
delete [] endpoints_;
|
796
|
+
endpoints_ = NULL;
|
797
|
+
size_ = 0;
|
798
|
+
}
|
799
|
+
|
800
|
+
|
801
|
+
Line_Segment::~Line_Segment()
|
802
|
+
{
|
803
|
+
delete [] endpoints_;
|
804
|
+
}
|
805
|
+
|
806
|
+
|
807
|
+
bool operator == (const Line_Segment& line_segment1,
|
808
|
+
const Line_Segment& line_segment2)
|
809
|
+
{
|
810
|
+
if( line_segment1.size() != line_segment2.size()
|
811
|
+
or line_segment1.size() == 0
|
812
|
+
or line_segment2.size() == 0 )
|
813
|
+
return false;
|
814
|
+
else if( line_segment1.first() == line_segment2.first()
|
815
|
+
and line_segment1.second() == line_segment2.second() )
|
816
|
+
return true;
|
817
|
+
else
|
818
|
+
return false;
|
819
|
+
}
|
820
|
+
|
821
|
+
|
822
|
+
bool operator != (const Line_Segment& line_segment1,
|
823
|
+
const Line_Segment& line_segment2)
|
824
|
+
{
|
825
|
+
return !( line_segment1 == line_segment2 );
|
826
|
+
}
|
827
|
+
|
828
|
+
|
829
|
+
bool equivalent(Line_Segment line_segment1,
|
830
|
+
Line_Segment line_segment2, double epsilon)
|
831
|
+
{
|
832
|
+
if( line_segment1.size() != line_segment2.size()
|
833
|
+
or line_segment1.size() == 0
|
834
|
+
or line_segment2.size() == 0 )
|
835
|
+
return false;
|
836
|
+
else if( ( distance( line_segment1.first(),
|
837
|
+
line_segment2.first() ) <= epsilon
|
838
|
+
and distance( line_segment1.second(),
|
839
|
+
line_segment2.second() ) <= epsilon )
|
840
|
+
or ( distance( line_segment1.first(),
|
841
|
+
line_segment2.second() ) <= epsilon
|
842
|
+
and distance( line_segment1.second(),
|
843
|
+
line_segment2.first() ) <= epsilon ) )
|
844
|
+
return true;
|
845
|
+
else
|
846
|
+
return false;
|
847
|
+
}
|
848
|
+
|
849
|
+
|
850
|
+
double distance(const Line_Segment& line_segment1,
|
851
|
+
const Line_Segment& line_segment2)
|
852
|
+
{
|
853
|
+
assert( line_segment1.size() > 0 and line_segment2.size() > 0 );
|
854
|
+
|
855
|
+
if(intersect_proper(line_segment1, line_segment2))
|
856
|
+
return 0;
|
857
|
+
//But if two line segments intersect improperly, the distance
|
858
|
+
//between them is equal to the minimum of the distances between
|
859
|
+
//all 4 endpoints_ and their respective projections onto the line
|
860
|
+
//segment they don't belong to.
|
861
|
+
double running_min, distance_temp;
|
862
|
+
running_min = distance(line_segment1.first(), line_segment2);
|
863
|
+
distance_temp = distance(line_segment1.second(), line_segment2);
|
864
|
+
if(distance_temp<running_min)
|
865
|
+
running_min = distance_temp;
|
866
|
+
distance_temp = distance(line_segment2.first(), line_segment1);
|
867
|
+
if(distance_temp<running_min)
|
868
|
+
running_min = distance_temp;
|
869
|
+
distance_temp = distance(line_segment2.second(), line_segment1);
|
870
|
+
if(distance_temp<running_min)
|
871
|
+
return distance_temp;
|
872
|
+
return running_min;
|
873
|
+
}
|
874
|
+
|
875
|
+
|
876
|
+
double boundary_distance(const Line_Segment& line_segment,
|
877
|
+
const Polygon& polygon)
|
878
|
+
{
|
879
|
+
assert( line_segment.size() > 0 and polygon.n() > 0 );
|
880
|
+
|
881
|
+
double running_min = distance( line_segment , polygon[0] );
|
882
|
+
if( polygon.n() > 1 )
|
883
|
+
for(unsigned i=0; i<polygon.n(); i++){
|
884
|
+
double d = distance( line_segment,
|
885
|
+
Line_Segment( polygon[i] , polygon[i+1] ) );
|
886
|
+
if( running_min > d )
|
887
|
+
running_min = d;
|
888
|
+
}
|
889
|
+
return running_min;
|
890
|
+
}
|
891
|
+
double boundary_distance(const Polygon& polygon,
|
892
|
+
const Line_Segment& line_segment)
|
893
|
+
{ return boundary_distance( line_segment , polygon ); }
|
894
|
+
|
895
|
+
|
896
|
+
bool intersect(const Line_Segment& line_segment1,
|
897
|
+
const Line_Segment& line_segment2, double epsilon)
|
898
|
+
{
|
899
|
+
if( line_segment1.size() == 0
|
900
|
+
or line_segment2.size() == 0 )
|
901
|
+
return false;
|
902
|
+
if( distance(line_segment1, line_segment2) <= epsilon )
|
903
|
+
return true;
|
904
|
+
return false;
|
905
|
+
}
|
906
|
+
|
907
|
+
|
908
|
+
bool intersect_proper(const Line_Segment& line_segment1,
|
909
|
+
const Line_Segment& line_segment2, double epsilon)
|
910
|
+
{
|
911
|
+
if( line_segment1.size() == 0
|
912
|
+
or line_segment2.size() == 0 )
|
913
|
+
return false;
|
914
|
+
|
915
|
+
//Declare new vars just for readability.
|
916
|
+
Point a( line_segment1.first() );
|
917
|
+
Point b( line_segment1.second() );
|
918
|
+
Point c( line_segment2.first() );
|
919
|
+
Point d( line_segment2.second() );
|
920
|
+
//First find the minimum of the distances between all 4 endpoints_
|
921
|
+
//and their respective projections onto the opposite line segment.
|
922
|
+
double running_min, distance_temp;
|
923
|
+
running_min = distance(a, line_segment2);
|
924
|
+
distance_temp = distance(b, line_segment2);
|
925
|
+
if(distance_temp<running_min)
|
926
|
+
running_min = distance_temp;
|
927
|
+
distance_temp = distance(c, line_segment1);
|
928
|
+
if(distance_temp<running_min)
|
929
|
+
running_min = distance_temp;
|
930
|
+
distance_temp = distance(d, line_segment1);
|
931
|
+
if(distance_temp<running_min)
|
932
|
+
running_min = distance_temp;
|
933
|
+
//If an endpoint is close enough to the other segment, the
|
934
|
+
//intersection is not considered proper.
|
935
|
+
if(running_min <= epsilon)
|
936
|
+
return false;
|
937
|
+
//This test is from O'Rourke's "Computational Geometry in C",
|
938
|
+
//p.30. Checks left and right turns.
|
939
|
+
if( cross(b-a, c-b) * cross(b-a, d-b) < 0
|
940
|
+
and cross(d-c, b-d) * cross(d-c, a-d) < 0 )
|
941
|
+
return true;
|
942
|
+
return false;
|
943
|
+
}
|
944
|
+
|
945
|
+
|
946
|
+
Line_Segment intersection(const Line_Segment& line_segment1,
|
947
|
+
const Line_Segment& line_segment2, double epsilon)
|
948
|
+
{
|
949
|
+
//Initially empty.
|
950
|
+
Line_Segment line_segment_temp;
|
951
|
+
|
952
|
+
if( line_segment1.size() == 0
|
953
|
+
or line_segment2.size() == 0 )
|
954
|
+
return line_segment_temp;
|
955
|
+
|
956
|
+
//No intersection => return empty segment.
|
957
|
+
if( !intersect(line_segment1, line_segment2, epsilon) )
|
958
|
+
return line_segment_temp;
|
959
|
+
//Declare new vars just for readability.
|
960
|
+
Point a( line_segment1.first() );
|
961
|
+
Point b( line_segment1.second() );
|
962
|
+
Point c( line_segment2.first() );
|
963
|
+
Point d( line_segment2.second() );
|
964
|
+
if( intersect_proper(line_segment1, line_segment2, epsilon) ){
|
965
|
+
//Use formula from O'Rourke's "Computational Geometry in C", p. 221.
|
966
|
+
//Note D=0 iff the line segments are parallel.
|
967
|
+
double D = a.x()*( d.y() - c.y() )
|
968
|
+
+ b.x()*( c.y() - d.y() )
|
969
|
+
+ d.x()*( b.y() - a.y() )
|
970
|
+
+ c.x()*( a.y() - b.y() );
|
971
|
+
double s = ( a.x()*( d.y() - c.y() )
|
972
|
+
+ c.x()*( a.y() - d.y() )
|
973
|
+
+ d.x()*( c.y() - a.y() ) ) / D;
|
974
|
+
line_segment_temp.set_first( a + s * ( b - a ) );
|
975
|
+
return line_segment_temp;
|
976
|
+
}
|
977
|
+
//Otherwise if improper...
|
978
|
+
double distance_temp_a = distance(a, line_segment2);
|
979
|
+
double distance_temp_b = distance(b, line_segment2);
|
980
|
+
double distance_temp_c = distance(c, line_segment1);
|
981
|
+
double distance_temp_d = distance(d, line_segment1);
|
982
|
+
//Check if the intersection is nondegenerate segment.
|
983
|
+
if( distance_temp_a <= epsilon and distance_temp_b <= epsilon ){
|
984
|
+
line_segment_temp.set_first(a, epsilon);
|
985
|
+
line_segment_temp.set_second(b, epsilon);
|
986
|
+
return line_segment_temp;
|
987
|
+
}
|
988
|
+
else if( distance_temp_c <= epsilon and distance_temp_d <= epsilon ){
|
989
|
+
line_segment_temp.set_first(c, epsilon);
|
990
|
+
line_segment_temp.set_second(d, epsilon);
|
991
|
+
return line_segment_temp;
|
992
|
+
}
|
993
|
+
else if( distance_temp_a <= epsilon and distance_temp_c <= epsilon ){
|
994
|
+
line_segment_temp.set_first(a, epsilon);
|
995
|
+
line_segment_temp.set_second(c, epsilon);
|
996
|
+
return line_segment_temp;
|
997
|
+
}
|
998
|
+
else if( distance_temp_a <= epsilon and distance_temp_d <= epsilon ){
|
999
|
+
line_segment_temp.set_first(a, epsilon);
|
1000
|
+
line_segment_temp.set_second(d, epsilon);
|
1001
|
+
return line_segment_temp;
|
1002
|
+
}
|
1003
|
+
else if( distance_temp_b <= epsilon and distance_temp_c <= epsilon ){
|
1004
|
+
line_segment_temp.set_first(b, epsilon);
|
1005
|
+
line_segment_temp.set_second(c, epsilon);
|
1006
|
+
return line_segment_temp;
|
1007
|
+
}
|
1008
|
+
else if( distance_temp_b <= epsilon and distance_temp_d <= epsilon ){
|
1009
|
+
line_segment_temp.set_first(b, epsilon);
|
1010
|
+
line_segment_temp.set_second(d, epsilon);
|
1011
|
+
return line_segment_temp;
|
1012
|
+
}
|
1013
|
+
//Check if the intersection is a single point.
|
1014
|
+
else if( distance_temp_a <= epsilon ){
|
1015
|
+
line_segment_temp.set_first(a, epsilon);
|
1016
|
+
return line_segment_temp;
|
1017
|
+
}
|
1018
|
+
else if( distance_temp_b <= epsilon ){
|
1019
|
+
line_segment_temp.set_first(b, epsilon);
|
1020
|
+
return line_segment_temp;
|
1021
|
+
}
|
1022
|
+
else if( distance_temp_c <= epsilon ){
|
1023
|
+
line_segment_temp.set_first(c, epsilon);
|
1024
|
+
return line_segment_temp;
|
1025
|
+
}
|
1026
|
+
else if( distance_temp_d <= epsilon ){
|
1027
|
+
line_segment_temp.set_first(d, epsilon);
|
1028
|
+
return line_segment_temp;
|
1029
|
+
}
|
1030
|
+
return line_segment_temp;
|
1031
|
+
}
|
1032
|
+
|
1033
|
+
|
1034
|
+
std::ostream& operator << (std::ostream& outs,
|
1035
|
+
const Line_Segment& line_segment_temp)
|
1036
|
+
{
|
1037
|
+
switch(line_segment_temp.size()){
|
1038
|
+
case 0:
|
1039
|
+
return outs;
|
1040
|
+
break;
|
1041
|
+
case 1:
|
1042
|
+
outs << line_segment_temp.first() << std::endl
|
1043
|
+
<< line_segment_temp.second() << std::endl;
|
1044
|
+
return outs;
|
1045
|
+
break;
|
1046
|
+
case 2:
|
1047
|
+
outs << line_segment_temp.first() << std::endl
|
1048
|
+
<< line_segment_temp.second() << std::endl;
|
1049
|
+
return outs;
|
1050
|
+
}
|
1051
|
+
return outs;
|
1052
|
+
}
|
1053
|
+
|
1054
|
+
|
1055
|
+
//Angle
|
1056
|
+
|
1057
|
+
|
1058
|
+
Angle::Angle(double data_temp)
|
1059
|
+
{
|
1060
|
+
if(data_temp >= 0)
|
1061
|
+
angle_radians_ = fmod(data_temp, 2*M_PI);
|
1062
|
+
else{
|
1063
|
+
angle_radians_ = 2*M_PI + fmod(data_temp, -2*M_PI);
|
1064
|
+
if(angle_radians_ == 2*M_PI)
|
1065
|
+
angle_radians_ = 0;
|
1066
|
+
}
|
1067
|
+
}
|
1068
|
+
|
1069
|
+
|
1070
|
+
Angle::Angle(double rise_temp, double run_temp)
|
1071
|
+
{
|
1072
|
+
if( rise_temp == 0 and run_temp == 0 )
|
1073
|
+
angle_radians_ = 0;
|
1074
|
+
//First calculate 4 quadrant inverse tangent into [-pi,+pi].
|
1075
|
+
angle_radians_ = std::atan2(rise_temp, run_temp);
|
1076
|
+
//Correct so angles specified in [0, 2*PI).
|
1077
|
+
if(angle_radians_ < 0)
|
1078
|
+
angle_radians_ = 2*M_PI + angle_radians_;
|
1079
|
+
}
|
1080
|
+
|
1081
|
+
|
1082
|
+
void Angle::set(double data_temp)
|
1083
|
+
{
|
1084
|
+
*this = Angle(data_temp);
|
1085
|
+
}
|
1086
|
+
|
1087
|
+
|
1088
|
+
void Angle::randomize()
|
1089
|
+
{
|
1090
|
+
angle_radians_ = fmod( uniform_random_sample(0, 2*M_PI), 2*M_PI );
|
1091
|
+
}
|
1092
|
+
|
1093
|
+
|
1094
|
+
bool operator == (const Angle& angle1, const Angle& angle2)
|
1095
|
+
{
|
1096
|
+
return (angle1.get() == angle2.get());
|
1097
|
+
}
|
1098
|
+
bool operator != (const Angle& angle1, const Angle& angle2)
|
1099
|
+
{
|
1100
|
+
return !(angle1.get() == angle2.get());
|
1101
|
+
}
|
1102
|
+
|
1103
|
+
|
1104
|
+
bool operator > (const Angle& angle1, const Angle& angle2)
|
1105
|
+
{
|
1106
|
+
return angle1.get() > angle2.get();
|
1107
|
+
}
|
1108
|
+
bool operator < (const Angle& angle1, const Angle& angle2)
|
1109
|
+
{
|
1110
|
+
return angle1.get() < angle2.get();
|
1111
|
+
}
|
1112
|
+
bool operator >= (const Angle& angle1, const Angle& angle2)
|
1113
|
+
{
|
1114
|
+
return angle1.get() >= angle2.get();
|
1115
|
+
}
|
1116
|
+
bool operator <= (const Angle& angle1, const Angle& angle2)
|
1117
|
+
{
|
1118
|
+
return angle1.get() <= angle2.get();
|
1119
|
+
}
|
1120
|
+
|
1121
|
+
|
1122
|
+
Angle operator + (const Angle& angle1, const Angle& angle2)
|
1123
|
+
{
|
1124
|
+
return Angle( angle1.get() + angle2.get() );
|
1125
|
+
}
|
1126
|
+
Angle operator - (const Angle& angle1, const Angle& angle2)
|
1127
|
+
{
|
1128
|
+
return Angle( angle1.get() - angle2.get() );
|
1129
|
+
}
|
1130
|
+
|
1131
|
+
|
1132
|
+
double geodesic_distance(const Angle& angle1, const Angle& angle2)
|
1133
|
+
{
|
1134
|
+
assert( angle1.get() == angle1.get()
|
1135
|
+
and angle2.get() == angle2.get() );
|
1136
|
+
|
1137
|
+
double distance1 = std::fabs( angle1.get()
|
1138
|
+
- angle2.get() );
|
1139
|
+
double distance2 = 2*M_PI - distance1;
|
1140
|
+
if(distance1 < distance2)
|
1141
|
+
return distance1;
|
1142
|
+
return distance2;
|
1143
|
+
}
|
1144
|
+
|
1145
|
+
|
1146
|
+
double geodesic_direction(const Angle& angle1, const Angle& angle2)
|
1147
|
+
{
|
1148
|
+
assert( angle1.get() == angle1.get()
|
1149
|
+
and angle2.get() == angle2.get() );
|
1150
|
+
|
1151
|
+
double distance1 = std::fabs( angle1.get()
|
1152
|
+
- angle2.get() );
|
1153
|
+
double distance2 = 2*M_PI - distance1;
|
1154
|
+
if(angle1 <= angle2){
|
1155
|
+
if(distance1 < distance2)
|
1156
|
+
return 1.0;
|
1157
|
+
return -1.0;
|
1158
|
+
}
|
1159
|
+
//Otherwise angle1 > angle2.
|
1160
|
+
if(distance1 < distance2)
|
1161
|
+
return -1.0;
|
1162
|
+
return 1.0;
|
1163
|
+
}
|
1164
|
+
|
1165
|
+
|
1166
|
+
std::ostream& operator << (std::ostream& outs, const Angle& angle_temp)
|
1167
|
+
{
|
1168
|
+
outs << angle_temp.get();
|
1169
|
+
return outs;
|
1170
|
+
}
|
1171
|
+
|
1172
|
+
|
1173
|
+
//Polar_Point
|
1174
|
+
|
1175
|
+
|
1176
|
+
Polar_Point::Polar_Point(const Point& polar_origin_temp,
|
1177
|
+
const Point& point_temp,
|
1178
|
+
double epsilon) : Point(point_temp)
|
1179
|
+
{
|
1180
|
+
polar_origin_ = polar_origin_temp;
|
1181
|
+
if( polar_origin_==polar_origin_
|
1182
|
+
and point_temp==point_temp
|
1183
|
+
and distance(polar_origin_, point_temp) <= epsilon ){
|
1184
|
+
bearing_ = Angle(0.0);
|
1185
|
+
range_ = 0.0;
|
1186
|
+
}
|
1187
|
+
else if( polar_origin_==polar_origin_
|
1188
|
+
and point_temp==point_temp){
|
1189
|
+
bearing_ = Angle( point_temp.y()-polar_origin_temp.y(),
|
1190
|
+
point_temp.x()-polar_origin_temp.x() );
|
1191
|
+
range_ = distance(polar_origin_temp, point_temp);
|
1192
|
+
}
|
1193
|
+
}
|
1194
|
+
|
1195
|
+
|
1196
|
+
void Polar_Point::set_polar_origin(const Point& polar_origin_temp)
|
1197
|
+
{
|
1198
|
+
*this = Polar_Point( polar_origin_temp, Point(x(), y()) );
|
1199
|
+
}
|
1200
|
+
|
1201
|
+
|
1202
|
+
void Polar_Point::set_x(double x_temp)
|
1203
|
+
{
|
1204
|
+
*this = Polar_Point( polar_origin_, Point(x_temp, y()) );
|
1205
|
+
}
|
1206
|
+
|
1207
|
+
|
1208
|
+
void Polar_Point::set_y(double y_temp)
|
1209
|
+
{
|
1210
|
+
*this = Polar_Point( polar_origin_, Point(x(), y_temp) );
|
1211
|
+
}
|
1212
|
+
|
1213
|
+
|
1214
|
+
void Polar_Point::set_range(double range_temp)
|
1215
|
+
{
|
1216
|
+
range_ = range_temp;
|
1217
|
+
x_ = polar_origin_.x()
|
1218
|
+
+ range_*std::cos( bearing_.get() );
|
1219
|
+
y_ = polar_origin_.y()
|
1220
|
+
+ range_*std::sin( bearing_.get() );
|
1221
|
+
}
|
1222
|
+
|
1223
|
+
|
1224
|
+
void Polar_Point::set_bearing(const Angle& bearing_temp)
|
1225
|
+
{
|
1226
|
+
bearing_ = bearing_temp;
|
1227
|
+
x_ = polar_origin_.x()
|
1228
|
+
+ range_*std::cos( bearing_.get() );
|
1229
|
+
y_ = polar_origin_.y()
|
1230
|
+
+ range_*std::sin( bearing_.get() );
|
1231
|
+
}
|
1232
|
+
|
1233
|
+
|
1234
|
+
bool operator == (const Polar_Point& polar_point1,
|
1235
|
+
const Polar_Point& polar_point2)
|
1236
|
+
{
|
1237
|
+
if( polar_point1.polar_origin() == polar_point2.polar_origin()
|
1238
|
+
and polar_point1.range() == polar_point2.range()
|
1239
|
+
and polar_point1.bearing() == polar_point2.bearing()
|
1240
|
+
)
|
1241
|
+
return true;
|
1242
|
+
return false;
|
1243
|
+
}
|
1244
|
+
bool operator != (const Polar_Point& polar_point1,
|
1245
|
+
const Polar_Point& polar_point2)
|
1246
|
+
{
|
1247
|
+
return !( polar_point1 == polar_point2 );
|
1248
|
+
}
|
1249
|
+
|
1250
|
+
|
1251
|
+
bool operator > (const Polar_Point& polar_point1,
|
1252
|
+
const Polar_Point& polar_point2)
|
1253
|
+
{
|
1254
|
+
if( polar_point1.polar_origin() != polar_point1.polar_origin()
|
1255
|
+
or polar_point1.range() != polar_point1.range()
|
1256
|
+
or polar_point1.bearing() != polar_point1.bearing()
|
1257
|
+
or polar_point2.polar_origin() != polar_point2.polar_origin()
|
1258
|
+
or polar_point2.range() != polar_point2.range()
|
1259
|
+
or polar_point2.bearing() != polar_point2.bearing()
|
1260
|
+
)
|
1261
|
+
return false;
|
1262
|
+
|
1263
|
+
if( polar_point1.bearing() > polar_point2.bearing() )
|
1264
|
+
return true;
|
1265
|
+
else if( polar_point1.bearing() == polar_point2.bearing()
|
1266
|
+
and polar_point1.range() > polar_point2.range() )
|
1267
|
+
return true;
|
1268
|
+
return false;
|
1269
|
+
}
|
1270
|
+
bool operator < (const Polar_Point& polar_point1,
|
1271
|
+
const Polar_Point& polar_point2)
|
1272
|
+
{
|
1273
|
+
if( polar_point1.polar_origin() != polar_point1.polar_origin()
|
1274
|
+
or polar_point1.range() != polar_point1.range()
|
1275
|
+
or polar_point1.bearing() != polar_point1.bearing()
|
1276
|
+
or polar_point2.polar_origin() != polar_point2.polar_origin()
|
1277
|
+
or polar_point2.range() != polar_point2.range()
|
1278
|
+
or polar_point2.bearing() != polar_point2.bearing()
|
1279
|
+
)
|
1280
|
+
return false;
|
1281
|
+
|
1282
|
+
if( polar_point1.bearing() < polar_point2.bearing() )
|
1283
|
+
return true;
|
1284
|
+
else if( polar_point1.bearing() == polar_point2.bearing()
|
1285
|
+
and polar_point1.range() < polar_point2.range() )
|
1286
|
+
return true;
|
1287
|
+
return false;
|
1288
|
+
|
1289
|
+
}
|
1290
|
+
bool operator >= (const Polar_Point& polar_point1,
|
1291
|
+
const Polar_Point& polar_point2)
|
1292
|
+
{
|
1293
|
+
if( polar_point1.polar_origin() != polar_point1.polar_origin()
|
1294
|
+
or polar_point1.range() != polar_point1.range()
|
1295
|
+
or polar_point1.bearing() != polar_point1.bearing()
|
1296
|
+
or polar_point2.polar_origin() != polar_point2.polar_origin()
|
1297
|
+
or polar_point2.range() != polar_point2.range()
|
1298
|
+
or polar_point2.bearing() != polar_point2.bearing()
|
1299
|
+
)
|
1300
|
+
return false;
|
1301
|
+
|
1302
|
+
return !(polar_point1<polar_point2);
|
1303
|
+
}
|
1304
|
+
bool operator <= (const Polar_Point& polar_point1,
|
1305
|
+
const Polar_Point& polar_point2)
|
1306
|
+
{
|
1307
|
+
if( polar_point1.polar_origin() != polar_point1.polar_origin()
|
1308
|
+
or polar_point1.range() != polar_point1.range()
|
1309
|
+
or polar_point1.bearing() != polar_point1.bearing()
|
1310
|
+
or polar_point2.polar_origin() != polar_point2.polar_origin()
|
1311
|
+
or polar_point2.range() != polar_point2.range()
|
1312
|
+
or polar_point2.bearing() != polar_point2.bearing()
|
1313
|
+
)
|
1314
|
+
return false;
|
1315
|
+
|
1316
|
+
return !(polar_point1>polar_point2);
|
1317
|
+
}
|
1318
|
+
|
1319
|
+
|
1320
|
+
std::ostream& operator << (std::ostream& outs,
|
1321
|
+
const Polar_Point& polar_point_temp)
|
1322
|
+
{
|
1323
|
+
outs << polar_point_temp.bearing() << " " << polar_point_temp.range();
|
1324
|
+
return outs;
|
1325
|
+
}
|
1326
|
+
|
1327
|
+
|
1328
|
+
//Ray
|
1329
|
+
|
1330
|
+
|
1331
|
+
Ray::Ray(Point base_point_temp, Point bearing_point)
|
1332
|
+
{
|
1333
|
+
assert( !( base_point_temp == bearing_point ) );
|
1334
|
+
|
1335
|
+
base_point_ = base_point_temp;
|
1336
|
+
bearing_ = Angle( bearing_point.y()-base_point_temp.y(),
|
1337
|
+
bearing_point.x()-base_point_temp.x() );
|
1338
|
+
}
|
1339
|
+
|
1340
|
+
bool operator == (const Ray& ray1,
|
1341
|
+
const Ray& ray2)
|
1342
|
+
{
|
1343
|
+
if( ray1.base_point() == ray2.base_point()
|
1344
|
+
and ray1.bearing() == ray2.bearing() )
|
1345
|
+
return true;
|
1346
|
+
else
|
1347
|
+
return false;
|
1348
|
+
}
|
1349
|
+
|
1350
|
+
|
1351
|
+
bool operator != (const Ray& ray1,
|
1352
|
+
const Ray& ray2)
|
1353
|
+
{
|
1354
|
+
return !( ray1 == ray2 );
|
1355
|
+
}
|
1356
|
+
|
1357
|
+
|
1358
|
+
Line_Segment intersection(const Ray ray_temp,
|
1359
|
+
const Line_Segment& line_segment_temp,
|
1360
|
+
double epsilon)
|
1361
|
+
{
|
1362
|
+
assert( ray_temp == ray_temp
|
1363
|
+
and line_segment_temp.size() > 0 );
|
1364
|
+
|
1365
|
+
//First construct a Line_Segment parallel with the Ray which is so
|
1366
|
+
//long, that it's intersection with line_segment_temp will be
|
1367
|
+
//equal to the intersection of ray_temp with line_segment_temp.
|
1368
|
+
double R = distance(ray_temp.base_point(), line_segment_temp)
|
1369
|
+
+ line_segment_temp.length();
|
1370
|
+
Line_Segment seg_approx =
|
1371
|
+
Line_Segment( ray_temp.base_point(), ray_temp.base_point() +
|
1372
|
+
Point( R*std::cos(ray_temp.bearing().get()),
|
1373
|
+
R*std::sin(ray_temp.bearing().get()) ) );
|
1374
|
+
Line_Segment intersect_seg = intersection(line_segment_temp,
|
1375
|
+
seg_approx,
|
1376
|
+
epsilon);
|
1377
|
+
//Make sure point closer to ray_temp's base_point is listed first.
|
1378
|
+
if( intersect_seg.size() == 2
|
1379
|
+
and distance( intersect_seg.first(), ray_temp.base_point() ) >
|
1380
|
+
distance( intersect_seg.second(), ray_temp.base_point() ) ){
|
1381
|
+
intersect_seg.reverse();
|
1382
|
+
}
|
1383
|
+
return intersect_seg;
|
1384
|
+
}
|
1385
|
+
|
1386
|
+
|
1387
|
+
Line_Segment intersection(const Line_Segment& line_segment_temp,
|
1388
|
+
const Ray& ray_temp,
|
1389
|
+
double epsilon)
|
1390
|
+
{
|
1391
|
+
return intersection( ray_temp , line_segment_temp , epsilon );
|
1392
|
+
}
|
1393
|
+
|
1394
|
+
|
1395
|
+
//Polyline
|
1396
|
+
|
1397
|
+
|
1398
|
+
double Polyline::length() const
|
1399
|
+
{
|
1400
|
+
double length_temp = 0;
|
1401
|
+
for(unsigned i=1; i <= vertices_.size()-1; i++)
|
1402
|
+
length_temp += distance( vertices_[i-1] , vertices_[i] );
|
1403
|
+
return length_temp;
|
1404
|
+
}
|
1405
|
+
|
1406
|
+
|
1407
|
+
double Polyline::diameter() const
|
1408
|
+
{
|
1409
|
+
//Precondition: nonempty Polyline.
|
1410
|
+
assert( size() > 0 );
|
1411
|
+
|
1412
|
+
double running_max=0;
|
1413
|
+
for(unsigned i=0; i<size()-1; i++){
|
1414
|
+
for(unsigned j=i+1; j<size(); j++){
|
1415
|
+
if( distance( (*this)[i] , (*this)[j] ) > running_max )
|
1416
|
+
running_max = distance( (*this)[i] , (*this)[j] );
|
1417
|
+
}}
|
1418
|
+
return running_max;
|
1419
|
+
}
|
1420
|
+
|
1421
|
+
|
1422
|
+
Bounding_Box Polyline::bbox () const
|
1423
|
+
{
|
1424
|
+
//Precondition: nonempty Polyline.
|
1425
|
+
assert( vertices_.size() > 0 );
|
1426
|
+
|
1427
|
+
Bounding_Box bounding_box;
|
1428
|
+
double x_min=vertices_[0].x(), x_max=vertices_[0].x(),
|
1429
|
+
y_min=vertices_[0].y(), y_max=vertices_[0].y();
|
1430
|
+
for(unsigned i = 1; i < vertices_.size(); i++){
|
1431
|
+
if(x_min > vertices_[i].x()) { x_min=vertices_[i].x(); }
|
1432
|
+
if(x_max < vertices_[i].x()) { x_max=vertices_[i].x(); }
|
1433
|
+
if(y_min > vertices_[i].y()) { y_min=vertices_[i].y(); }
|
1434
|
+
if(y_max < vertices_[i].y()) { y_max=vertices_[i].y(); }
|
1435
|
+
}
|
1436
|
+
bounding_box.x_min=x_min; bounding_box.x_max=x_max;
|
1437
|
+
bounding_box.y_min=y_min; bounding_box.y_max=y_max;
|
1438
|
+
return bounding_box;
|
1439
|
+
}
|
1440
|
+
|
1441
|
+
|
1442
|
+
void Polyline::eliminate_redundant_vertices(double epsilon)
|
1443
|
+
{
|
1444
|
+
//Trivial case
|
1445
|
+
if(vertices_.size() < 3)
|
1446
|
+
return;
|
1447
|
+
|
1448
|
+
//Store new minimal length list of vertices
|
1449
|
+
std::vector<Point> vertices_temp;
|
1450
|
+
vertices_temp.reserve(vertices_.size());
|
1451
|
+
|
1452
|
+
//Place holders
|
1453
|
+
unsigned first = 0;
|
1454
|
+
unsigned second = 1;
|
1455
|
+
unsigned third = 2;
|
1456
|
+
|
1457
|
+
//Add first vertex
|
1458
|
+
vertices_temp.push_back((*this)[first]);
|
1459
|
+
|
1460
|
+
while( third < vertices_.size() ){
|
1461
|
+
//if second redundant
|
1462
|
+
if( distance( Line_Segment( (*this)[first],
|
1463
|
+
(*this)[third] ) ,
|
1464
|
+
(*this)[second] )
|
1465
|
+
<= epsilon ){
|
1466
|
+
//=>skip it
|
1467
|
+
second = third;
|
1468
|
+
third++;
|
1469
|
+
}
|
1470
|
+
//else second not redundant
|
1471
|
+
else{
|
1472
|
+
//=>add it.
|
1473
|
+
vertices_temp.push_back((*this)[second]);
|
1474
|
+
first = second;
|
1475
|
+
second = third;
|
1476
|
+
third++;
|
1477
|
+
}
|
1478
|
+
}
|
1479
|
+
|
1480
|
+
//Add last vertex
|
1481
|
+
vertices_temp.push_back(vertices_.back());
|
1482
|
+
|
1483
|
+
//Update list of vertices
|
1484
|
+
vertices_ = vertices_temp;
|
1485
|
+
}
|
1486
|
+
|
1487
|
+
|
1488
|
+
void Polyline::reverse()
|
1489
|
+
{
|
1490
|
+
std::reverse( vertices_.begin() , vertices_.end() );
|
1491
|
+
}
|
1492
|
+
|
1493
|
+
|
1494
|
+
std::ostream& operator << (std::ostream& outs,
|
1495
|
+
const Polyline& polyline_temp)
|
1496
|
+
{
|
1497
|
+
for(unsigned i=0; i<polyline_temp.size(); i++)
|
1498
|
+
outs << polyline_temp[i] << std::endl;
|
1499
|
+
return outs;
|
1500
|
+
}
|
1501
|
+
|
1502
|
+
|
1503
|
+
void Polyline::append( const Polyline& polyline ){
|
1504
|
+
vertices_.reserve( vertices_.size() + polyline.vertices_.size() );
|
1505
|
+
for(unsigned i=0; i<polyline.vertices_.size(); i++){
|
1506
|
+
vertices_.push_back( polyline.vertices_[i] );
|
1507
|
+
}
|
1508
|
+
}
|
1509
|
+
|
1510
|
+
|
1511
|
+
//Polygon
|
1512
|
+
|
1513
|
+
|
1514
|
+
Polygon::Polygon (const std::string& filename)
|
1515
|
+
{
|
1516
|
+
std::ifstream fin(filename.c_str());
|
1517
|
+
//if(fin.fail()) { std::cerr << "\x1b[5;31m" << "Input file
|
1518
|
+
//opening failed." << "\x1b[0m\n" << "\a \n"; exit(1);}
|
1519
|
+
assert( !fin.fail() );
|
1520
|
+
|
1521
|
+
Point point_temp;
|
1522
|
+
double x_temp, y_temp;
|
1523
|
+
while (fin >> x_temp and fin >> y_temp){
|
1524
|
+
point_temp.set_x(x_temp);
|
1525
|
+
point_temp.set_y(y_temp);
|
1526
|
+
vertices_.push_back(point_temp);
|
1527
|
+
}
|
1528
|
+
fin.close();
|
1529
|
+
}
|
1530
|
+
|
1531
|
+
|
1532
|
+
Polygon::Polygon(const std::vector<Point>& vertices_temp)
|
1533
|
+
{
|
1534
|
+
vertices_ = vertices_temp;
|
1535
|
+
}
|
1536
|
+
|
1537
|
+
|
1538
|
+
Polygon::Polygon(const Point& point0,
|
1539
|
+
const Point& point1,
|
1540
|
+
const Point& point2)
|
1541
|
+
{
|
1542
|
+
vertices_.push_back(point0);
|
1543
|
+
vertices_.push_back(point1);
|
1544
|
+
vertices_.push_back(point2);
|
1545
|
+
}
|
1546
|
+
|
1547
|
+
|
1548
|
+
unsigned Polygon::r () const
|
1549
|
+
{
|
1550
|
+
int r_count = 0;
|
1551
|
+
if( vertices_.size() > 1 ){
|
1552
|
+
//Use cross product to count right turns.
|
1553
|
+
for(unsigned i=0; i<=n()-1; i++)
|
1554
|
+
if( ((*this)[i+1].x()-(*this)[i].x())
|
1555
|
+
*((*this)[i+2].y()-(*this)[i].y())
|
1556
|
+
- ((*this)[i+1].y()-(*this)[i].y())
|
1557
|
+
*((*this)[i+2].x()-(*this)[i].x()) < 0 )
|
1558
|
+
r_count++;
|
1559
|
+
if( area() < 0 ){
|
1560
|
+
r_count = n() - r_count;
|
1561
|
+
}
|
1562
|
+
}
|
1563
|
+
return r_count;
|
1564
|
+
}
|
1565
|
+
|
1566
|
+
|
1567
|
+
bool Polygon::is_simple(double epsilon) const
|
1568
|
+
{
|
1569
|
+
|
1570
|
+
if(n()==0 or n()==1 or n()==2)
|
1571
|
+
return false;
|
1572
|
+
|
1573
|
+
//Make sure adjacent edges only intersect at a single point.
|
1574
|
+
for(unsigned i=0; i<=n()-1; i++)
|
1575
|
+
if( intersection( Line_Segment((*this)[i],(*this)[i+1]) ,
|
1576
|
+
Line_Segment((*this)[i+1],(*this)[i+2]) ,
|
1577
|
+
epsilon ).size() > 1 )
|
1578
|
+
return false;
|
1579
|
+
|
1580
|
+
//Make sure nonadjacent edges do not intersect.
|
1581
|
+
for(unsigned i=0; i<n()-2; i++)
|
1582
|
+
for(unsigned j=i+2; j<=n()-1; j++)
|
1583
|
+
if( 0!=(j+1)%vertices_.size()
|
1584
|
+
and distance( Line_Segment((*this)[i],(*this)[i+1]) ,
|
1585
|
+
Line_Segment((*this)[j],(*this)[j+1]) ) <= epsilon )
|
1586
|
+
return false;
|
1587
|
+
|
1588
|
+
return true;
|
1589
|
+
}
|
1590
|
+
|
1591
|
+
|
1592
|
+
bool Polygon::is_in_standard_form() const
|
1593
|
+
{
|
1594
|
+
if(vertices_.size() > 1) //if more than one point in the polygon.
|
1595
|
+
for(unsigned i=1; i<vertices_.size(); i++)
|
1596
|
+
if(vertices_[0] > vertices_[i])
|
1597
|
+
return false;
|
1598
|
+
return true;
|
1599
|
+
}
|
1600
|
+
|
1601
|
+
|
1602
|
+
double Polygon::boundary_length() const
|
1603
|
+
{
|
1604
|
+
double length_temp=0;
|
1605
|
+
if(n()==0 or n()==1)
|
1606
|
+
return 0;
|
1607
|
+
for(unsigned i=0; i<n()-1; i++)
|
1608
|
+
length_temp += distance( vertices_[i] , vertices_[i+1] );
|
1609
|
+
length_temp += distance( vertices_[n()-1] ,
|
1610
|
+
vertices_[0] );
|
1611
|
+
return length_temp;
|
1612
|
+
}
|
1613
|
+
|
1614
|
+
|
1615
|
+
double Polygon::area() const
|
1616
|
+
{
|
1617
|
+
double area_temp = 0;
|
1618
|
+
if(n()==0)
|
1619
|
+
return 0;
|
1620
|
+
for(unsigned i=0; i<=n()-1; i++)
|
1621
|
+
area_temp += (*this)[i].x()*(*this)[i+1].y()
|
1622
|
+
- (*this)[i+1].x()*(*this)[i].y();
|
1623
|
+
return area_temp/2.0;
|
1624
|
+
}
|
1625
|
+
|
1626
|
+
|
1627
|
+
Point Polygon::centroid() const
|
1628
|
+
{
|
1629
|
+
assert( vertices_.size() > 0 );
|
1630
|
+
|
1631
|
+
double area_temp=area();
|
1632
|
+
if(area_temp==0)
|
1633
|
+
{ std::cerr << "\x1b[5;31m"
|
1634
|
+
<< "Warning: tried to compute centoid of polygon with zero area!"
|
1635
|
+
<< "\x1b[0m\n" << "\a \n"; exit(1); }
|
1636
|
+
double x_temp=0;
|
1637
|
+
for(unsigned i=0; i<=n()-1; i++)
|
1638
|
+
x_temp += ( (*this)[i].x() + (*this)[i+1].x() )
|
1639
|
+
* ( (*this)[i].x()*(*this)[i+1].y()
|
1640
|
+
- (*this)[i+1].x()*(*this)[i].y() );
|
1641
|
+
double y_temp=0;
|
1642
|
+
for(unsigned i=0; i<=n()-1; i++)
|
1643
|
+
y_temp += ( (*this)[i].y() + (*this)[i+1].y() )
|
1644
|
+
* ( (*this)[i].x()*(*this)[i+1].y()
|
1645
|
+
- (*this)[i+1].x()*(*this)[i].y() );
|
1646
|
+
return Point(x_temp/(6*area_temp), y_temp/(6*area_temp));
|
1647
|
+
}
|
1648
|
+
|
1649
|
+
|
1650
|
+
double Polygon::diameter() const
|
1651
|
+
{
|
1652
|
+
//Precondition: nonempty Polygon.
|
1653
|
+
assert( n() > 0 );
|
1654
|
+
|
1655
|
+
double running_max=0;
|
1656
|
+
for(unsigned i=0; i<n()-1; i++){
|
1657
|
+
for(unsigned j=i+1; j<n(); j++){
|
1658
|
+
if( distance( (*this)[i] , (*this)[j] ) > running_max )
|
1659
|
+
running_max = distance( (*this)[i] , (*this)[j] );
|
1660
|
+
}}
|
1661
|
+
return running_max;
|
1662
|
+
}
|
1663
|
+
|
1664
|
+
|
1665
|
+
Bounding_Box Polygon::bbox () const
|
1666
|
+
{
|
1667
|
+
//Precondition: nonempty Polygon.
|
1668
|
+
assert( vertices_.size() > 0 );
|
1669
|
+
|
1670
|
+
Bounding_Box bounding_box;
|
1671
|
+
double x_min=vertices_[0].x(), x_max=vertices_[0].x(),
|
1672
|
+
y_min=vertices_[0].y(), y_max=vertices_[0].y();
|
1673
|
+
for(unsigned i = 1; i < vertices_.size(); i++){
|
1674
|
+
if(x_min > vertices_[i].x()) { x_min=vertices_[i].x(); }
|
1675
|
+
if(x_max < vertices_[i].x()) { x_max=vertices_[i].x(); }
|
1676
|
+
if(y_min > vertices_[i].y()) { y_min=vertices_[i].y(); }
|
1677
|
+
if(y_max < vertices_[i].y()) { y_max=vertices_[i].y(); }
|
1678
|
+
}
|
1679
|
+
bounding_box.x_min=x_min; bounding_box.x_max=x_max;
|
1680
|
+
bounding_box.y_min=y_min; bounding_box.y_max=y_max;
|
1681
|
+
return bounding_box;
|
1682
|
+
}
|
1683
|
+
|
1684
|
+
|
1685
|
+
std::vector<Point> Polygon::random_points(const unsigned& count,
|
1686
|
+
double epsilon) const
|
1687
|
+
{
|
1688
|
+
//Precondition: nonempty Polygon.
|
1689
|
+
assert( vertices_.size() > 0 );
|
1690
|
+
|
1691
|
+
Bounding_Box bounding_box = bbox();
|
1692
|
+
std::vector<Point> pts_in_polygon; pts_in_polygon.reserve(count);
|
1693
|
+
Point pt_temp( uniform_random_sample(bounding_box.x_min,
|
1694
|
+
bounding_box.x_max),
|
1695
|
+
uniform_random_sample(bounding_box.y_min,
|
1696
|
+
bounding_box.y_max) );
|
1697
|
+
while(pts_in_polygon.size() < count){
|
1698
|
+
while(!pt_temp.in(*this, epsilon)){
|
1699
|
+
pt_temp.set_x( uniform_random_sample(bounding_box.x_min,
|
1700
|
+
bounding_box.x_max) );
|
1701
|
+
pt_temp.set_y( uniform_random_sample(bounding_box.y_min,
|
1702
|
+
bounding_box.y_max) );
|
1703
|
+
}
|
1704
|
+
pts_in_polygon.push_back(pt_temp);
|
1705
|
+
pt_temp.set_x( uniform_random_sample(bounding_box.x_min,
|
1706
|
+
bounding_box.x_max) );
|
1707
|
+
pt_temp.set_y( uniform_random_sample(bounding_box.y_min,
|
1708
|
+
bounding_box.y_max) );
|
1709
|
+
}
|
1710
|
+
return pts_in_polygon;
|
1711
|
+
}
|
1712
|
+
|
1713
|
+
|
1714
|
+
void Polygon::write_to_file(const std::string& filename,
|
1715
|
+
int fios_precision_temp)
|
1716
|
+
{
|
1717
|
+
assert( fios_precision_temp >= 1 );
|
1718
|
+
|
1719
|
+
std::ofstream fout( filename.c_str() );
|
1720
|
+
//fout.open( filename.c_str() ); //Alternatives.
|
1721
|
+
//fout << *this;
|
1722
|
+
fout.setf(std::ios::fixed);
|
1723
|
+
fout.setf(std::ios::showpoint);
|
1724
|
+
fout.precision(fios_precision_temp);
|
1725
|
+
for(unsigned i=0; i<n(); i++)
|
1726
|
+
fout << vertices_[i] << std::endl;
|
1727
|
+
fout.close();
|
1728
|
+
}
|
1729
|
+
|
1730
|
+
|
1731
|
+
void Polygon::enforce_standard_form()
|
1732
|
+
{
|
1733
|
+
int point_count=vertices_.size();
|
1734
|
+
if(point_count > 1){ //if more than one point in the polygon.
|
1735
|
+
std::vector<Point> vertices_temp;
|
1736
|
+
vertices_temp.reserve(point_count);
|
1737
|
+
//Find index of lexicographically smallest point.
|
1738
|
+
int index_of_smallest=0;
|
1739
|
+
int i; //counter.
|
1740
|
+
for(i=1; i<point_count; i++)
|
1741
|
+
if(vertices_[i]<vertices_[index_of_smallest])
|
1742
|
+
index_of_smallest=i;
|
1743
|
+
//Fill vertices_temp starting with lex. smallest.
|
1744
|
+
for(i=index_of_smallest; i<point_count; i++)
|
1745
|
+
vertices_temp.push_back(vertices_[i]);
|
1746
|
+
for(i=0; i<index_of_smallest; i++)
|
1747
|
+
vertices_temp.push_back(vertices_[i]);
|
1748
|
+
vertices_=vertices_temp;
|
1749
|
+
}
|
1750
|
+
}
|
1751
|
+
|
1752
|
+
|
1753
|
+
void Polygon::eliminate_redundant_vertices(double epsilon)
|
1754
|
+
{
|
1755
|
+
//Degenerate case.
|
1756
|
+
if( vertices_.size() < 4 )
|
1757
|
+
return;
|
1758
|
+
|
1759
|
+
//Store new minimal length list of vertices.
|
1760
|
+
std::vector<Point> vertices_temp;
|
1761
|
+
vertices_temp.reserve( vertices_.size() );
|
1762
|
+
|
1763
|
+
//Place holders.
|
1764
|
+
unsigned first = 0;
|
1765
|
+
unsigned second = 1;
|
1766
|
+
unsigned third = 2;
|
1767
|
+
|
1768
|
+
while( third <= vertices_.size() ){
|
1769
|
+
//if second is redundant
|
1770
|
+
if( distance( Line_Segment( (*this)[first],
|
1771
|
+
(*this)[third] ) ,
|
1772
|
+
(*this)[second] )
|
1773
|
+
<= epsilon ){
|
1774
|
+
//=>skip it
|
1775
|
+
second = third;
|
1776
|
+
third++;
|
1777
|
+
}
|
1778
|
+
//else second not redundant
|
1779
|
+
else{
|
1780
|
+
//=>add it
|
1781
|
+
vertices_temp.push_back( (*this)[second] );
|
1782
|
+
first = second;
|
1783
|
+
second = third;
|
1784
|
+
third++;
|
1785
|
+
}
|
1786
|
+
}
|
1787
|
+
|
1788
|
+
//decide whether to add original first point
|
1789
|
+
if( distance( Line_Segment( vertices_temp.front(),
|
1790
|
+
vertices_temp.back() ) ,
|
1791
|
+
vertices_.front() )
|
1792
|
+
> epsilon )
|
1793
|
+
vertices_temp.push_back( vertices_.front() );
|
1794
|
+
|
1795
|
+
//Update list of vertices.
|
1796
|
+
vertices_ = vertices_temp;
|
1797
|
+
}
|
1798
|
+
|
1799
|
+
|
1800
|
+
void Polygon::reverse()
|
1801
|
+
{
|
1802
|
+
if( n() > 2 )
|
1803
|
+
std::reverse( ++vertices_.begin() , vertices_.end() );
|
1804
|
+
}
|
1805
|
+
|
1806
|
+
|
1807
|
+
bool operator == (Polygon polygon1, Polygon polygon2)
|
1808
|
+
{
|
1809
|
+
if( polygon1.n() != polygon2.n()
|
1810
|
+
or polygon1.n() == 0
|
1811
|
+
or polygon2.n() == 0 )
|
1812
|
+
return false;
|
1813
|
+
for(unsigned i=0; i<polygon1.n(); i++)
|
1814
|
+
if( !(polygon1[i] == polygon2[i]) )
|
1815
|
+
return false;
|
1816
|
+
return true;
|
1817
|
+
}
|
1818
|
+
bool operator != (Polygon polygon1, Polygon polygon2)
|
1819
|
+
{
|
1820
|
+
return !( polygon1 == polygon2 );
|
1821
|
+
}
|
1822
|
+
bool equivalent(Polygon polygon1, Polygon polygon2, double epsilon)
|
1823
|
+
{
|
1824
|
+
if( polygon1.n() == 0 or polygon2.n() == 0 )
|
1825
|
+
return false;
|
1826
|
+
if( polygon1.n() != polygon2.n() )
|
1827
|
+
return false;
|
1828
|
+
//Try all cyclic matches
|
1829
|
+
unsigned n = polygon1.n();//=polygon2.n()
|
1830
|
+
for( unsigned offset = 0 ; offset < n ; offset++ ){
|
1831
|
+
bool successful_match = true;
|
1832
|
+
for(unsigned i=0; i<n; i++){
|
1833
|
+
if( distance( polygon1[ i ] , polygon2[ i + offset ] ) > epsilon )
|
1834
|
+
{ successful_match = false; break; }
|
1835
|
+
}
|
1836
|
+
if( successful_match )
|
1837
|
+
return true;
|
1838
|
+
}
|
1839
|
+
return false;
|
1840
|
+
}
|
1841
|
+
|
1842
|
+
|
1843
|
+
double boundary_distance(const Polygon& polygon1, const Polygon& polygon2)
|
1844
|
+
{
|
1845
|
+
assert( polygon1.n() > 0 and polygon2.n() > 0 );
|
1846
|
+
|
1847
|
+
//Handle single point degeneracy.
|
1848
|
+
if(polygon1.n() == 1)
|
1849
|
+
return boundary_distance(polygon1[0], polygon2);
|
1850
|
+
else if(polygon2.n() == 1)
|
1851
|
+
return boundary_distance(polygon2[0], polygon1);
|
1852
|
+
//Handle cases where each polygon has at least 2 points.
|
1853
|
+
//Initialize to an upper bound.
|
1854
|
+
double running_min = boundary_distance(polygon1[0], polygon2);
|
1855
|
+
double distance_temp;
|
1856
|
+
//Loop over all possible pairs of line segments.
|
1857
|
+
for(unsigned i=0; i<=polygon1.n()-1; i++){
|
1858
|
+
for(unsigned j=0; j<=polygon2.n()-1; j++){
|
1859
|
+
distance_temp = distance( Line_Segment(polygon1[i], polygon1[i+1]) ,
|
1860
|
+
Line_Segment(polygon2[j], polygon2[j+1]) );
|
1861
|
+
if(distance_temp < running_min)
|
1862
|
+
running_min = distance_temp;
|
1863
|
+
}}
|
1864
|
+
return running_min;
|
1865
|
+
}
|
1866
|
+
|
1867
|
+
|
1868
|
+
std::ostream& operator << (std::ostream& outs,
|
1869
|
+
const Polygon& polygon_temp)
|
1870
|
+
{
|
1871
|
+
for(unsigned i=0; i<polygon_temp.n(); i++)
|
1872
|
+
outs << polygon_temp[i] << std::endl;
|
1873
|
+
return outs;
|
1874
|
+
}
|
1875
|
+
|
1876
|
+
|
1877
|
+
//Environment
|
1878
|
+
|
1879
|
+
|
1880
|
+
Environment::Environment(const std::vector<Polygon>& polygons)
|
1881
|
+
{
|
1882
|
+
outer_boundary_ = polygons[0];
|
1883
|
+
for(unsigned i=1; i<polygons.size(); i++)
|
1884
|
+
holes_.push_back( polygons[i] );
|
1885
|
+
update_flattened_index_key();
|
1886
|
+
}
|
1887
|
+
Environment::Environment(const std::string& filename)
|
1888
|
+
{
|
1889
|
+
std::ifstream fin(filename.c_str());
|
1890
|
+
//if(fin.fail()) { std::cerr << "\x1b[5;31m" << "Input file
|
1891
|
+
//opening failed." << "\x1b[0m\n" << "\a \n"; exit(1);}
|
1892
|
+
assert( !fin.fail() );
|
1893
|
+
|
1894
|
+
//Temporary vars for numbers to be read from file.
|
1895
|
+
double x_temp, y_temp;
|
1896
|
+
std::vector<Point> vertices_temp;
|
1897
|
+
|
1898
|
+
//Skip comments
|
1899
|
+
while( fin.peek() == '/' )
|
1900
|
+
fin.ignore(200,'\n');
|
1901
|
+
|
1902
|
+
//Read outer_boundary.
|
1903
|
+
while ( fin.peek() != '/' ){
|
1904
|
+
fin >> x_temp >> y_temp;
|
1905
|
+
//Skip to next line.
|
1906
|
+
fin.ignore(1);
|
1907
|
+
if( fin.eof() )
|
1908
|
+
{
|
1909
|
+
outer_boundary_.set_vertices(vertices_temp);
|
1910
|
+
fin.close();
|
1911
|
+
update_flattened_index_key(); return;
|
1912
|
+
}
|
1913
|
+
vertices_temp.push_back( Point(x_temp, y_temp) );
|
1914
|
+
}
|
1915
|
+
outer_boundary_.set_vertices(vertices_temp);
|
1916
|
+
vertices_temp.clear();
|
1917
|
+
|
1918
|
+
//Read holes.
|
1919
|
+
Polygon polygon_temp;
|
1920
|
+
while(1){
|
1921
|
+
//Skip comments
|
1922
|
+
while( fin.peek() == '/' )
|
1923
|
+
fin.ignore(200,'\n');
|
1924
|
+
if( fin.eof() )
|
1925
|
+
{ fin.close(); update_flattened_index_key(); return; }
|
1926
|
+
while( fin.peek() != '/' ){
|
1927
|
+
fin >> x_temp >> y_temp;
|
1928
|
+
if( fin.eof() )
|
1929
|
+
{
|
1930
|
+
polygon_temp.set_vertices(vertices_temp);
|
1931
|
+
holes_.push_back(polygon_temp);
|
1932
|
+
fin.close();
|
1933
|
+
update_flattened_index_key(); return;
|
1934
|
+
}
|
1935
|
+
vertices_temp.push_back( Point(x_temp, y_temp) );
|
1936
|
+
//Skips to next line.
|
1937
|
+
fin.ignore(1);
|
1938
|
+
}
|
1939
|
+
polygon_temp.set_vertices(vertices_temp);
|
1940
|
+
holes_.push_back(polygon_temp);
|
1941
|
+
vertices_temp.clear();
|
1942
|
+
}
|
1943
|
+
|
1944
|
+
update_flattened_index_key();
|
1945
|
+
}
|
1946
|
+
|
1947
|
+
|
1948
|
+
const Point& Environment::operator () (unsigned k) const
|
1949
|
+
{
|
1950
|
+
//std::pair<unsigned,unsigned> ij(one_to_two(k));
|
1951
|
+
std::pair<unsigned,unsigned> ij( flattened_index_key_[k] );
|
1952
|
+
return (*this)[ ij.first ][ ij.second ];
|
1953
|
+
}
|
1954
|
+
|
1955
|
+
|
1956
|
+
unsigned Environment::n() const
|
1957
|
+
{
|
1958
|
+
int n_count = 0;
|
1959
|
+
n_count = outer_boundary_.n();
|
1960
|
+
for(unsigned i=0; i<h(); i++)
|
1961
|
+
n_count += holes_[i].n();
|
1962
|
+
return n_count;
|
1963
|
+
}
|
1964
|
+
|
1965
|
+
|
1966
|
+
unsigned Environment::r() const
|
1967
|
+
{
|
1968
|
+
int r_count = 0;
|
1969
|
+
Polygon polygon_temp;
|
1970
|
+
r_count = outer_boundary_.r();
|
1971
|
+
for(unsigned i=0; i<h(); i++){
|
1972
|
+
r_count += holes_[i].n() - holes_[i].r();
|
1973
|
+
}
|
1974
|
+
return r_count;
|
1975
|
+
}
|
1976
|
+
|
1977
|
+
|
1978
|
+
bool Environment::is_in_standard_form() const
|
1979
|
+
{
|
1980
|
+
if( outer_boundary_.is_in_standard_form() == false
|
1981
|
+
or outer_boundary_.area() < 0 )
|
1982
|
+
return false;
|
1983
|
+
for(unsigned i=0; i<holes_.size(); i++)
|
1984
|
+
if( holes_[i].is_in_standard_form() == false
|
1985
|
+
or holes_[i].area() > 0 )
|
1986
|
+
return false;
|
1987
|
+
return true;
|
1988
|
+
}
|
1989
|
+
|
1990
|
+
|
1991
|
+
bool Environment::is_valid(double epsilon) const
|
1992
|
+
{
|
1993
|
+
if( n() <= 2 )
|
1994
|
+
return false;
|
1995
|
+
|
1996
|
+
//Check all Polygons are simple.
|
1997
|
+
if( !outer_boundary_.is_simple(epsilon) ){
|
1998
|
+
std::cerr << std::endl << "\x1b[31m"
|
1999
|
+
<< "The outer boundary is not simple."
|
2000
|
+
<< "\x1b[0m" << std::endl;
|
2001
|
+
return false;
|
2002
|
+
}
|
2003
|
+
for(unsigned i=0; i<h(); i++)
|
2004
|
+
if( !holes_[i].is_simple(epsilon) ){
|
2005
|
+
std::cerr << std::endl << "\x1b[31m"
|
2006
|
+
<< "Hole " << i << " is not simple."
|
2007
|
+
<< "\x1b[0m" << std::endl;
|
2008
|
+
return false;
|
2009
|
+
}
|
2010
|
+
|
2011
|
+
//Check none of the Polygons' boundaries intersect w/in epsilon.
|
2012
|
+
for(unsigned i=0; i<h(); i++)
|
2013
|
+
if( boundary_distance(outer_boundary_, holes_[i]) <= epsilon ){
|
2014
|
+
std::cerr << std::endl << "\x1b[31m"
|
2015
|
+
<< "The outer boundary intersects the boundary of hole " << i << "."
|
2016
|
+
<< "\x1b[0m" << std::endl;
|
2017
|
+
return false;
|
2018
|
+
}
|
2019
|
+
for(unsigned i=0; i<h(); i++)
|
2020
|
+
for(unsigned j=i+1; j<h(); j++)
|
2021
|
+
if( boundary_distance(holes_[i], holes_[j]) <= epsilon ){
|
2022
|
+
std::cerr << std::endl << "\x1b[31m"
|
2023
|
+
<< "The boundary of hole " << i
|
2024
|
+
<< " intersects the boundary of hole " << j << "."
|
2025
|
+
<< "\x1b[0m" << std::endl;
|
2026
|
+
return false;
|
2027
|
+
}
|
2028
|
+
|
2029
|
+
//Check that the vertices of each hole are in the outside_boundary
|
2030
|
+
//and not in any other holes.
|
2031
|
+
//Loop over holes.
|
2032
|
+
for(unsigned i=0; i<h(); i++){
|
2033
|
+
//Loop over vertices of a hole
|
2034
|
+
for(unsigned j=0; j<holes_[i].n(); j++){
|
2035
|
+
if( !holes_[i][j].in(outer_boundary_, epsilon) ){
|
2036
|
+
std::cerr << std::endl << "\x1b[31m"
|
2037
|
+
<< "Vertex " << j << " of hole " << i
|
2038
|
+
<< " is not within the outer boundary."
|
2039
|
+
<< "\x1b[0m" << std::endl;
|
2040
|
+
return false;
|
2041
|
+
}
|
2042
|
+
//Second loop over holes.
|
2043
|
+
for(unsigned k=0; k<h(); k++)
|
2044
|
+
if( i!=k and holes_[i][j].in(holes_[k], epsilon) ){
|
2045
|
+
std::cerr << std::endl << "\x1b[31m"
|
2046
|
+
<< "Vertex " << j
|
2047
|
+
<< " of hole " << i
|
2048
|
+
<< " is in hole " << k << "."
|
2049
|
+
<< "\x1b[0m" << std::endl;
|
2050
|
+
return false;
|
2051
|
+
}
|
2052
|
+
}
|
2053
|
+
}
|
2054
|
+
|
2055
|
+
//Check outer_boundary is ccw and holes are cw.
|
2056
|
+
if( outer_boundary_.area() <= 0 ){
|
2057
|
+
std::cerr << std::endl << "\x1b[31m"
|
2058
|
+
<< "The outer boundary vertices are not listed ccw."
|
2059
|
+
<< "\x1b[0m" << std::endl;
|
2060
|
+
return false;
|
2061
|
+
}
|
2062
|
+
for(unsigned i=0; i<h(); i++)
|
2063
|
+
if( holes_[i].area() >= 0 ){
|
2064
|
+
std::cerr << std::endl << "\x1b[31m"
|
2065
|
+
<< "The vertices of hole " << i << " are not listed cw."
|
2066
|
+
<< "\x1b[0m" << std::endl;
|
2067
|
+
return false;
|
2068
|
+
}
|
2069
|
+
|
2070
|
+
return true;
|
2071
|
+
}
|
2072
|
+
|
2073
|
+
|
2074
|
+
double Environment::boundary_length() const
|
2075
|
+
{
|
2076
|
+
//Precondition: nonempty Environment.
|
2077
|
+
assert( outer_boundary_.n() > 0 );
|
2078
|
+
|
2079
|
+
double length_temp = outer_boundary_.boundary_length();
|
2080
|
+
for(unsigned i=0; i<h(); i++)
|
2081
|
+
length_temp += holes_[i].boundary_length();
|
2082
|
+
return length_temp;
|
2083
|
+
}
|
2084
|
+
|
2085
|
+
|
2086
|
+
double Environment::area() const
|
2087
|
+
{
|
2088
|
+
double area_temp = outer_boundary_.area();
|
2089
|
+
for(unsigned i=0; i<h(); i++)
|
2090
|
+
area_temp += holes_[i].area();
|
2091
|
+
return area_temp;
|
2092
|
+
}
|
2093
|
+
|
2094
|
+
|
2095
|
+
std::vector<Point> Environment::random_points(const unsigned& count,
|
2096
|
+
double epsilon) const
|
2097
|
+
{
|
2098
|
+
assert( area() > 0 );
|
2099
|
+
|
2100
|
+
Bounding_Box bounding_box = bbox();
|
2101
|
+
std::vector<Point> pts_in_environment;
|
2102
|
+
pts_in_environment.reserve(count);
|
2103
|
+
Point pt_temp( uniform_random_sample(bounding_box.x_min,
|
2104
|
+
bounding_box.x_max),
|
2105
|
+
uniform_random_sample(bounding_box.y_min,
|
2106
|
+
bounding_box.y_max) );
|
2107
|
+
while(pts_in_environment.size() < count){
|
2108
|
+
while(!pt_temp.in(*this, epsilon)){
|
2109
|
+
pt_temp.set_x( uniform_random_sample(bounding_box.x_min,
|
2110
|
+
bounding_box.x_max) );
|
2111
|
+
pt_temp.set_y( uniform_random_sample(bounding_box.y_min,
|
2112
|
+
bounding_box.y_max) );
|
2113
|
+
}
|
2114
|
+
pts_in_environment.push_back(pt_temp);
|
2115
|
+
pt_temp.set_x( uniform_random_sample(bounding_box.x_min,
|
2116
|
+
bounding_box.x_max) );
|
2117
|
+
pt_temp.set_y( uniform_random_sample(bounding_box.y_min,
|
2118
|
+
bounding_box.y_max) );
|
2119
|
+
}
|
2120
|
+
return pts_in_environment;
|
2121
|
+
}
|
2122
|
+
|
2123
|
+
|
2124
|
+
Polyline Environment::shortest_path(const Point& start,
|
2125
|
+
const Point& finish,
|
2126
|
+
const Visibility_Graph& visibility_graph,
|
2127
|
+
double epsilon)
|
2128
|
+
{
|
2129
|
+
//true => data printed to terminal
|
2130
|
+
//false => silent
|
2131
|
+
const bool PRINTING_DEBUG_DATA = false;
|
2132
|
+
|
2133
|
+
//For now, just find one shortest path, later change this to a
|
2134
|
+
//vector to find all shortest paths (w/in epsilon).
|
2135
|
+
Polyline shortest_path_output;
|
2136
|
+
Visibility_Polygon start_visibility_polygon(start, *this, epsilon);
|
2137
|
+
|
2138
|
+
//Trivial cases
|
2139
|
+
if( distance(start,finish) <= epsilon ){
|
2140
|
+
shortest_path_output.push_back(start);
|
2141
|
+
return shortest_path_output;
|
2142
|
+
}
|
2143
|
+
else if( finish.in(start_visibility_polygon, epsilon) ){
|
2144
|
+
shortest_path_output.push_back(start);
|
2145
|
+
shortest_path_output.push_back(finish);
|
2146
|
+
return shortest_path_output;
|
2147
|
+
}
|
2148
|
+
|
2149
|
+
Visibility_Polygon finish_visibility_polygon(finish, *this, epsilon);
|
2150
|
+
|
2151
|
+
//Connect start and finish Points to the visibility graph
|
2152
|
+
bool *start_visible; //start row of visibility graph
|
2153
|
+
bool *finish_visible; //finish row of visibility graph
|
2154
|
+
start_visible = new bool[n()];
|
2155
|
+
finish_visible = new bool[n()];
|
2156
|
+
for(unsigned k=0; k<n(); k++){
|
2157
|
+
if( (*this)(k).in( start_visibility_polygon , epsilon ) )
|
2158
|
+
start_visible[k] = true;
|
2159
|
+
else
|
2160
|
+
start_visible[k] = false;
|
2161
|
+
if( (*this)(k).in( finish_visibility_polygon , epsilon ) )
|
2162
|
+
finish_visible[k] = true;
|
2163
|
+
else
|
2164
|
+
finish_visible[k] = false;
|
2165
|
+
}
|
2166
|
+
|
2167
|
+
//Initialize search tree of visited nodes
|
2168
|
+
std::list<Shortest_Path_Node> T;
|
2169
|
+
//:WARNING:
|
2170
|
+
//If T is a vector it is crucial to make T large enough that it
|
2171
|
+
//will not be resized. If T were resized, any iterators pointing
|
2172
|
+
//to its contents would be invalidated, thus causing the program
|
2173
|
+
//to fail.
|
2174
|
+
//T.reserve( n() + 3 );
|
2175
|
+
|
2176
|
+
//Initialize priority queue of unexpanded nodes
|
2177
|
+
std::set<Shortest_Path_Node> Q;
|
2178
|
+
|
2179
|
+
//Construct initial node
|
2180
|
+
Shortest_Path_Node current_node;
|
2181
|
+
//convention vertex_index == n() => corresponds to start Point
|
2182
|
+
//vertex_index == n() + 1 => corresponds to finish Point
|
2183
|
+
current_node.vertex_index = n();
|
2184
|
+
current_node.cost_to_come = 0;
|
2185
|
+
current_node.estimated_cost_to_go = distance( start , finish );
|
2186
|
+
//Put in T and on Q
|
2187
|
+
T.push_back( current_node );
|
2188
|
+
T.begin()->search_tree_location = T.begin();
|
2189
|
+
current_node.search_tree_location = T.begin();
|
2190
|
+
T.begin()->parent_search_tree_location = T.begin();
|
2191
|
+
current_node.parent_search_tree_location = T.begin();
|
2192
|
+
Q.insert( current_node );
|
2193
|
+
|
2194
|
+
//Initialize temporary variables
|
2195
|
+
Shortest_Path_Node child; //children of current_node
|
2196
|
+
std::vector<Shortest_Path_Node> children;
|
2197
|
+
//flags
|
2198
|
+
bool solution_found = false;
|
2199
|
+
bool child_already_visited = false;
|
2200
|
+
//-----------Begin Main Loop-----------
|
2201
|
+
while( !Q.empty() ){
|
2202
|
+
|
2203
|
+
//Pop top element off Q onto current_node
|
2204
|
+
current_node = *Q.begin(); Q.erase( Q.begin() );
|
2205
|
+
|
2206
|
+
if(PRINTING_DEBUG_DATA){
|
2207
|
+
std::cout << std::endl
|
2208
|
+
<<"=============="
|
2209
|
+
<<" current_node just poped off of Q "
|
2210
|
+
<<"=============="
|
2211
|
+
<< std::endl;
|
2212
|
+
current_node.print();
|
2213
|
+
std::cout << std::endl;
|
2214
|
+
}
|
2215
|
+
|
2216
|
+
//Check for goal state
|
2217
|
+
//(if current node corresponds to finish)
|
2218
|
+
if( current_node.vertex_index == n() + 1 ){
|
2219
|
+
|
2220
|
+
if( PRINTING_DEBUG_DATA ){
|
2221
|
+
std::cout <<"solution found!"
|
2222
|
+
<< std::endl
|
2223
|
+
<< std::endl;
|
2224
|
+
}
|
2225
|
+
|
2226
|
+
solution_found = true;
|
2227
|
+
break;
|
2228
|
+
}
|
2229
|
+
|
2230
|
+
//Expand current_node (compute children)
|
2231
|
+
children.clear();
|
2232
|
+
|
2233
|
+
if( PRINTING_DEBUG_DATA ){
|
2234
|
+
std::cout << "-------------------------------------------"
|
2235
|
+
<< std::endl
|
2236
|
+
<< "Expanding Current Node (Computing Children)"
|
2237
|
+
<< std::endl
|
2238
|
+
<< "current size of search tree T = "
|
2239
|
+
<< T.size()
|
2240
|
+
<< std::endl
|
2241
|
+
<< "-------------------------------------------"
|
2242
|
+
<< std::endl;
|
2243
|
+
}
|
2244
|
+
|
2245
|
+
//if current_node corresponds to start
|
2246
|
+
if( current_node.vertex_index == n() ){
|
2247
|
+
//loop over environment vertices
|
2248
|
+
for(unsigned i=0; i < n(); i++){
|
2249
|
+
if( start_visible[i] ){
|
2250
|
+
child.vertex_index = i;
|
2251
|
+
child.parent_search_tree_location
|
2252
|
+
= current_node.search_tree_location;
|
2253
|
+
child.cost_to_come = distance( start , (*this)(i) );
|
2254
|
+
child.estimated_cost_to_go = distance( (*this)(i) , finish );
|
2255
|
+
children.push_back( child );
|
2256
|
+
|
2257
|
+
if( PRINTING_DEBUG_DATA ){
|
2258
|
+
std::cout << std::endl << "computed child: "
|
2259
|
+
<< std::endl;
|
2260
|
+
child.print();
|
2261
|
+
}
|
2262
|
+
|
2263
|
+
}
|
2264
|
+
}
|
2265
|
+
}
|
2266
|
+
//else current_node corresponds to a vertex of the environment
|
2267
|
+
else{
|
2268
|
+
//check which environment vertices are visible
|
2269
|
+
for(unsigned i=0; i < n(); i++){
|
2270
|
+
if( current_node.vertex_index != i )
|
2271
|
+
if( visibility_graph( current_node.vertex_index , i ) ){
|
2272
|
+
child.vertex_index = i;
|
2273
|
+
child.parent_search_tree_location
|
2274
|
+
= current_node.search_tree_location;
|
2275
|
+
child.cost_to_come = current_node.cost_to_come
|
2276
|
+
+ distance( (*this)(current_node.vertex_index),
|
2277
|
+
(*this)(i) );
|
2278
|
+
child.estimated_cost_to_go = distance( (*this)(i) , finish );
|
2279
|
+
children.push_back( child );
|
2280
|
+
|
2281
|
+
if( PRINTING_DEBUG_DATA ){
|
2282
|
+
std::cout << std::endl << "computed child: "
|
2283
|
+
<< std::endl;
|
2284
|
+
child.print();
|
2285
|
+
}
|
2286
|
+
|
2287
|
+
}
|
2288
|
+
}
|
2289
|
+
//check if finish is visible
|
2290
|
+
if( finish_visible[ current_node.vertex_index ] ){
|
2291
|
+
child.vertex_index = n() + 1;
|
2292
|
+
child.parent_search_tree_location
|
2293
|
+
= current_node.search_tree_location;
|
2294
|
+
child.cost_to_come = current_node.cost_to_come
|
2295
|
+
+ distance( (*this)(current_node.vertex_index) , finish );
|
2296
|
+
child.estimated_cost_to_go = 0;
|
2297
|
+
children.push_back( child );
|
2298
|
+
|
2299
|
+
if( PRINTING_DEBUG_DATA ){
|
2300
|
+
std::cout << std::endl << "computed child: "
|
2301
|
+
<< std::endl;
|
2302
|
+
child.print();
|
2303
|
+
}
|
2304
|
+
|
2305
|
+
}
|
2306
|
+
}
|
2307
|
+
|
2308
|
+
if( PRINTING_DEBUG_DATA ){
|
2309
|
+
std::cout << std::endl
|
2310
|
+
<<"-----------------------------------------"
|
2311
|
+
<< std::endl
|
2312
|
+
<< "Processing " << children.size()
|
2313
|
+
<< " children" << std::endl
|
2314
|
+
<< "-----------------------------------------"
|
2315
|
+
<< std::endl;
|
2316
|
+
}
|
2317
|
+
|
2318
|
+
//Process children
|
2319
|
+
for( std::vector<Shortest_Path_Node>::iterator
|
2320
|
+
children_itr = children.begin();
|
2321
|
+
children_itr != children.end();
|
2322
|
+
children_itr++ ){
|
2323
|
+
child_already_visited = false;
|
2324
|
+
|
2325
|
+
if( PRINTING_DEBUG_DATA ){
|
2326
|
+
std::cout << std::endl << "current child being processed: "
|
2327
|
+
<< std::endl;
|
2328
|
+
children_itr->print();
|
2329
|
+
}
|
2330
|
+
|
2331
|
+
//Check if child state has already been visited
|
2332
|
+
//(by looking in search tree T)
|
2333
|
+
for( std::list<Shortest_Path_Node>::iterator T_itr = T.begin();
|
2334
|
+
T_itr != T.end(); T_itr++ ){
|
2335
|
+
if( children_itr->vertex_index
|
2336
|
+
== T_itr->vertex_index ){
|
2337
|
+
children_itr->search_tree_location = T_itr;
|
2338
|
+
child_already_visited = true;
|
2339
|
+
break;
|
2340
|
+
}
|
2341
|
+
}
|
2342
|
+
|
2343
|
+
if( !child_already_visited ){
|
2344
|
+
//Add child to search tree T
|
2345
|
+
T.push_back( *children_itr );
|
2346
|
+
(--T.end())->search_tree_location = --T.end();
|
2347
|
+
children_itr->search_tree_location = --T.end();
|
2348
|
+
Q.insert( *children_itr );
|
2349
|
+
}
|
2350
|
+
else if( children_itr->search_tree_location->cost_to_come >
|
2351
|
+
children_itr->cost_to_come ){
|
2352
|
+
//redirect parent pointer in search tree
|
2353
|
+
children_itr->search_tree_location->parent_search_tree_location
|
2354
|
+
= children_itr->parent_search_tree_location;
|
2355
|
+
//and update cost data
|
2356
|
+
children_itr->search_tree_location->cost_to_come
|
2357
|
+
= children_itr->cost_to_come;
|
2358
|
+
//update Q
|
2359
|
+
for(std::set<Shortest_Path_Node>::iterator
|
2360
|
+
Q_itr = Q.begin();
|
2361
|
+
Q_itr!= Q.end();
|
2362
|
+
Q_itr++){
|
2363
|
+
if( children_itr->vertex_index == Q_itr->vertex_index ){
|
2364
|
+
Q.erase( Q_itr );
|
2365
|
+
break;
|
2366
|
+
}
|
2367
|
+
}
|
2368
|
+
Q.insert( *children_itr );
|
2369
|
+
}
|
2370
|
+
|
2371
|
+
//If not already visited, insert into Q
|
2372
|
+
if( !child_already_visited )
|
2373
|
+
Q.insert( *children_itr );
|
2374
|
+
|
2375
|
+
if( PRINTING_DEBUG_DATA ){
|
2376
|
+
std::cout << "child already visited? "
|
2377
|
+
<< child_already_visited
|
2378
|
+
<< std::endl;
|
2379
|
+
}
|
2380
|
+
|
2381
|
+
}
|
2382
|
+
}
|
2383
|
+
//-----------End Main Loop-----------
|
2384
|
+
|
2385
|
+
//Recover solution
|
2386
|
+
if( solution_found ){
|
2387
|
+
shortest_path_output.push_back( finish );
|
2388
|
+
std::list<Shortest_Path_Node>::iterator
|
2389
|
+
backtrace_itr = current_node.parent_search_tree_location;
|
2390
|
+
Point waypoint;
|
2391
|
+
|
2392
|
+
if( PRINTING_DEBUG_DATA ){
|
2393
|
+
std::cout << "----------------------------" << std::endl
|
2394
|
+
<< "backtracing to find solution" << std::endl
|
2395
|
+
<< "----------------------------" << std::endl;
|
2396
|
+
|
2397
|
+
}
|
2398
|
+
|
2399
|
+
while( true ){
|
2400
|
+
|
2401
|
+
if( PRINTING_DEBUG_DATA ){
|
2402
|
+
std::cout << "backtrace node is "
|
2403
|
+
<< std::endl;
|
2404
|
+
backtrace_itr->print();
|
2405
|
+
std::cout << std::endl;
|
2406
|
+
}
|
2407
|
+
|
2408
|
+
if( backtrace_itr->vertex_index < n() )
|
2409
|
+
waypoint = (*this)( backtrace_itr->vertex_index );
|
2410
|
+
else if( backtrace_itr->vertex_index == n() )
|
2411
|
+
waypoint = start;
|
2412
|
+
//Add vertex if not redundant
|
2413
|
+
if( shortest_path_output.size() > 0
|
2414
|
+
and distance( shortest_path_output[ shortest_path_output.size()
|
2415
|
+
- 1 ],
|
2416
|
+
waypoint ) > epsilon )
|
2417
|
+
shortest_path_output.push_back( waypoint );
|
2418
|
+
if( backtrace_itr->cost_to_come == 0 )
|
2419
|
+
break;
|
2420
|
+
backtrace_itr = backtrace_itr->parent_search_tree_location;
|
2421
|
+
}
|
2422
|
+
shortest_path_output.reverse();
|
2423
|
+
}
|
2424
|
+
|
2425
|
+
//free memory
|
2426
|
+
delete [] start_visible;
|
2427
|
+
delete [] finish_visible;
|
2428
|
+
|
2429
|
+
//shortest_path_output.eliminate_redundant_vertices( epsilon );
|
2430
|
+
//May not be desirable to eliminate redundant vertices, because
|
2431
|
+
//those redundant vertices can make successive waypoints along the
|
2432
|
+
//shortest path robustly visible (and thus easier for a robot to
|
2433
|
+
//navigate)
|
2434
|
+
|
2435
|
+
return shortest_path_output;
|
2436
|
+
}
|
2437
|
+
Polyline Environment::shortest_path(const Point& start,
|
2438
|
+
const Point& finish,
|
2439
|
+
double epsilon)
|
2440
|
+
{
|
2441
|
+
return shortest_path( start,
|
2442
|
+
finish,
|
2443
|
+
Visibility_Graph(*this, epsilon),
|
2444
|
+
epsilon );
|
2445
|
+
}
|
2446
|
+
|
2447
|
+
|
2448
|
+
void Environment::write_to_file(const std::string& filename,
|
2449
|
+
int fios_precision_temp)
|
2450
|
+
{
|
2451
|
+
assert( fios_precision_temp >= 1 );
|
2452
|
+
|
2453
|
+
std::ofstream fout( filename.c_str() );
|
2454
|
+
//fout.open( filename.c_str() ); //Alternatives.
|
2455
|
+
//fout << *this;
|
2456
|
+
fout.setf(std::ios::fixed);
|
2457
|
+
fout.setf(std::ios::showpoint);
|
2458
|
+
fout.precision(fios_precision_temp);
|
2459
|
+
fout << "//Environment Model" << std::endl;
|
2460
|
+
fout << "//Outer Boundary" << std::endl << outer_boundary_;
|
2461
|
+
for(unsigned i=0; i<h(); i++)
|
2462
|
+
{
|
2463
|
+
fout << "//Hole" << std::endl << holes_[i];
|
2464
|
+
}
|
2465
|
+
//fout << "//EOF marker";
|
2466
|
+
fout.close();
|
2467
|
+
}
|
2468
|
+
|
2469
|
+
|
2470
|
+
Point& Environment::operator () (unsigned k)
|
2471
|
+
{
|
2472
|
+
//std::pair<unsigned,unsigned> ij( one_to_two(k) );
|
2473
|
+
std::pair<unsigned,unsigned> ij( flattened_index_key_[k] );
|
2474
|
+
return (*this)[ ij.first ][ ij.second ];
|
2475
|
+
}
|
2476
|
+
|
2477
|
+
|
2478
|
+
void Environment::enforce_standard_form()
|
2479
|
+
{
|
2480
|
+
if( outer_boundary_.area() < 0 )
|
2481
|
+
outer_boundary_.reverse();
|
2482
|
+
outer_boundary_.enforce_standard_form();
|
2483
|
+
for(unsigned i=0; i<h(); i++){
|
2484
|
+
if( holes_[i].area() > 0 )
|
2485
|
+
holes_[i].reverse();
|
2486
|
+
holes_[i].enforce_standard_form();
|
2487
|
+
}
|
2488
|
+
}
|
2489
|
+
|
2490
|
+
|
2491
|
+
void Environment::eliminate_redundant_vertices(double epsilon)
|
2492
|
+
{
|
2493
|
+
outer_boundary_.eliminate_redundant_vertices(epsilon);
|
2494
|
+
for(unsigned i=0; i<holes_.size(); i++)
|
2495
|
+
holes_[i].eliminate_redundant_vertices(epsilon);
|
2496
|
+
|
2497
|
+
update_flattened_index_key();
|
2498
|
+
}
|
2499
|
+
|
2500
|
+
|
2501
|
+
void Environment::reverse_holes()
|
2502
|
+
{
|
2503
|
+
for(unsigned i=0; i < holes_.size(); i++)
|
2504
|
+
holes_[i].reverse();
|
2505
|
+
}
|
2506
|
+
|
2507
|
+
|
2508
|
+
void Environment::update_flattened_index_key()
|
2509
|
+
{
|
2510
|
+
flattened_index_key_.clear();
|
2511
|
+
std::pair<unsigned, unsigned> pair_temp;
|
2512
|
+
for(unsigned i=0; i<=h(); i++){
|
2513
|
+
for(unsigned j=0; j<(*this)[i].n(); j++){
|
2514
|
+
pair_temp.first = i;
|
2515
|
+
pair_temp.second = j;
|
2516
|
+
flattened_index_key_.push_back( pair_temp );
|
2517
|
+
}}
|
2518
|
+
}
|
2519
|
+
|
2520
|
+
|
2521
|
+
std::pair<unsigned,unsigned> Environment::one_to_two(unsigned k) const
|
2522
|
+
{
|
2523
|
+
std::pair<unsigned,unsigned> two(0,0);
|
2524
|
+
//Strategy: add up vertex count of each Polygon (outer boundary +
|
2525
|
+
//holes) until greater than k
|
2526
|
+
unsigned current_polygon_index = 0;
|
2527
|
+
unsigned vertex_count_up_to_current_polygon = (*this)[0].n();
|
2528
|
+
unsigned vertex_count_up_to_last_polygon = 0;
|
2529
|
+
|
2530
|
+
while( k >= vertex_count_up_to_current_polygon
|
2531
|
+
and current_polygon_index < (*this).h() ){
|
2532
|
+
current_polygon_index++;
|
2533
|
+
two.first = two.first + 1;
|
2534
|
+
vertex_count_up_to_last_polygon = vertex_count_up_to_current_polygon;
|
2535
|
+
vertex_count_up_to_current_polygon += (*this)[current_polygon_index].n();
|
2536
|
+
}
|
2537
|
+
two.second = k - vertex_count_up_to_last_polygon;
|
2538
|
+
|
2539
|
+
return two;
|
2540
|
+
}
|
2541
|
+
|
2542
|
+
|
2543
|
+
std::ostream& operator << (std::ostream& outs,
|
2544
|
+
const Environment& environment_temp)
|
2545
|
+
{
|
2546
|
+
outs << "//Environment Model" << std::endl;
|
2547
|
+
outs << "//Outer Boundary" << std::endl << environment_temp[0];
|
2548
|
+
for(unsigned i=1; i<=environment_temp.h(); i++){
|
2549
|
+
outs << "//Hole" << std::endl << environment_temp[i];
|
2550
|
+
}
|
2551
|
+
//outs << "//EOF marker";
|
2552
|
+
return outs;
|
2553
|
+
}
|
2554
|
+
|
2555
|
+
|
2556
|
+
//Guards
|
2557
|
+
|
2558
|
+
|
2559
|
+
Guards::Guards(const std::string& filename)
|
2560
|
+
{
|
2561
|
+
std::ifstream fin(filename.c_str());
|
2562
|
+
//if(fin.fail()) { std::cerr << "\x1b[5;31m" << "Input file
|
2563
|
+
//opening failed." << "\x1b[0m\n" << "\a \n"; exit(1);}
|
2564
|
+
assert( !fin.fail() );
|
2565
|
+
|
2566
|
+
//Temp vars for numbers to be read from file.
|
2567
|
+
double x_temp, y_temp;
|
2568
|
+
|
2569
|
+
//Skip comments
|
2570
|
+
while( fin.peek() == '/' )
|
2571
|
+
fin.ignore(200,'\n');
|
2572
|
+
|
2573
|
+
//Read positions.
|
2574
|
+
while (1){
|
2575
|
+
fin >> x_temp >> y_temp;
|
2576
|
+
if( fin.eof() )
|
2577
|
+
{ fin.close(); return; }
|
2578
|
+
positions_.push_back( Point(x_temp, y_temp) );
|
2579
|
+
//Skip to next line.
|
2580
|
+
fin.ignore(1);
|
2581
|
+
//Skip comments
|
2582
|
+
while( fin.peek() == '/' )
|
2583
|
+
fin.ignore(200,'\n');
|
2584
|
+
}
|
2585
|
+
}
|
2586
|
+
|
2587
|
+
|
2588
|
+
bool Guards::are_lex_ordered() const
|
2589
|
+
{
|
2590
|
+
//if more than one guard.
|
2591
|
+
if(positions_.size() > 1)
|
2592
|
+
for(unsigned i=0; i<positions_.size()-1; i++)
|
2593
|
+
if(positions_[i] > positions_[i+1])
|
2594
|
+
return false;
|
2595
|
+
return true;
|
2596
|
+
}
|
2597
|
+
|
2598
|
+
|
2599
|
+
bool Guards::noncolocated(double epsilon) const
|
2600
|
+
{
|
2601
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2602
|
+
for(unsigned j=i+1; j<positions_.size(); j++)
|
2603
|
+
if( distance(positions_[i], positions_[j]) <= epsilon )
|
2604
|
+
return false;
|
2605
|
+
return true;
|
2606
|
+
}
|
2607
|
+
|
2608
|
+
|
2609
|
+
bool Guards::in(const Polygon& polygon_temp, double epsilon) const
|
2610
|
+
{
|
2611
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2612
|
+
if(!positions_[i].in(polygon_temp, epsilon))
|
2613
|
+
return false;
|
2614
|
+
return true;
|
2615
|
+
}
|
2616
|
+
|
2617
|
+
|
2618
|
+
bool Guards::in(const Environment& environment_temp, double epsilon) const
|
2619
|
+
{
|
2620
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2621
|
+
if(!positions_[i].in(environment_temp, epsilon))
|
2622
|
+
return false;
|
2623
|
+
return true;
|
2624
|
+
}
|
2625
|
+
|
2626
|
+
|
2627
|
+
double Guards::diameter() const
|
2628
|
+
{
|
2629
|
+
//Precondition: more than 0 guards
|
2630
|
+
assert( N() > 0 );
|
2631
|
+
|
2632
|
+
double running_max=0;
|
2633
|
+
for(unsigned i=0; i<N()-1; i++){
|
2634
|
+
for(unsigned j=i+1; j<N(); j++){
|
2635
|
+
if( distance( (*this)[i] , (*this)[j] ) > running_max )
|
2636
|
+
running_max = distance( (*this)[i] , (*this)[j] );
|
2637
|
+
}}
|
2638
|
+
return running_max;
|
2639
|
+
}
|
2640
|
+
|
2641
|
+
|
2642
|
+
Bounding_Box Guards::bbox() const
|
2643
|
+
{
|
2644
|
+
//Precondition: nonempty Guard set
|
2645
|
+
assert( positions_.size() > 0 );
|
2646
|
+
|
2647
|
+
Bounding_Box bounding_box;
|
2648
|
+
double x_min=positions_[0].x(), x_max=positions_[0].x(),
|
2649
|
+
y_min=positions_[0].y(), y_max=positions_[0].y();
|
2650
|
+
for(unsigned i = 1; i < positions_.size(); i++){
|
2651
|
+
if(x_min > positions_[i].x()) { x_min=positions_[i].x(); }
|
2652
|
+
if(x_max < positions_[i].x()) { x_max=positions_[i].x(); }
|
2653
|
+
if(y_min > positions_[i].y()) { y_min=positions_[i].y(); }
|
2654
|
+
if(y_max < positions_[i].y()) { y_max=positions_[i].y(); }
|
2655
|
+
}
|
2656
|
+
bounding_box.x_min=x_min; bounding_box.x_max=x_max;
|
2657
|
+
bounding_box.y_min=y_min; bounding_box.y_max=y_max;
|
2658
|
+
return bounding_box;
|
2659
|
+
}
|
2660
|
+
|
2661
|
+
|
2662
|
+
void Guards::write_to_file(const std::string& filename,
|
2663
|
+
int fios_precision_temp)
|
2664
|
+
{
|
2665
|
+
assert( fios_precision_temp >= 1 );
|
2666
|
+
|
2667
|
+
std::ofstream fout( filename.c_str() );
|
2668
|
+
//fout.open( filename.c_str() ); //Alternatives.
|
2669
|
+
//fout << *this;
|
2670
|
+
fout.setf(std::ios::fixed);
|
2671
|
+
fout.setf(std::ios::showpoint);
|
2672
|
+
fout.precision(fios_precision_temp);
|
2673
|
+
fout << "//Guard Positions" << std::endl;
|
2674
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2675
|
+
fout << positions_[i].x() << " " << positions_[i].y() << std::endl;
|
2676
|
+
//fout << "//EOF marker";
|
2677
|
+
fout.close();
|
2678
|
+
}
|
2679
|
+
|
2680
|
+
|
2681
|
+
void Guards::enforce_lex_order()
|
2682
|
+
{
|
2683
|
+
//std::stable_sort(positions_.begin(), positions_.end());
|
2684
|
+
std::sort(positions_.begin(), positions_.end());
|
2685
|
+
}
|
2686
|
+
|
2687
|
+
|
2688
|
+
void Guards::reverse()
|
2689
|
+
{
|
2690
|
+
std::reverse( positions_.begin() , positions_.end() );
|
2691
|
+
}
|
2692
|
+
|
2693
|
+
|
2694
|
+
void Guards::snap_to_vertices_of(const Environment& environment_temp,
|
2695
|
+
double epsilon)
|
2696
|
+
{
|
2697
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2698
|
+
positions_[i].snap_to_vertices_of(environment_temp);
|
2699
|
+
}
|
2700
|
+
|
2701
|
+
|
2702
|
+
void Guards::snap_to_vertices_of(const Polygon& polygon_temp,
|
2703
|
+
double epsilon)
|
2704
|
+
{
|
2705
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2706
|
+
positions_[i].snap_to_vertices_of(polygon_temp);
|
2707
|
+
}
|
2708
|
+
|
2709
|
+
|
2710
|
+
void Guards::snap_to_boundary_of(const Environment& environment_temp,
|
2711
|
+
double epsilon)
|
2712
|
+
{
|
2713
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2714
|
+
positions_[i].snap_to_boundary_of(environment_temp);
|
2715
|
+
}
|
2716
|
+
|
2717
|
+
|
2718
|
+
void Guards::snap_to_boundary_of(const Polygon& polygon_temp,
|
2719
|
+
double epsilon)
|
2720
|
+
{
|
2721
|
+
for(unsigned i=0; i<positions_.size(); i++)
|
2722
|
+
positions_[i].snap_to_boundary_of(polygon_temp);
|
2723
|
+
}
|
2724
|
+
|
2725
|
+
|
2726
|
+
std::ostream& operator << (std::ostream& outs, const Guards& guards)
|
2727
|
+
{
|
2728
|
+
outs << "//Guard Positions" << std::endl;
|
2729
|
+
for(unsigned i=0; i<guards.N(); i++)
|
2730
|
+
outs << guards[i].x() << " " << guards[i].y() << std::endl;
|
2731
|
+
//outs << "//EOF marker";
|
2732
|
+
return outs;
|
2733
|
+
}
|
2734
|
+
|
2735
|
+
|
2736
|
+
//Visibility_Polygon
|
2737
|
+
|
2738
|
+
|
2739
|
+
bool Visibility_Polygon::is_spike( const Point& observer,
|
2740
|
+
const Point& point1,
|
2741
|
+
const Point& point2,
|
2742
|
+
const Point& point3,
|
2743
|
+
double epsilon) const
|
2744
|
+
{
|
2745
|
+
|
2746
|
+
return(
|
2747
|
+
//Make sure observer not colocated with any of the points.
|
2748
|
+
distance( observer , point1 ) > epsilon
|
2749
|
+
and distance( observer , point2 ) > epsilon
|
2750
|
+
and distance( observer , point3 ) > epsilon
|
2751
|
+
//Test whether there is a spike with point2 as the tip
|
2752
|
+
and ( ( distance(observer,point2)
|
2753
|
+
>= distance(observer,point1)
|
2754
|
+
and distance(observer,point2)
|
2755
|
+
>= distance(observer,point3) )
|
2756
|
+
or ( distance(observer,point2)
|
2757
|
+
<= distance(observer,point1)
|
2758
|
+
and distance(observer,point2)
|
2759
|
+
<= distance(observer,point3) ) )
|
2760
|
+
//and the pike is sufficiently sharp,
|
2761
|
+
and std::max( distance( Ray(observer, point1), point2 ),
|
2762
|
+
distance( Ray(observer, point3), point2 ) )
|
2763
|
+
<= epsilon
|
2764
|
+
);
|
2765
|
+
//Formerly used
|
2766
|
+
//std::fabs( Polygon(point1, point2, point3).area() ) < epsilon
|
2767
|
+
}
|
2768
|
+
|
2769
|
+
|
2770
|
+
void Visibility_Polygon::chop_spikes_at_back(const Point& observer,
|
2771
|
+
double epsilon)
|
2772
|
+
{
|
2773
|
+
//Eliminate "special case" vertices of the visibility polygon.
|
2774
|
+
//While the top three vertices form a spike.
|
2775
|
+
while( vertices_.size() >= 3
|
2776
|
+
and is_spike( observer,
|
2777
|
+
vertices_[vertices_.size()-3],
|
2778
|
+
vertices_[vertices_.size()-2],
|
2779
|
+
vertices_[vertices_.size()-1], epsilon ) ){
|
2780
|
+
vertices_[vertices_.size()-2] = vertices_[vertices_.size()-1];
|
2781
|
+
vertices_.pop_back();
|
2782
|
+
}
|
2783
|
+
}
|
2784
|
+
|
2785
|
+
|
2786
|
+
void Visibility_Polygon::chop_spikes_at_wrap_around(const Point& observer,
|
2787
|
+
double epsilon)
|
2788
|
+
{
|
2789
|
+
//Eliminate "special case" vertices of the visibility polygon at
|
2790
|
+
//wrap-around. While the there's a spike at the wrap-around,
|
2791
|
+
while( vertices_.size() >= 3
|
2792
|
+
and is_spike( observer,
|
2793
|
+
vertices_[vertices_.size()-2],
|
2794
|
+
vertices_[vertices_.size()-1],
|
2795
|
+
vertices_[0], epsilon ) ){
|
2796
|
+
//Chop off the tip of the spike.
|
2797
|
+
vertices_.pop_back();
|
2798
|
+
}
|
2799
|
+
}
|
2800
|
+
|
2801
|
+
|
2802
|
+
void Visibility_Polygon::chop_spikes(const Point& observer,
|
2803
|
+
double epsilon)
|
2804
|
+
{
|
2805
|
+
std::set<Point> spike_tips;
|
2806
|
+
std::vector<Point> vertices_temp;
|
2807
|
+
//Middle point is potentially the tip of a spike
|
2808
|
+
for(unsigned i=0; i<vertices_.size(); i++)
|
2809
|
+
if( distance( (*this)[i+2],
|
2810
|
+
Line_Segment( (*this)[i], (*this)[i+1] ) )
|
2811
|
+
<= epsilon
|
2812
|
+
or
|
2813
|
+
distance( (*this)[i],
|
2814
|
+
Line_Segment( (*this)[i+1], (*this)[i+2] ) )
|
2815
|
+
<= epsilon )
|
2816
|
+
spike_tips.insert( (*this)[i+1] );
|
2817
|
+
|
2818
|
+
for(unsigned i=0; i<vertices_.size(); i++)
|
2819
|
+
if( spike_tips.find(vertices_[i]) == spike_tips.end() )
|
2820
|
+
vertices_temp.push_back( vertices_[i] );
|
2821
|
+
vertices_.swap( vertices_temp );
|
2822
|
+
}
|
2823
|
+
|
2824
|
+
|
2825
|
+
void Visibility_Polygon::
|
2826
|
+
print_cv_and_ae(const Polar_Point_With_Edge_Info& current_vertex,
|
2827
|
+
const std::list<Polar_Edge>::iterator&
|
2828
|
+
active_edge)
|
2829
|
+
{
|
2830
|
+
std::cout << " current_vertex [x y bearing range is_first] = ["
|
2831
|
+
<< current_vertex.x() << " "
|
2832
|
+
<< current_vertex.y() << " "
|
2833
|
+
<< current_vertex.bearing() << " "
|
2834
|
+
<< current_vertex.range() << " "
|
2835
|
+
<< current_vertex.is_first << "]" << std::endl;
|
2836
|
+
std::cout << "1st point of current_vertex's edge [x y bearing range] = ["
|
2837
|
+
<< (current_vertex.incident_edge->first).x() << " "
|
2838
|
+
<< (current_vertex.incident_edge->first).y() << " "
|
2839
|
+
<< (current_vertex.incident_edge->first).bearing() << " "
|
2840
|
+
<< (current_vertex.incident_edge->first).range() << "]"
|
2841
|
+
<< std::endl;
|
2842
|
+
std::cout << "2nd point of current_vertex's edge [x y bearing range] = ["
|
2843
|
+
<< (current_vertex.incident_edge->second).x() << " "
|
2844
|
+
<< (current_vertex.incident_edge->second).y() << " "
|
2845
|
+
<< (current_vertex.incident_edge->second).bearing() << " "
|
2846
|
+
<< (current_vertex.incident_edge->second).range() << "]"
|
2847
|
+
<< std::endl;
|
2848
|
+
std::cout << " 1st point of active_edge [x y bearing range] = ["
|
2849
|
+
<< (active_edge->first).x() << " "
|
2850
|
+
<< (active_edge->first).y() << " "
|
2851
|
+
<< (active_edge->first).bearing() << " "
|
2852
|
+
<< (active_edge->first).range() << "]" << std::endl;
|
2853
|
+
std::cout << " 2nd point of active_edge [x y bearing range] = ["
|
2854
|
+
<< (active_edge->second).x() << " "
|
2855
|
+
<< (active_edge->second).y() << " "
|
2856
|
+
<< (active_edge->second).bearing() << " "
|
2857
|
+
<< (active_edge->second).range() << "]" << std::endl;
|
2858
|
+
}
|
2859
|
+
|
2860
|
+
|
2861
|
+
Visibility_Polygon::Visibility_Polygon(const Point& observer,
|
2862
|
+
const Environment& environment_temp,
|
2863
|
+
double epsilon)
|
2864
|
+
: observer_(observer)
|
2865
|
+
{
|
2866
|
+
//Visibility polygon algorithm for environments with holes
|
2867
|
+
//Radial line (AKA angular plane) sweep technique.
|
2868
|
+
//
|
2869
|
+
//Based on algorithms described in
|
2870
|
+
//
|
2871
|
+
//[1] "Automated Camera Layout to Satisfy Task-Specific and
|
2872
|
+
//Floorplan-Specific Coverage Requirements" by Ugur Murat Erdem
|
2873
|
+
//and Stan Scarloff, April 15, 2004
|
2874
|
+
//available at BUCS Technical Report Archive:
|
2875
|
+
//http://www.cs.bu.edu/techreports/pdf/2004-015-camera-layout.pdf
|
2876
|
+
//
|
2877
|
+
//[2] "Art Gallery Theorems and Algorithms" by Joseph O'Rourke
|
2878
|
+
//
|
2879
|
+
//[3] "Visibility Algorithms in the Plane" by Ghosh
|
2880
|
+
//
|
2881
|
+
|
2882
|
+
//We define a k-point is a point seen on the other side of a
|
2883
|
+
//visibility occluding corner. This name is appropriate because
|
2884
|
+
//the vertical line in the letter "k" is like a line-of-sight past
|
2885
|
+
//the corner of the "k".
|
2886
|
+
|
2887
|
+
//
|
2888
|
+
//Preconditions:
|
2889
|
+
//(1) the Environment is epsilon-valid,
|
2890
|
+
//(2) the Point observer is actually in the Environment
|
2891
|
+
// environment_temp,
|
2892
|
+
//(3) the guard has been epsilon-snapped to the boundary, followed
|
2893
|
+
// by vertices of the environment (the order of the snapping
|
2894
|
+
// is important).
|
2895
|
+
//
|
2896
|
+
//:WARNING:
|
2897
|
+
//For efficiency, the assertions corresponding to these
|
2898
|
+
//preconditions have been excluded.
|
2899
|
+
//
|
2900
|
+
//assert( environment_temp.is_valid(epsilon) );
|
2901
|
+
//assert( environment_temp.is_in_standard_form() );
|
2902
|
+
//assert( observer.in(environment_temp, epsilon) );
|
2903
|
+
|
2904
|
+
//true => data printed to terminal
|
2905
|
+
//false => silent
|
2906
|
+
const bool PRINTING_DEBUG_DATA = false;
|
2907
|
+
|
2908
|
+
//The visibility polygon cannot have more vertices than the environment.
|
2909
|
+
vertices_.reserve( environment_temp.n() );
|
2910
|
+
|
2911
|
+
//
|
2912
|
+
//--------PREPROCESSING--------
|
2913
|
+
//
|
2914
|
+
|
2915
|
+
//Construct a POLAR EDGE LIST from environment_temp's outer
|
2916
|
+
//boundary and holes. During this construction, those edges are
|
2917
|
+
//split which either (1) cross the ray emanating from the observer
|
2918
|
+
//parallel to the x-axis (of world coords), or (2) contain the
|
2919
|
+
//observer in their relative interior (w/in epsilon). Also, edges
|
2920
|
+
//having first vertex bearing >= second vertex bearing are
|
2921
|
+
//eliminated because they cannot possibly contribute to the
|
2922
|
+
//visibility polygon.
|
2923
|
+
std::list<Polar_Edge> elp;
|
2924
|
+
Polar_Point ppoint1, ppoint2;
|
2925
|
+
Polar_Point split_bottom, split_top;
|
2926
|
+
double t;
|
2927
|
+
//If the observer is standing on the Enviroment boundary with its
|
2928
|
+
//back to the wall, these will be the bearings of the next vertex
|
2929
|
+
//to the right and to the left, respectively.
|
2930
|
+
Angle right_wall_bearing;
|
2931
|
+
Angle left_wall_bearing;
|
2932
|
+
for(unsigned i=0; i<=environment_temp.h(); i++){
|
2933
|
+
for(unsigned j=0; j<environment_temp[i].n(); j++){
|
2934
|
+
ppoint1 = Polar_Point( observer, environment_temp[i][j] );
|
2935
|
+
ppoint2 = Polar_Point( observer, environment_temp[i][j+1] );
|
2936
|
+
|
2937
|
+
//If the observer is in the relative interior of the edge.
|
2938
|
+
if( observer.in_relative_interior_of( Line_Segment(ppoint1, ppoint2),
|
2939
|
+
epsilon ) ){
|
2940
|
+
//Split the edge at the observer and add the resulting two
|
2941
|
+
//edges to elp (the polar edge list).
|
2942
|
+
split_bottom = Polar_Point(observer, observer);
|
2943
|
+
split_top = Polar_Point(observer, observer);
|
2944
|
+
|
2945
|
+
if( ppoint2.bearing() == Angle(0.0) )
|
2946
|
+
ppoint2.set_bearing_to_2pi();
|
2947
|
+
|
2948
|
+
left_wall_bearing = ppoint1.bearing();
|
2949
|
+
right_wall_bearing = ppoint2.bearing();
|
2950
|
+
|
2951
|
+
elp.push_back( Polar_Edge( ppoint1 , split_bottom ) );
|
2952
|
+
elp.push_back( Polar_Edge( split_top , ppoint2 ) );
|
2953
|
+
continue;
|
2954
|
+
}
|
2955
|
+
|
2956
|
+
//Else if the observer is on first vertex of edge.
|
2957
|
+
else if( distance(observer, ppoint1) <= epsilon ){
|
2958
|
+
if( ppoint2.bearing() == Angle(0.0) )
|
2959
|
+
ppoint2.set_bearing_to_2pi();
|
2960
|
+
//Get right wall bearing.
|
2961
|
+
right_wall_bearing = ppoint2.bearing();
|
2962
|
+
elp.push_back( Polar_Edge( Polar_Point(observer, observer),
|
2963
|
+
ppoint2 ) );
|
2964
|
+
continue;
|
2965
|
+
}
|
2966
|
+
//Else if the observer is on second vertex of edge.
|
2967
|
+
else if( distance(observer, ppoint2) <= epsilon ){
|
2968
|
+
//Get left wall bearing.
|
2969
|
+
left_wall_bearing = ppoint1.bearing();
|
2970
|
+
elp.push_back( Polar_Edge( ppoint1,
|
2971
|
+
Polar_Point(observer, observer) ) );
|
2972
|
+
continue;
|
2973
|
+
}
|
2974
|
+
|
2975
|
+
//Otherwise the observer is not on the edge.
|
2976
|
+
|
2977
|
+
//If edge not horizontal (w/in epsilon).
|
2978
|
+
else if( std::fabs( ppoint1.y() - ppoint2.y() ) > epsilon ){
|
2979
|
+
//Possible source of numerical instability?
|
2980
|
+
t = ( observer.y() - ppoint2.y() )
|
2981
|
+
/ ( ppoint1.y() - ppoint2.y() );
|
2982
|
+
//If edge crosses the ray emanating horizontal and right of
|
2983
|
+
//the observer.
|
2984
|
+
if( 0 < t and t < 1 and
|
2985
|
+
observer.x() < t*ppoint1.x() + (1-t)*ppoint2.x() ){
|
2986
|
+
//If first point is above, omit edge because it runs
|
2987
|
+
//'against the grain'.
|
2988
|
+
if( ppoint1.y() > observer.y() )
|
2989
|
+
continue;
|
2990
|
+
//Otherwise split the edge, making sure angles are assigned
|
2991
|
+
//correctly on each side of the split point.
|
2992
|
+
split_bottom = split_top
|
2993
|
+
= Polar_Point( observer,
|
2994
|
+
Point( t*ppoint1.x() + (1-t)*ppoint2.x(),
|
2995
|
+
observer.y() ) );
|
2996
|
+
split_top.set_bearing( Angle(0.0) );
|
2997
|
+
split_bottom.set_bearing_to_2pi();
|
2998
|
+
elp.push_back( Polar_Edge( ppoint1 , split_bottom ) );
|
2999
|
+
elp.push_back( Polar_Edge( split_top , ppoint2 ) );
|
3000
|
+
continue;
|
3001
|
+
}
|
3002
|
+
//If the edge is not horizontal and doesn't cross the ray
|
3003
|
+
//emanating horizontal and right of the observer.
|
3004
|
+
else if( ppoint1.bearing() >= ppoint2.bearing()
|
3005
|
+
and ppoint2.bearing() == Angle(0.0)
|
3006
|
+
and ppoint1.bearing() > Angle(M_PI) )
|
3007
|
+
ppoint2.set_bearing_to_2pi();
|
3008
|
+
//Filter out edges which run 'against the grain'.
|
3009
|
+
else if( ( ppoint1.bearing() == Angle(0,0)
|
3010
|
+
and ppoint2.bearing() > Angle(M_PI) )
|
3011
|
+
or ppoint1.bearing() >= ppoint2.bearing() )
|
3012
|
+
continue;
|
3013
|
+
elp.push_back( Polar_Edge( ppoint1, ppoint2 ) );
|
3014
|
+
continue;
|
3015
|
+
}
|
3016
|
+
//If edge is horizontal (w/in epsilon).
|
3017
|
+
else{
|
3018
|
+
//Filter out edges which run 'against the grain'.
|
3019
|
+
if( ppoint1.bearing() >= ppoint2.bearing() )
|
3020
|
+
continue;
|
3021
|
+
elp.push_back( Polar_Edge( ppoint1, ppoint2 ) );
|
3022
|
+
}
|
3023
|
+
}}
|
3024
|
+
|
3025
|
+
//Construct a SORTED LIST, q1, OF VERTICES represented by
|
3026
|
+
//Polar_Point_With_Edge_Info objects. A
|
3027
|
+
//Polar_Point_With_Edge_Info is a derived class of Polar_Point
|
3028
|
+
//which includes (1) a pointer to the corresponding edge
|
3029
|
+
//(represented as a Polar_Edge) in the polar edge list elp, and
|
3030
|
+
//(2) a boolean (is_first) which is true iff that vertex is the
|
3031
|
+
//first Point of the respective edge (is_first == false => it's
|
3032
|
+
//second Point). q1 is sorted according to lex. order of polar
|
3033
|
+
//coordinates just as Polar_Points are, but with the additional
|
3034
|
+
//requirement that if two vertices have equal polar coordinates,
|
3035
|
+
//the vertex which is the first point of its respective edge is
|
3036
|
+
//considered greater. q1 will serve as an event point queue for
|
3037
|
+
//the radial sweep.
|
3038
|
+
std::list<Polar_Point_With_Edge_Info> q1;
|
3039
|
+
Polar_Point_With_Edge_Info ppoint_wei1, ppoint_wei2;
|
3040
|
+
std::list<Polar_Edge>::iterator elp_iterator;
|
3041
|
+
for(elp_iterator=elp.begin();
|
3042
|
+
elp_iterator!=elp.end();
|
3043
|
+
elp_iterator++){
|
3044
|
+
ppoint_wei1.set_polar_point( elp_iterator->first );
|
3045
|
+
ppoint_wei1.incident_edge = elp_iterator;
|
3046
|
+
ppoint_wei1.is_first = true;
|
3047
|
+
ppoint_wei2.set_polar_point( elp_iterator->second );
|
3048
|
+
ppoint_wei2.incident_edge = elp_iterator;
|
3049
|
+
ppoint_wei2.is_first = false;
|
3050
|
+
//If edge contains the observer, then adjust the bearing of
|
3051
|
+
//the Polar_Point containing the observer.
|
3052
|
+
if( distance(observer, ppoint_wei1) <= epsilon ){
|
3053
|
+
if( right_wall_bearing > left_wall_bearing ){
|
3054
|
+
ppoint_wei1.set_bearing( right_wall_bearing );
|
3055
|
+
(elp_iterator->first).set_bearing( right_wall_bearing );
|
3056
|
+
}
|
3057
|
+
else{
|
3058
|
+
ppoint_wei1.set_bearing( Angle(0.0) );
|
3059
|
+
(elp_iterator->first).set_bearing( Angle(0.0) );
|
3060
|
+
}
|
3061
|
+
}
|
3062
|
+
else if( distance(observer, ppoint_wei2) <= epsilon ){
|
3063
|
+
if( right_wall_bearing > left_wall_bearing ){
|
3064
|
+
ppoint_wei2.set_bearing(right_wall_bearing);
|
3065
|
+
(elp_iterator->second).set_bearing( right_wall_bearing );
|
3066
|
+
}
|
3067
|
+
else{
|
3068
|
+
ppoint_wei2.set_bearing_to_2pi();
|
3069
|
+
(elp_iterator->second).set_bearing_to_2pi();
|
3070
|
+
}
|
3071
|
+
}
|
3072
|
+
q1.push_back(ppoint_wei1);
|
3073
|
+
q1.push_back(ppoint_wei2);
|
3074
|
+
}
|
3075
|
+
//Put event point in correct order.
|
3076
|
+
//STL list's sort method is a stable sort.
|
3077
|
+
q1.sort();
|
3078
|
+
|
3079
|
+
if(PRINTING_DEBUG_DATA){
|
3080
|
+
std::cout << std::endl
|
3081
|
+
<< "\E[1;37;40m"
|
3082
|
+
<< "COMPUTING VISIBILITY POLYGON " << std::endl
|
3083
|
+
<< "for an observer located at [x y] = ["
|
3084
|
+
<< observer << "]"
|
3085
|
+
<< "\x1b[0m"
|
3086
|
+
<< std::endl << std::endl
|
3087
|
+
<< "\E[1;37;40m" <<"PREPROCESSING" << "\x1b[0m"
|
3088
|
+
<< std::endl << std::endl
|
3089
|
+
<< "q1 is" << std::endl;
|
3090
|
+
std::list<Polar_Point_With_Edge_Info>::iterator q1_itr;
|
3091
|
+
for(q1_itr=q1.begin(); q1_itr!=q1.end(); q1_itr++){
|
3092
|
+
std::cout << "[x y bearing range is_first] = ["
|
3093
|
+
<< q1_itr->x() << " "
|
3094
|
+
<< q1_itr->y() << " "
|
3095
|
+
<< q1_itr->bearing() << " "
|
3096
|
+
<< q1_itr->range() << " "
|
3097
|
+
<< q1_itr->is_first << "]"
|
3098
|
+
<< std::endl;
|
3099
|
+
}
|
3100
|
+
}
|
3101
|
+
|
3102
|
+
//
|
3103
|
+
//-------PREPARE FOR MAIN LOOP-------
|
3104
|
+
//
|
3105
|
+
|
3106
|
+
//current_vertex is used to hold the event point (from q1)
|
3107
|
+
//considered at iteration of the main loop.
|
3108
|
+
Polar_Point_With_Edge_Info current_vertex;
|
3109
|
+
//Note active_edge and e are not actually edges themselves, but
|
3110
|
+
//iterators pointing to edges. active_edge keeps track of the
|
3111
|
+
//current edge visibile during the sweep. e is an auxiliary
|
3112
|
+
//variable used in calculation of k-points
|
3113
|
+
std::list<Polar_Edge>::iterator active_edge, e;
|
3114
|
+
//More aux vars for computing k-points.
|
3115
|
+
Polar_Point k;
|
3116
|
+
double k_range;
|
3117
|
+
Line_Segment xing;
|
3118
|
+
|
3119
|
+
//Priority queue of edges, where higher priority indicates closer
|
3120
|
+
//range to observer along current ray (of ray sweep).
|
3121
|
+
Incident_Edge_Compare my_iec(observer, current_vertex, epsilon);
|
3122
|
+
std::priority_queue<std::list<Polar_Edge>::iterator,
|
3123
|
+
std::vector<std::list<Polar_Edge>::iterator>,
|
3124
|
+
Incident_Edge_Compare> q2(my_iec);
|
3125
|
+
|
3126
|
+
//Initialize main loop.
|
3127
|
+
current_vertex = q1.front(); q1.pop_front();
|
3128
|
+
active_edge = current_vertex.incident_edge;
|
3129
|
+
|
3130
|
+
if(PRINTING_DEBUG_DATA){
|
3131
|
+
std::cout << std::endl
|
3132
|
+
<< "\E[1;37;40m"
|
3133
|
+
<< "INITIALIZATION"
|
3134
|
+
<< "\x1b[0m"
|
3135
|
+
<< std::endl << std::endl
|
3136
|
+
<< "\x1b[35m"
|
3137
|
+
<< "Pop first vertex off q1"
|
3138
|
+
<< "\x1b[0m"
|
3139
|
+
<< ", set as current_vertex, \n"
|
3140
|
+
<< "and set active_edge to the corresponding "
|
3141
|
+
<< "incident edge."
|
3142
|
+
<< std::endl;
|
3143
|
+
print_cv_and_ae(current_vertex, active_edge);
|
3144
|
+
}
|
3145
|
+
|
3146
|
+
//Insert e into q2 as long as it doesn't contain the
|
3147
|
+
//observer.
|
3148
|
+
if( distance(observer,active_edge->first) > epsilon
|
3149
|
+
and distance(observer,active_edge->second) > epsilon ){
|
3150
|
+
|
3151
|
+
if(PRINTING_DEBUG_DATA){
|
3152
|
+
std::cout << std::endl
|
3153
|
+
<< "Push current_vertex's edge onto q2."
|
3154
|
+
<< std::endl;
|
3155
|
+
}
|
3156
|
+
|
3157
|
+
q2.push(active_edge);
|
3158
|
+
}
|
3159
|
+
|
3160
|
+
if(PRINTING_DEBUG_DATA){
|
3161
|
+
std::cout << std::endl
|
3162
|
+
<< "\E[32m"
|
3163
|
+
<< "Add current_vertex to visibility polygon."
|
3164
|
+
<< "\x1b[0m"
|
3165
|
+
<< std::endl << std::endl
|
3166
|
+
<< "\E[1;37;40m"
|
3167
|
+
<< "MAIN LOOP"
|
3168
|
+
<< "\x1b[0m"
|
3169
|
+
<< std::endl;
|
3170
|
+
}
|
3171
|
+
|
3172
|
+
vertices_.push_back(current_vertex);
|
3173
|
+
|
3174
|
+
//-------BEGIN MAIN LOOP-------//
|
3175
|
+
//
|
3176
|
+
//Perform radial sweep by sequentially considering each vertex
|
3177
|
+
//(event point) in q1.
|
3178
|
+
while( !q1.empty() ){
|
3179
|
+
|
3180
|
+
//Pop current_vertex from q1.
|
3181
|
+
current_vertex = q1.front(); q1.pop_front();
|
3182
|
+
|
3183
|
+
if(PRINTING_DEBUG_DATA){
|
3184
|
+
std::cout << std::endl
|
3185
|
+
<< "\x1b[35m"
|
3186
|
+
<< "Pop next vertex off q1" << "\x1b[0m"
|
3187
|
+
<< " and set as current_vertex."
|
3188
|
+
<< std::endl;
|
3189
|
+
print_cv_and_ae(current_vertex, active_edge);
|
3190
|
+
}
|
3191
|
+
|
3192
|
+
//---Handle Event Point---
|
3193
|
+
|
3194
|
+
//TYPE 1: current_vertex is the _second_vertex_ of active_edge.
|
3195
|
+
if( current_vertex.incident_edge == active_edge
|
3196
|
+
and !current_vertex.is_first ){
|
3197
|
+
|
3198
|
+
if(PRINTING_DEBUG_DATA){
|
3199
|
+
std::cout << std::endl
|
3200
|
+
<< "\E[36m" << "TYPE 1:" << "\x1b[0m"
|
3201
|
+
<< " current_vertex is the second vertex of active_edge."
|
3202
|
+
<< std::endl;
|
3203
|
+
}
|
3204
|
+
|
3205
|
+
if( !q1.empty() ){
|
3206
|
+
//If the next vertex in q1 is contiguous.
|
3207
|
+
if( distance( current_vertex, q1.front() ) <= epsilon ){
|
3208
|
+
|
3209
|
+
if(PRINTING_DEBUG_DATA){
|
3210
|
+
std::cout << std::endl
|
3211
|
+
<< "current_vertex is contiguous "
|
3212
|
+
<< "with the next vertex in q1."
|
3213
|
+
<< std::endl;
|
3214
|
+
}
|
3215
|
+
|
3216
|
+
continue;
|
3217
|
+
}
|
3218
|
+
}
|
3219
|
+
|
3220
|
+
if(PRINTING_DEBUG_DATA){
|
3221
|
+
std::cout << std::endl
|
3222
|
+
<< "\E[32m" << "Add current_vertex to visibility polygon."
|
3223
|
+
<< "\x1b[0m" << std::endl;
|
3224
|
+
}
|
3225
|
+
|
3226
|
+
//Push current_vertex onto visibility polygon
|
3227
|
+
vertices_.push_back( current_vertex );
|
3228
|
+
chop_spikes_at_back(observer, epsilon);
|
3229
|
+
|
3230
|
+
while( !q2.empty() ){
|
3231
|
+
e = q2.top();
|
3232
|
+
|
3233
|
+
if(PRINTING_DEBUG_DATA){
|
3234
|
+
std::cout << std::endl
|
3235
|
+
<< "Examine edge at top of q2." << std::endl
|
3236
|
+
<< "1st point of e [x y bearing range] = ["
|
3237
|
+
<< (e->first).x() << " "
|
3238
|
+
<< (e->first).y() << " "
|
3239
|
+
<< (e->first).bearing() << " "
|
3240
|
+
<< (e->first).range() << "]" << std::endl
|
3241
|
+
<< "2nd point of e [x y bearing range] = ["
|
3242
|
+
<< (e->second).x() << " "
|
3243
|
+
<< (e->second).y() << " "
|
3244
|
+
<< (e->second).bearing() << " "
|
3245
|
+
<< (e->second).range() << "]" << std::endl;
|
3246
|
+
}
|
3247
|
+
|
3248
|
+
//If the current_vertex bearing has not passed, in the
|
3249
|
+
//lex. order sense, the bearing of the second point of the
|
3250
|
+
//edge at the front of q2.
|
3251
|
+
if( ( current_vertex.bearing().get()
|
3252
|
+
<= e->second.bearing().get() )
|
3253
|
+
//For robustness.
|
3254
|
+
and distance( Ray(observer, current_vertex.bearing()),
|
3255
|
+
e->second ) >= epsilon
|
3256
|
+
/* was
|
3257
|
+
and std::min( distance(Ray(observer, current_vertex.bearing()),
|
3258
|
+
e->second),
|
3259
|
+
distance(Ray(observer, e->second.bearing()),
|
3260
|
+
current_vertex)
|
3261
|
+
) >= epsilon
|
3262
|
+
*/
|
3263
|
+
){
|
3264
|
+
//Find intersection point k of ray (through
|
3265
|
+
//current_vertex) with edge e.
|
3266
|
+
xing = intersection( Ray(observer, current_vertex.bearing()),
|
3267
|
+
Line_Segment(e->first,
|
3268
|
+
e->second),
|
3269
|
+
epsilon );
|
3270
|
+
|
3271
|
+
//assert( xing.size() > 0 );
|
3272
|
+
|
3273
|
+
if( xing.size() > 0 ){
|
3274
|
+
k = Polar_Point( observer , xing.first() );
|
3275
|
+
}
|
3276
|
+
else{ //Error contingency.
|
3277
|
+
k = current_vertex;
|
3278
|
+
e = current_vertex.incident_edge;
|
3279
|
+
}
|
3280
|
+
|
3281
|
+
if(PRINTING_DEBUG_DATA){
|
3282
|
+
std::cout << std::endl
|
3283
|
+
<< "\E[32m"
|
3284
|
+
<< "Add a type 1 k-point to visibility polygon."
|
3285
|
+
<< "\x1b[0m" << std::endl
|
3286
|
+
<< std::endl
|
3287
|
+
<< "Set active_edge to edge at top of q2."
|
3288
|
+
<< std::endl;
|
3289
|
+
}
|
3290
|
+
|
3291
|
+
//Push k onto the visibility polygon.
|
3292
|
+
vertices_.push_back(k);
|
3293
|
+
chop_spikes_at_back(observer, epsilon);
|
3294
|
+
active_edge = e;
|
3295
|
+
break;
|
3296
|
+
}
|
3297
|
+
|
3298
|
+
if(PRINTING_DEBUG_DATA){
|
3299
|
+
std::cout << std::endl
|
3300
|
+
<< "Pop edge off top of q2." << std::endl;
|
3301
|
+
}
|
3302
|
+
|
3303
|
+
q2.pop();
|
3304
|
+
}
|
3305
|
+
} //Close Type 1.
|
3306
|
+
|
3307
|
+
//If current_vertex is the _first_vertex_ of its edge.
|
3308
|
+
if( current_vertex.is_first ){
|
3309
|
+
//Find intersection point k of ray (through current_vertex)
|
3310
|
+
//with active_edge.
|
3311
|
+
xing = intersection( Ray(observer, current_vertex.bearing()),
|
3312
|
+
Line_Segment(active_edge->first,
|
3313
|
+
active_edge->second),
|
3314
|
+
epsilon );
|
3315
|
+
if( xing.size() == 0
|
3316
|
+
or ( distance(active_edge->first, observer) <= epsilon
|
3317
|
+
and active_edge->second.bearing()
|
3318
|
+
<= current_vertex.bearing() )
|
3319
|
+
or active_edge->second < current_vertex ){
|
3320
|
+
k_range = INFINITY;
|
3321
|
+
}
|
3322
|
+
else{
|
3323
|
+
k = Polar_Point( observer , xing.first() );
|
3324
|
+
k_range = k.range();
|
3325
|
+
}
|
3326
|
+
|
3327
|
+
//Incident edge of current_vertex.
|
3328
|
+
e = current_vertex.incident_edge;
|
3329
|
+
|
3330
|
+
if(PRINTING_DEBUG_DATA){
|
3331
|
+
std::cout << std::endl
|
3332
|
+
<< " k_range = "
|
3333
|
+
<< k_range
|
3334
|
+
<< " (range of active edge along "
|
3335
|
+
<< "bearing of current vertex)" << std::endl
|
3336
|
+
<< "current_vertex.range() = "
|
3337
|
+
<< current_vertex.range() << std::endl;
|
3338
|
+
}
|
3339
|
+
|
3340
|
+
//Insert e into q2 as long as it doesn't contain the
|
3341
|
+
//observer.
|
3342
|
+
if( distance(observer, e->first) > epsilon
|
3343
|
+
and distance(observer, e->second) > epsilon ){
|
3344
|
+
|
3345
|
+
if(PRINTING_DEBUG_DATA){
|
3346
|
+
std::cout << std::endl
|
3347
|
+
<< "Push current_vertex's edge onto q2."
|
3348
|
+
<< std::endl;
|
3349
|
+
}
|
3350
|
+
|
3351
|
+
q2.push(e);
|
3352
|
+
}
|
3353
|
+
|
3354
|
+
//TYPE 2: current_vertex is (1) a first vertex of some edge
|
3355
|
+
//other than active_edge, and (2) that edge should not become
|
3356
|
+
//the next active_edge. This happens, e.g., if that edge is
|
3357
|
+
//(rangewise) in back along the current bearing.
|
3358
|
+
if( k_range < current_vertex.range() ){
|
3359
|
+
|
3360
|
+
if(PRINTING_DEBUG_DATA){
|
3361
|
+
std::cout << std::endl
|
3362
|
+
<< "\E[36m" << "TYPE 2:" << "\x1b[0m"
|
3363
|
+
<< " current_vertex is" << std::endl
|
3364
|
+
<< "(1) a first vertex of some edge "
|
3365
|
+
"other than active_edge, and" << std::endl
|
3366
|
+
<< "(2) that edge should not become "
|
3367
|
+
<< "the next active_edge."
|
3368
|
+
<< std::endl;
|
3369
|
+
|
3370
|
+
}
|
3371
|
+
|
3372
|
+
} //Close Type 2.
|
3373
|
+
|
3374
|
+
//TYPE 3: current_vertex is (1) the first vertex of some edge
|
3375
|
+
//other than active_edge, and (2) that edge should become the
|
3376
|
+
//next active_edge. This happens, e.g., if that edge is
|
3377
|
+
//(rangewise) in front along the current bearing.
|
3378
|
+
if( k_range >= current_vertex.range()
|
3379
|
+
){
|
3380
|
+
|
3381
|
+
if(PRINTING_DEBUG_DATA){
|
3382
|
+
std::cout << std::endl
|
3383
|
+
<< "\E[36m" << "TYPE 3:" << "\x1b[0m"
|
3384
|
+
<< " current_vertex is" << std::endl
|
3385
|
+
<< "(1) the first vertex of some edge "
|
3386
|
+
"other than active edge, and" << std::endl
|
3387
|
+
<< "(2) that edge should become "
|
3388
|
+
<< "the next active_edge."
|
3389
|
+
<< std::endl;
|
3390
|
+
}
|
3391
|
+
|
3392
|
+
//Push k onto the visibility polygon unless effectively
|
3393
|
+
//contiguous with current_vertex.
|
3394
|
+
if( xing.size() > 0
|
3395
|
+
//and k == k
|
3396
|
+
and k_range != INFINITY
|
3397
|
+
and distance(k, current_vertex) > epsilon
|
3398
|
+
and distance(active_edge->first, observer) > epsilon
|
3399
|
+
){
|
3400
|
+
|
3401
|
+
if(PRINTING_DEBUG_DATA){
|
3402
|
+
std::cout << std::endl
|
3403
|
+
<< "\E[32m"
|
3404
|
+
<< "Add type 3 k-point to visibility polygon."
|
3405
|
+
<< "\x1b[0m" << std::endl;
|
3406
|
+
}
|
3407
|
+
|
3408
|
+
//Push k-point onto the visibility polygon.
|
3409
|
+
vertices_.push_back(k);
|
3410
|
+
chop_spikes_at_back(observer, epsilon);
|
3411
|
+
}
|
3412
|
+
|
3413
|
+
//Push current_vertex onto the visibility polygon.
|
3414
|
+
vertices_.push_back(current_vertex);
|
3415
|
+
chop_spikes_at_back(observer, epsilon);
|
3416
|
+
//Set active_edge to edge of current_vertex.
|
3417
|
+
active_edge = e;
|
3418
|
+
|
3419
|
+
if(PRINTING_DEBUG_DATA){
|
3420
|
+
std::cout << std::endl
|
3421
|
+
<< "\E[32m" << "Add current_vertex to visibility polygon."
|
3422
|
+
<< "\x1b[0m" << std::endl
|
3423
|
+
<< std::endl
|
3424
|
+
<< "Set active_edge to edge of current_vertex."
|
3425
|
+
<< std::endl;
|
3426
|
+
}
|
3427
|
+
|
3428
|
+
} //Close Type 3.
|
3429
|
+
}
|
3430
|
+
|
3431
|
+
if(PRINTING_DEBUG_DATA){
|
3432
|
+
std::cout << std::endl
|
3433
|
+
<< "visibility polygon vertices so far are \n"
|
3434
|
+
<< Polygon(vertices_) << std::endl
|
3435
|
+
<< std::endl;
|
3436
|
+
}
|
3437
|
+
} //
|
3438
|
+
//
|
3439
|
+
//-------END MAIN LOOP-------//
|
3440
|
+
|
3441
|
+
//The Visibility_Polygon should have a minimal representation
|
3442
|
+
chop_spikes_at_wrap_around( observer , epsilon );
|
3443
|
+
eliminate_redundant_vertices( epsilon );
|
3444
|
+
chop_spikes( observer, epsilon );
|
3445
|
+
enforce_standard_form();
|
3446
|
+
|
3447
|
+
if(PRINTING_DEBUG_DATA){
|
3448
|
+
std::cout << std::endl
|
3449
|
+
<< "Final visibility polygon vertices are \n"
|
3450
|
+
<< Polygon(vertices_) << std::endl
|
3451
|
+
<< std::endl;
|
3452
|
+
}
|
3453
|
+
|
3454
|
+
}
|
3455
|
+
Visibility_Polygon::Visibility_Polygon(const Point& observer,
|
3456
|
+
const Polygon& polygon_temp,
|
3457
|
+
double epsilon)
|
3458
|
+
{
|
3459
|
+
*this = Visibility_Polygon( observer, Environment(polygon_temp), epsilon );
|
3460
|
+
}
|
3461
|
+
|
3462
|
+
|
3463
|
+
//Visibility_Graph
|
3464
|
+
|
3465
|
+
|
3466
|
+
Visibility_Graph::Visibility_Graph( const Visibility_Graph& vg2 )
|
3467
|
+
{
|
3468
|
+
n_ = vg2.n_;
|
3469
|
+
vertex_counts_ = vg2.vertex_counts_;
|
3470
|
+
|
3471
|
+
//Allocate adjacency matrix
|
3472
|
+
adjacency_matrix_ = new bool*[n_];
|
3473
|
+
adjacency_matrix_[0] = new bool[n_*n_];
|
3474
|
+
for(unsigned i=1; i<n_; i++)
|
3475
|
+
adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
|
3476
|
+
|
3477
|
+
//copy each entry
|
3478
|
+
for(unsigned i=0; i<n_; i++){
|
3479
|
+
for(unsigned j=0; j<n_; j++){
|
3480
|
+
adjacency_matrix_[i][j]
|
3481
|
+
= vg2.adjacency_matrix_[i][j];
|
3482
|
+
}}
|
3483
|
+
}
|
3484
|
+
|
3485
|
+
|
3486
|
+
Visibility_Graph::Visibility_Graph(const Environment& environment,
|
3487
|
+
double epsilon)
|
3488
|
+
{
|
3489
|
+
n_ = environment.n();
|
3490
|
+
|
3491
|
+
//fill vertex_counts_
|
3492
|
+
vertex_counts_.reserve( environment.h() );
|
3493
|
+
for(unsigned i=0; i<environment.h(); i++)
|
3494
|
+
vertex_counts_.push_back( environment[i].n() );
|
3495
|
+
|
3496
|
+
//allocate a contiguous chunk of memory for adjacency_matrix_
|
3497
|
+
adjacency_matrix_ = new bool*[n_];
|
3498
|
+
adjacency_matrix_[0] = new bool[n_*n_];
|
3499
|
+
for(unsigned i=1; i<n_; i++)
|
3500
|
+
adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
|
3501
|
+
|
3502
|
+
// fill adjacency matrix by checking for inclusion in the
|
3503
|
+
// visibility polygons
|
3504
|
+
Polygon polygon_temp;
|
3505
|
+
for(unsigned k1=0; k1<n_; k1++){
|
3506
|
+
polygon_temp = Visibility_Polygon( environment(k1),
|
3507
|
+
environment,
|
3508
|
+
epsilon );
|
3509
|
+
for(unsigned k2=0; k2<n_; k2++){
|
3510
|
+
if( k1 == k2 )
|
3511
|
+
adjacency_matrix_[ k1 ][ k1 ] = true;
|
3512
|
+
else
|
3513
|
+
adjacency_matrix_[ k1 ][ k2 ] =
|
3514
|
+
adjacency_matrix_[ k2 ][ k1 ] =
|
3515
|
+
environment(k2).in( polygon_temp , epsilon );
|
3516
|
+
}
|
3517
|
+
}
|
3518
|
+
}
|
3519
|
+
|
3520
|
+
|
3521
|
+
Visibility_Graph::Visibility_Graph(const std::vector<Point> points,
|
3522
|
+
const Environment& environment,
|
3523
|
+
double epsilon)
|
3524
|
+
{
|
3525
|
+
n_ = points.size();
|
3526
|
+
|
3527
|
+
//fill vertex_counts_
|
3528
|
+
vertex_counts_.push_back( n_ );
|
3529
|
+
|
3530
|
+
//allocate a contiguous chunk of memory for adjacency_matrix_
|
3531
|
+
adjacency_matrix_ = new bool*[n_];
|
3532
|
+
adjacency_matrix_[0] = new bool[n_*n_];
|
3533
|
+
for(unsigned i=1; i<n_; i++)
|
3534
|
+
adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
|
3535
|
+
|
3536
|
+
// fill adjacency matrix by checking for inclusion in the
|
3537
|
+
// visibility polygons
|
3538
|
+
Polygon polygon_temp;
|
3539
|
+
for(unsigned k1=0; k1<n_; k1++){
|
3540
|
+
polygon_temp = Visibility_Polygon( points[k1],
|
3541
|
+
environment,
|
3542
|
+
epsilon );
|
3543
|
+
for(unsigned k2=0; k2<n_; k2++){
|
3544
|
+
if( k1 == k2 )
|
3545
|
+
adjacency_matrix_[ k1 ][ k1 ] = true;
|
3546
|
+
else
|
3547
|
+
adjacency_matrix_[ k1 ][ k2 ] =
|
3548
|
+
adjacency_matrix_[ k2 ][ k1 ] =
|
3549
|
+
points[k2].in( polygon_temp , epsilon );
|
3550
|
+
}
|
3551
|
+
}
|
3552
|
+
}
|
3553
|
+
|
3554
|
+
|
3555
|
+
Visibility_Graph::Visibility_Graph(const Guards& guards,
|
3556
|
+
const Environment& environment,
|
3557
|
+
double epsilon)
|
3558
|
+
{
|
3559
|
+
*this = Visibility_Graph( guards.positions_,
|
3560
|
+
environment,
|
3561
|
+
epsilon );
|
3562
|
+
}
|
3563
|
+
|
3564
|
+
|
3565
|
+
bool Visibility_Graph::operator () (unsigned i1,
|
3566
|
+
unsigned j1,
|
3567
|
+
unsigned i2,
|
3568
|
+
unsigned j2) const
|
3569
|
+
{
|
3570
|
+
return adjacency_matrix_[ two_to_one(i1,j1) ][ two_to_one(i2,j2) ];
|
3571
|
+
}
|
3572
|
+
bool Visibility_Graph::operator () (unsigned k1,
|
3573
|
+
unsigned k2) const
|
3574
|
+
{
|
3575
|
+
return adjacency_matrix_[ k1 ][ k2 ];
|
3576
|
+
}
|
3577
|
+
bool& Visibility_Graph::operator () (unsigned i1,
|
3578
|
+
unsigned j1,
|
3579
|
+
unsigned i2,
|
3580
|
+
unsigned j2)
|
3581
|
+
{
|
3582
|
+
return adjacency_matrix_[ two_to_one(i1,j1) ][ two_to_one(i2,j2) ];
|
3583
|
+
}
|
3584
|
+
bool& Visibility_Graph::operator () (unsigned k1,
|
3585
|
+
unsigned k2)
|
3586
|
+
{
|
3587
|
+
return adjacency_matrix_[ k1 ][ k2 ];
|
3588
|
+
}
|
3589
|
+
|
3590
|
+
|
3591
|
+
Visibility_Graph& Visibility_Graph::operator =
|
3592
|
+
(const Visibility_Graph& visibility_graph_temp)
|
3593
|
+
{
|
3594
|
+
if( this == &visibility_graph_temp )
|
3595
|
+
return *this;
|
3596
|
+
|
3597
|
+
n_ = visibility_graph_temp.n_;
|
3598
|
+
vertex_counts_ = visibility_graph_temp.vertex_counts_;
|
3599
|
+
|
3600
|
+
//resize adjacency_matrix_
|
3601
|
+
if( adjacency_matrix_ != NULL ){
|
3602
|
+
delete [] adjacency_matrix_[0];
|
3603
|
+
delete [] adjacency_matrix_;
|
3604
|
+
}
|
3605
|
+
adjacency_matrix_ = new bool*[n_];
|
3606
|
+
adjacency_matrix_[0] = new bool[n_*n_];
|
3607
|
+
for(unsigned i=1; i<n_; i++)
|
3608
|
+
adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
|
3609
|
+
|
3610
|
+
//copy each entry
|
3611
|
+
for(unsigned i=0; i<n_; i++){
|
3612
|
+
for(unsigned j=0; j<n_; j++){
|
3613
|
+
adjacency_matrix_[i][j]
|
3614
|
+
= visibility_graph_temp.adjacency_matrix_[i][j];
|
3615
|
+
}}
|
3616
|
+
|
3617
|
+
return *this;
|
3618
|
+
}
|
3619
|
+
|
3620
|
+
|
3621
|
+
unsigned Visibility_Graph::two_to_one(unsigned i,
|
3622
|
+
unsigned j) const
|
3623
|
+
{
|
3624
|
+
unsigned k=0;
|
3625
|
+
|
3626
|
+
for(unsigned counter=0; counter<i; counter++)
|
3627
|
+
k += vertex_counts_[counter];
|
3628
|
+
k += j;
|
3629
|
+
|
3630
|
+
return k;
|
3631
|
+
}
|
3632
|
+
|
3633
|
+
|
3634
|
+
Visibility_Graph::~Visibility_Graph()
|
3635
|
+
{
|
3636
|
+
if( adjacency_matrix_ != NULL ){
|
3637
|
+
delete [] adjacency_matrix_[0];
|
3638
|
+
delete [] adjacency_matrix_;
|
3639
|
+
}
|
3640
|
+
}
|
3641
|
+
|
3642
|
+
|
3643
|
+
std::ostream& operator << (std::ostream& outs,
|
3644
|
+
const Visibility_Graph& visibility_graph)
|
3645
|
+
{
|
3646
|
+
for(unsigned k1=0; k1<visibility_graph.n(); k1++){
|
3647
|
+
for(unsigned k2=0; k2<visibility_graph.n(); k2++){
|
3648
|
+
outs << visibility_graph( k1, k2 );
|
3649
|
+
if( k2 < visibility_graph.n()-1 )
|
3650
|
+
outs << " ";
|
3651
|
+
else
|
3652
|
+
outs << std::endl;
|
3653
|
+
}
|
3654
|
+
}
|
3655
|
+
|
3656
|
+
return outs;
|
3657
|
+
}
|
3658
|
+
|
3659
|
+
}
|