SelfIntersectionUtil.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.checks.util;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import org.locationtech.jts.geom.Coordinate;
import org.locationtech.jts.geom.CoordinateSequence;
import org.locationtech.jts.geom.GeometryCollection;
import org.locationtech.jts.geom.GeometryFactory;
import org.locationtech.jts.geom.LineString;
import org.locationtech.jts.geom.MultiLineString;
import org.locationtech.jts.geom.MultiPoint;
import org.locationtech.jts.geom.Point;
import org.locationtech.jts.geom.PrecisionModel;
import org.locationtech.jts.geom.impl.CoordinateArraySequence;
import org.locationtech.jts.operation.overlay.OverlayOp;
import org.locationtech.jts.operation.overlay.snap.SnapIfNeededOverlayOp;

import de.hft.stuttgart.citydoctor2.check.GeometrySelfIntersection;
import de.hft.stuttgart.citydoctor2.datastructure.ConcretePolygon;
import de.hft.stuttgart.citydoctor2.datastructure.Geometry;
import de.hft.stuttgart.citydoctor2.datastructure.LinearRing;
import de.hft.stuttgart.citydoctor2.datastructure.LinearRing.LinearRingType;
import de.hft.stuttgart.citydoctor2.datastructure.Polygon;
import de.hft.stuttgart.citydoctor2.datastructure.Vertex;
import de.hft.stuttgart.citydoctor2.edge.MeshSurface;
import de.hft.stuttgart.citydoctor2.edge.MeshSurfaceUtils;
import de.hft.stuttgart.citydoctor2.edge.PolygonPolygonIntersection;
import de.hft.stuttgart.citydoctor2.math.MovedPolygon;
import de.hft.stuttgart.citydoctor2.math.MovedRing;
import de.hft.stuttgart.citydoctor2.math.Plane;
import de.hft.stuttgart.citydoctor2.math.PlaneSegmentIntersection;
import de.hft.stuttgart.citydoctor2.math.PlaneSegmentIntersection.Type;
import de.hft.stuttgart.citydoctor2.math.Polygon2d;
import de.hft.stuttgart.citydoctor2.math.ProjectionAxis;
import de.hft.stuttgart.citydoctor2.math.Ring2d;
import de.hft.stuttgart.citydoctor2.math.Segment3d;
import de.hft.stuttgart.citydoctor2.math.Triangle3d;
import de.hft.stuttgart.citydoctor2.math.Vector2d;
import de.hft.stuttgart.citydoctor2.math.Vector3d;
import de.hft.stuttgart.citydoctor2.tesselation.JoglTesselator;
import de.hft.stuttgart.citydoctor2.tesselation.TesselatedPolygon;
import de.hft.stuttgart.citydoctor2.utils.PolygonIntersection;
import de.hft.stuttgart.citydoctor2.utils.PolygonIntersection.IntersectionType;

/**
 * Utility class for checking for intersections
 * 
 * @author Matthias Betz
 *
 */
public class SelfIntersectionUtil {


	private static final Logger logger = LogManager.getLogger(SelfIntersectionUtil.class);
	
	private static boolean useEdge = true;

	private static GeometryFactory factory = new GeometryFactory(new PrecisionModel(PrecisionModel.FLOATING));

	private SelfIntersectionUtil() {

	}

	public static GeometrySelfIntersection doesSolidSelfIntersect(Geometry g) {
		return selfIntersectionJava(g);
	}

	public static List<PolygonIntersection> doesSolidSelfIntersect2(Geometry g) {
		List<Polygon> polygons = g.getPolygons();
		List<PolygonIntersection> intersections = new ArrayList<>();
		if (useEdge) {
			MeshSurface meshSurface = MeshSurface.of(g);
			List<PolygonPolygonIntersection> selfIntersects = MeshSurfaceUtils.selfIntersects(meshSurface, 0.000001, 0.001);
			if (!selfIntersects.isEmpty()) {
				PolygonPolygonIntersection polygonPolygonIntersection = selfIntersects.get(0);
				Polygon p1 = polygonPolygonIntersection.getPolygon1().getOriginal();
				Polygon p2 = polygonPolygonIntersection.getPolygon2().getOriginal();
				intersections.add(PolygonIntersection.lines(Collections.emptyList(), p1, p2));
			}
		}
		for (int i = 0; i < polygons.size() - 1; i++) {
			Polygon p1 = polygons.get(i);
			for (int j = i + 1; j < polygons.size(); j++) {
				Polygon p2 = polygons.get(j);
				if (useEdge && p1.getInnerRings().isEmpty() && p2.getInnerRings().isEmpty()) {
				} else {
					PolygonIntersection intersect = polygonIntersection(p1, p2, 0.1, 0.01);
					if (intersect.getType() != IntersectionType.NONE) {
						intersections.add(intersect);
					}
				}
			}
		}
		return intersections;
	}

