IntersectPlanarPolygons.java

/*-
 *  Copyright 2020 Beuth Hochschule für Technik Berlin, Hochschule für Technik Stuttgart
 * 
 *  This file is part of CityDoctor2.
 *
 *  CityDoctor2 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 3 of the License, or
 *  (at your option) any later version.
 *
 *  CityDoctor2 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.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with CityDoctor2.  If not, see <https://www.gnu.org/licenses/>.
 */
package de.hft.stuttgart.citydoctor2.edge;

import java.util.ArrayList;
import java.util.Collections;
import java.util.IdentityHashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.TreeSet;

public class IntersectPlanarPolygons {

	private static final double FULLY_EMBEDDED_TOLERANCE = Global.getHighAccuracyTolerance() * 1e2;
	private static final double PROJECTED_POINT_DISTANCE_EPSILON = 1e-8;

	public static List<PolygonPolygonIntersection> intersectPolygons(EdgePolygon p1, EdgePolygon p2, double epsilon,
			double angleEpsilon) {
		GmPlane plane1 = p1.getPlane();
		GmPlane plane2 = p2.getPlane();

		GmStraight straight = plane1.planeIntersection(plane2);
		if (straight == null) {
			// planes are parallel
			double dist1 = plane1.getDistance();
			double dist2 = plane2.getDistance();
			if (Vector3d.areAntiParallel(plane1.getNormal(), plane2.getNormal())) {
				dist2 *= -1;
			}
			if (Math.abs(dist1 - dist2) > epsilon) {
				// not in the same plane
				return Collections.emptyList();
			} else {
				// planes are equal
				return handlePolygonsInSamePlane(plane1, p1, p2, epsilon);
			}

		}

		// plane is intersecting
		// handle embedded edges
		List<PolygonPolygonIntersection> intersections = new ArrayList<>();
		handleEmbeddedEdges(p1, p2, straight, intersections, epsilon, angleEpsilon);

		List<Interval> intervals1 = calculateIntersectionIntervals2d(p1, p1, p2, straight, epsilon, angleEpsilon);
		if (intervals1.isEmpty()) {
			// no intersection
			return intersections;
		}

		List<Interval> intervals2 = calculateIntersectionIntervals2d(p2, p1, p2, straight, epsilon, angleEpsilon);
		if (intervals2.isEmpty()) {
			// no intersection
			return intersections;
		}

		createIntersectionResults(p1, p2, intervals1, intervals2, straight, intersections, epsilon);
		return intersections;
	}

	private static void createIntersectionResults(EdgePolygon cpPolygon1, EdgePolygon cpPolygon2,
			List<Interval> intervals1, List<Interval> intervals2, GmStraight straight,
			List<PolygonPolygonIntersection> intersections, double epsilon) {
		for (Interval in1 : intervals1) {
			for (Interval in2 : intervals2) {
				if (in1.isOverlapping(in2)) {
					Interval overlap = in1.overlap(in2);
					Point3d start = straight.evaluate(overlap.getStart());
					if (overlap.getLength() < Global.getHighAccuracyTolerance() * 1e6) {
						checkAndAddRealIntersectionPoint(cpPolygon1, cpPolygon2, start, intersections, epsilon);
					} else {
						Point3d end = straight.evaluate(overlap.getEnd());
						PolyLine pPL = new PolyLine(new Coordinate3d(start), new Coordinate3d(end));

						if (!isPolyLineASharedEdge(cpPolygon1, pPL)) {
							PolygonPolygonIntersection pPPI = new PolygonPolygonIntersection(cpPolygon1, cpPolygon2,
									pPL);
							intersections.add(pPPI);
						}

					}
				} else if (Math.abs(in1.getStart() - in2.getEnd()) < Global.getHighAccuracyTolerance()) {
					// check if the overlaps with a tolerance (numeric errors)
					Point3d point = straight.evaluate(in1.getStart());
					checkAndAddRealIntersectionPoint(cpPolygon1, cpPolygon2, point, intersections, epsilon);
				} else if (Math.abs(in1.getEnd() - in2.getStart()) < Global.getHighAccuracyTolerance()) {
					Point3d point = straight.evaluate(in1.getEnd());
					checkAndAddRealIntersectionPoint(cpPolygon1, cpPolygon2, point, intersections, epsilon);
				}
			}
		}
	}

