diff options
Diffstat (limited to 'src/3rdparty/adaptagrams/libavoid/geometry.cpp')
| -rw-r--r-- | src/3rdparty/adaptagrams/libavoid/geometry.cpp | 641 |
1 files changed, 641 insertions, 0 deletions
diff --git a/src/3rdparty/adaptagrams/libavoid/geometry.cpp b/src/3rdparty/adaptagrams/libavoid/geometry.cpp new file mode 100644 index 0000000..7f50800 --- /dev/null +++ b/src/3rdparty/adaptagrams/libavoid/geometry.cpp @@ -0,0 +1,641 @@ +/* + * vim: ts=4 sw=4 et tw=0 wm=0 + * + * libavoid - Fast, Incremental, Object-avoiding Line Router + * + * Copyright (C) 2004-2011 Monash University + * + * -------------------------------------------------------------------- + * Much of the code in this module is based on code published with + * and/or described in "Computational Geometry in C" (Second Edition), + * Copyright (C) 1998 Joseph O'Rourke <orourke@cs.smith.edu> + * -------------------------------------------------------------------- + * The segmentIntersectPoint function is based on code published and + * described in Franklin Antonio, Faster Line Segment Intersection, + * Graphics Gems III, p. 199-202, code: p. 500-501. + * -------------------------------------------------------------------- + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * See the file LICENSE.LGPL distributed with the library. + * + * Licensees holding a valid commercial license may use this file in + * accordance with the commercial license agreement provided with the + * library. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * Author(s): Michael Wybrow +*/ + + +// For M_PI: +#define _USE_MATH_DEFINES +#include <cmath> + +#include <limits> + +#include "libavoid/graph.h" +#include "libavoid/geometry.h" +#include "libavoid/assertions.h" + + +namespace Avoid { + + +// Returns true iff the point c lies on the closed segment ab. +// To be used when the points are known to be collinear. +// +// Based on the code of 'Between'. +// +bool inBetween(const Point& a, const Point& b, const Point& c) +{ + double epsilon = std::numeric_limits<double>::epsilon(); + + // We only call this when we know the points are collinear, + // otherwise we should be checking this here. + COLA_ASSERT(vecDir(a, b, c, epsilon) == 0); + + if (fabs(a.x - b.x) > epsilon) + { + // not vertical + return (((a.x < c.x) && (c.x < b.x)) || + ((b.x < c.x) && (c.x < a.x))); + } + else + { + return (((a.y < c.y) && (c.y < b.y)) || + ((b.y < c.y) && (c.y < a.y))); + } +} + + +// Returns true iff the three points are colinear. +// +bool colinear(const Point& a, const Point& b, const Point& c, + const double tolerance) +{ + + // Do this a bit more optimally for orthogonal AB line segments. + if (a == b) + { + return true; + } + else if (a.x == b.x) + { + return (a.x == c.x); + } + else if (a.y == b.y) + { + return (a.y == c.y); + } + + // Or use the general case. + return (vecDir(a, b, c, tolerance) == 0); + +} + + +// Returns true iff the point c lies on the closed segment ab. +// +bool pointOnLine(const Point& a, const Point& b, const Point& c, + const double tolerance) +{ + // Do this a bit more optimally for orthogonal AB line segments. + if (a.x == b.x) + { + return (a.x == c.x) && + (((a.y < c.y) && (c.y < b.y)) || + ((b.y < c.y) && (c.y < a.y))); + } + else if (a.y == b.y) + { + return (a.y == c.y) && + (((a.x < c.x) && (c.x < b.x)) || + ((b.x < c.x) && (c.x < a.x))); + } + + // Or use the general case. + return (vecDir(a, b, c, tolerance) == 0) && inBetween(a, b, c); +} + + +// Returns true if the segment cd intersects the segment ab, blocking +// visibility. +// +// Based on the code of 'IntersectProp' and 'Intersect'. +// +bool segmentIntersect(const Point& a, const Point& b, const Point& c, + const Point& d) +{ + int ab_c = vecDir(a, b, c); + if (ab_c == 0) + { + return false; + } + + int ab_d = vecDir(a, b, d); + if (ab_d == 0) + { + return false; + } + + // It's ok for either of the points a or b to be on the line cd, + // so we don't have to check the other two cases. + + int cd_a = vecDir(c, d, a); + int cd_b = vecDir(c, d, b); + + // Is an intersection if a and b are on opposite sides of cd, + // and c and d are on opposite sides of the line ab. + // + // Note: this is safe even though the textbook warns about it + // since, unlike them, our vecDir is equivilent to 'AreaSign' + // rather than 'Area2'. + return (((ab_c * ab_d) < 0) && ((cd_a * cd_b) < 0)); +} + + +// Returns true if the segment e1-e2 intersects the shape boundary +// segment s1-s2, blocking visibility. +// +bool segmentShapeIntersect(const Point& e1, const Point& e2, const Point& s1, + const Point& s2, bool& seenIntersectionAtEndpoint) +{ + if (segmentIntersect(e1, e2, s1, s2)) + { + // Basic intersection of segments. + return true; + } + else if ( (((s2 == e1) || pointOnLine(s1, s2, e1)) && + (vecDir(s1, s2, e2) != 0)) + || + (((s2 == e2) || pointOnLine(s1, s2, e2)) && + (vecDir(s1, s2, e1) != 0)) ) + { + // Segments intersect at the endpoint of one of the segments. We + // allow this once, but the second one blocks visibility. Otherwise + // shapes butted up against each other could have visibility through + // shapes. + if (seenIntersectionAtEndpoint) + { + return true; + } + seenIntersectionAtEndpoint = true; + } + return false; +} + + +// Returns true iff the point p in a valid region that can contain +// shortest paths. a0, a1, a2 are ordered vertices of a shape. +// +// Based on the code of 'InCone'. +// +bool inValidRegion(bool IgnoreRegions, const Point& a0, const Point& a1, + const Point& a2, const Point& b) +{ + // r is a0--a1 + // s is a1--a2 + + int rSide = vecDir(b, a0, a1); + int sSide = vecDir(b, a1, a2); + + bool rOutOn = (rSide <= 0); + bool sOutOn = (sSide <= 0); + + bool rOut = (rSide < 0); + bool sOut = (sSide < 0); + + if (vecDir(a0, a1, a2) > 0) + { + // Convex at a1: + // + // !rO rO + // sO sO + // + // ---s---+ + // | + // !rO r rO + // !sO | !sO + // + // + if (IgnoreRegions) + { + return (rOutOn && !sOut) || (!rOut && sOutOn); + } + return (rOutOn || sOutOn); + } + else + { + // Concave at a1: + // + // !rO rO + // !sO !sO + // + // +---s--- + // | + // !rO r rO + // sO | sO + // + // + return (IgnoreRegions ? false : (rOutOn && sOutOn)); + } +} + + +// Gives the side of a corner that a point lies on: +// 1 anticlockwise +// -1 clockwise +// e.g. /|s2 +// /s3 -1 / | +// / / | +// 1 |s2 -1 / 1 | -1 +// | / | +// |s1 s3/ |s1 +// +int cornerSide(const Point &c1, const Point &c2, const Point &c3, + const Point& p) +{ + int s123 = vecDir(c1, c2, c3); + int s12p = vecDir(c1, c2, p); + int s23p = vecDir(c2, c3, p); + + if (s123 == 1) + { + if ((s12p >= 0) && (s23p >= 0)) + { + return 1; + } + return -1; + } + else if (s123 == -1) + { + if ((s12p <= 0) && (s23p <= 0)) + { + return -1; + } + return 1; + } + + // c1-c2-c3 are collinear, so just return vecDir from c1-c2 + return s12p; +} + + +// Returns the Euclidean distance between points a and b. +// +double euclideanDist(const Point& a, const Point& b) +{ + double xdiff = a.x - b.x; + double ydiff = a.y - b.y; + + return sqrt((xdiff * xdiff) + (ydiff * ydiff)); +} + +// Returns the Manhattan distance between points a and b. +// +double manhattanDist(const Point& a, const Point& b) +{ + return fabs(a.x - b.x) + fabs(a.y - b.y); +} + + +// Returns the Euclidean distance between points a and b. +// +double dist(const Point& a, const Point& b) +{ + double xdiff = a.x - b.x; + double ydiff = a.y - b.y; + + return sqrt((xdiff * xdiff) + (ydiff * ydiff)); +} + +// Returns the total length of all line segments in the polygon +double totalLength(const Polygon& poly) +{ + double l = 0; + for (size_t i = 1; i < poly.size(); ++i) + { + l += dist(poly.ps[i-1], poly.ps[i]); + } + return l; +} + +// Uses the dot-product rule to find the angle (radians) between ab and bc +double angle(const Point& a, const Point& b, const Point& c) +{ + double ux = b.x - a.x, + uy = b.y - a.y, + vx = c.x - b.x, + vy = c.y - b.y, + lu = sqrt(ux*ux+uy*uy), + lv = sqrt(vx*vx+vy*vy), + udotv = ux * vx + uy * vy, + costheta = udotv / (lu * lv); + return acos(costheta); +} + +// Returns true iff the point q is inside (or on the edge of) the +// polygon argpoly. +// +// This is a fast version that only works for convex shapes. The +// other version (inPolyGen) is more general. +// +bool inPoly(const Polygon& poly, const Point& q, bool countBorder) +{ + size_t n = poly.size(); + const std::vector<Point>& P = poly.ps; + bool onBorder = false; + for (size_t i = 0; i < n; i++) + { + // point index; i1 = i-1 mod n + size_t prev = (i + n - 1) % n; + int dir = vecDir(P[prev], P[i], q); + if (dir == -1) + { + // Point is outside + return false; + } + // Record if point was on a boundary. + onBorder |= (dir == 0); + } + if (!countBorder && onBorder) + { + return false; + } + return true; +} + + +// Returns true iff the point q is inside (or on the edge of) the +// polygon argpoly. +// +// Based on the code of 'InPoly'. +// +bool inPolyGen(const PolygonInterface& argpoly, const Point& q) +{ + // Numbers of right and left edge/ray crossings. + int Rcross = 0; + int Lcross = 0; + + // Copy the argument polygon + Polygon poly = argpoly; + std::vector<Point>& P = poly.ps; + size_t n = poly.size(); + + // Shift so that q is the origin. This is done for pedagogical clarity. + for (size_t i = 0; i < n; ++i) + { + P[i].x = P[i].x - q.x; + P[i].y = P[i].y - q.y; + } + + // For each edge e=(i-1,i), see if crosses ray. + for (size_t i = 0; i < n; ++i) + { + // First see if q=(0,0) is a vertex. + if ((P[i].x == 0) && (P[i].y == 0)) + { + // We count a vertex as inside. + return true; + } + + // point index; i1 = i-1 mod n + size_t i1 = ( i + n - 1 ) % n; + + // if e "straddles" the x-axis... + // The commented-out statement is logically equivalent to the one + // following. + // if( ((P[i].y > 0) && (P[i1].y <= 0)) || + // ((P[i1].y > 0) && (P[i].y <= 0)) ) + + if ((P[i].y > 0) != (P[i1].y > 0)) + { + // e straddles ray, so compute intersection with ray. + double x = (P[i].x * P[i1].y - P[i1].x * P[i].y) + / (P[i1].y - P[i].y); + + // crosses ray if strictly positive intersection. + if (x > 0) + { + Rcross++; + } + } + + // if e straddles the x-axis when reversed... + // if( ((P[i].y < 0) && (P[i1].y >= 0)) || + // ((P[i1].y < 0) && (P[i].y >= 0)) ) + + if ((P[i].y < 0) != (P[i1].y < 0)) + { + // e straddles ray, so compute intersection with ray. + double x = (P[i].x * P[i1].y - P[i1].x * P[i].y) + / (P[i1].y - P[i].y); + + // crosses ray if strictly positive intersection. + if (x < 0) + { + Lcross++; + } + } + } + + // q on the edge if left and right cross are not the same parity. + if ( (Rcross % 2) != (Lcross % 2) ) + { + // We count the edge as inside. + return true; + } + + // Inside iff an odd number of crossings. + if ((Rcross % 2) == 1) + { + return true; + } + + // Outside. + return false; +} + + + +// Line Segment Intersection +// Original code by Franklin Antonio +// +// The SAME_SIGNS macro assumes arithmetic where the exclusive-or +// operation will work on sign bits. This works for twos-complement, +// and most other machine arithmetic. +#define SAME_SIGNS( a, b ) \ + (((long) ((unsigned long) a ^ (unsigned long) b)) >= 0 ) +// +int segmentIntersectPoint(const Point& a1, const Point& a2, + const Point& b1, const Point& b2, double *x, double *y) +{ + double Ax,Bx,Cx,Ay,By,Cy,d,e,f,num; + double x1lo,x1hi,y1lo,y1hi; + + Ax = a2.x - a1.x; + Bx = b1.x - b2.x; + + // X bound box test: + if (Ax < 0) + { + x1lo = a2.x; + x1hi = a1.x; + } + else + { + x1hi = a2.x; + x1lo = a1.x; + } + if (Bx > 0) + { + if (x1hi < b2.x || b1.x < x1lo) return DONT_INTERSECT; + } + else + { + if (x1hi < b1.x || b2.x < x1lo) return DONT_INTERSECT; + } + + Ay = a2.y - a1.y; + By = b1.y - b2.y; + + // Y bound box test: + if (Ay < 0) + { + y1lo = a2.y; + y1hi = a1.y; + } + else + { + y1hi = a2.y; + y1lo = a1.y; + } + if (By > 0) + { + if (y1hi < b2.y || b1.y < y1lo) return DONT_INTERSECT; + } + else + { + if (y1hi < b1.y || b2.y < y1lo) return DONT_INTERSECT; + } + + Cx = a1.x - b1.x; + Cy = a1.y - b1.y; + // alpha numerator: + d = By*Cx - Bx*Cy; + // Both denominator: + f = Ay*Bx - Ax*By; + // alpha tests: + if (f > 0) + { + if (d < 0 || d > f) return DONT_INTERSECT; + } + else + { + if (d > 0 || d < f) return DONT_INTERSECT; + } + + // beta numerator: + e = Ax*Cy - Ay*Cx; + // beta tests: + if (f > 0) + { + if (e < 0 || e > f) return DONT_INTERSECT; + } + else + { + if (e > 0 || e < f) return DONT_INTERSECT; + } + + // compute intersection coordinates: + + if (f == 0) return PARALLEL; + + // Numerator: + num = d*Ax; + // Intersection X: + *x = a1.x + (num) / f; + + num = d*Ay; + // Intersection Y: + *y = a1.y + (num) / f; + + return DO_INTERSECT; +} + + +// Line Segment Intersection +// Original code by Franklin Antonio +// +int rayIntersectPoint(const Point& a1, const Point& a2, + const Point& b1, const Point& b2, double *x, double *y) +{ + double Ax,Bx,Cx,Ay,By,Cy,d,f,num; + + Ay = a2.y - a1.y; + By = b1.y - b2.y; + Ax = a2.x - a1.x; + Bx = b1.x - b2.x; + + Cx = a1.x - b1.x; + Cy = a1.y - b1.y; + // alpha numerator: + d = By*Cx - Bx*Cy; + // Both denominator: + f = Ay*Bx - Ax*By; + + // compute intersection coordinates: + + if (f == 0) return PARALLEL; + + // Numerator: + num = d*Ax; + // Intersection X: + *x = a1.x + (num) / f; + + num = d*Ay; + // Intersection Y: + *y = a1.y + (num) / f; + + return DO_INTERSECT; +} + +// Returns the rotationalAngle, between 0 and 360, of this point from (0,0). +// +double rotationalAngle(const Point& p) +{ + if (p.y == 0) + { + return ((p.x < 0) ? 180 : 0); + } + else if (p.x == 0) + { + return ((p.y < 0) ? 270 : 90); + } + + double ang = atan(p.y / p.x); + ang = (ang * 180) / M_PI; + + if (p.x < 0) + { + ang += 180; + } + else if (p.y < 0) + { + ang += 360; + } + COLA_ASSERT(ang >= 0); + COLA_ASSERT(ang <= 360); + + return ang; +} + + +} + |