1 /*
2 * vim: ts=4 sw=4 et tw=0 wm=0
3 *
4 * libavoid - Fast, Incremental, Object-avoiding Line Router
5 *
6 * Copyright (C) 2004-2011 Monash University
7 *
8 * --------------------------------------------------------------------
9 * Much of the code in this module is based on code published with
10 * and/or described in "Computational Geometry in C" (Second Edition),
11 * Copyright (C) 1998 Joseph O'Rourke <orourke@cs.smith.edu>
12 * --------------------------------------------------------------------
13 * The segmentIntersectPoint function is based on code published and
14 * described in Franklin Antonio, Faster Line Segment Intersection,
15 * Graphics Gems III, p. 199-202, code: p. 500-501.
16 * --------------------------------------------------------------------
17 *
18 * This library is free software; you can redistribute it and/or
19 * modify it under the terms of the GNU Lesser General Public
20 * License as published by the Free Software Foundation; either
21 * version 2.1 of the License, or (at your option) any later version.
22 * See the file LICENSE.LGPL distributed with the library.
23 *
24 * Licensees holding a valid commercial license may use this file in
25 * accordance with the commercial license agreement provided with the
26 * library.
27 *
28 * This library is distributed in the hope that it will be useful,
29 * but WITHOUT ANY WARRANTY; without even the implied warranty of
30 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
31 *
32 * Author(s): Michael Wybrow
33 */
34
35
36 // For M_PI:
37 #define _USE_MATH_DEFINES
38 #include <cmath>
39
40 #include <limits>
41
42 #include "libavoid/graph.h"
43 #include "libavoid/geometry.h"
44 #include "libavoid/assertions.h"
45
46
47 namespace Avoid {
48
49
50 // Returns true iff the point c lies on the closed segment ab.
51 // To be used when the points are known to be collinear.
52 //
53 // Based on the code of 'Between'.
54 //
inBetween(const Point & a,const Point & b,const Point & c)55 bool inBetween(const Point& a, const Point& b, const Point& c)
56 {
57 double epsilon = std::numeric_limits<double>::epsilon();
58
59 // We only call this when we know the points are collinear,
60 // otherwise we should be checking this here.
61 COLA_ASSERT(vecDir(a, b, c, epsilon) == 0);
62
63 if (fabs(a.x - b.x) > epsilon)
64 {
65 // not vertical
66 return (((a.x < c.x) && (c.x < b.x)) ||
67 ((b.x < c.x) && (c.x < a.x)));
68 }
69 else
70 {
71 return (((a.y < c.y) && (c.y < b.y)) ||
72 ((b.y < c.y) && (c.y < a.y)));
73 }
74 }
75
76
77 // Returns true iff the three points are colinear.
78 //
colinear(const Point & a,const Point & b,const Point & c,const double tolerance)79 bool colinear(const Point& a, const Point& b, const Point& c,
80 const double tolerance)
81 {
82
83 // Do this a bit more optimally for orthogonal AB line segments.
84 if (a == b)
85 {
86 return true;
87 }
88 else if (a.x == b.x)
89 {
90 return (a.x == c.x);
91 }
92 else if (a.y == b.y)
93 {
94 return (a.y == c.y);
95 }
96
97 // Or use the general case.
98 return (vecDir(a, b, c, tolerance) == 0);
99
100 }
101
102
103 // Returns true iff the point c lies on the closed segment ab.
104 //
pointOnLine(const Point & a,const Point & b,const Point & c,const double tolerance)105 bool pointOnLine(const Point& a, const Point& b, const Point& c,
106 const double tolerance)
107 {
108 // Do this a bit more optimally for orthogonal AB line segments.
109 if (a.x == b.x)
110 {
111 return (a.x == c.x) &&
112 (((a.y < c.y) && (c.y < b.y)) ||
113 ((b.y < c.y) && (c.y < a.y)));
114 }
115 else if (a.y == b.y)
116 {
117 return (a.y == c.y) &&
118 (((a.x < c.x) && (c.x < b.x)) ||
119 ((b.x < c.x) && (c.x < a.x)));
120 }
121
122 // Or use the general case.
123 return (vecDir(a, b, c, tolerance) == 0) && inBetween(a, b, c);
124 }
125
126
127 // Returns true if the segment cd intersects the segment ab, blocking
128 // visibility.
129 //
130 // Based on the code of 'IntersectProp' and 'Intersect'.
131 //
segmentIntersect(const Point & a,const Point & b,const Point & c,const Point & d)132 bool segmentIntersect(const Point& a, const Point& b, const Point& c,
133 const Point& d)
134 {
135 int ab_c = vecDir(a, b, c);
136 if (ab_c == 0)
137 {
138 return false;
139 }
140
141 int ab_d = vecDir(a, b, d);
142 if (ab_d == 0)
143 {
144 return false;
145 }
146
147 // It's ok for either of the points a or b to be on the line cd,
148 // so we don't have to check the other two cases.
149
150 int cd_a = vecDir(c, d, a);
151 int cd_b = vecDir(c, d, b);
152
153 // Is an intersection if a and b are on opposite sides of cd,
154 // and c and d are on opposite sides of the line ab.
155 //
156 // Note: this is safe even though the textbook warns about it
157 // since, unlike them, our vecDir is equivilent to 'AreaSign'
158 // rather than 'Area2'.
159 return (((ab_c * ab_d) < 0) && ((cd_a * cd_b) < 0));
160 }
161
162
163 // Returns true if the segment e1-e2 intersects the shape boundary
164 // segment s1-s2, blocking visibility.
165 //
segmentShapeIntersect(const Point & e1,const Point & e2,const Point & s1,const Point & s2,bool & seenIntersectionAtEndpoint)166 bool segmentShapeIntersect(const Point& e1, const Point& e2, const Point& s1,
167 const Point& s2, bool& seenIntersectionAtEndpoint)
168 {
169 if (segmentIntersect(e1, e2, s1, s2))
170 {
171 // Basic intersection of segments.
172 return true;
173 }
174 else if ( (((s2 == e1) || pointOnLine(s1, s2, e1)) &&
175 (vecDir(s1, s2, e2) != 0))
176 ||
177 (((s2 == e2) || pointOnLine(s1, s2, e2)) &&
178 (vecDir(s1, s2, e1) != 0)) )
179 {
180 // Segments intersect at the endpoint of one of the segments. We
181 // allow this once, but the second one blocks visibility. Otherwise
182 // shapes butted up against each other could have visibility through
183 // shapes.
184 if (seenIntersectionAtEndpoint)
185 {
186 return true;
187 }
188 seenIntersectionAtEndpoint = true;
189 }
190 return false;
191 }
192
193
194 // Returns true iff the point p in a valid region that can contain
195 // shortest paths. a0, a1, a2 are ordered vertices of a shape.
196 //
197 // Based on the code of 'InCone'.
198 //
inValidRegion(bool IgnoreRegions,const Point & a0,const Point & a1,const Point & a2,const Point & b)199 bool inValidRegion(bool IgnoreRegions, const Point& a0, const Point& a1,
200 const Point& a2, const Point& b)
201 {
202 // r is a0--a1
203 // s is a1--a2
204
205 int rSide = vecDir(b, a0, a1);
206 int sSide = vecDir(b, a1, a2);
207
208 bool rOutOn = (rSide <= 0);
209 bool sOutOn = (sSide <= 0);
210
211 bool rOut = (rSide < 0);
212 bool sOut = (sSide < 0);
213
214 if (vecDir(a0, a1, a2) > 0)
215 {
216 // Convex at a1:
217 //
218 // !rO rO
219 // sO sO
220 //
221 // ---s---+
222 // |
223 // !rO r rO
224 // !sO | !sO
225 //
226 //
227 if (IgnoreRegions)
228 {
229 return (rOutOn && !sOut) || (!rOut && sOutOn);
230 }
231 return (rOutOn || sOutOn);
232 }
233 else
234 {
235 // Concave at a1:
236 //
237 // !rO rO
238 // !sO !sO
239 //
240 // +---s---
241 // |
242 // !rO r rO
243 // sO | sO
244 //
245 //
246 return (IgnoreRegions ? false : (rOutOn && sOutOn));
247 }
248 }
249
250
251 // Gives the side of a corner that a point lies on:
252 // 1 anticlockwise
253 // -1 clockwise
254 // e.g. /|s2
255 // /s3 -1 / |
256 // / / |
257 // 1 |s2 -1 / 1 | -1
258 // | / |
259 // |s1 s3/ |s1
260 //
cornerSide(const Point & c1,const Point & c2,const Point & c3,const Point & p)261 int cornerSide(const Point &c1, const Point &c2, const Point &c3,
262 const Point& p)
263 {
264 int s123 = vecDir(c1, c2, c3);
265 int s12p = vecDir(c1, c2, p);
266 int s23p = vecDir(c2, c3, p);
267
268 if (s123 == 1)
269 {
270 if ((s12p >= 0) && (s23p >= 0))
271 {
272 return 1;
273 }
274 return -1;
275 }
276 else if (s123 == -1)
277 {
278 if ((s12p <= 0) && (s23p <= 0))
279 {
280 return -1;
281 }
282 return 1;
283 }
284
285 // c1-c2-c3 are collinear, so just return vecDir from c1-c2
286 return s12p;
287 }
288
289
290 // Returns the Euclidean distance between points a and b.
291 //
euclideanDist(const Point & a,const Point & b)292 double euclideanDist(const Point& a, const Point& b)
293 {
294 double xdiff = a.x - b.x;
295 double ydiff = a.y - b.y;
296
297 return sqrt((xdiff * xdiff) + (ydiff * ydiff));
298 }
299
300 // Returns the Manhattan distance between points a and b.
301 //
manhattanDist(const Point & a,const Point & b)302 double manhattanDist(const Point& a, const Point& b)
303 {
304 return fabs(a.x - b.x) + fabs(a.y - b.y);
305 }
306
307
308 // Returns the Euclidean distance between points a and b.
309 //
dist(const Point & a,const Point & b)310 double dist(const Point& a, const Point& b)
311 {
312 double xdiff = a.x - b.x;
313 double ydiff = a.y - b.y;
314
315 return sqrt((xdiff * xdiff) + (ydiff * ydiff));
316 }
317
318 // Returns the total length of all line segments in the polygon
totalLength(const Polygon & poly)319 double totalLength(const Polygon& poly)
320 {
321 double l = 0;
322 for (size_t i = 1; i < poly.size(); ++i)
323 {
324 l += dist(poly.ps[i-1], poly.ps[i]);
325 }
326 return l;
327 }
328
329 // Uses the dot-product rule to find the angle (radians) between ab and bc
angle(const Point & a,const Point & b,const Point & c)330 double angle(const Point& a, const Point& b, const Point& c)
331 {
332 double ux = b.x - a.x,
333 uy = b.y - a.y,
334 vx = c.x - b.x,
335 vy = c.y - b.y,
336 lu = sqrt(ux*ux+uy*uy),
337 lv = sqrt(vx*vx+vy*vy),
338 udotv = ux * vx + uy * vy,
339 costheta = udotv / (lu * lv);
340 return acos(costheta);
341 }
342
343 // Returns true iff the point q is inside (or on the edge of) the
344 // polygon argpoly.
345 //
346 // This is a fast version that only works for convex shapes. The
347 // other version (inPolyGen) is more general.
348 //
inPoly(const Polygon & poly,const Point & q,bool countBorder)349 bool inPoly(const Polygon& poly, const Point& q, bool countBorder)
350 {
351 size_t n = poly.size();
352 const std::vector<Point>& P = poly.ps;
353 bool onBorder = false;
354 for (size_t i = 0; i < n; i++)
355 {
356 // point index; i1 = i-1 mod n
357 size_t prev = (i + n - 1) % n;
358 int dir = vecDir(P[prev], P[i], q);
359 if (dir == -1)
360 {
361 // Point is outside
362 return false;
363 }
364 // Record if point was on a boundary.
365 onBorder |= (dir == 0);
366 }
367 if (!countBorder && onBorder)
368 {
369 return false;
370 }
371 return true;
372 }
373
374
375 // Returns true iff the point q is inside (or on the edge of) the
376 // polygon argpoly.
377 //
378 // Based on the code of 'InPoly'.
379 //
inPolyGen(const PolygonInterface & argpoly,const Point & q)380 bool inPolyGen(const PolygonInterface& argpoly, const Point& q)
381 {
382 // Numbers of right and left edge/ray crossings.
383 int Rcross = 0;
384 int Lcross = 0;
385
386 // Copy the argument polygon
387 Polygon poly = argpoly;
388 std::vector<Point>& P = poly.ps;
389 size_t n = poly.size();
390
391 // Shift so that q is the origin. This is done for pedagogical clarity.
392 for (size_t i = 0; i < n; ++i)
393 {
394 P[i].x = P[i].x - q.x;
395 P[i].y = P[i].y - q.y;
396 }
397
398 // For each edge e=(i-1,i), see if crosses ray.
399 for (size_t i = 0; i < n; ++i)
400 {
401 // First see if q=(0,0) is a vertex.
402 if ((P[i].x == 0) && (P[i].y == 0))
403 {
404 // We count a vertex as inside.
405 return true;
406 }
407
408 // point index; i1 = i-1 mod n
409 size_t i1 = ( i + n - 1 ) % n;
410
411 // if e "straddles" the x-axis...
412 // The commented-out statement is logically equivalent to the one
413 // following.
414 // if( ((P[i].y > 0) && (P[i1].y <= 0)) ||
415 // ((P[i1].y > 0) && (P[i].y <= 0)) )
416
417 if ((P[i].y > 0) != (P[i1].y > 0))
418 {
419 // e straddles ray, so compute intersection with ray.
420 double x = (P[i].x * P[i1].y - P[i1].x * P[i].y)
421 / (P[i1].y - P[i].y);
422
423 // crosses ray if strictly positive intersection.
424 if (x > 0)
425 {
426 Rcross++;
427 }
428 }
429
430 // if e straddles the x-axis when reversed...
431 // if( ((P[i].y < 0) && (P[i1].y >= 0)) ||
432 // ((P[i1].y < 0) && (P[i].y >= 0)) )
433
434 if ((P[i].y < 0) != (P[i1].y < 0))
435 {
436 // e straddles ray, so compute intersection with ray.
437 double x = (P[i].x * P[i1].y - P[i1].x * P[i].y)
438 / (P[i1].y - P[i].y);
439
440 // crosses ray if strictly positive intersection.
441 if (x < 0)
442 {
443 Lcross++;
444 }
445 }
446 }
447
448 // q on the edge if left and right cross are not the same parity.
449 if ( (Rcross % 2) != (Lcross % 2) )
450 {
451 // We count the edge as inside.
452 return true;
453 }
454
455 // Inside iff an odd number of crossings.
456 if ((Rcross % 2) == 1)
457 {
458 return true;
459 }
460
461 // Outside.
462 return false;
463 }
464
465
466
467 // Line Segment Intersection
468 // Original code by Franklin Antonio
469 //
470 // The SAME_SIGNS macro assumes arithmetic where the exclusive-or
471 // operation will work on sign bits. This works for twos-complement,
472 // and most other machine arithmetic.
473 #define SAME_SIGNS( a, b ) \
474 (((long) ((unsigned long) a ^ (unsigned long) b)) >= 0 )
475 //
segmentIntersectPoint(const Point & a1,const Point & a2,const Point & b1,const Point & b2,double * x,double * y)476 int segmentIntersectPoint(const Point& a1, const Point& a2,
477 const Point& b1, const Point& b2, double *x, double *y)
478 {
479 double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
480 double x1lo,x1hi,y1lo,y1hi;
481
482 Ax = a2.x - a1.x;
483 Bx = b1.x - b2.x;
484
485 // X bound box test:
486 if (Ax < 0)
487 {
488 x1lo = a2.x;
489 x1hi = a1.x;
490 }
491 else
492 {
493 x1hi = a2.x;
494 x1lo = a1.x;
495 }
496 if (Bx > 0)
497 {
498 if (x1hi < b2.x || b1.x < x1lo) return DONT_INTERSECT;
499 }
500 else
501 {
502 if (x1hi < b1.x || b2.x < x1lo) return DONT_INTERSECT;
503 }
504
505 Ay = a2.y - a1.y;
506 By = b1.y - b2.y;
507
508 // Y bound box test:
509 if (Ay < 0)
510 {
511 y1lo = a2.y;
512 y1hi = a1.y;
513 }
514 else
515 {
516 y1hi = a2.y;
517 y1lo = a1.y;
518 }
519 if (By > 0)
520 {
521 if (y1hi < b2.y || b1.y < y1lo) return DONT_INTERSECT;
522 }
523 else
524 {
525 if (y1hi < b1.y || b2.y < y1lo) return DONT_INTERSECT;
526 }
527
528 Cx = a1.x - b1.x;
529 Cy = a1.y - b1.y;
530 // alpha numerator:
531 d = By*Cx - Bx*Cy;
532 // Both denominator:
533 f = Ay*Bx - Ax*By;
534 // alpha tests:
535 if (f > 0)
536 {
537 if (d < 0 || d > f) return DONT_INTERSECT;
538 }
539 else
540 {
541 if (d > 0 || d < f) return DONT_INTERSECT;
542 }
543
544 // beta numerator:
545 e = Ax*Cy - Ay*Cx;
546 // beta tests:
547 if (f > 0)
548 {
549 if (e < 0 || e > f) return DONT_INTERSECT;
550 }
551 else
552 {
553 if (e > 0 || e < f) return DONT_INTERSECT;
554 }
555
556 // compute intersection coordinates:
557
558 if (f == 0) return PARALLEL;
559
560 // Numerator:
561 num = d*Ax;
562 // Intersection X:
563 *x = a1.x + (num) / f;
564
565 num = d*Ay;
566 // Intersection Y:
567 *y = a1.y + (num) / f;
568
569 return DO_INTERSECT;
570 }
571
572
573 // Line Segment Intersection
574 // Original code by Franklin Antonio
575 //
rayIntersectPoint(const Point & a1,const Point & a2,const Point & b1,const Point & b2,double * x,double * y)576 int rayIntersectPoint(const Point& a1, const Point& a2,
577 const Point& b1, const Point& b2, double *x, double *y)
578 {
579 double Ax,Bx,Cx,Ay,By,Cy,d,f,num;
580
581 Ay = a2.y - a1.y;
582 By = b1.y - b2.y;
583 Ax = a2.x - a1.x;
584 Bx = b1.x - b2.x;
585
586 Cx = a1.x - b1.x;
587 Cy = a1.y - b1.y;
588 // alpha numerator:
589 d = By*Cx - Bx*Cy;
590 // Both denominator:
591 f = Ay*Bx - Ax*By;
592
593 // compute intersection coordinates:
594
595 if (f == 0) return PARALLEL;
596
597 // Numerator:
598 num = d*Ax;
599 // Intersection X:
600 *x = a1.x + (num) / f;
601
602 num = d*Ay;
603 // Intersection Y:
604 *y = a1.y + (num) / f;
605
606 return DO_INTERSECT;
607 }
608
609 // Returns the rotationalAngle, between 0 and 360, of this point from (0,0).
610 //
rotationalAngle(const Point & p)611 double rotationalAngle(const Point& p)
612 {
613 if (p.y == 0)
614 {
615 return ((p.x < 0) ? 180 : 0);
616 }
617 else if (p.x == 0)
618 {
619 return ((p.y < 0) ? 270 : 90);
620 }
621
622 double ang = atan(p.y / p.x);
623 ang = (ang * 180) / M_PI;
624
625 if (p.x < 0)
626 {
627 ang += 180;
628 }
629 else if (p.y < 0)
630 {
631 ang += 360;
632 }
633 COLA_ASSERT(ang >= 0);
634 COLA_ASSERT(ang <= 360);
635
636 return ang;
637 }
638
639
640 }
641
642