	/**
	 * Checks if two polygons are intersecting. Result may be nothing, a line or a
	 * polygon if they are planar and intersecting.
	 * 
	 * @param p1                  the first polygon
	 * @param p2                  the second polygon
	 * @param planarEpsilon       the deviation of the normal before they are
	 *                            considered planar. In degrees.
	 * @param intersectionEpsilon epsilon to have some lee-way before polygons are
	 *                            intersecting.
	 * @return the intersection result.
	 */
	public static PolygonIntersection polygonIntersection(Polygon p1, Polygon p2, double planarEpsilon,
			double intersectionEpsilon) {
		Vector3d movedBy = p1.getExteriorRing().getVertices().get(0);
		MovedPolygon indPoly1 = MovedPolygon.ofPolygon(p1, movedBy);
		MovedPolygon indPoly2 = MovedPolygon.ofPolygon(p2, movedBy);
		Plane plane1 = Plane.of(indPoly1);
		// check p2 points all above or under plane1
		if (isPolygonOnOneSide(intersectionEpsilon, plane1, indPoly2.getExteriorRing())) {
			return PolygonIntersection.none();
		}
		Plane plane2 = Plane.of(indPoly2);
		// check if p1 points all above or under plane2
		if (isPolygonOnOneSide(intersectionEpsilon, plane2, indPoly1.getExteriorRing())) {
			return PolygonIntersection.none();
		}

		// are polygons planar
		double dot = Math.abs(plane1.getNormal().dot(plane2.getNormal()));
		double radians = Math.toRadians(planarEpsilon);
		if (dot > 1 - radians && dot < 1 + radians) {
			// polygons are planar and on the same plane more or less
			return polygon2DIntersection(indPoly1, indPoly2, plane1);
		}

		// possible intersection, get all intersection points
		List<Vector3d> intersectionPoints = new ArrayList<>();
		getIntersectionPoints(indPoly1, indPoly2, intersectionEpsilon, plane2, intersectionPoints);
		getIntersectionPoints(indPoly2, indPoly1, intersectionEpsilon, plane1, intersectionPoints);
		if (intersectionPoints.isEmpty()) {
			// no points found: no intersection
			return PolygonIntersection.none();
		}
		int size = intersectionPoints.size();
		if (size == 1) {
			List<Segment3d> lines = new ArrayList<>();
			Vector3d v = intersectionPoints.get(0);
			Segment3d seg = new Segment3d(v, v);
			lines.add(seg);
			return PolygonIntersection.lines(lines, p1, p2);
		}
		if (size % 2 != 0) {
			throw new IllegalStateException(
					"Polygon intersection has uneven number of intersection points: " + intersectionPoints);
		}
		List<Segment3d> lines = new ArrayList<>();
		for (int i = 0; i < size / 2; i++) {
			createLine(intersectionPoints, lines);
		}
		return PolygonIntersection.lines(lines, p1, p2);
	}

	private static PolygonIntersection polygon2DIntersection(MovedPolygon p1, MovedPolygon p2, Plane plane1) {
		ProjectionAxis projectionAxis = ProjectionAxis.of(plane1);
		Polygon2d projectedP1 = Polygon2d.withProjection(p1, projectionAxis);
		Polygon2d projectedP2 = Polygon2d.withProjection(p2, projectionAxis);

		org.locationtech.jts.geom.Polygon jtsP1 = createJtsPolygon(projectedP1);
		org.locationtech.jts.geom.Polygon jtsP2 = createJtsPolygon(projectedP2);
		// use JTS to get intersection
		// NOTE: OverlayOp.overlayOp sometimes throws "side location conflict"
		// TopologyException.
		// SnapIfNeededOverlayOp should be more robust.
		org.locationtech.jts.geom.Geometry intersection = SnapIfNeededOverlayOp.overlayOp(jtsP1, jtsP2,
				OverlayOp.INTERSECTION);
		if (intersection instanceof LineString || intersection instanceof MultiPoint
				|| intersection instanceof MultiLineString || intersection instanceof Point || intersection.isEmpty()) {
			// intersection is only an edge, not a polygon
			// edge intersections are allowed
			return PolygonIntersection.none();
		} else if (intersection instanceof GeometryCollection) {
			GeometryCollection col = (GeometryCollection) intersection;
			for (int i = 0; i < col.getNumGeometries(); i++) {
				org.locationtech.jts.geom.Geometry interGeom = col.getGeometryN(i);
				if (interGeom instanceof org.locationtech.jts.geom.Polygon) {
					org.locationtech.jts.geom.Polygon intPoly = (org.locationtech.jts.geom.Polygon) interGeom;
					ConcretePolygon poly = convertToPolygon(plane1, projectionAxis, intPoly);
					return PolygonIntersection.polygon(poly, p1.getOriginal(), p2.getOriginal());
				}
			}
			// no polygon in collection, so no intersection
			return PolygonIntersection.none();
		}
		if (intersection instanceof org.locationtech.jts.geom.Polygon) {
			org.locationtech.jts.geom.Polygon intPoly = (org.locationtech.jts.geom.Polygon) intersection;
			ConcretePolygon poly = convertToPolygon(plane1, projectionAxis, intPoly);
			return PolygonIntersection.polygon(poly, p1.getOriginal(), p2.getOriginal());
		} else {
			throw new IllegalStateException("Unknown intersection class: " + intersection);
		}
	}

