fix wrong polygon intersection computation, add an additional...

fix wrong polygon intersection computation, add an additional PedestrianFundamentalDiagramProcessor.

VadereMeshing/src/org/vadere/meshing/WeilerAtherton.java

@@ -9,6 +9,7 @@ import org.vadere.meshing.mesh.gen.PMesh;
 import org.vadere.meshing.mesh.gen.PVertex;
 import org.vadere.meshing.mesh.inter.IPolyConnectivity;
 import org.vadere.util.geometry.GeometryUtils;
+import org.vadere.util.geometry.shapes.IPoint;
 import org.vadere.util.geometry.shapes.VLine;
 import org.vadere.util.geometry.shapes.VPoint;
 import org.vadere.util.geometry.shapes.VPolygon;
@@ -349,6 +350,10 @@ public class WeilerAtherton {
 		}
 	}
 
+	private boolean contains(@NotNull final VPolygon polygon, @NotNull final IPoint point) {
+		return polygon.contains(point) || Math.abs(polygon.distance(point)) <= GeometryUtils.DOUBLE_EPS;
+	}
+
 	private List<VPolygon> construct(
 			@NotNull final VPolygon subjectCandidat,
 			@NotNull final VPolygon clippingCandidat,
@@ -375,15 +380,18 @@ public class WeilerAtherton {
 		Set<PHalfEdge<WeilerPoint>> subjectExitingEdges = subjectMesh
 				.streamEdges(subjectFace)
 				.filter(edge -> subjectMesh.getPoint(edge).isIntersectionPoint())
-				.filter(edge -> !subjectMesh.getPoint(subjectMesh.getNext(edge)).isIntersectionPoint() &&
-						!clipping.contains(subjectMesh.getPoint(subjectMesh.getNext(edge))))
+				.filter(edge ->
+						contains(clipping, subjectMesh.getPoint(subjectMesh.getPrev(edge))) &&
+						!contains(clipping, subjectMesh.getPoint(subjectMesh.getNext(edge))))
 				.collect(Collectors.toSet());
 
 		Set<PHalfEdge<WeilerPoint>> subjectEnteringEdges = subjectMesh
 				.streamEdges(subjectFace)
 				.filter(edge -> subjectMesh.getPoint(edge).isIntersectionPoint())
-				.filter(edge -> subjectMesh.getPoint(subjectMesh.getNext(edge)).isIntersectionPoint() ||
-						clipping.contains(subjectMesh.getPoint(subjectMesh.getNext(edge))))
+				.filter(edge ->
+						contains(clipping, subjectMesh.getPoint(subjectMesh.getNext(edge))) &&
+						!contains(clipping, subjectMesh.getPoint(subjectMesh.getPrev(edge)))
+				)
 				.collect(Collectors.toSet());
 
 		List<VPoint> points = new ArrayList<>();
@@ -393,13 +401,42 @@ public class WeilerAtherton {
 		Set<PHalfEdge<WeilerPoint>> intersectionSet = cap ? subjectEnteringEdges : subjectExitingEdges;
 
 		// cup will preserve the polyons.
-		if(!cap && intersectionSet.isEmpty()) {
+		if(intersectionSet.isEmpty()) {
+			boolean subInClip = subjectMesh
+					.streamPoints(subjectFace)
+					.allMatch(p -> contains(clipping, p));
+
+			boolean clipInSub = clippingMesh
+					.streamPoints(clippingFace)
+					.allMatch(p -> contains(subject, p));
+
+			if(cap) {
+				if(subInClip) {
+					polygons.add(subjectCandidat);
+				}
+				else if(clipInSub) {
+					polygons.add(clippingCandidat);
+				}
+			}
+			else {
+				if(subInClip) {
+					polygons.add(clippingCandidat);
+				}
+				else if(clipInSub) {
+					polygons.add(subjectCandidat);
+				}
+				else {
 					polygons.add(subjectCandidat);
 					polygons.add(clippingCandidat);
 				}
+			}
+
+			return polygons;
+		}
 
 		while (!intersectionSet.isEmpty()) {
 			PHalfEdge<WeilerPoint> subjectEdge = intersectionSet.iterator().next();
+			PHalfEdge<WeilerPoint> subjectTwin = clippingMesh.getEdge(subjectMesh.getPoint(subjectEdge).getTwinPoint());
 			PHalfEdge<WeilerPoint> next = subjectEdge;
 			intersectionSet.remove(subjectEdge);
 
@@ -407,14 +444,28 @@ public class WeilerAtherton {
 				next = mesh.getNext(next);
 
 				// adaptPath
-				if(mesh.getPoint(next).isIntersectionPoint() && !intersectionSet.contains(next)) {
+				if(mesh.getPoint(next).isIntersectionPoint()) {
+					/*
+					 * Special case!
+					 */
+					PVertex<WeilerPoint> twinPoint = mesh.getPoint(next).getTwinPoint();
+					PMesh<WeilerPoint> twinMesh = mesh.equals(subjectMesh) ? clippingMesh : subjectMesh;
+					PHalfEdge<WeilerPoint> twinPointEdge = twinMesh.getEdge(twinPoint);
+					VPoint prevTwinPoint = new VPoint(twinMesh.getPoint(twinMesh.getPrev(twinPointEdge)));
+					VPoint prevPoint = new VPoint(mesh.getPoint(mesh.getPrev(next)));
+
+					intersectionSet.remove(next);
+					intersectionSet.remove(twinMesh.getEdge(twinPoint));
+
+					if(!prevTwinPoint.equals(prevPoint)) {
 						mesh = mesh.equals(subjectMesh) ? clippingMesh : subjectMesh;
 						next = mesh.getEdge(mesh.getPoint(next).getTwinPoint());
-					intersectionSet.remove(next);
+					}
+
 				}
 				points.add(new VPoint(mesh.getPoint(next)));
 
-			} while (!next.equals(subjectEdge));
+			} while (!next.equals(subjectEdge) && !next.equals(subjectTwin));
 
 			polygons.add(GeometryUtils.toPolygon(GeometryUtils.filterUselessPoints(points, EPSILON)));
 		}

VadereMeshing/src/org/vadere/meshing/mesh/inter/IPolyConnectivity.java

@@ -162,6 +162,15 @@ public interface IPolyConnectivity<P extends IPoint, V extends IVertex<P>, E ext
 			@NotNull final E edge, @NotNull P p, @NotNull IMesh<P, V, E, F> mesh) {
 
 		E twin = mesh.getTwin(edge);
+
+		if(mesh.getPoint(edge).equals(p)) {
+			return mesh.getVertex(edge);
+		}
+
+		if(mesh.getPoint(twin).equals(p)) {
+			return mesh.getVertex(mesh.getPrev(edge));
+		}
+
 		E prev = mesh.getPrev(edge);
 		E tNext = mesh.getNext(twin);
 

VadereMeshing/src/org/vadere/meshing/mesh/triangulation/triangulator/UniformRefinementTriangulatorSFC.java

@@ -603,7 +603,7 @@ public class UniformRefinementTriangulatorSFC<P extends IPoint, V extends IVerte
 		F twin = getMesh().getTwinFace(edge);
 
 		VTriangle triangle = getMesh().toTriangle(face);
-		return (!triangle.intersects(bbox) && (getMesh().isBoundary(twin) || !getMesh().toTriangle(twin).intersects(bbox)));
+		return (!triangle.intersectsRectangleLine(bbox) && (getMesh().isBoundary(twin) || !getMesh().toTriangle(twin).intersectsRectangleLine(bbox)));
 	}
 
 	/**

VadereMeshing/src/org/vadere/meshing/mesh/triangulation/triangulator/UniformSFCTriangulator.java

@@ -212,7 +212,7 @@ public class UniformSFCTriangulator<P extends IPoint, V extends IVertex<P>, E ex
         VLine line = mesh.toLine(edge);
 
         return (line.length() <= lenFunc.apply(line.midPoint()) && random.nextDouble() < 0.96)
-                || (!triangle.intersects(bbox) && (mesh.isBoundary(twin) || !mesh.toTriangle(twin).intersects(bbox)))
+                || (!triangle.intersectsRectangleLine(bbox) && (mesh.isBoundary(twin) || !mesh.toTriangle(twin).intersectsRectangleLine(bbox)))
                 || boundary.stream().anyMatch(shape -> shape.contains(triangle.getBounds2D()) || (!mesh.isBoundary(twin) && shape.contains(mesh.toTriangle(twin).getBounds2D())));
     }
 

VadereMeshing/tests/org/vadere/geometry/TestWeilerAtherton.java

@@ -27,6 +27,19 @@ public class TestWeilerAtherton {
 	@Before
 	public void setUp() throws Exception {}
 
+	@Test
+	public void testRectangleIntersectionSpecialCase() {
+		VRectangle rec1 = new VRectangle(0, 0, 5, 5);
+		VRectangle rec2 = new VRectangle(2, 0, 5, 5);
+		VRectangle expectedResult = new VRectangle(2,0,3,5);
+		List<VPolygon> originalList = Arrays.asList(new VPolygon(rec1), new VPolygon(rec2));
+		WeilerAtherton weilerAtherton = new WeilerAtherton(originalList);
+
+		Optional<VPolygon> optPolygon = weilerAtherton.cap();
+		assertTrue(optPolygon.isPresent());
+		assertTrue(GeometryUtils.equalsPolygons(new VPolygon(expectedResult), optPolygon.get()));
+	}
+
 	@Test
 	public void testRectangleIntersection() {
 		VRectangle rec1 = new VRectangle(0, 0, 5, 5);

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/IntegralVoronoiAlgorithm.java

@@ -12,6 +12,7 @@ import org.vadere.util.voronoi.Face;
 import org.vadere.util.voronoi.HalfEdge;
 import org.vadere.util.voronoi.VoronoiDiagram;
 
+import java.awt.geom.Area;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collections;
@@ -48,15 +49,28 @@ public class IntegralVoronoiAlgorithm extends AreaDensityAlgorithm implements IA
         for (Face face : faces) {
             if (intersectMeasurementArea(face)) {
 				VPolygon cell = face.toPolygon();
-	            WeilerAtherton weilerAtherton = new WeilerAtherton(Arrays.asList(cell, measurementAreaPolygon));
-	            VPolygon capPolygon = weilerAtherton.cap().get();
-	            area += cell.getArea() / capPolygon.getArea();
+
+	            VPolygon capPolygon = computeIntersection2(cell);
+	            area += capPolygon.getArea() / cell.getArea();
+				assert capPolygon.getArea() <= cell.getArea();
             }
         }
 
         return area / measurementArea.getArea();
     }
 
+    private VPolygon computeIntersection2(@NotNull final VPolygon cell) {
+	    try {
+		    WeilerAtherton weilerAtherton = new WeilerAtherton(Arrays.asList(cell, measurementAreaPolygon));
+		    VPolygon capPolygon = weilerAtherton.cap().get();
+		    return capPolygon;
+	    } catch (Exception e) {
+		    System.out.println(e.getMessage());
+		    //VPolygon capPolygon = weilerAtherton.cap().get();
+	    }
+	    return null;
+    }
+
     private List<Face> generateFaces(@NotNull final SimulationState state) {
 	    VoronoiDiagram voronoiDiagram = new VoronoiDiagram(this.voronoiArea);
 
@@ -71,24 +85,10 @@ public class IntegralVoronoiAlgorithm extends AreaDensityAlgorithm implements IA
 
 	@Override
 	public double getVelocity(SimulationState state) {
-		VoronoiDiagram voronoiDiagram = new VoronoiDiagram(voronoiArea);
-
-		// convert pedestrians to positions
-		List<VPoint> pedestrianPositions = Agent.getPositions(state.getTopography().getElements(Agent.class));
-
-		// compute the voronoi diagram
-		voronoiDiagram.computeVoronoiDiagram(pedestrianPositions);
-
-		// filter invalid faces (which do not form a simple polygon)
-		List<Face> faces = voronoiDiagram.getFaces().stream().filter(f -> f.getNumberOfEdges() > 2).collect(Collectors.toList());
-
-		Map<Integer, Double> areaMap = new TreeMap<>();
-		Map<Integer, Face> faceMap = new TreeMap<>();
+		List<Face> faces = generateFaces(state);
 
 		double velocity = 0.0;
 		for (Face face : faces) {
-			areaMap.put(face.getId(), face.computeArea());
-			faceMap.put(face.getId(), face);
 
 			if (intersectMeasurementArea(face)) {
 				VPoint center = face.getSite();
@@ -97,7 +97,7 @@ public class IntegralVoronoiAlgorithm extends AreaDensityAlgorithm implements IA
 						.filter(agent -> center.distance(agent.getPosition()) < 0.01)
 						.findAny().get();
 
-				velocity += (face.computeArea() * ped.getVelocity().getLength());
+				velocity += (computeIntersection2(face.toPolygon()).getArea() * ped.getVelocity().getLength());
 			}
 		}
 
@@ -105,7 +105,7 @@ public class IntegralVoronoiAlgorithm extends AreaDensityAlgorithm implements IA
 	}
 
     private boolean intersectMeasurementArea(@NotNull final Face face) {
-		return measurementArea.intersects(toPolygon(face));
+		return measurementArea.intersects(face.toPolygon());
     }
 
     private VPolygon toPolygon(@NotNull final Face face) {

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/PedestrianFundamentalDiagramBProcessor.java

 package org.vadere.simulator.projects.dataprocessing.processor;
 
 import org.apache.commons.lang3.tuple.Pair;
-import org.apache.commons.lang3.tuple.Triple;
 import org.jetbrains.annotations.NotNull;
 import org.vadere.annotation.factories.dataprocessors.DataProcessorClass;
 import org.vadere.simulator.control.SimulationState;
@@ -12,13 +11,8 @@ import org.vadere.state.attributes.processor.AttributesProcessor;
 import org.vadere.state.simulation.VTrajectory;
 import org.vadere.util.geometry.shapes.VRectangle;
 
-import java.util.Comparator;
 import java.util.HashMap;
-import java.util.LinkedList;
-import java.util.List;
 import java.util.Map;
-import java.util.PriorityQueue;
-import java.util.stream.Collectors;
 
 /**
  * <p>This processor computes the fundamental diagram by computing an (average) velocity and the density

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/PedestrianFundamentalDiagramEProcessor.java

+package org.vadere.simulator.projects.dataprocessing.processor;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.jetbrains.annotations.NotNull;
+import org.vadere.annotation.factories.dataprocessors.DataProcessorClass;
+import org.vadere.simulator.control.SimulationState;
+import org.vadere.simulator.projects.dataprocessing.ProcessorManager;
+import org.vadere.simulator.projects.dataprocessing.datakey.TimestepKey;
+import org.vadere.state.attributes.processor.AttributesFundamentalDiagramEProcessor;
+import org.vadere.state.attributes.processor.AttributesProcessor;
+
+/**
+ *
+ * @author Benedikt Zoennchen
+ */
+@DataProcessorClass()
+public class PedestrianFundamentalDiagramEProcessor extends AreaDataProcessor<Pair<Double, Double>>  {
+
+	private SumVoronoiAlgorithm sumVoronoiAlgorithm;
+
+	public PedestrianFundamentalDiagramEProcessor() {
+		super("velocity", "density");
+	}
+
+	@Override
+	public void init(final ProcessorManager manager) {
+		super.init(manager);
+		AttributesFundamentalDiagramEProcessor att = (AttributesFundamentalDiagramEProcessor) this.getAttributes();
+		sumVoronoiAlgorithm = new SumVoronoiAlgorithm(att.getMeasurementArea(), att.getVoronoiArea());
+	}
+
+	@Override
+	public AttributesProcessor getAttributes() {
+		if (super.getAttributes() == null) {
+			setAttributes(new AttributesFundamentalDiagramEProcessor());
+		}
+		return super.getAttributes();
+	}
+
+	@Override
+	public void preLoop(SimulationState state) {
+		super.preLoop(state);
+	}
+
+	@Override
+	protected void doUpdate(SimulationState state) {
+		putValue(new TimestepKey(state.getStep()), Pair.of(
+				sumVoronoiAlgorithm.getVelocity(state),
+				sumVoronoiAlgorithm.getDensity(state)));
+	}
+
+	@Override
+	public String[] toStrings(@NotNull final TimestepKey key) {
+		return new String[]{ Double.toString(getValue(key).getLeft()), Double.toString(getValue(key).getRight()) };
+	}
+}

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/SumVoronoiAlgorithm.java

+package org.vadere.simulator.projects.dataprocessing.processor;
+
+import org.jetbrains.annotations.NotNull;
+import org.vadere.meshing.WeilerAtherton;
+import org.vadere.simulator.control.SimulationState;
+import org.vadere.state.scenario.Agent;
+import org.vadere.util.geometry.GeometryUtils;
+import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.geometry.shapes.VPolygon;
+import org.vadere.util.geometry.shapes.VRectangle;
+import org.vadere.util.voronoi.Face;
+import org.vadere.util.voronoi.HalfEdge;
+import org.vadere.util.voronoi.VoronoiDiagram;
+
+import java.awt.geom.Area;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.List;
+import java.util.Map;
+import java.util.TreeMap;
+
+public class SumVoronoiAlgorithm extends AreaDensityAlgorithm implements IAreaVelocityAlgorithm {
+	private VRectangle measurementArea;
+	private VPolygon measurementAreaPolygon;
+	private VRectangle voronoiArea;
+
+	public SumVoronoiAlgorithm(@NotNull final VRectangle measurementArea, @NotNull final VRectangle voronoiArea) {
+		super("areaVoronoi");
+
+		this.measurementArea = measurementArea;
+		this.measurementAreaPolygon = new VPolygon(measurementArea);
+		this.voronoiArea = voronoiArea;
+	}
+
+	@Override
+	public double getDensity(final SimulationState state) {
+		List<Face> faces = generateFaces(state);
+
+		double area = 0.0;
+		int N = 0;
+		for (Face face : faces) {
+			if (measurementArea.contains(face.getSite())) {
+				VPolygon cell = face.toPolygon();
+				N++;
+				area += cell.getArea();
+			}
+		}
+
+		return area > 0 ?  N / area : 0;
+	}
+
+	private List<Face> generateFaces(@NotNull final SimulationState state) {
+		VoronoiDiagram voronoiDiagram = new VoronoiDiagram(this.voronoiArea);
+
+		// convert pedestrians to positions
+		List<VPoint> pedestrianPositions = Agent.getPositions(state.getTopography().getElements(Agent.class));
+		voronoiDiagram.computeVoronoiDiagram(pedestrianPositions);
+
+		// compute everything
+		List<Face> faces = voronoiDiagram.getFaces();
+		return faces == null ? Collections.emptyList() : faces;
+	}
+
+	@Override
+	public double getVelocity(SimulationState state) {
+		List<Face> faces = generateFaces(state);
+
+		Map<Integer, Double> areaMap = new TreeMap<>();
+		Map<Integer, Face> faceMap = new TreeMap<>();
+
+		double velocity = 0.0;
+		double area = 0.0;
+		for (Face face : faces) {
+
+			if (measurementArea.contains(face.getSite())) {
+				VPoint center = face.getSite();
+				Agent ped = state.getTopography().getSpatialMap(Agent.class).getObjects(center, 0.2)
+						.stream()
+						.filter(agent -> center.distance(agent.getPosition()) < 0.01)
+						.findAny().get();
+
+				double faceArea = face.toPolygon().getArea();
+				area += faceArea;
+				velocity += (faceArea * ped.getVelocity().getLength());
+			}
+		}
+
+		return velocity / area;
+	}
+
+	private VPolygon toPolygon(@NotNull final Face face) {
+		List<VPoint> points = new ArrayList<>();
+		HalfEdge start = face.getOuterComponent();
+		HalfEdge next = start;
+
+		do {
+			next = next.getNext();
+			points.add(next.getOrigin());
+		} while (!start.equals(next));
+
+		return GeometryUtils.toPolygon(points);
+	}
+}

VadereState/src/org/vadere/state/attributes/processor/AttributesFundamentalDiagramEProcessor.java

+package org.vadere.state.attributes.processor;
+
+import org.vadere.util.geometry.shapes.VRectangle;
+
+public class AttributesFundamentalDiagramEProcessor extends AttributesAreaProcessor {
+	private VRectangle measurementArea;
+	private VRectangle voronoiArea;
+
+	public VRectangle getMeasurementArea() {
+		return measurementArea;
+	}
+
+	public VRectangle getVoronoiArea() {
+		return voronoiArea;
+	}
+
+	public void setVoronoiArea(VRectangle voronoiArea) {
+		checkSealed();
+		this.voronoiArea = voronoiArea;
+	}
+
+	public void setMeasurementArea(VRectangle measurementArea) {
+		checkSealed();
+		this.measurementArea = measurementArea;
+	}
+}

VadereState/src/org/vadere/state/simulation/VTrajectory.java

 package org.vadere.state.simulation;
 
 
+import org.apache.commons.lang3.tuple.Pair;
 import org.jetbrains.annotations.NotNull;
 import org.vadere.util.geometry.shapes.VRectangle;
 
+import java.util.ArrayList;
 import java.util.Iterator;
 import java.util.LinkedList;
 import java.util.List;
@@ -85,28 +87,36 @@ public class VTrajectory implements Iterable<FootStep> {
 	}
 
 	public VTrajectory cut(@NotNull final VRectangle rectangle) {
-		List<FootStep> intersectionSteps = footSteps.stream().filter(footStep -> footStep.intersects(rectangle)).collect(Collectors.toList());
-		if(intersectionSteps.size() == 2) {
-			double startSimTime = intersectionSteps.get(0).computeIntersectionTime(rectangle);
-			double endSimTime = intersectionSteps.get(1).computeIntersectionTime(rectangle);
-			return cut(startSimTime, endSimTime);
-		}
-		else if(intersectionSteps.size() == 1) {
-			double simTime = intersectionSteps.get(0).computeIntersectionTime(rectangle);
-			VTrajectory clone = clone();
+		LinkedList<FootStep> newFootSteps = new LinkedList<>();
+		boolean inside = rectangle.contains(footSteps.peekFirst().getStart());
+
+		for(FootStep footStep : footSteps) {
+			if(footStep.intersects(rectangle)) {
+				Pair<FootStep, FootStep> splitStep = footStep.cut(footStep.computeIntersectionTime(rectangle));
 
-			if(rectangle.contains(footSteps.peekLast().getEnd())) {
-				clone.cutHead(simTime);
+				if(!inside) {
+					newFootSteps.clear();
+					newFootSteps.add(splitStep.getRight());
 				}
 				else {
-				clone.cutTail(simTime);
+					newFootSteps.add(splitStep.getLeft());
 				}
-			return clone;
+
+				inside = !inside;
+
 			}
-		else if(intersectionSteps.size() > 0) {
-			throw new IllegalArgumentException("the number of intersection points is not zero or 2.");
+			else if(inside) {
+				newFootSteps.add(footStep);
 			}
-		return this;
+		}
+
+		VTrajectory copy = new VTrajectory();
+		copy.footSteps = newFootSteps;
+		return copy;
+	}
+
+	private boolean isEntering(@NotNull final VRectangle rectangle, @NotNull FootStep intersectionStep) {
+		return rectangle.contains(intersectionStep.getEnd());
 	}
 
 	public void cutTail(final double simStartTime) {

VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java

 package org.vadere.util.geometry;
 
+import java.awt.geom.Area;
 import java.awt.geom.Path2D;
+import java.awt.geom.PathIterator;
 import java.awt.geom.Point2D;
 import java.util.ArrayList;
 import java.util.Collection;
@@ -1123,15 +1125,21 @@ public class GeometryUtils {
 			return true;
 		}
 
+		boolean found = false;
 		int j = -1;
 		for(int i = 0; i < pointList1.size(); i++) {
 			VPoint p0 = pointList2.get(0);
 			if(p0.equals(pointList1.get(i))) {
 				j = i;
+				found = true;
 				break;
 			}
 		}
 
+		if(!found) {