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-2009 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 <mjwybrow@users.sourceforge.net>
33 */
36 #include <cmath>
38 #include "libavoid/graph.h"
39 #include "libavoid/geometry.h"
40 #include "libavoid/assertions.h"
42 namespace Avoid {
46 // Returns true iff the point c lies on the closed segment ab.
47 // To be used when the points are known to be collinear.
48 //
49 // Based on the code of 'Between'.
50 //
51 bool inBetween(const Point& a, const Point& b, const Point& c)
52 {
53 // We only call this when we know the points are collinear,
54 // otherwise we should be checking this here.
55 COLA_ASSERT(vecDir(a, b, c, 0.0001) == 0);
57 if ((fabs(a.x - b.x) > 1) && (a.x != b.x))
58 {
59 // not vertical
60 return (((a.x < c.x) && (c.x < b.x)) ||
61 ((b.x < c.x) && (c.x < a.x)));
62 }
63 else
64 {
65 return (((a.y < c.y) && (c.y < b.y)) ||
66 ((b.y < c.y) && (c.y < a.y)));
67 }
68 }
71 // Returns true iff the point c lies on the closed segment ab.
72 //
73 bool pointOnLine(const Point& a, const Point& b, const Point& c,
74 const double tolerance)
75 {
76 return (vecDir(a, b, c, tolerance) == 0) && inBetween(a, b, c);
77 }
80 // Returns true if the segment cd intersects the segment ab, blocking
81 // visibility.
82 //
83 // Based on the code of 'IntersectProp' and 'Intersect'.
84 //
85 bool segmentIntersect(const Point& a, const Point& b, const Point& c,
86 const Point& d)
87 {
88 int ab_c = vecDir(a, b, c);
89 if (ab_c == 0)
90 {
91 return false;
92 }
94 int ab_d = vecDir(a, b, d);
95 if (ab_d == 0)
96 {
97 return false;
98 }
100 // It's ok for either of the points a or b to be on the line cd,
101 // so we don't have to check the other two cases.
103 int cd_a = vecDir(c, d, a);
104 int cd_b = vecDir(c, d, b);
106 // Is an intersection if a and b are on opposite sides of cd,
107 // and c and d are on opposite sides of the line ab.
108 //
109 // Note: this is safe even though the textbook warns about it
110 // since, unlike them, our vecDir is equivilent to 'AreaSign'
111 // rather than 'Area2'.
112 return (((ab_c * ab_d) < 0) && ((cd_a * cd_b) < 0));
113 }
116 // Returns true if the segment e1-e2 intersects the shape boundary
117 // segment s1-s2, blocking visibility.
118 //
119 bool segmentShapeIntersect(const Point& e1, const Point& e2, const Point& s1,
120 const Point& s2, bool& seenIntersectionAtEndpoint)
121 {
122 if (segmentIntersect(e1, e2, s1, s2))
123 {
124 // Basic intersection of segments.
125 return true;
126 }
127 else if ( (((s2 == e1) || pointOnLine(s1, s2, e1)) &&
128 (vecDir(s1, s2, e2) != 0))
129 ||
130 (((s2 == e2) || pointOnLine(s1, s2, e2)) &&
131 (vecDir(s1, s2, e1) != 0)) )
132 {
133 // Segments intersect at the endpoint of one of the segments. We
134 // allow this once, but the second one blocks visibility. Otherwise
135 // shapes butted up against each other could have visibility through
136 // shapes.
137 if (seenIntersectionAtEndpoint)
138 {
139 return true;
140 }
141 seenIntersectionAtEndpoint = true;
142 }
143 return false;
144 }
147 // Returns true iff the point p in a valid region that can contain
148 // shortest paths. a0, a1, a2 are ordered vertices of a shape.
149 //
150 // Based on the code of 'InCone'.
151 //
152 bool inValidRegion(bool IgnoreRegions, const Point& a0, const Point& a1,
153 const Point& a2, const Point& b)
154 {
155 // r is a0--a1
156 // s is a1--a2
158 int rSide = vecDir(b, a0, a1);
159 int sSide = vecDir(b, a1, a2);
161 bool rOutOn = (rSide <= 0);
162 bool sOutOn = (sSide <= 0);
164 bool rOut = (rSide < 0);
165 bool sOut = (sSide < 0);
167 if (vecDir(a0, a1, a2) > 0)
168 {
169 // Convex at a1:
170 //
171 // !rO rO
172 // sO sO
173 //
174 // ---s---+
175 // |
176 // !rO r rO
177 // !sO | !sO
178 //
179 //
180 if (IgnoreRegions)
181 {
182 return (rOutOn && !sOut) || (!rOut && sOutOn);
183 }
184 return (rOutOn || sOutOn);
185 }
186 else
187 {
188 // Concave at a1:
189 //
190 // !rO rO
191 // !sO !sO
192 //
193 // +---s---
194 // |
195 // !rO r rO
196 // sO | sO
197 //
198 //
199 return (IgnoreRegions ? false : (rOutOn && sOutOn));
200 }
201 }
204 // Gives the side of a corner that a point lies on:
205 // 1 anticlockwise
206 // -1 clockwise
207 // e.g. /|s2
208 // /s3 -1 / |
209 // / / |
210 // 1 |s2 -1 / 1 | -1
211 // | / |
212 // |s1 s3/ |s1
213 //
214 int cornerSide(const Point &c1, const Point &c2, const Point &c3,
215 const Point& p)
216 {
217 int s123 = vecDir(c1, c2, c3);
218 int s12p = vecDir(c1, c2, p);
219 int s23p = vecDir(c2, c3, p);
221 if (s123 == 1)
222 {
223 if ((s12p >= 0) && (s23p >= 0))
224 {
225 return 1;
226 }
227 return -1;
228 }
229 else if (s123 == -1)
230 {
231 if ((s12p <= 0) && (s23p <= 0))
232 {
233 return -1;
234 }
235 return 1;
236 }
238 // c1-c2-c3 are collinear, so just return vecDir from c1-c2
239 return s12p;
240 }
243 // Returns the Euclidean distance between points a and b.
244 //
245 double euclideanDist(const Point& a, const Point& b)
246 {
247 double xdiff = a.x - b.x;
248 double ydiff = a.y - b.y;
250 return sqrt((xdiff * xdiff) + (ydiff * ydiff));
251 }
253 // Returns the Manhattan distance between points a and b.
254 //
255 double manhattanDist(const Point& a, const Point& b)
256 {
257 return fabs(a.x - b.x) + fabs(a.y - b.y);
258 }
261 // Returns the Euclidean distance between points a and b.
262 //
263 double dist(const Point& a, const Point& b)
264 {
265 double xdiff = a.x - b.x;
266 double ydiff = a.y - b.y;
268 return sqrt((xdiff * xdiff) + (ydiff * ydiff));
269 }
271 // Returns the total length of all line segments in the polygon
272 double totalLength(const Polygon& poly)
273 {
274 double l = 0;
275 for (size_t i = 1; i < poly.size(); ++i)
276 {
277 l += dist(poly.ps[i-1], poly.ps[i]);
278 }
279 return l;
280 }
282 // Uses the dot-product rule to find the angle (radians) between ab and bc
283 double angle(const Point& a, const Point& b, const Point& c)
284 {
285 double ux = b.x - a.x,
286 uy = b.y - a.y,
287 vx = c.x - b.x,
288 vy = c.y - b.y,
289 lu = sqrt(ux*ux+uy*uy),
290 lv = sqrt(vx*vx+vy*vy),
291 udotv = ux * vx + uy * vy,
292 costheta = udotv / (lu * lv);
293 return acos(costheta);
294 }
296 // Returns true iff the point q is inside (or on the edge of) the
297 // polygon argpoly.
298 //
299 // This is a fast version that only works for convex shapes. The
300 // other version (inPolyGen) is more general.
301 //
302 bool inPoly(const Polygon& poly, const Point& q, bool countBorder)
303 {
304 size_t n = poly.size();
305 const std::vector<Point>& P = poly.ps;
306 bool onBorder = false;
307 for (size_t i = 0; i < n; i++)
308 {
309 // point index; i1 = i-1 mod n
310 size_t prev = (i + n - 1) % n;
311 int dir = vecDir(P[prev], P[i], q);
312 if (dir == -1)
313 {
314 // Point is outside
315 return false;
316 }
317 // Record if point was on a boundary.
318 onBorder |= (dir == 0);
319 }
320 if (!countBorder && onBorder)
321 {
322 return false;
323 }
324 return true;
325 }
328 // Returns true iff the point q is inside (or on the edge of) the
329 // polygon argpoly.
330 //
331 // Based on the code of 'InPoly'.
332 //
333 bool inPolyGen(const PolygonInterface& argpoly, const Point& q)
334 {
335 // Numbers of right and left edge/ray crossings.
336 int Rcross = 0;
337 int Lcross = 0;
339 // Copy the argument polygon
340 Polygon poly = argpoly;
341 std::vector<Point>& P = poly.ps;
342 size_t n = poly.size();
344 // Shift so that q is the origin. This is done for pedogical clarity.
345 for (size_t i = 0; i < n; ++i)
346 {
347 P[i].x = P[i].x - q.x;
348 P[i].y = P[i].y - q.y;
349 }
351 // For each edge e=(i-1,i), see if crosses ray.
352 for (size_t i = 0; i < n; ++i)
353 {
354 // First see if q=(0,0) is a vertex.
355 if ((P[i].x == 0) && (P[i].y == 0))
356 {
357 // We count a vertex as inside.
358 return true;
359 }
361 // point index; i1 = i-1 mod n
362 size_t i1 = ( i + n - 1 ) % n;
364 // if e "straddles" the x-axis...
365 // The commented-out statement is logically equivalent to the one
366 // following.
367 // if( ((P[i].y > 0) && (P[i1].y <= 0)) ||
368 // ((P[i1].y > 0) && (P[i].y <= 0)) )
370 if ((P[i].y > 0) != (P[i1].y > 0))
371 {
372 // e straddles ray, so compute intersection with ray.
373 double x = (P[i].x * P[i1].y - P[i1].x * P[i].y)
374 / (P[i1].y - P[i].y);
376 // crosses ray if strictly positive intersection.
377 if (x > 0)
378 {
379 Rcross++;
380 }
381 }
383 // if e straddles the x-axis when reversed...
384 // if( ((P[i].y < 0) && (P[i1].y >= 0)) ||
385 // ((P[i1].y < 0) && (P[i].y >= 0)) )
387 if ((P[i].y < 0) != (P[i1].y < 0))
388 {
389 // e straddles ray, so compute intersection with ray.
390 double x = (P[i].x * P[i1].y - P[i1].x * P[i].y)
391 / (P[i1].y - P[i].y);
393 // crosses ray if strictly positive intersection.
394 if (x < 0)
395 {
396 Lcross++;
397 }
398 }
399 }
401 // q on the edge if left and right cross are not the same parity.
402 if ( (Rcross % 2) != (Lcross % 2) )
403 {
404 // We count the edge as inside.
405 return true;
406 }
408 // Inside iff an odd number of crossings.
409 if ((Rcross % 2) == 1)
410 {
411 return true;
412 }
414 // Outside.
415 return false;
416 }
420 // Line Segment Intersection
421 // Original code by Franklin Antonio
422 //
423 // The SAME_SIGNS macro assumes arithmetic where the exclusive-or
424 // operation will work on sign bits. This works for twos-complement,
425 // and most other machine arithmetic.
426 #define SAME_SIGNS( a, b ) \
427 (((long) ((unsigned long) a ^ (unsigned long) b)) >= 0 )
428 //
429 int segmentIntersectPoint(const Point& a1, const Point& a2,
430 const Point& b1, const Point& b2, double *x, double *y)
431 {
432 double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num;
433 double x1lo,x1hi,y1lo,y1hi;
435 Ax = a2.x - a1.x;
436 Bx = b1.x - b2.x;
438 // X bound box test:
439 if (Ax < 0)
440 {
441 x1lo = a2.x;
442 x1hi = a1.x;
443 }
444 else
445 {
446 x1hi = a2.x;
447 x1lo = a1.x;
448 }
449 if (Bx > 0)
450 {
451 if (x1hi < b2.x || b1.x < x1lo) return DONT_INTERSECT;
452 }
453 else
454 {
455 if (x1hi < b1.x || b2.x < x1lo) return DONT_INTERSECT;
456 }
458 Ay = a2.y - a1.y;
459 By = b1.y - b2.y;
461 // Y bound box test:
462 if (Ay < 0)
463 {
464 y1lo = a2.y;
465 y1hi = a1.y;
466 }
467 else
468 {
469 y1hi = a2.y;
470 y1lo = a1.y;
471 }
472 if (By > 0)
473 {
474 if (y1hi < b2.y || b1.y < y1lo) return DONT_INTERSECT;
475 }
476 else
477 {
478 if (y1hi < b1.y || b2.y < y1lo) return DONT_INTERSECT;
479 }
481 Cx = a1.x - b1.x;
482 Cy = a1.y - b1.y;
483 // alpha numerator:
484 d = By*Cx - Bx*Cy;
485 // Both denominator:
486 f = Ay*Bx - Ax*By;
487 // alpha tests:
488 if (f > 0)
489 {
490 if (d < 0 || d > f) return DONT_INTERSECT;
491 }
492 else
493 {
494 if (d > 0 || d < f) return DONT_INTERSECT;
495 }
497 // beta numerator:
498 e = Ax*Cy - Ay*Cx;
499 // beta tests:
500 if (f > 0)
501 {
502 if (e < 0 || e > f) return DONT_INTERSECT;
503 }
504 else
505 {
506 if (e > 0 || e < f) return DONT_INTERSECT;
507 }
509 // compute intersection coordinates:
511 if (f == 0) return PARALLEL;
513 // Numerator:
514 num = d*Ax;
515 // Intersection X:
516 *x = a1.x + (num) / f;
518 num = d*Ay;
519 // Intersection Y:
520 *y = a1.y + (num) / f;
522 return DO_INTERSECT;
523 }
526 // Line Segment Intersection
527 // Original code by Franklin Antonio
528 //
529 int rayIntersectPoint(const Point& a1, const Point& a2,
530 const Point& b1, const Point& b2, double *x, double *y)
531 {
532 double Ax,Bx,Cx,Ay,By,Cy,d,f,num;
534 Ay = a2.y - a1.y;
535 By = b1.y - b2.y;
536 Ax = a2.x - a1.x;
537 Bx = b1.x - b2.x;
539 Cx = a1.x - b1.x;
540 Cy = a1.y - b1.y;
541 // alpha numerator:
542 d = By*Cx - Bx*Cy;
543 // Both denominator:
544 f = Ay*Bx - Ax*By;
546 // compute intersection coordinates:
548 if (f == 0) return PARALLEL;
550 // Numerator:
551 num = d*Ax;
552 // Intersection X:
553 *x = a1.x + (num) / f;
555 num = d*Ay;
556 // Intersection Y:
557 *y = a1.y + (num) / f;
559 return DO_INTERSECT;
560 }
563 }