fixing the problem with non-acute triangles for the FMM

VadereUtils/src/org/vadere/util/geometry/shapes/IPoint.java

@@ -27,4 +27,8 @@ public interface IPoint {
 	double distance(IPoint other);
 
 	double distanceToOrigin();
+
+	default double crossProduct(IPoint point) {
+		return getX() * point.getY() - point.getX() * getY();
+	}
 }

VadereUtils/src/org/vadere/util/geometry/shapes/VCone.java

+package org.vadere.util.geometry.shapes;
+
+import org.jetbrains.annotations.NotNull;
+import org.vadere.util.geometry.GeometryUtils;
+
+/**
+ * @author Benedikt Zoennchen
+ */
+public class VCone {
+	private VPoint position;
+	private VPoint direction;
+	private double angle;
+
+	public VCone(@NotNull final VPoint position, @NotNull final VPoint direction, double angle) {
+		if(angle <= 0) {
+			throw new IllegalArgumentException("angle of a cone has to be greater than 0.");
+		}
+
+		this.position = position;
+		this.direction = direction.norm();
+		this.angle = angle;
+	}
+
+	public boolean contains(final IPoint point) {
+		double angle = GeometryUtils.angle(point, position, position.add(direction));
+		return angle <= this.angle / 2;
+	}
+
+	public boolean overlapLineSegment(final VLine line) {
+		VPoint a = new VPoint(line.getX1(), line.getY1());
+		VPoint b = new VPoint(line.getX2(), line.getY2());
+
+		if(contains(a) ||contains(b)) {
+			return false;
+		}
+
+		VPoint v1 = position.subtract(a);
+		VPoint v2 = b.subtract(a);
+		VPoint v3 = new VPoint(-direction.getY(), direction.getX());
+		double t1 = v2.crossProduct(v1) / v2.scalarProduct(v3);
+		double t2 = v1.scalarProduct(v3) / v2.scalarProduct(v3);
+
+		assert Double.isFinite(t1) && Double.isFinite(t2);
+
+		// the line segment from a to b intersect the cone?
+		return t1 >= 0 && t2 <= 1 && t2 >= 0;
+	}
+}

VadereUtils/src/org/vadere/util/math/InterpolationUtil.java