	private static boolean isPolyLineASharedEdge(EdgePolygon p1, PolyLine polyLine) {
		// get coordinates of the poly line
		Point3d polyLineStart = polyLine.getStart().getPoint();
		Point3d polyLineEnd = polyLine.getEnd().getPoint();

		// check if there exists a HalfEdge with same coordinates
		List<HalfEdge> polyHEs = p1.getHalfEdges();
		for (HalfEdge he : polyHEs) {
			// only check half edges with partners. NOTE: we are just looking
			// for half edges, shared by both polygons!
			if (he.getPartner() != null) {
				Point3d start = he.getStart().getPoint();
				Point3d end = he.getEnd().getPoint();
				if ((start.isAlmostEqual(polyLineStart, 1e-6) && end.isAlmostEqual(polyLineEnd, 1e-6))
						|| (start.isAlmostEqual(polyLineEnd, 1e-6) && end.isAlmostEqual(polyLineStart, 1e-6))) {
					// current half edge has a partner and the same start and
					// end points of the polyline -> intersection is the edge
					// which is shared by both polygons
					return true;
				}
			}
		}
		return false;
	}

	/**
	 * 
	 * Checks if the given point is a "real" intersection point and add's it to the
	 * intersection segments member variable. Real means, that this point isn't
	 * shared by both polygons as corner point.
	 * 
	 * @param point A possible intersection point
	 */
	private static void checkAndAddRealIntersectionPoint(EdgePolygon p1, EdgePolygon p2, Point3d point,
			List<PolygonPolygonIntersection> intersections, double epsilon) {
		// check if both polygons have this point in common
		if (!sharingBothPolygonsThisPoint(p1, p2, point, epsilon)) {
			PolyLine pPL = new PolyLine(new Coordinate3d(point), new Coordinate3d(point));
			PolygonPolygonIntersection pPPI = new PolygonPolygonIntersection(p1, p2, pPL);
			intersections.add(pPPI);
		}
	}

	/**
	 * 
	 * This method checks if the point is a border point of one of the polygons,
	 * which should be intersected. If this check is successfull, the method tries
	 * to find the same point in the other polygon.
	 * 
	 * <br>
	 * Equality in this case means, both polygons sharing the same instance of
	 * corner point! Otherwise this is just a util method to purge the code.
	 * 
	 * @param point A computed intersection point
	 * 
	 * @return true, if the point is shared by the member variables, holding the
	 *         given polygons
	 * 
	 */
	private static boolean sharingBothPolygonsThisPoint(EdgePolygon p1, EdgePolygon p2, Point3d point, double epsilon) {
		// search for point in one Polygon
		Coordinate3d pointWithSameCoordinates = null;
		List<HalfEdge> hl1 = p1.getHalfEdges();
		for (HalfEdge he1 : hl1) {
			Point3d currHEPoint = he1.getStart().getPoint();
			if ((currHEPoint.minus(point)).getLength() < epsilon) {
				pointWithSameCoordinates = he1.getStart();
			}
		}

		boolean bothPolygonsSharingThisPoint = false;

		// check if the second polygon shares the same point (same instance!!)
		if (null != pointWithSameCoordinates) {
			List<EdgePolygon> polygons = pointWithSameCoordinates.getPolygons();
			for (EdgePolygon poly : polygons) {
				if (p2 == poly) {
					bothPolygonsSharingThisPoint = true;
					break;
				}
			}
		}
		return bothPolygonsSharingThisPoint;
	}