	private static ConcretePolygon convertToPolygon(Plane plane1, ProjectionAxis projectionAxis,
			org.locationtech.jts.geom.Polygon intPoly) {
		List<Vertex> extRing = convertTo3dRing(plane1, intPoly.getExteriorRing(), projectionAxis);
		ConcretePolygon poly = new ConcretePolygon();
		LinearRing ext = new LinearRing(LinearRingType.EXTERIOR);
		poly.setExteriorRing(ext);
		ext.addAllVertices(extRing);
		for (int i = 0; i < intPoly.getNumInteriorRing(); i++) {
			List<Vertex> intRing = convertTo3dRing(plane1, intPoly.getInteriorRingN(i), projectionAxis);
			LinearRing inner = new LinearRing(LinearRingType.INTERIOR);
			poly.addInteriorRing(inner);
			inner.addAllVertices(intRing);
		}
		return poly;
	}

	private static List<Vertex> convertTo3dRing(Plane plane, LineString lineString, ProjectionAxis axis) {
		List<Vertex> ring = new ArrayList<>();
		for (Coordinate coord : lineString.getCoordinates()) {
			Vertex v = projectPointToPlane(plane, axis, coord.getX(), coord.getY());
			ring.add(v);
		}
		return ring;
	}

	private static Vertex projectPointToPlane(Plane plane, ProjectionAxis axis, double oldX, double oldY) {
		return new Vertex(axis.projectToPlane(plane, oldX, oldY));
	}

	private static org.locationtech.jts.geom.Polygon createJtsPolygon(Polygon2d poly) {
		org.locationtech.jts.geom.LinearRing shell = createJtsRing(poly.getExterior());
		org.locationtech.jts.geom.LinearRing[] holes = null;
		List<Ring2d> interiorRings = poly.getInteriorRings();
		if (!interiorRings.isEmpty()) {
			holes = new org.locationtech.jts.geom.LinearRing[interiorRings.size()];
			for (int i = 0; i < interiorRings.size(); i++) {
				holes[i] = createJtsRing(interiorRings.get(i));
			}
		}
		return new org.locationtech.jts.geom.Polygon(shell, holes, factory);
	}

	private static org.locationtech.jts.geom.LinearRing createJtsRing(Ring2d ring) {
		Coordinate[] coords = new Coordinate[ring.getVertices().size()];
		for (int i = 0; i < ring.getVertices().size(); i++) {
			Vector2d v = ring.getVertices().get(i);
			coords[i] = new Coordinate(v.getX(), v.getY());
		}
		CoordinateSequence points = new CoordinateArraySequence(coords);
		return new org.locationtech.jts.geom.LinearRing(points, factory);
	}

	private static void createLine(List<Vector3d> intersectionPoints, List<Segment3d> lines) {
		Vector3d startPoint = getStartPoint(intersectionPoints);
		intersectionPoints.remove(startPoint);
		Vector3d endPoint = getNearestPoint(intersectionPoints, startPoint);
		intersectionPoints.remove(endPoint);
		Segment3d line = new Segment3d(startPoint, endPoint);
		lines.add(line);
	}

	private static Vector3d getNearestPoint(List<Vector3d> intersectionPoints, Vector3d startPoint) {
		if (intersectionPoints.size() == 1) {
			return intersectionPoints.get(0);
		}
		Vector3d nearestPoint = intersectionPoints.get(0);
		double distance = startPoint.getDistanceSquare(nearestPoint);
		for (int i = 1; i < intersectionPoints.size(); i++) {
			Vector3d checkPoint = intersectionPoints.get(i);
			double checkDistance = checkPoint.getDistanceSquare(startPoint);
			if (checkDistance < distance) {
				nearestPoint = checkPoint;
				distance = checkDistance;
			}
		}
		return nearestPoint;
	}