@@ -16,11 +16,8 @@ import java.util.List;
  */
 public class InterpolationUtil {
 
-	public static double barycentricInterpolation(final Face<PotentialPoint> triangle, final double x, final double y){
-		List<PotentialPoint> points = triangle.getPoints();
-		if(points.size() != 3) {
-			System.out.println("error");
-		}
+	public static double barycentricInterpolation(final Face<? extends PotentialPoint> triangle, final double x, final double y){
+		List<? extends PotentialPoint> points = triangle.getPoints();
 		assert points.size() == 3;
 
 		PotentialPoint p1 = points.get(0);

VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMMAcuteTriangulation.java

+package org.vadere.util.potential.calculators;
+
+import org.vadere.util.geometry.GeometryUtils;
+import org.vadere.util.geometry.data.Face;
+import org.vadere.util.geometry.data.HalfEdge;
+import org.vadere.util.geometry.data.Triangulation;
+import org.vadere.util.geometry.shapes.IPoint;
+import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.math.InterpolationUtil;
+import org.vadere.util.potential.PathFindingTag;
+import org.vadere.util.potential.timecost.ITimeCostFunction;
+
+import java.util.Collection;
+import java.util.Comparator;
+import java.util.Iterator;
+import java.util.List;
+import java.util.PriorityQueue;
+
+
+public class EikonalSolverFMMAcuteTriangulation implements EikonalSolver  {
+
+	private ITimeCostFunction timeCostFunction;
+	private Triangulation<? extends PotentialPoint> triangulation;
+	private boolean calculationFinished;
+	private PriorityQueue<FFMHalfEdge> narrowBand;
+	private final Collection<IPoint> targetPoints;
+
+	private Comparator<FFMHalfEdge> pointComparator = (he1, he2) -> {
+		if (he1.halfEdge.getEnd().getPotential() < he2.halfEdge.getEnd().getPotential()) {
+			return -1;
+		} else if(he1.halfEdge.getEnd().getPotential() > he2.halfEdge.getEnd().getPotential()) {
+			return 1;
+		}
+		else {
+			return 0;
+		}
+	};
+
+	public EikonalSolverFMMAcuteTriangulation(final Collection<IPoint> targetPoints,
+	                                          final ITimeCostFunction timeCostFunction,
+	                                          final Triangulation<? extends PotentialPoint> triangulation) {
+		this.triangulation = triangulation;
+		this.calculationFinished = false;
+		this.timeCostFunction = timeCostFunction;
+		this.targetPoints = targetPoints;
+	}
+
+	@Override
+	public void initialize() {
+		this.narrowBand = new PriorityQueue<>(pointComparator);
+
+		for(IPoint point : targetPoints) {
+			Face<? extends PotentialPoint> face = triangulation.locate(point);
+
+			for(HalfEdge<? extends PotentialPoint> halfEdge : face) {
+				PotentialPoint potentialPoint = halfEdge.getEnd();
+				double distance = point.distance(potentialPoint);
+
+				if(potentialPoint.getPathFindingTag() != PathFindingTag.Undefined) {
+					narrowBand.remove(new FFMHalfEdge(halfEdge));
+				}
+
+				potentialPoint.setPotential(Math.min(potentialPoint.getPotential(), distance / timeCostFunction.costAt(potentialPoint)));
+				potentialPoint.setPathFindingTag(PathFindingTag.Reached);
+				narrowBand.add(new FFMHalfEdge(halfEdge));
+			}
+		}
+
+		calculate();
+	}
+
+	@Override
+	public double getValue(double x, double y) {
+		Face<? extends PotentialPoint> triangle = triangulation.locate(new VPoint(x, y));
+		return InterpolationUtil.barycentricInterpolation(triangle, x, y);
+	}
+
+
+	/**
+	 * Calculate the fast marching solution. This is called only once,
+	 * subsequent calls only return the result of the first.
+	 */
+	private void calculate() {
+		if (!calculationFinished) {
+			while (this.narrowBand.size() > 0) {
+				//System.out.println(narrowBand.size());
+				// poll the point with lowest data value
+				FFMHalfEdge ffmHalfEdge = this.narrowBand.poll();
+				// add it to the frozen points
+				ffmHalfEdge.halfEdge.getEnd().setPathFindingTag(PathFindingTag.Reached);
+				// recalculate the value based on the adjacent triangles
+				double potential = recalculatePoint(ffmHalfEdge.halfEdge);
+				ffmHalfEdge.halfEdge.getEnd().setPotential(Math.min(ffmHalfEdge.halfEdge.getEnd().getPotential(), potential));
+				// add narrow points
+				setNeighborDistances(ffmHalfEdge.halfEdge);
+			}
+
+			this.calculationFinished = true;
+		}
+	}
+
+	/**
+	 * Gets points in the narrow band around p.
+	 *
+	 * @param halfEdge
+	 * @return a set of points in the narrow band that are close to p.
+	 */
+	private void setNeighborDistances(final HalfEdge<? extends PotentialPoint> halfEdge) {
+		// remove frozen points
+		Iterator<? extends HalfEdge<? extends PotentialPoint>> it = halfEdge.incidentVertexIterator();
+
+		while (it.hasNext()) {
+			HalfEdge<? extends PotentialPoint> neighbour = it.next();
+			if(neighbour.getEnd().getPathFindingTag() == PathFindingTag.Undefined) {
+				double potential = recalculatePoint(neighbour);
+				neighbour.getEnd().setPotential(potential);
+				neighbour.getEnd().setPathFindingTag(PathFindingTag.Reachable);
+				narrowBand.add(new FFMHalfEdge(neighbour));
+
+			}
+			else if(neighbour.getEnd().getPathFindingTag() == PathFindingTag.Reachable) {
+				//double potential = neighbour.getEnd().getPotential();
+				double potential = recalculatePoint(neighbour);
+
+				// neighbour might be already in the narrowBand => update it
+				if (potential < neighbour.getEnd().getPotential()) {
+					FFMHalfEdge ffmHalfEdge = new FFMHalfEdge(neighbour);
+					narrowBand.remove(new FFMHalfEdge(neighbour));
+					neighbour.getEnd().setPotential(potential);
+					narrowBand.add(ffmHalfEdge);
+				}
+			}
+		}
+	}
+
+	/**
+	 * Recalculates the vertex given by the formulas in Sethian-1999.
+	 *
+	 * @param point
+	 * @return the same point, with a (possibly) changed data value.
+	 */
+	private double recalculatePoint(final HalfEdge<? extends PotentialPoint> point) {
+		// loop over all, check whether the point is contained and update its
+		// value accordingly
+		double potential = Double.MAX_VALUE;
+		Iterator<? extends Face<? extends PotentialPoint>> it = point.incidentFaceIterator();
+		while (it.hasNext()) {
+			Face<? extends PotentialPoint> face = it.next();
+			if(!face.isBorder()) {
+				potential = Math.min(updatePoint(point, face), potential);
+			}
+		}
+		return potential;
+	}
+
+	/**
+	 * Updates a point given a triangle. The point can only be updated if the
+	 * triangle contains it and the other two points are in the frozen band.
+	 *
+	 * @param halfEdge
+	 * @param face
+	 */
+	private double updatePoint(final HalfEdge<? extends PotentialPoint> halfEdge, final Face<? extends PotentialPoint> face) {
+		// check whether the triangle does contain useful data
+		List<? extends PotentialPoint> points = face.getPoints();
+		points.removeIf(p -> p.equals(halfEdge.getEnd()));
+
+		assert points.size() == 2;
+
+		PotentialPoint p1 = points.get(0);
+		PotentialPoint p2 = points.get(1);
+		PotentialPoint point = halfEdge.getEnd();
+
+		if ((Double.isInfinite(p1.getPotential()) && Double.isInfinite((p2.getPotential())))
+				|| (Double.isInfinite(p1.getPotential()) && Double.isInfinite(point.getPotential()))
+				|| (Double.isInfinite(p2.getPotential()) && Double.isInfinite(point.getPotential()))) {
+			return halfEdge.getEnd().getPotential();
+		}
+
+		// check whether they are in the frozen set. only if they are, we can
+		// continue.
+		// if(this.frozenPoints.contains(points.first()) &&
+		// this.frozenPoints.contains(points.last()))
+
+		if(p1.getPathFindingTag().frozen && p2.getPathFindingTag().frozen)
+		{
+			// see: Sethian, Level Set Methods and Fast Marching Methods, page
+			// 124.
+			double u = p2.getPotential() - p1.getX();
+			double a = p2.distance(point);
+			double b = p1.distance(point);
+			double c = p1.distance(p2);
+			double TA = p1.getPotential();
+			double TB = p2.getPotential();
+
+			double phi = GeometryUtils.angle(p1, point, p2);
+			double cosphi = Math.cos(phi);
+
+			double F = 1.0 / this.timeCostFunction.costAt(point);
+
+			// solve x2 t^2 + x1 t + x0 == 0
+			double x2 = a * a + b * b - 2 * a * b * cosphi;
+			double x1 = 2 * b * u * (a * cosphi - b);
+			double x0 = b * b
+					* (u * u - F * F * a * a * Math.sin(phi) * Math.sin(phi));
+			double t = solveQuadratic(x2, x1, x0);
+
+			double inTriangle = (b * (t - u) / t);
+			if (u < t && a * cosphi < inTriangle && inTriangle < a / cosphi) {
+				return t + TA;
+			} else {
+				return Math.min(b * F + TA, c * F + TB);
+			}
+		}
+		else {
+			return halfEdge.getEnd().getPotential();
+		}
+	}
+
+	/**
+	 * Solves the quadratic equation given by a x^2+bx+c=0.
+	 *
+	 * @param a
+	 * @param b
+	 * @param c
+	 * @return the maximum of both solutions, if any. If det=b^2-4ac < 0, it
+	 *         returns Double.MIN_VALUE
+	 */
+	private double solveQuadratic(double a, double b, double c) {
+		double det = b * b - 4 * a * c;
+		if (det < 0) {
+			return Double.MIN_VALUE;
+		}
+
+		return Math.max((-b + Math.sqrt(det)) / (2 * a), (-b - Math.sqrt(det))
+				/ (2 * a));
+	}
+
+	/**
+	 * We require a half-edge that has an equals which only depends on the end-vertex.
+	 */
+	private class FFMHalfEdge {
+		private HalfEdge<? extends PotentialPoint> halfEdge;
+
+		public FFMHalfEdge(final HalfEdge<? extends PotentialPoint> halfEdge){
+			this.halfEdge = halfEdge;
+		}
+
+		@Override
+		public boolean equals(Object o) {
+			if (this == o) return true;
+			if (o == null || getClass() != o.getClass()) return false;
+
+			FFMHalfEdge that = (FFMHalfEdge) o;
+
+			return halfEdge.getEnd().equals(that.halfEdge.getEnd());
+		}
+
+		@Override
+		public int hashCode() {
+			return halfEdge.getEnd().hashCode();
+		}
+
+		@Override
+		public String toString() {
+			return halfEdge.toString();
+		}
+	}
+}

VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverFMMTriangulation.java

 package org.vadere.util.potential.calculators;
 
+import org.vadere.util.geometry.Geometry;
 import org.vadere.util.geometry.GeometryUtils;
 import org.vadere.util.geometry.data.Face;
 import org.vadere.util.geometry.data.HalfEdge;
 import org.vadere.util.geometry.data.Triangulation;
+import org.vadere.util.geometry.shapes.IPoint;
+import org.vadere.util.geometry.shapes.VCone;
+import org.vadere.util.geometry.shapes.VLine;
 import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.geometry.shapes.VTriangle;
 import org.vadere.util.math.InterpolationUtil;
 import org.vadere.util.potential.PathFindingTag;
 import org.vadere.util.potential.timecost.ITimeCostFunction;
@@ -19,10 +24,10 @@ import java.util.PriorityQueue;
 public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 
 	private ITimeCostFunction timeCostFunction;
-	private Triangulation<PotentialPoint> triangulation;
+	private Triangulation<? extends PotentialPoint> triangulation;
 	private boolean calculationFinished;
 	private PriorityQueue<FFMHalfEdge> narrowBand;
-	private final Collection<HalfEdge<PotentialPoint>> targetPoints;
+	private final Collection<IPoint> targetPoints;
 
 	private Comparator<FFMHalfEdge> pointComparator = (he1, he2) -> {
 		if (he1.halfEdge.getEnd().getPotential() < he2.halfEdge.getEnd().getPotential()) {
@@ -35,9 +40,9 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 		}
 	};
 
-	public EikonalSolverFMMTriangulation(final Collection<HalfEdge<PotentialPoint>> targetPoints,
-	                                     final ITimeCostFunction timeCostFunction,
-	                                     final Triangulation<PotentialPoint> triangulation) {
+	public EikonalSolverFMMTriangulation(final Collection<IPoint> targetPoints,
+	                                          final ITimeCostFunction timeCostFunction,
+	                                          final Triangulation<? extends PotentialPoint> triangulation) {
 		this.triangulation = triangulation;
 		this.calculationFinished = false;
 		this.timeCostFunction = timeCostFunction;
@@ -48,10 +53,21 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 	public void initialize() {
 		this.narrowBand = new PriorityQueue<>(pointComparator);
 
-		for(HalfEdge<PotentialPoint> halfEdge : targetPoints) {
-			halfEdge.getEnd().setPathFindingTag(PathFindingTag.Target);
-			halfEdge.getEnd().setPotential(0.0);
-			narrowBand.add(new FFMHalfEdge(halfEdge));
+		for(IPoint point : targetPoints) {
+			Face<? extends PotentialPoint> face = triangulation.locate(point);
+
+			for(HalfEdge<? extends PotentialPoint> halfEdge : face) {
+				PotentialPoint potentialPoint = halfEdge.getEnd();
+				double distance = point.distance(potentialPoint);
+
+				if(potentialPoint.getPathFindingTag() != PathFindingTag.Undefined) {
+					narrowBand.remove(new FFMHalfEdge(halfEdge));
+				}
+
+				potentialPoint.setPotential(Math.min(potentialPoint.getPotential(), distance / timeCostFunction.costAt(potentialPoint)));
+				potentialPoint.setPathFindingTag(PathFindingTag.Reached);
+				narrowBand.add(new FFMHalfEdge(halfEdge));
+			}
 		}
 
 		calculate();
@@ -59,13 +75,8 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 
 	@Override
 	public double getValue(double x, double y) {
-		Face<PotentialPoint> triangle = triangulation.locate(new VPoint(x, y));
-		if(triangle != null) {
-			return InterpolationUtil.barycentricInterpolation(triangle, x, y);
-		}
-		else {
-			return Double.MAX_VALUE;
-		}
+		Face<? extends PotentialPoint> triangle = triangulation.locate(new VPoint(x, y));
+		return InterpolationUtil.barycentricInterpolation(triangle, x, y);
 	}
 
 
@@ -76,7 +87,7 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 	private void calculate() {
 		if (!calculationFinished) {
 			while (this.narrowBand.size() > 0) {
-				System.out.println(narrowBand.size());
+				//System.out.println(narrowBand.size());
 				// poll the point with lowest data value
 				FFMHalfEdge ffmHalfEdge = this.narrowBand.poll();
 				// add it to the frozen points
@@ -98,12 +109,12 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 	 * @param halfEdge
 	 * @return a set of points in the narrow band that are close to p.
 	 */
-	private void setNeighborDistances(final HalfEdge<PotentialPoint> halfEdge) {
+	private void setNeighborDistances(final HalfEdge<? extends PotentialPoint> halfEdge) {
 		// remove frozen points
-		Iterator<HalfEdge<PotentialPoint>> it = halfEdge.incidentVertexIterator();
+		Iterator<? extends HalfEdge<? extends PotentialPoint>> it = halfEdge.incidentVertexIterator();
 
 		while (it.hasNext()) {
-			HalfEdge<PotentialPoint> neighbour = it.next();
+			HalfEdge<? extends PotentialPoint> neighbour = it.next();
 			if(neighbour.getEnd().getPathFindingTag() == PathFindingTag.Undefined) {
 				double potential = recalculatePoint(neighbour);
 				neighbour.getEnd().setPotential(potential);
@@ -132,15 +143,15 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 	 * @param point
 	 * @return the same point, with a (possibly) changed data value.
 	 */
-	private double recalculatePoint(final HalfEdge<PotentialPoint> point) {
+	private double recalculatePoint(final HalfEdge<? extends PotentialPoint> point) {
 		// loop over all, check whether the point is contained and update its
 		// value accordingly
 		double potential = Double.MAX_VALUE;
-		Iterator<Face<PotentialPoint>> it = point.incidentFaceIterator();
+		Iterator<? extends Face<? extends PotentialPoint>> it = point.incidentFaceIterator();
 		while (it.hasNext()) {
-			Face<PotentialPoint> face = it.next();
+			Face<? extends PotentialPoint> face = it.next();
 			if(!face.isBorder()) {
-				potential = Math.min(updatePoint(point, face), potential);
+				potential = Math.min(updatePoint(point.getEnd(), face), potential);
 			}
 		}
 		return potential;
@@ -150,24 +161,65 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 	 * Updates a point given a triangle. The point can only be updated if the
 	 * triangle contains it and the other two points are in the frozen band.
 	 *
-	 * @param halfEdge
+	 * @param point
 	 * @param face
 	 */
-	private double updatePoint(final HalfEdge<PotentialPoint> halfEdge, final Face<PotentialPoint> face) {
+	private double updatePoint(final PotentialPoint point, final Face<? extends PotentialPoint> face) {
 		// check whether the triangle does contain useful data
-		List<PotentialPoint> points = face.getPoints();
-		points.removeIf(p -> p.equals(halfEdge.getEnd()));
+		List<? extends PotentialPoint> points = face.getPoints();
+		HalfEdge<? extends PotentialPoint> halfEdge = face.stream().filter(p -> p.getEnd().equals(point)).findAny().get();
+		points.removeIf(p -> p.equals(point));
 
 		assert points.size() == 2;
-
 		PotentialPoint p1 = points.get(0);
 		PotentialPoint p2 = points.get(1);
-		PotentialPoint point = halfEdge.getEnd();
+
+		// search another vertex in the acute cone
+		VTriangle triangle = face.toTriangle();
+		if(GeometryUtils.angle(p1, point, p2) > Math.PI / 2) {
+			// 1. construct the acute cone
+			VPoint direction = triangle.getIncenter().subtract(point);
+			double angle = Math.PI - GeometryUtils.angle(p1, point, p2);
+			VPoint o = new VPoint(point);
+			VCone cone = new VCone(new VPoint(point.getX(), point.getY()), direction, angle);
+
+			// 2. search for the nearest point inside the cone
+			HalfEdge<? extends PotentialPoint> he = halfEdge.getPrevious().getTwin();
+			while(!cone.contains(he.getNext().getEnd()) && !cone.contains(he.getPrevious().getEnd())) {
+				VLine line1 = new VLine(new VPoint(he.getEnd()), new VPoint(he.getNext().getEnd()));
+				VLine line2 = new VLine(new VPoint(he.getNext().getEnd()), new VPoint(he.getNext().getNext().getEnd()));
+				// the line segment from a to b intersect the cone?
+				if(cone.overlapLineSegment(line1)) {
+					he = he.getNext().getTwin();
+				}
+				else if(cone.overlapLineSegment(line2)) {
+					he = he.getNext().getNext().getTwin();
+				}
+				else {
+					cone.overlapLineSegment(line1);
+					cone.overlapLineSegment(line2);
+					throw new IllegalStateException("no line overlap the acute cone!");
+				}
+			}
+
+			he = cone.contains(he.getNext().getEnd()) ? he.getNext() : he.getPrevious();
+
+			double pot1 = updatePoint(point, he.getEnd(), p1);
+			double pot2 = updatePoint(point, he.getEnd(), p2);
+			return Math.min(pot1, pot2);
+
+		}
+		else {
+			return updatePoint(point, p1, p2);
+		}
+	}
+
+	private double updatePoint(final PotentialPoint point, final PotentialPoint p1, final PotentialPoint p2) {
 
 		if ((Double.isInfinite(p1.getPotential()) && Double.isInfinite((p2.getPotential())))
 				|| (Double.isInfinite(p1.getPotential()) && Double.isInfinite(point.getPotential()))
 				|| (Double.isInfinite(p2.getPotential()) && Double.isInfinite(point.getPotential()))) {
-			return halfEdge.getEnd().getPotential();
+			return point.getPotential();
 		}
 
 		// check whether they are in the frozen set. only if they are, we can
@@ -175,7 +227,7 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 		// if(this.frozenPoints.contains(points.first()) &&
 		// this.frozenPoints.contains(points.last()))
 
-		if(p1.getPathFindingTag().frozen && p2.getPathFindingTag().frozen)
+		//if(p1.getPathFindingTag().frozen && p2.getPathFindingTag().frozen)
 		{
 			// see: Sethian, Level Set Methods and Fast Marching Methods, page
 			// 124.
@@ -205,9 +257,9 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver  {
 				return Math.min(b * F + TA, c * F + TB);
 			}
 		}
-		else {
+		/*else {
 			return halfEdge.getEnd().getPotential();