src/libavoid/geometry.cpp

   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 */
  34
  35
  36 #include <cmath>
  37
  38 #include "libavoid/graph.h"
  39 #include "libavoid/geometry.h"
  40 #include "libavoid/assertions.h"
  41
  42 namespace Avoid {
  43
  44
  45
  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);
  56
  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 }
  69
  70
  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 }
  78
  79
  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     }
  93
  94     int ab_d = vecDir(a, b, d);
  95     if (ab_d == 0)
  96     {
  97         return false;
  98     }
  99
 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.
 102
 103     int cd_a = vecDir(c, d, a);
 104     int cd_b = vecDir(c, d, b);
 105
 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 }
 114
 115
 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 }
 145
 146
 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
 157
 158     int rSide = vecDir(b, a0, a1);
 159     int sSide = vecDir(b, a1, a2);
 160
 161     bool rOutOn = (rSide <= 0);
 162     bool sOutOn = (sSide <= 0);
 163
 164     bool rOut = (rSide < 0);
 165     bool sOut = (sSide < 0);
 166
 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 }
 202
 203
 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);
 220
 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     }
 237
 238     // c1-c2-c3 are collinear, so just return vecDir from c1-c2
 239     return s12p;
 240 }
 241
 242
 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;
 249
 250     return sqrt((xdiff * xdiff) + (ydiff * ydiff));
 251 }
 252
 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 }
 259
 260
 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;
 267
 268     return sqrt((xdiff * xdiff) + (ydiff * ydiff));
 269 }
 270
 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 }
 281
 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 }
 295
 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 }
 326
 327
 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;
 338
 339     // Copy the argument polygon
 340     Polygon poly = argpoly;
 341     std::vector<Point>& P = poly.ps;
 342     size_t    n = poly.size();
 343
 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     }
 350
 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         }
 360
 361         // point index; i1 = i-1 mod n
 362         size_t i1 = ( i + n - 1 ) % n;
 363
 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)) )
 369
 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);
 375
 376             // crosses ray if strictly positive intersection.
 377             if (x > 0)
 378             {
 379                 Rcross++;
 380             }
 381         }
 382
 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)) )
 386
 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);
 392
 393             // crosses ray if strictly positive intersection.
 394             if (x < 0)
 395             {
 396                 Lcross++;
 397             }
 398         }
 399     }
 400
 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     }
 407
 408     // Inside iff an odd number of crossings.
 409     if ((Rcross % 2) == 1)
 410     {
 411         return true;
 412     }
 413
 414     // Outside.
 415     return false;
 416 }
 417
 418
 419
 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;
 434
 435     Ax = a2.x - a1.x;
 436     Bx = b1.x - b2.x;
 437
 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     }
 457
 458     Ay = a2.y - a1.y;
 459     By = b1.y - b2.y;
 460
 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     }
 480
 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     }
 496
 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     }
 508
 509     // compute intersection coordinates:
 510
 511     if (f == 0) return PARALLEL;
 512
 513     // Numerator:
 514     num = d*Ax;
 515     // Intersection X:
 516     *x = a1.x + (num) / f;
 517
 518     num = d*Ay;
 519     // Intersection Y:
 520     *y = a1.y + (num) / f;
 521
 522     return DO_INTERSECT;
 523 }
 524
 525
 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;
 533
 534     Ay = a2.y - a1.y;
 535     By = b1.y - b2.y;
 536     Ax = a2.x - a1.x;
 537     Bx = b1.x - b2.x;
 538
 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;
 545
 546     // compute intersection coordinates:
 547
 548     if (f == 0) return PARALLEL;
 549
 550     // Numerator:
 551     num = d*Ax;
 552     // Intersection X:
 553     *x = a1.x + (num) / f;
 554
 555     num = d*Ay;
 556     // Intersection Y:
 557     *y = a1.y + (num) / f;
 558
 559     return DO_INTERSECT;
 560 }
 561
 562
 563 }
 564