	private static List<Interval> calculateIntersectionIntervals2d(EdgePolygon p, EdgePolygon cpPolygon1,
			EdgePolygon cpPolygon2, GmStraight intersectingStraight, double epsilon, double angleEpsilon) {
		List<Interval> intersectionIntervals = new ArrayList<>();
		TreeSet<Double> intersectionValues = new TreeSet<>(Global.getDoubleTolCompare(epsilon));
		TreeSet<Double> intersectedPolygonPoints = new TreeSet<>(Global.getDoubleTolCompare(epsilon));

		GmPlane polyPlane = p.getPlane();
		GmStraight2d intersectingStraight2d = polyPlane.projectOn2dStraight(intersectingStraight);

		EdgePolygon otherPolygon;
		if (cpPolygon1 == p) {
			otherPolygon = cpPolygon2;
		} else {
			otherPolygon = cpPolygon1;
		}

		List<HalfEdge> he = p.getHalfEdges();
		for (HalfEdge e : he) {
			if (e.getPartner() != null && e.getPartner().getPolygon() == otherPolygon) {
				// both polygons sharing this edge, no need to intersect it
				// NOTE: it's assumed, that the polygons aren't self intersecting
				continue;
			}

			GmBoundedStraight heStraight = e.getStraight();
			// Straights are colinear; checked in the handleEmdeddedEdges method
			if (heStraight.isColinear(intersectingStraight, angleEpsilon, epsilon)) {
				continue;
			}

			// Check if the current HalfEdge shares the start or end coordinate with
			// the other polygon
			boolean currHESharesAPntWithTheOtherPolygon = isHalfEdgeSharingAPointWithOtherPolygon(otherPolygon,
					e.getStart().getPolygons());
			if (!currHESharesAPntWithTheOtherPolygon) {
				List<EdgePolygon> endPolygons = e.getEnd().getPolygons();
				currHESharesAPntWithTheOtherPolygon = isHalfEdgeSharingAPointWithOtherPolygon(otherPolygon,
						endPolygons);
			}

			// check if the straight of the HalfEdge is collinear to the intersection
			// straight; this might result in an embedded edge intersection result
			// TODO MW: really needed? this check is also made some lines above
			if (heStraight.isColinear(intersectingStraight, angleEpsilon, epsilon)) {
				currHESharesAPntWithTheOtherPolygon = false;
			}

			// The current HalfEdge shares a point with the other polygon and is not
			// collinear with the intersection straight. Hence, there is now way, that
			// this HalfEdge can intersect the other one, because both are planar.
//			if (currHESharesAPntWithTheOtherPolygon) {
//				continue;
//			}

			// project straight on plane
			GmBoundedStraight2d heStraight2d = polyPlane.projectOn2dStraight(heStraight);

			GmStraight2dIntersectionResult intersectionResult = heStraight2d.intersect(intersectingStraight2d);
			if (intersectionResult.areParallel()) {
				Vector2d dir = intersectingStraight2d.getDirection();
				Vector2d diffVec = heStraight2d.getOrigin().minus(intersectingStraight2d.getOrigin());
				double diffVecDotPerpDir = diffVec.dot(dir.getPerpendicularVector());

				if (Math.abs(diffVecDotPerpDir) < Global.getZeroAngleCosinus()) {
					// Straights are identical
					Point2d p1 = heStraight2d.getOrigin();
					Point2d p2 = heStraight2d.getTarget();
					Point2d orig = intersectingStraight2d.getOrigin();

					double[] params = new double[2];

					if (Math.abs(dir.getX()) > Global.getHighAccuracyTolerance()) {
						params[0] = (p1.getX() - orig.getX()) / dir.getX();
						params[1] = (p2.getX() - orig.getX()) / dir.getX();
					} else if (Math.abs(dir.getY()) > Global.getHighAccuracyTolerance()) {
						params[0] = (p1.getY() - orig.getY()) / dir.getY();
						params[1] = (p2.getY() - orig.getY()) / dir.getY();
					} else {
						throw new IllegalStateException(
								"Directional vector of identical straights is equal to the zero vector " + dir);
					}

					assignParameterToCorrectList(params[0], intersectionValues, intersectedPolygonPoints);
					assignParameterToCorrectList(params[1], intersectionValues, intersectedPolygonPoints);
				}
			} else {
				if (heStraight2d.isWithinBoundaries(intersectionResult.getParamHE(), 1e-9)) {
					assignParameterToCorrectList(intersectionResult.getParamInt(), intersectionValues,
							intersectedPolygonPoints);
				}
			}
		}

		processIntersectionIntervals(p, intersectingStraight, intersectionValues, intersectedPolygonPoints,
				intersectionIntervals);
		return intersectionIntervals;

	}

