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.
@@ -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
+ }