# ORourke.java :  » Java-3D » jinngine » jinngine » geometry » util » Java Open Source

 Java Open Source » Java 3D » jinngine
 jinngine » jinngine » geometry » util » ORourke.java ``````/** * Copyright (c) 2008-2010 Morten Silcowitz. * * This file is part of the Jinngine physics library * * Jinngine is published under the GPL license, available * at http://www.gnu.org/copyleft/gpl.html. */ package jinngine.geometry.util; import java.util.Iterator; import java.util.List; import jinngine.math.Vector3; /** * 2D convex convex intersection. Algorithm due to O'Rourke et al. 1981 "A New Linear * Algorithm for Intersecting Polygons". Intersection is done in the XY plane, the Z component * is completely ignored. However, the returned intersection will contain the appropriate Z values * for each vertex of the returned polygon, including vertices originating from edge-edge intersections. */ public class ORourke { /** * Handler for obtaining the result from a 2d intersection. Intersections will be reported in * counter clock-wise order, wrt. the positive Z axis */ public interface ResultHandler { /** * Called for each point in the intersection result. p will be the point on P and * like wise q will be the point on Q. The xy coordinates obviously be the same for * p and q, but the z-value may differ. */ public void intersection(Vector3 p, Vector3 q); } private final static double epsilon = 1e-8; /** * Internal algorithm state */ private enum State { none, P, Q }; /** * Perform XY-plane intersection of poly1 and poly2. The given polygons must be in counter clockwise order. * @param poly1 A closed polygon in the XY plane * @param poly2 A closed polygon in the XY plane * @param result Intersection of poly1 and poly2 in counter clock wise order */ public static void run( final List poly1, final List poly2, final List result ) { run( poly1, poly2, new ResultHandler() { public void intersection(Vector3 arg0, Vector3 arg1) { if (arg1 == null) result.add( new Vector3( arg0.x, arg0.y, arg0.z)); else result.add( new Vector3( arg1.x, arg1.y, arg1.z)); } }); } /** * @param poly1 * @param poly2 * @param result */ public static void run( final List poly1, final List poly2, final ResultHandler result ) { // transform polygons into projection space, leave for now int iter = 0; int intersections = 0; int addedpoints = 0; int firstIntersectionIter = 0; State inside = State.none; final Vector3 p = new Vector3(), pm = new Vector3(), pd = new Vector3(); final Vector3 q = new Vector3(), qm = new Vector3(), qd = new Vector3(); final Vector3 parameter = new Vector3(); final Vector3 firstIntersection = new Vector3(); final Vector3 firstAddedPoint = new Vector3(); final Vector3 previouslyAddedPoint = new Vector3(); final Vector3 ipp = new Vector3(); final Vector3 ipq = new Vector3(); // final Vector3 pnormal = new Vector3(); // final Vector3 qnormal = new Vector3(); final int N = poly1.size(); final int M = poly2.size(); // get iterators Iterator poly1points = poly1.iterator(); Iterator poly2points = poly2.iterator(); // System.out.println(N+","+M); // trival cases // if any one polygon is empty, so is intersection if (N == 0 || M == 0) return; // point-point case if ( N==1 && M == 1) { p.assign(poly1points.next()); q.assign(poly2points.next()); // report intersection if ( p.sub(q).xynorm() < epsilon) result.intersection(p, q); return; } // point-line case if ( N==1 && M == 2) { Vector3 x = poly1points.next(); Vector3 p1 = poly2points.next(); Vector3 p2 = poly2points.next(); Vector3 lp = new Vector3(); if ( pointLineIntersection(x, p1, p2, lp)) result.intersection(x, lp); return; } // line-point case if ( N==2 && M == 1) { Vector3 x = poly2points.next(); Vector3 p1 = poly1points.next(); Vector3 p2 = poly1points.next(); Vector3 lp = new Vector3(); if ( pointLineIntersection(x, p1, p2, lp)) result.intersection(lp, x); return; } // point-poly case if (N==1 && M > 2) { Vector3 x = poly1points.next(); if (isContained(x, poly2points)) { // polygon normal in 3d Vector3 polypoint = poly2.get(0); final Vector3 poly2normal = new Vector3(); polyNormal(poly2, poly2normal); // report result.intersection(x, projectToPlane(x, poly2normal, polypoint)); } return; } // poly-point case if (N>2 && M==1) { Vector3 x = poly2points.next(); if (isContained(x, poly1points)) { // polygon normal in 3d Vector3 polypoint = poly1.get(0); final Vector3 poly1normal = new Vector3(); polyNormal(poly1, poly1normal); // report result.intersection(projectToPlane(x, poly1normal, polypoint),x); } return; } // line-line case if (N==2 && M==2) { final Vector3 p1 = poly1points.next(); final Vector3 p2 = poly1points.next(); final Vector3 p3 = poly2points.next(); final Vector3 p4 = poly2points.next(); final Vector3 par = new Vector3(); // alternative version // the two lines are // l1(t) = p1 + (p2-p1)t // l2(s) = p3 + (p4-p3)s // if lines are orthogonal, use the unique intersection test (this is rare) final double p4p3Tp2p1 = p4.sub(p3).xydot(p2.sub(p1)); if ( Math.abs(p4p3Tp2p1) < epsilon) { lineLineIntersection(p1, p2, p3, p4, par, epsilon); // intersection in internal part of lines? if (par.x>=-epsilon && par.x <=1+epsilon && par.y>=-epsilon && par.y<=1+epsilon) { // report intersection result.intersection(p1.add(p2.sub(p1).multiply(par.x)), p3.add(p4.sub(p3).multiply(par.y))); // done return; } else { // no intersection return; } } // we require (p4-p3)T(p2-p1) > 0. if not, swap p3 and p4 else if ( p4p3Tp2p1<0) { // swap Vector3 tmp = new Vector3(p3); p3.assign(p4); p4.assign(tmp); } final double e = 0.1; final Vector3 p4p3 = p4.sub(p3); final Vector3 p2p1 = p2.sub(p1); final Vector3 p3p1 = p3.sub(p1); // d is the normalised tangent of l2 final double p4p3n =p4p3.xynorm(); final Vector3 d = new Vector3( -p4p3.y/p4p3n, p4p3.x/p4p3n, 0 ); // if lines are not parallel, we can compute two points on l1 where // its distance to l2 is equal to e final double z = p2p1.dot(d); double tlow; double thigh; if (Math.abs(z)>epsilon) { // TODO include derivation tlow = (-e+p3p1.xydot(d))/z; thigh = (e+p3p1.xydot(d))/z; // enforce t1 s final double k1 = (1/(p4p3n*p4p3n))*p4p3.xydot(p1.sub(p3)); final double k2 = (1/(p4p3n*p4p3n))*p4p3.xydot(p2p1); // System.out.println("(k1,k2)=("+k1+","+k2+")"); // all candidate points end-points final double slow = k1+k2*tlow; final double st0 = k1; final double s0 = 0; final double shigh = k1+k2*thigh; final double st1 = k1+k2; final double s1 = 1; // highest possible plow double plow = slow>st0? (slow>s0? slow:s0) : (st0>s0? st0:s0); // smallest possible phigh double phigh = shigh2) { final Vector3 p1 = poly1points.next(); final Vector3 p2 = poly1points.next(); final Vector3 p3 = poly2points.next(); final Vector3 p4 = poly2points.next(); final Vector3 p5 = poly2points.next(); // counter clock-wise normal final Vector3 poly2normal = p5.sub(p3).cross(p3.sub(p4)); linePolyIntersection(p1, p2, poly2, poly2normal, result); return; } // poly-line if (N>2 && M==2) { // System.out.println("switch"); // turn arguments around and wrap result TODO, could be more effective? final Vector3 p1 = poly2points.next(); final Vector3 p2 = poly2points.next(); final Vector3 p3 = poly1points.next(); final Vector3 p4 = poly1points.next(); final Vector3 p5 = poly1points.next(); // counter clock-wise normal final Vector3 poly1normal = p5.sub(p3).cross(p3.sub(p4)); linePolyIntersection(p1, p2, poly1, poly1normal, new ResultHandler() { public final void intersection(Vector3 arg0, Vector3 arg1) { result.intersection(arg1, arg0); }}); return; } // if (poly1.size() < 2 || poly2.size() < 2 ) { // throw new IllegalArgumentException("Polygons must contain at least 2 vertices, N="+N+", M="+M ); // } // polygon normal in 3d final Vector3 poly1normal = new Vector3(); polyNormal(poly1, poly1normal); final Vector3 poly2normal = new Vector3(); polyNormal(poly2, poly2normal); // get end-vertices p.assign(poly1.get(poly1.size()-1)); q.assign(poly2.get(poly2.size()-1)); // first iteration pm.assign(p); qm.assign(q); // next vertices p.assign(poly1points.next()); q.assign(poly2points.next()); // deltas pd.assign( p.sub(pm)); qd.assign( q.sub(qm)); // run iterations while (true) { iter = iter+1; //System.out.println("iteration " + iter); // if intersection is in the interior of the lines if (lineLineIntersection(pm,p,qm,q,parameter,epsilon) ) { if (parameter.x >= 0 && parameter.x <= 1 && parameter.y >= 0 && parameter.y <= 1 ) { ipp.assign(pm.add( pd.multiply(parameter.x))); ipq.assign(qm.add( qd.multiply(parameter.y))); intersections = intersections+1; //System.out.println("computed intersection="+ipp); // check if intersection is the same point // as first time an intersection was encountered. This // means that the algorithm should terminate if (intersections > 1) { // the firstIntersectionIter condition makes the algorithm able to handle some // degenerate cases, as treated in the O'Rourke paper if (firstIntersection.sub(ipp).xynorm() < epsilon && firstIntersectionIter!=iter-1) { // termination //System.out.println("intersection termination"); return; } } else { // track the first discovered intersection firstIntersection.assign(ipp); firstIntersectionIter = iter; } // add intersection point if (testPoint(firstAddedPoint, previouslyAddedPoint, ipp, addedpoints)) { //System.out.println("point added from intersection"); result.intersection(ipp, ipq); addedpoints = addedpoints+1; } // determine inside setting if ( isInHalfplane( p, qm, q)) { inside = State.P; } else { inside = State.Q; } } } // advance q or p if (qd.cross(pd).z >= 0) { if (isInHalfplane(p, qm, q)) { //advance q if (inside == State.Q) //intersection.add(q.copy()); //addPoint(previouslyAddedPoint, q, result); if ( testPoint(firstAddedPoint, previouslyAddedPoint, q, addedpoints)) { result.intersection(projectToPlane(q, poly1normal, p), q); addedpoints = addedpoints+1; } qm.assign(q); // rewind iterator if (!poly2points.hasNext()) poly2points = poly2.iterator(); q.assign(poly2points.next()); qd.assign( q.sub(qm)); } else { //advance p if (inside == State.P) if ( testPoint(firstAddedPoint, previouslyAddedPoint, p, addedpoints)) { result.intersection(p, projectToPlane(p, poly2normal, q)); addedpoints = addedpoints+1; } pm.assign(p); // rewind iterator if (!poly1points.hasNext()) poly1points = poly1.iterator(); p.assign(poly1points.next()); pd.assign( p.sub(pm)); } } else { // qd X pd < 0 if (isInHalfplane(q, pm, p)) { //advance p if (inside == State.P) if ( testPoint(firstAddedPoint, previouslyAddedPoint, p, addedpoints)) { result.intersection(p, projectToPlane(p, poly2normal, q)); addedpoints = addedpoints+1; } pm.assign(p); // rewind iterator if (!poly1points.hasNext()) poly1points = poly1.iterator(); p.assign(poly1points.next()); pd.assign( p.sub(pm)); } else { //advance q if (inside == State.Q) if ( testPoint(firstAddedPoint, previouslyAddedPoint, q, addedpoints)) { result.intersection(projectToPlane(q, poly1normal, p), q); addedpoints = addedpoints+1; } qm.assign(q); // rewind iterator if (!poly2points.hasNext()) poly2points = poly2.iterator(); q.assign(poly2points.next()); qd.assign( q.sub(qm)); } } if (iter > 2*(N+M)) break; } // while true //System.out.println("separation or inclusion termination"); // if we end up here, the polygons is either // separated or contained inside each other if ( isContained(p, poly2.iterator()) && M > 2) { // add all points from P as intersection for (Vector3 pi: poly1) { result.intersection(pi, projectToPlane(pi, poly2normal, q)); } return; } else if ( isContained(q, poly1.iterator()) && N > 2) { // add all points from Q as intersection for (Vector3 qi: poly2) { result.intersection(projectToPlane(qi, poly1normal, p), qi); } return; } // P and Q are separated return; } /** * Private method that checks if a new point is equal to the previous point or the first point. The * method also updates the first and previous point vectors. It returns true if the point can be accepted * and false otherwise */ private static final boolean testPoint( Vector3 first, Vector3 prev, Vector3 point, int addedPoints ) { if (addedPoints < 1) { first.assign(point); prev.assign(point); return true; } else if ( addedPoints < 2 ) { if ( first.sub(point).xynorm()>epsilon) { prev.assign(point); return true; } else { return false; } } else { if ( first.sub(point).xynorm()>epsilon && prev.sub(point).xynorm()>epsilon ) { prev.assign(point); return true; } else { return false; } } } private static final boolean pointLineIntersection( Vector3 x, Vector3 p1, Vector3 p2, Vector3 intPoint) { // check point line-intersection // l(t) = p1 + (p2-p1)t // (l(t)-x)T(p2-p1) = 0 // l(t)T(p2-p1) - xT(p2-p1) = 0 // p1T(p2-p1) + (p2-p1)T(p2-p1) t - xT(p2-p1) = 0 // t = xT(p2-p1) - p1T(p2-p1) / (p2-p1)T(p2-p1) // t = (x-p1)T(p2-p1) / |(p2-p1)|^2 final Vector3 p2p1 = p2.sub(p1); p2p1.z = 0; // we only work in the xy plane final double t = x.sub(p1).dot(p2p1) / p2p1.squaredNorm(); if (t>= -epsilon && t <= 1+epsilon) { // closest point on line Vector3 lp = p1.add(p2.sub(p1).multiply(t)); if ( lp.sub(x).xynorm() < epsilon ) { intPoint.assign(lp); return true; } } return false; } private static final void linePolyIntersection( Vector3 p1, Vector3 p2, List poly, Vector3 polynormal, ResultHandler result ) { // we maintain two candidate points final Vector3 out1 = new Vector3(); final Vector3 out2 = new Vector3(); boolean keepp1 = true; boolean keepp2 = true; int intersections = 0; final int N = poly.size(); int iter = 0; final Vector3 qm = new Vector3(); final Vector3 q = new Vector3(poly.get(N-1)); final Vector3 polypoint = new Vector3(q); final Vector3 param = new Vector3(); while (iter epsilon) { out1.assign(ipp); //report result.intersection(ipp, ipq); intersections++; } break; case 1: if (out2.sub(ipp).norm() > epsilon) { out2.assign(ipp); //report result.intersection(ipp, ipq); intersections++; } break; } if (intersections > 1) { // terminate return; } } } else { // poly edge is parallel with line. There is a special case where the line // is coincident with and edge of the poly } // go forward to next edge iter++; } // if no intersections, check if boundary line points have survived if (intersections < 1) { if (keepp1 && keepp2) { // return both end-points result.intersection(p1, projectToPlane(p1, polynormal, polypoint)); result.intersection(p2, projectToPlane(p2, polynormal, polypoint)); return; } if (!keepp1 && !keepp2) { // empty intersection return; } //if here, only one boundary point survived, which should not happen. // We default to separation in this case return; } if (intersections < 2) { if (keepp1) { // return p1 result.intersection(p1, projectToPlane(p1, polynormal, polypoint)); return; } else if (keepp2) { // report p2 result.intersection(p2, projectToPlane(p2, polynormal, polypoint)); return; } else { // no boundary points return; } } } /** * assign to projected the point that results from projecting point onto the plane defined by normal and ref, * along the positive z axis */ private static final Vector3 projectToPlane( Vector3 point, Vector3 normal, Vector3 ref) { return point.add(0,0,ref.sub(point).dot(normal)/normal.z); } /** * Return true if a is in the positive half-plane define by the two points bs->bt. * @param a * @param bs * @param bt * @return */ public static final boolean isInHalfplane(final Vector3 a, final Vector3 bs, final Vector3 bt) { return (bt.sub(bs)).cross(a.sub(bs)).z >= 0; } /** * Return the counter clock-wise normal of the given polygon * @param poly * @param normal */ private static final void polyNormal(List poly, Vector3 normal) { final Vector3 p3 = poly.get(0); final Vector3 p4 = poly.get(1); final Vector3 p5 = poly.get(2); // counter clock-wise normal final Vector3 poly1normal = p5.sub(p3).cross(p3.sub(p4)); // return normal.assign(poly1normal.normalize()); } /** * Return true if p is contained inside poly. Poly is required to contain at least 3 affine independent points * @param p * @param poly * @return */ public static final boolean isContained( final Vector3 p, final Iterator poly ) { Vector3 p0 = poly.next(); Vector3 pm = p0; // test each edge while(poly.hasNext()) { Vector3 pi = poly.next(); if (!isInHalfplane(p, pm, pi)) return false; pm = pi; } // test last edge from final vertex to // the first vertex, closing the polygon if (!isInHalfplane(p, pm, p0)) return false; // all tests passed return true; } /** * Intersect lines in 2d. Lines are given by * p(a) = ps + (pt-ps)a * q(b) = qs + (qt-qs)b * * Returns the values of a and b at the intersection point * in the (x,y) values in given vector st. If lines p and q are * parallel or overlapping along a line, the function returns false. * Otherwise the return value is true * * @param ps Starting point for line p * @param pt End-point for line p * @param qs Starting point for line q * @param qt End-point for line q * @param st Return vector for parameter values at intersection * @param epsilon Precision value. * @return True if result is well defined. False solution is non-existent or * not unique */ public static final boolean lineLineIntersection( final Vector3 ps, final Vector3 pt, final Vector3 qs, final Vector3 qt, final Vector3 st, final double epsilon ) { // intersect edges in 2d // // p(a) = pm + a (p-pm) // q(b) = qm + b (q-qm) // we want to know a and b so p(a) = q(b) // rewriting // pm + a(p-pm) = qm + b(q-qm) // a(p-pm) - b(q-qm) = qm - pm // [ (p-pm) (qm-q) ] [a,b] = [qm-pm] // take inverse and obtain a and b. // inverse of 2d matrix is // [c d]^-1 [ f -d] // [e f] = (cf-ed)^-1 [-e c] // so // [a] [ pd.x qd.x ]^-1 // [b] = [ pd.y qd.y ] (qm-pm) // and then // a = 1/(pd.x*qd.y-pd.y*qd.x)( qd.y*b.x + (-qd.x)*b.y ); // b = 1/(pd.x*qd.y-pd.y*qd.x)( (-pd.y)*b.x + pd.x*b.y ); // line deltas Vector3 pd = pt.sub(ps); Vector3 qd = qs.sub(qt); // turned around, see derivation // the b-side in the equation in the comments above Vector3 b = qs.sub(ps); // determinant calculation double det = pd.x*qd.y-pd.y*qd.x; // ill-posed problem? if (Math.abs(det)