	/**
	 * This methods computes the intervals of the intersection between the given
	 * straight and polygon. It gets only parameters where the straight hits the
	 * polygon. <br>
	 * The additional set of intersected polygon points is necessary, because each
	 * intersection of a corner point with the straight results in the computation
	 * of the same intersection parameter twice, because at every corner point of
	 * the polygon two edges are joining. These points has to be treated special.
	 * 
	 * @param pcPolygon                A polygon
	 * 
	 * @param intersectingStraight     The straight which have been intersected with
	 *                                 the given polygon
	 * 
	 * @param intersectionValues       The calculated parameters of the intersection
	 * 
	 * @param intersectedPolygonPoints A set of values, containing parameter values
	 *                                 where the straight intersects the polygon in
	 *                                 a corner point.
	 * 
	 * @param intersectionIntervals    A list of computed intersection intervals of
	 *                                 the straight with the polygon
	 */
	private static void processIntersectionIntervals(EdgePolygon pcPolygon, GmStraight intersectingStraight,
			TreeSet<Double> intersectionValues, TreeSet<Double> intersectedPolygonPoints,
			List<Interval> intersectionIntervals) {

		// insert points as intervals with length 0
		// insert a interval with zero length for every crossed point
		for (Double d : intersectedPolygonPoints) {
			intersectionIntervals.add(new Interval(d, d));
		}

		// create other intervals
		// create list for easier random access
		List<Double> valuesList = new ArrayList<>(intersectionValues);
		for (int i = 0; i < valuesList.size(); i++) {
			double i1 = valuesList.get(i);
			i = i + 1;
			if (i < valuesList.size()) {
				double i2 = valuesList.get(i);
				// check if the double values are corner points of the polygon
				boolean gotPolygonPoint = false;
				if (intersectedPolygonPoints.contains(i1) || intersectedPolygonPoints.contains(i2)) {
					gotPolygonPoint = true;
				}

				if (gotPolygonPoint) {
					// maybe an interval
					// check if the point between the two parameters is inside the
					// polygon or outside ( i.e. i1 and i2 are both corner points of
					// a concave polygon, so the connection line don't need to lie
					// inside the polygon )
					Point3d pnt = intersectingStraight.evaluate((i1 + i2) / 2);
					if (pcPolygon.isPointInsidePolygon(pnt, 1e-9)) {
						Interval newLineInt = new Interval(i1, i2);
						// there is already at least one point interval present. We need
						// to remove this first, otherwise we would get a normal interval
						// and one or two point intervals at the end of the normal int.
						Iterator<Interval> intervalIterator = intersectionIntervals.iterator();
						while (intervalIterator.hasNext() && !intersectionIntervals.isEmpty()) {
							for (Interval inter = intervalIterator.next(); intervalIterator
									.hasNext(); inter = intervalIterator.next()) {
								if (Math.abs(inter.getLength()) < Global.getHighAccuracyTolerance()) {
									if (Math.abs(inter.getStart() - i1) < 1e-9
											|| Math.abs(inter.getStart() - i2) < 1e-9) {
										intervalIterator.remove();
										intervalIterator = intersectionIntervals.iterator();
										break;
									}
								}
							}
						}

						intersectionIntervals.add(newLineInt);
					} else {
						i--;
					}
				} else {
					intersectionIntervals.add(new Interval(i1, i2));
				}
			}

			// it's possible, that 'it' equals 'endIt' at the end of the loop.
			// this would lead to an increment of an iterator pointing to the end
			// iterator. don't ask me why, but incrementing such an iterator
			// is forbidden and you get an assertion.
			if (i >= valuesList.size()) {
				break;
			}
		}
	}

	/**
	 * Util method. Checks if the given parameter hase been allready inserted as
	 * intersction parameter. If this is true, the parameter will be added as
	 * intersected polygon corner point. Otherwise it's just a normal intersection
	 * parameter till now.
	 * 
	 * @param param                    Calculated intersection parameter
	 * 
	 * @param intersectionValues       Set of intersection parameters
	 * 
	 * @param intersectedPolygonPoints Set of intersected polygon corner points
	 * 
	 */
	private static void assignParameterToCorrectList(double param, TreeSet<Double> intersectionValues,
			TreeSet<Double> intersectedPolygonPoints) {
		if (!intersectionValues.add(param)) {
			// value is allready present ==> this must be a point of the polygon
			// note, it's assumed, that the polygon do not intersect itself
			intersectedPolygonPoints.add(param);
		}
	}

