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  * Copyright (C) 2004-2006  Michael Wybrow <mjwybrow@users.sourceforge.net>
   6  *
   7  * --------------------------------------------------------------------
   8  * Much of the code in this module is based on code published with
   9  * and/or described in "Computational Geometry in C" (Second Edition),
  10  * Copyright (C) 1998  Joseph O'Rourke <orourke@cs.smith.edu>
  11  * --------------------------------------------------------------------
  12  * The segmentIntersectPoint function is based on code published and
  13  * described in Franklin Antonio, Faster Line Segment Intersection,
  14  * Graphics Gems III, p. 199-202, code: p. 500-501.
  15  * --------------------------------------------------------------------
  16  *
  17  * This library is free software; you can redistribute it and/or
  18  * modify it under the terms of the GNU Lesser General Public
  19  * License as published by the Free Software Foundation; either
  20  * version 2.1 of the License, or (at your option) any later version.
  21  *
  22  * This library is distributed in the hope that it will be useful,
  23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  25  * Lesser General Public License for more details.
  26  *
  27  * You should have received a copy of the GNU Lesser General Public
  28  * License along with this library; if not, write to the Free Software
  29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  30  *
  31 */
  32
  33 #include "libavoid/graph.h"
  34 #include "libavoid/geometry.h"
  35 #include "libavoid/polyutil.h"
  36
  37 #include <math.h>
  38
  39 namespace Avoid {
  40
  41
  42 Point::Point()
  43 {
  44 }
  45
  46
  47 Point::Point(const double xv, const double yv)
  48     : x(xv)
  49     , y(yv)
  50 {
  51 }
  52
  53
  54 bool Point::operator==(const Point& rhs) const
  55 {
  56     if ((x == rhs.x) && (y == rhs.y))
  57     {
  58         return true;
  59     }
  60     return false;
  61 }
  62
  63
  64 bool Point::operator!=(const Point& rhs) const
  65 {
  66     if ((x != rhs.x) || (y != rhs.y))
  67     {
  68         return true;
  69     }
  70     return false;
  71 }
  72
  73
  74 // Returns true iff the point c lies on the closed segment ab.
  75 //
  76 // Based on the code of 'Between'.
  77 //
  78 static const bool inBetween(const Point& a, const Point& b, const Point& c)
  79 {
  80     // We only call this when we know the points are collinear,
  81     // otherwise we should be checking this here.
  82     assert(vecDir(a, b, c) == 0);
  83
  84     if (a.x != b.x)
  85     {
  86         // not vertical
  87         return (((a.x < c.x) && (c.x < b.x)) ||
  88                 ((b.x < c.x) && (c.x < a.x)));
  89     }
  90     else
  91     {
  92         return (((a.y < c.y) && (c.y < b.y)) ||
  93                 ((b.y < c.y) && (c.y < a.y)));
  94     }
  95 }
  96
  97
  98 // Returns true if the segment cd intersects the segment ab, blocking
  99 // visibility.
 100 //
 101 // Based on the code of 'IntersectProp' and 'Intersect'.
 102 //
 103 bool segmentIntersect(const Point& a, const Point& b, const Point& c,
 104         const Point& d)
 105 {
 106     int ab_c = vecDir(a, b, c);
 107     if ((ab_c == 0) && inBetween(a, b, c))
 108     {
 109         return true;
 110     }
 111
 112     int ab_d = vecDir(a, b, d);
 113     if ((ab_d == 0) && inBetween(a, b, d))
 114     {
 115         return true;
 116     }
 117
 118     // It's ok for either of the points a or b to be on the line cd,
 119     // so we don't have to check the other two cases.
 120
 121     int cd_a = vecDir(c, d, a);
 122     int cd_b = vecDir(c, d, b);
 123
 124     // Is an intersection if a and b are on opposite sides of cd,
 125     // and c and d are on opposite sides of the line ab.
 126     //
 127     // Note: this is safe even though the textbook warns about it
 128     // since, unlike them, our vecDir is equivilent to 'AreaSign'
 129     // rather than 'Area2'.
 130     return (((ab_c * ab_d) < 0) && ((cd_a * cd_b) < 0));
 131 }
 132
 133
 134 // Returns true iff the point p in a valid region that can contain
 135 // shortest paths.  a0, a1, a2 are ordered vertices of a shape.
 136 // This function may seem 'backwards' to the user due to some of
 137 // the code being reversed due to screen cooridinated being the
 138 // opposite of graph paper coords.
 139 // TODO: Rewrite this after checking whether it works for Inkscape.
 140 //
 141 // Based on the code of 'InCone'.
 142 //
 143 bool inValidRegion(bool IgnoreRegions, const Point& a0, const Point& a1,
 144         const Point& a2, const Point& b)
 145 {
 146     int rSide = vecDir(b, a0, a1);
 147     int sSide = vecDir(b, a1, a2);
 148
 149     bool rOutOn = (rSide >= 0);
 150     bool sOutOn = (sSide >= 0);
 151
 152     bool rOut = (rSide > 0);
 153     bool sOut = (sSide > 0);
 154
 155     if (vecDir(a0, a1, a2) > 0)
 156     {
 157         // Concave at a1:
 158         //
 159         //   !rO      rO
 160         //   !sO     !sO
 161         //
 162         //        +---s---
 163         //        |
 164         //   !rO  r   rO
 165         //    sO  |   sO
 166         //
 167         //
 168         return (IgnoreRegions ? false : (rOutOn && sOutOn));
 169     }
 170     else
 171     {
 172         // Convex at a1:
 173         //
 174         //   !rO      rO
 175         //    sO      sO
 176         //
 177         // ---s---+
 178         //        |
 179         //   !rO  r   rO
 180         //   !sO  |  !sO
 181         //
 182         //
 183         if (IgnoreRegions)
 184         {
 185             return (rOutOn && !sOut) || (!rOut && sOutOn);
 186         }
 187         return (rOutOn || sOutOn);
 188     }
 189 }
 190
 191
 192 // Gives the side of a corner that a point lies on:
 193 //      1   anticlockwise
 194 //     -1   clockwise
 195 // e.g.                     /|
 196 //       /s2          -1   / |s1
 197 //      /                 /  |
 198 //  1  |s1  -1           / 1 |  -1
 199 //     |                /    |
 200 //     |s0           s2/     |s0
 201 //
 202 int cornerSide(const Point &c1, const Point &c2, const Point &c3,
 203         const Point& p)
 204 {
 205     int s123 = vecDir(c1, c2, c3);
 206     int s12p = vecDir(c1, c2, p);
 207     int s23p = vecDir(c2, c3, p);
 208
 209     if (s12p == 0)
 210     {
 211         // Case of p being somewhere on c1-c2.
 212         return s23p;
 213     }
 214     if (s23p == 0)
 215     {
 216         // Case of p being somewhere on c2-c3.
 217         return s12p;
 218     }
 219
 220     if (s123 == 1)
 221     {
 222         if ((s12p == 1) && (s23p == 1))
 223         {
 224             return 1;
 225         }
 226         return -1;
 227     }
 228     else if (s123 == -1)
 229     {
 230         if ((s12p == -1) && (s23p == -1))
 231         {
 232             return -1;
 233         }
 234         return 1;
 235     }
 236     // Case of c3 being somewhere on c1-c2.
 237     return s12p;
 238 }
 239
 240
 241 // Returns the distance between points a and b.
 242 //
 243 double dist(const Point& a, const Point& b)
 244 {
 245     double xdiff = a.x - b.x;
 246     double ydiff = a.y - b.y;
 247
 248     return sqrt((xdiff * xdiff) + (ydiff * ydiff));
 249 }
 250
 251 // Returns the total length of all line segments in the polygon
 252 double totalLength(const Polygn& poly)
 253 {
 254     double l = 0;
 255     for (int i = 0; i < poly.pn-1; ++i) {
 256         l += dist(poly.ps[i], poly.ps[i+1]);
 257     }
 258     return l;
 259 }
 260
 261 // Uses the dot-product rule to find the angle (radians) between ab and bc
 262 double angle(const Point& a, const Point& b, const Point& c)
 263 {
 264     double ux = b.x - a.x,
 265            uy = b.y - a.y,
 266            vx = c.x - b.x,
 267            vy = c.y - b.y,
 268            lu = sqrt(ux*ux+uy*uy),
 269            lv = sqrt(vx*vx+vy*vy),
 270            udotv = ux * vx + uy * vy,
 271            costheta = udotv / (lu * lv);
 272     return acos(costheta);
 273 }
 274
 275 // Returns true iff the point q is inside (or on the edge of) the
 276 // polygon argpoly.
 277 //
 278 // This is a fast version that only works for convex shapes.  The
 279 // other version (inPolyGen) is more general.
 280 //
 281 bool inPoly(const Polygn& poly, const Point& q)
 282 {
 283     int n = poly.pn;
 284     Point *P = poly.ps;
 285     for (int i = 0; i < n; i++)
 286     {
 287         // point index; i1 = i-1 mod n
 288         int prev = (i + n - 1) % n;
 289         if (vecDir(P[prev], P[i], q) == 1)
 290         {
 291             return false;
 292         }
 293     }
 294     return true;
 295 }
 296
 297
 298 // Returns true iff the point q is inside (or on the edge of) the
 299 // polygon argpoly.
 300 //
 301 // Based on the code of 'InPoly'.
 302 //
 303 bool inPolyGen(const Polygn& argpoly, const Point& q)
 304 {
 305     // Numbers of right and left edge/ray crossings.
 306     int Rcross = 0;
 307     int Lcross = 0;
 308
 309     // Copy the argument polygon
 310     Polygn poly = copyPoly(argpoly);
 311     Point *P = poly.ps;
 312     int    n = poly.pn;
 313
 314     // Shift so that q is the origin. This is done for pedogical clarity.
 315     for (int i = 0; i < n; ++i)
 316     {
 317         P[i].x = P[i].x - q.x;
 318         P[i].y = P[i].y - q.y;
 319     }
 320
 321     // For each edge e=(i-1,i), see if crosses ray.
 322     for (int i = 0; i < n; ++i)
 323     {
 324         // First see if q=(0,0) is a vertex.
 325         if ((P[i].x == 0) && (P[i].y == 0))
 326         {
 327             // We count a vertex as inside.
 328             freePoly(poly);
 329             return true;
 330         }
 331
 332         // point index; i1 = i-1 mod n
 333         int i1 = ( i + n - 1 ) % n;
 334
 335         // if e "straddles" the x-axis...
 336         // The commented-out statement is logically equivalent to the one
 337         // following.
 338         // if( ((P[i].y > 0) && (P[i1].y <= 0)) ||
 339         //         ((P[i1].y > 0) && (P[i].y <= 0)) )
 340
 341         if ((P[i].y > 0) != (P[i1].y > 0))
 342         {
 343             // e straddles ray, so compute intersection with ray.
 344             double x = (P[i].x * P[i1].y - P[i1].x * P[i].y)
 345                     / (P[i1].y - P[i].y);
 346
 347             // crosses ray if strictly positive intersection.
 348             if (x > 0)
 349             {
 350                 Rcross++;
 351             }
 352         }
 353
 354         // if e straddles the x-axis when reversed...
 355         // if( ((P[i].y < 0) && (P[i1].y >= 0)) ||
 356         //         ((P[i1].y < 0) && (P[i].y >= 0)) )
 357
 358         if ((P[i].y < 0) != (P[i1].y < 0))
 359         {
 360             // e straddles ray, so compute intersection with ray.
 361             double x = (P[i].x * P[i1].y - P[i1].x * P[i].y)
 362                     / (P[i1].y - P[i].y);
 363
 364             // crosses ray if strictly positive intersection.
 365             if (x < 0)
 366             {
 367                 Lcross++;
 368             }
 369         }
 370     }
 371     freePoly(poly);
 372
 373     // q on the edge if left and right cross are not the same parity.
 374     if ( (Rcross % 2) != (Lcross % 2) )
 375     {
 376         // We count the edge as inside.
 377         return true;
 378     }
 379
 380     // Inside iff an odd number of crossings.
 381     if ((Rcross % 2) == 1)
 382     {
 383         return true;
 384     }
 385
 386     // Outside.
 387     return false;
 388 }
 389
 390
 391
 392 // Line Segment Intersection
 393 // Original code by Franklin Antonio
 394 //
 395 // The SAME_SIGNS macro assumes arithmetic where the exclusive-or
 396 // operation will work on sign bits.  This works for twos-complement,
 397 // and most other machine arithmetic.
 398 #define SAME_SIGNS( a, b ) \
 399         (((long) ((unsigned long) a ^ (unsigned long) b)) >= 0 )
 400 //
 401 int segmentIntersectPoint(const Point& a1, const Point& a2,
 402         const Point& b1, const Point& b2, double *x, double *y)
 403 {
 404
 405     double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num,offset;
 406     double x1lo,x1hi,y1lo,y1hi;
 407
 408     Ax = a2.x - a1.x;
 409     Bx = b1.x - b2.x;
 410
 411     // X bound box test:
 412     if (Ax < 0)
 413     {
 414         x1lo = a2.x;
 415         x1hi = a1.x;
 416     }
 417     else
 418     {
 419         x1hi = a2.x;
 420         x1lo = a1.x;
 421     }
 422     if (Bx > 0)
 423     {
 424         if (x1hi < b2.x || b1.x < x1lo) return DONT_INTERSECT;
 425     }
 426     else
 427     {
 428         if (x1hi < b1.x || b2.x < x1lo) return DONT_INTERSECT;
 429     }
 430
 431     Ay = a2.y - a1.y;
 432     By = b1.y - b2.y;
 433
 434     // Y bound box test:
 435     if (Ay < 0)
 436     {
 437         y1lo = a2.y;
 438         y1hi = a1.y;
 439     }
 440     else
 441     {
 442         y1hi = a2.y;
 443         y1lo = a1.y;
 444     }
 445     if (By > 0)
 446     {
 447         if (y1hi < b2.y || b1.y < y1lo) return DONT_INTERSECT;
 448     }
 449     else
 450     {
 451         if (y1hi < b1.y || b2.y < y1lo) return DONT_INTERSECT;
 452     }
 453
 454
 455     Cx = a1.x - b1.x;
 456     Cy = a1.y - b1.y;
 457     // alpha numerator:
 458     d = By*Cx - Bx*Cy;
 459     // Both denominator:
 460     f = Ay*Bx - Ax*By;
 461     // aplha tests:
 462     if (f > 0)
 463     {
 464         if (d < 0 || d > f) return DONT_INTERSECT;
 465     }
 466     else
 467     {
 468         if (d > 0 || d < f) return DONT_INTERSECT;
 469     }
 470
 471     // beta numerator:
 472     e = Ax*Cy - Ay*Cx;
 473     // beta tests:
 474     if (f > 0)
 475     {
 476         if (e < 0 || e > f) return DONT_INTERSECT;
 477     }
 478     else
 479     {
 480         if (e > 0 || e < f) return DONT_INTERSECT;
 481     }
 482
 483     // compute intersection coordinates:
 484
 485     if (f == 0) return PARALLEL;
 486
 487     // Numerator:
 488     num = d*Ax;
 489     // Round direction:
 490     offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
 491     // Intersection X:
 492     *x = a1.x + (num+offset) / f;
 493
 494     num = d*Ay;
 495     offset = SAME_SIGNS(num,f) ? f/2 : -f/2;
 496     // Intersection Y:
 497     *y = a1.y + (num+offset) / f;
 498
 499     return DO_INTERSECT;
 500 }
 501
 502
 503 }
 504