	private static Vector3d getStartPoint(List<Vector3d> intersectionPoints) {
		Vector3d point = intersectionPoints.get(0);
		for (int i = 1; i < intersectionPoints.size(); i++) {
			Vector3d checkPoint = intersectionPoints.get(i);
			if (checkPoint.getX() < point.getX()
					|| (checkPoint.getX() == point.getX() && (checkPoint.getY() < point.getY()
							|| (checkPoint.getY() == point.getY() && checkPoint.getZ() < point.getZ())))) {
				point = checkPoint;
			}
		}
		return point;
	}

	private static void getIntersectionPoints(MovedPolygon p1, MovedPolygon p2, double intersectionEpsilon, Plane plane,
			List<Vector3d> intersectionPoints) {
		List<Segment3d> segments = getSegments(p1);
		for (Segment3d segment : segments) {
			PlaneSegmentIntersection intersection = plane.intersects(segment, intersectionEpsilon);
			if (intersection.getType() == Type.INTERSECTION_POINT
					&& isPointInsidePolygon(p2, intersection.getPoint())) {
				intersectionPoints.add(intersection.getPoint());
			}
		}
	}

	private static List<Segment3d> getSegments(MovedPolygon p1) {
		List<Segment3d> segments = new ArrayList<>();
		getSegments(p1.getExteriorRing(), segments);
		for (MovedRing inner : p1.getInnerRings()) {
			getSegments(inner, segments);
		}
		return segments;
	}

	private static void getSegments(MovedRing movedRing, List<Segment3d> segments) {
		List<Vector3d> vertices = movedRing.getVertices();
		for (int i = 0; i < vertices.size() - 1; i++) {
			Segment3d seg = new Segment3d(vertices.get(i), vertices.get(i + 1));
			segments.add(seg);
		}
	}

	private static boolean isPointInsidePolygon(MovedPolygon p, Vector3d v) {
		if (p.getExteriorRing().isPointInside(v)) {
			for (MovedRing r : p.getInnerRings()) {
				if (r.isPointInside(v)) {
					return false;
				}
			}
			return true;
		}
		return false;
	}

	private static boolean isPolygonOnOneSide(double intersectionEpsilon, Plane plane, MovedRing ring) {
		double sign = 0;
		for (Vector3d v : ring.getVertices()) {
			double signedDistance = plane.getSignedDistance(v);
			if (signedDistance > -intersectionEpsilon && signedDistance < intersectionEpsilon) {
				// point is slightly off the plane, does not count as possible intersection yet
				signedDistance = 0;
			}
			if ((signedDistance < 0 && sign > 0) || (signedDistance > 0 && sign < 0)) {
				// possible intersection detected
				return false;
			}
			if (signedDistance != 0) {
				sign = Math.signum(signedDistance);
			}
		}
		// all points are on the same plane, therefore possible intersection
		return sign != 0;
	}

	private static GeometrySelfIntersection selfIntersectionJava(Geometry g) {
		List<TesselatedPolygon> tessPolys = new ArrayList<>();
		for (Polygon p : g.getPolygons()) {
			tessPolys.add(JoglTesselator.tesselatePolygon(p));
		}
		for (int i = 0; i < tessPolys.size() - 1; i++) {
			TesselatedPolygon p1 = tessPolys.get(i);
			for (int j = i + 1; j < tessPolys.size(); j++) {
				TesselatedPolygon p2 = tessPolys.get(j);
				GeometrySelfIntersection intersection = doPolygonsIntersect(p1, p2);
				if (intersection != null) {
					return intersection;
				}
			}
		}
		return null;
	}

	private static GeometrySelfIntersection doPolygonsIntersect(TesselatedPolygon p1, TesselatedPolygon p2) {
		for (int p1Index = 0; p1Index < p1.getTriangles().size(); p1Index++) {
			for (int p2Index = 0; p2Index < p2.getTriangles().size(); p2Index++) {
				Triangle3d t1 = p1.getTriangles().get(p1Index);
				Triangle3d t2 = p2.getTriangles().get(p2Index);
				if (t1.doesIntersect(t2)) {
					logger.trace("{} intersects {}", t1, t2);
					logger.trace("{} intersects {}", t1.getPartOf().getOriginal().getGmlId(),
							t2.getPartOf().getOriginal().getGmlId());
					return new GeometrySelfIntersection(t1.getPartOf().getOriginal(), t2.getPartOf().getOriginal(), t1,
							t2);
				}
			}
		}
		return null;
	}
}