	private static boolean isHalfEdgeSharingAPointWithOtherPolygon(EdgePolygon otherPolygon, List<EdgePolygon> polys) {
		boolean currHESharesAPntWithTheOtherPolygon = false;
		for (EdgePolygon polyIt : polys) {
			if (polyIt == otherPolygon) {
				currHESharesAPntWithTheOtherPolygon = true;
				break;
			}
		}
		return currHESharesAPntWithTheOtherPolygon;
	}

	private static void handleEmbeddedEdges(EdgePolygon p1, EdgePolygon p2, GmStraight straight,
			List<PolygonPolygonIntersection> intersections, double epsilon, double angleEpsilon) {
		// merge both lists
		List<HalfEdge> heList = new ArrayList<>(p1.getHalfEdges());
		heList.addAll(p2.getHalfEdges());

		List<HalfEdge> heListColinear = new ArrayList<>();
		for (HalfEdge he : heList) {

			if (null != he.getPartner()) {
				// this half edge is shared by both polygons ==> no intersection
				// NOTE: The issue of more than 2 half edges per edge is solved by
				// circular pointer, so we have to cycle threw all partners
				HalfEdge pStartHE = he;
				HalfEdge pPartnerHE = pStartHE.getPartner();
				boolean bothPolysShareThisEdge = false;
				while (pStartHE != pPartnerHE && !bothPolysShareThisEdge) {
					if (pPartnerHE.getPolygon() == p1 || pPartnerHE.getPolygon() == p2) {
						bothPolysShareThisEdge = true;
					}
					pPartnerHE = pPartnerHE.getPartner();
				}
				if (bothPolysShareThisEdge) {
					continue;
				}
			}

			GmBoundedStraight straightHe = he.getStraight();
			Point3d origin = straightHe.getOrigin();
			Point3d target = straightHe.getTarget();

			boolean straightsAreColinear = IntersectPlanarPolygons.areStraightsColinear(straightHe, straight, epsilon,
					angleEpsilon);
			ProjectedPoint3d projPoint1 = straight.project(origin);
			ProjectedPoint3d projPoint2 = straight.project(target);

			boolean originLiesOnIntStraight = projPoint1.getPoint().isAlmostEqual(origin,
					PROJECTED_POINT_DISTANCE_EPSILON);
			boolean targetLiesOnIntStraight = projPoint2.getPoint().isAlmostEqual(target,
					PROJECTED_POINT_DISTANCE_EPSILON);
			if (straightsAreColinear && (originLiesOnIntStraight || targetLiesOnIntStraight)) {
				heListColinear.add(he);
			}
		}

		for (HalfEdge he : heListColinear) {
			// 1.2) determine if fully or partially or not at all embedded
			// create parameter interval of the first projected half edge
			Point3d startPoint1 = he.getStraight().getOrigin();
			Point3d endPoint1 = he.getStraight().getTarget();
			ProjectedPoint3d projP1 = straight.project(startPoint1);
			ProjectedPoint3d projP2 = straight.project(endPoint1);
			Interval int1 = new Interval(projP1.getParameter(), projP2.getParameter());
			for (HalfEdge he2 : heListColinear) {
				if (he == he2) {
					continue;
				}
				Point3d startPoint2 = he2.getStraight().getOrigin();
				Point3d endPoint2 = he2.getStraight().getTarget();
				ProjectedPoint3d proj2P1 = straight.project(startPoint2);
				ProjectedPoint3d proj2P2 = straight.project(endPoint2);
				Interval int2 = new Interval(proj2P1.getParameter(), proj2P2.getParameter());

				Interval intOverlap = int1.overlap(int2);
				double intOverlapLength = intOverlap.getLength();
				if (intOverlapLength < Global.getHighAccuracyTolerance()) {
					// no overlap
					continue;
				}

				Coordinate3d polyLineStart = new Coordinate3d(straight.evaluate(intOverlap.getStart()));
				Coordinate3d polyLineEnd = new Coordinate3d(straight.evaluate(intOverlap.getEnd()));
				PolyLine polyLine = new PolyLine(polyLineStart, polyLineEnd);
				double int1Length = int1.getLength();
				double int2Length = int2.getLength();
				IntersectionType type;
				if (Math.abs(intOverlapLength - int1Length) < FULLY_EMBEDDED_TOLERANCE
						|| Math.abs(intOverlapLength - int2Length) < FULLY_EMBEDDED_TOLERANCE) {
					// fully embedded
					type = IntersectionType.FULLY_EMBEDDED_EDGE;
				} else {
					type = IntersectionType.PARTIALLY_EMBEDDED_EDGE;
				}

				PolygonPolygonIntersection intersection = new PolygonPolygonIntersection(p1, p2, polyLine, type);
				intersections.add(intersection);
			}
		}
	}

	private static boolean areStraightsColinear(GmBoundedStraight straight1, GmStraight straight2, double epsilon,
			double angleEpsilon) {

		UnitVector3d rDir1 = straight1.getDir();
		UnitVector3d rDir2 = straight2.getDir();

		if ((!Vector3d.areParallel(rDir1, rDir2, angleEpsilon))
				&& (!Vector3d.areAntiParallel(rDir1, rDir2, angleEpsilon))) {
			return false;
		}

		Point3d rOrigin = straight1.getOrigin();
		ProjectedPoint3d foot1 = straight2.project(rOrigin);
		Point3d rTarget = straight1.getTarget();
		ProjectedPoint3d foot2 = straight2.project(rTarget);
		if ((foot1.getPoint().minus(rOrigin)).getLength() > epsilon
				|| (foot2.getPoint().minus(rTarget)).getLength() > epsilon) {
			return false;
		}
		return true;
	}

	private static List<PolygonPolygonIntersection> handlePolygonsInSamePlane(GmPlane plane, EdgePolygon p1,
			EdgePolygon p2, double epsilon) {
		Map<Coordinate3d, Coordinate2d> coordMap = new IdentityHashMap<>();

		List<Coordinate2d> poly2dCoords1 = new ArrayList<>();
		List<Coordinate3d> polyCoords1 = p1.getCoordinates();
		for (Coordinate3d c : polyCoords1) {
			Point2d proj = plane.project(c.getPoint());
			Coordinate2d pc2d = new Coordinate2d(proj);
			poly2dCoords1.add(pc2d);
			coordMap.put(c, pc2d);
		}

		List<Coordinate2d> poly2dCoords2 = new ArrayList<>();
		List<Coordinate3d> polyCoords2 = p2.getCoordinates();
		for (Coordinate3d c : polyCoords2) {
			Coordinate2d pc2d = coordMap.get(c);
			if (pc2d == null) {
				Point2d proj = plane.project(c.getPoint());
				pc2d = new Coordinate2d(proj);
			}
			poly2dCoords2.add(pc2d);
		}

		Polygon2d poly2d1 = new Polygon2dNs(poly2dCoords1, true);
		Polygon2d poly2d2 = new Polygon2dNs(poly2dCoords2, true);

		List<PolygonPolygonIntersection> result = new ArrayList<>();
		List<Polygon2dPolygon2dInt> ppi2ds = IntersectPolygon2d.getIntersections(poly2d1, poly2d2, epsilon);
		for (Polygon2dPolygon2dInt ppi2d : ppi2ds) {
			PolyLine2d pl2d = ppi2d.getPolyLine2d();

			List<Coordinate2d> coords2d = pl2d.getCoordinates();
			List<Coordinate3d> coords3d = new ArrayList<>();
			for (Coordinate2d coord2d : coords2d) {
				Point3d pnt3d = plane.evaluate(coord2d.getPoint());
				coords3d.add(new Coordinate3d(pnt3d));
			}

			PolyLine pl = new PolyLine(coords3d, false);
			PolygonPolygonIntersection ppi = new PolygonPolygonIntersection(p1, p2, pl, ppi2d.getIntType());
			result.add(ppi);
		}
		return result;
	}

	private IntersectPlanarPolygons() {
	}

}