before changing initial point generation of DSMesh

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

@@ -82,7 +82,7 @@ public class GeometryUtils {
 
 		for (int i = 0; i < allPoints.size(); i++) {
 			Vector2D p = new Vector2D(allPoints.get(i));
-			orderedList.add(new DataPoint(p.x, p.y, p.angleTo(center)));
+			orderedList.add(new DataPoint(p.x, p.y, GeometryUtils.angleTo(p, center)));
 		}
 		// sort by angle
 		Collections.sort(orderedList, DataPoint.getComparator());
@@ -221,12 +221,31 @@ public class GeometryUtils {
 	 * @return
 	 */
 	public static double angle(IPoint A, IPoint C, IPoint B) {
-		double phi1 = new Vector2D(A).angleTo(C);
-		double phi2 = new Vector2D(B).angleTo(C);
+		double phi1 = angleTo(A, C);
+		double phi2 = angleTo(B, C);
 		double phi = Math.abs(phi1 - phi2);
 		return Math.min(phi, 2 * Math.PI - phi);
 	}
 
+	/**
+	 *
+	 * Computes the angle between the x-axis through the given Point "center" and this.
+	 * Result is in interval (0,2*PI) according to standard math usage.
+	 */
+	public static double angleTo(final IPoint from, final IPoint to) {
+		double atan2 = Math.atan2(from.getY() - to.getY(), from.getX() - to.getX());
+
+		if (atan2 < 0.0) {
+			atan2 = Math.PI * 2 + atan2;
+		}
+
+		return atan2;
+	}
+
+	public static double angleTo(final IPoint to) {
+		return angleTo(new VPoint(0,0), to);
+	}
+
 	/**
 	 * Returns the angle between line1 and line2 in clock wise order (cw).
 	 * @param line1

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

@@ -65,21 +65,6 @@ public class Vector2D extends VPoint {
 		return atan2;
 	}
 
-	/**
-	 * 
-	 * Computes the angle between the x-axis through the given Point "center" and this.
-	 * Result is in interval (0,2*PI) according to standard math usage.
-	 */
-	public double angleTo(IPoint center) {
-		double atan2 = Math.atan2(this.y - center.getY(), this.x - center.getX());
-
-		if (atan2 < 0.0) {
-			atan2 = Math.PI * 2 + atan2;
-		}
-
-		return atan2;
-	}
-
 	public Vector2D add(VPoint p) {
 		return new Vector2D(this.x + p.x, this.y + p.y);
 	}

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

 package org.vadere.util.geometry.shapes;
 
 import org.jetbrains.annotations.NotNull;
+import org.omg.CORBA.portable.ValueOutputStream;
 import org.vadere.util.geometry.GeometryUtils;
 
 /**
  * @author Benedikt Zoennchen
  */
 public class VCone {
+	private VPoint rayDirection1;
+	private VPoint rayDirection2;
 	private VPoint position;
+	private VPoint rayPosition1;
+	private VPoint rayPosition2;
 	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.");
+		if(angle <= 0 || angle >= Math.PI) {
+			throw new IllegalArgumentException("angle of a cone has to be greater than 0 and smaller than pi.");
 		}
 
 		this.position = position;
 		this.direction = direction.norm();
+		this.rayDirection1 = direction.rotate(angle/2).norm();
+		this.rayDirection2 = direction.rotate(-angle/2).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 contains(final VPoint point) {
+		double angle1 = GeometryUtils.angleTo(point.subtract(position));
+		double angle2 = GeometryUtils.angleTo(direction);
+		return Math.abs(angle1 - angle2) < angle / 2;
 	}
 
+	/*public boolean intersect(final VTriangle triangle) {
+		double max = triangle.maxCoordinate();
+		this.rayPosition1 = position.add(rayDirection1.scalarMultiply(max));
+		this.rayPosition2 = position.add(rayDirection2.scalarMultiply(max));
+		if(rayPosition1.equals(rayPosition2)) {
+			System.out.println("ww");
+		}
+		return new VTriangle(position, rayPosition1, rayPosition2).intersect(triangle);
+	}*/
+
+	/*
+	 * TODO: test
+	 */
 	public boolean overlapLineSegment(final VLine line) {
 		VPoint a = new VPoint(line.getX1(), line.getY1());
 		VPoint b = new VPoint(line.getX2(), line.getY2());
@@ -39,10 +61,22 @@ public class VCone {
 		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;
 	}
+
+	private boolean isLeft(IPoint a, IPoint b, IPoint c) {
+		return ((b.getY() - a.getX())*(c.getY() - a.getY()) - (b.getY() - a.getY())*(c.getX() - a.getX())) > 0;
+	}
+
+	private boolean isRight(IPoint a, IPoint b, IPoint c) {
+		return ((b.getY() - a.getX())*(c.getY() - a.getY()) - (b.getY() - a.getY())*(c.getX() - a.getX())) < 0;
+	}
+
+	private boolean isOn(IPoint a, IPoint b, IPoint c) {
+		return ((b.getY() - a.getX())*(c.getY() - a.getY()) - (b.getY() - a.getY())*(c.getX() - a.getX())) == 0;
+	}
 }

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

@@ -14,6 +14,9 @@ public class VLine extends Line2D.Double {
 
 	public VLine(final VPoint p1, final VPoint p2) {
 		super(p1.getX(), p1.getY(), p2.getX(), p2.getY());
+		if(p1.equals(p2)) {
+			throw new IllegalArgumentException(p1 + " is equal " + p2);
+		}
 		this.p1 = p1;
 		this.p2 = p2;
 	}

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

@@ -204,6 +204,13 @@ public class VTriangle extends VPolygon {
 		return Arrays.stream(getLines());
 	}
 
+	public double maxCoordinate() {
+		double max = Math.max(Math.abs(p1.getX()), Math.abs(p1.getY()));
+		max = Math.max(max, Math.max(Math.abs(p2.getX()), Math.abs(p2.getY())));
+		max = Math.max(max, Math.max(Math.abs(p3.getX()), Math.abs(p3.getY())));
+		return max;
+	}
+
 	@Override
 	public String toString() {
 		return p1 + "-" + p2 + "-" + p3;

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

@@ -371,10 +371,10 @@ public class MathUtil {
 			} else if (discr > 0) {
 				Collections.addAll(result, (-b + Math.sqrt(discr)) / (2.0 * a), (-b - Math.sqrt(discr)) / (2.0 * a));
 			}
-		} else if (c != 0) {
+		} else if (b != 0) {
 			result.add(-c / b);
 		} else {
-			throw new IllegalArgumentException("ax^2 + bx + c = 0 is not a valid quadratic equation for a=b=0.");
+			//throw new IllegalArgumentException("ax^2 + bx + c = 0 is not a valid quadratic equation for a=b=0.");
 		}
 
 		return result;

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

@@ -2,7 +2,6 @@ package org.vadere.util.potential.calculators;
 
 import org.apache.log4j.LogManager;
 import org.apache.log4j.Logger;
-import org.vadere.util.geometry.Geometry;
 import org.vadere.util.geometry.GeometryUtils;
 import org.vadere.util.geometry.LineIterator;
 import org.vadere.util.geometry.data.Face;
@@ -13,23 +12,25 @@ 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.VRectangle;
-import org.vadere.util.geometry.shapes.VShape;
 import org.vadere.util.geometry.shapes.VTriangle;
 import org.vadere.util.math.InterpolationUtil;
 import org.vadere.util.math.MathUtil;
 import org.vadere.util.potential.PathFindingTag;
 import org.vadere.util.potential.timecost.ITimeCostFunction;
+import org.vadere.util.triangulation.FaceIterator;
 import org.vadere.util.triangulation.PointConstructor;
-import org.vadere.util.triangulation.adaptive.MeshPoint;
 
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.Comparator;
+import java.util.HashSet;
 import java.util.Iterator;
+import java.util.LinkedList;
 import java.util.List;
-import java.util.Optional;
 import java.util.PriorityQueue;
+import java.util.Set;
+import java.util.stream.Collectors;
 
 
 public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements EikonalSolver  {
@@ -41,7 +42,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 	private boolean calculationFinished;
 	private PriorityQueue<FFMHalfEdge> narrowBand;
 	private final Collection<VRectangle> targetAreas;
-	private final PointConstructor<P> pointConstructor;
+	private PointConstructor<P> pointConstructor;
 
 	private Comparator<FFMHalfEdge> pointComparator = (he1, he2) -> {
 		if (he1.halfEdge.getEnd().getPotential() < he2.halfEdge.getEnd().getPotential()) {
@@ -64,6 +65,35 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 		this.timeCostFunction = timeCostFunction;
 		this.targetAreas = targetAreas;
 		this.pointConstructor = pointConstructor;
+		this.narrowBand = new PriorityQueue<>(pointComparator);
+	}
+
+	public EikonalSolverFMMTriangulation(final Collection<IPoint> targetPoints,
+	                                     final ITimeCostFunction timeCostFunction,
+	                                     final Triangulation<P> triangulation
+	) {
+		this.triangulation = triangulation;
+		this.calculationFinished = false;
+		this.timeCostFunction = timeCostFunction;
+		this.targetAreas = new ArrayList<>();
+		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));
+			}
+		}
 	}
 
 
@@ -113,7 +143,6 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 
 	@Override
 	public void initialize() {
-		narrowBand = new PriorityQueue<>(pointComparator);
 		initializeTargetAreas();
 		/*for(IPoint point : targetPoints) {
 			Face<? extends PotentialPoint> face = triangulation.locate(point);
@@ -186,7 +215,6 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 			HalfEdge<? extends PotentialPoint> neighbour = it.next();
 			if(neighbour.getEnd().getPathFindingTag() == PathFindingTag.Undefined) {
 				double potential = recalculatePoint(neighbour);
-
 				// if not, it was not possible to compute a valid potential. TODO?
 				if(potential < neighbour.getEnd().getPotential()) {
 					neighbour.getEnd().setPotential(potential);
@@ -194,9 +222,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 					narrowBand.add(new FFMHalfEdge(neighbour));
 				}
 				else {
-					logger.warn("could not set neighbour vertex" + neighbour + "," + neighbour.getFace().isBorder());
-					potential = recalculatePoint(neighbour);
-					potential = recalculatePoint(neighbour);
+					//logger.warn("could not set neighbour vertex" + neighbour + "," + neighbour.getFace().isBorder());
 				}
 			}
 			else if(neighbour.getEnd().getPathFindingTag() == PathFindingTag.Reachable) {
@@ -228,7 +254,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 		while (it.hasNext()) {
 			Face<? extends PotentialPoint> face = it.next();
 			if(!face.isBorder()) {
-				potential = Math.min(updatePoint(point.getEnd(), face), potential);
+				potential = Math.min(computeValue(point.getEnd(), face), potential);
 			}
 		}
 		return potential;
@@ -241,7 +267,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 	 * @param point
 	 * @param face
 	 */
-	private double updatePoint(final PotentialPoint point, final Face<? extends PotentialPoint> face) {
+	private double computeValue(final PotentialPoint point, final Face<? extends PotentialPoint> face) {
 		// check whether the triangle does contain useful data
 		List<? extends PotentialPoint> points = face.getPoints();
 		HalfEdge<? extends PotentialPoint> halfEdge = face.stream().filter(p -> p.getEnd().equals(point)).findAny().get();
@@ -251,32 +277,98 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 		PotentialPoint p1 = points.get(0);
 		PotentialPoint p2 = points.get(1);
 
-		/*if(!p1.getPathFindingTag().frozen && !p2.getPathFindingTag().frozen) {
-			return Double.MAX_VALUE;
+		if(feasableForComputation(p1) && feasableForComputation(p2) && !face.toTriangle().isNonAcute()){
+			return computeValue(point, p1, p2);
+		}
+		return point.getPotential();
+
+		// the general case
+		/*double angle = GeometryUtils.angle(p1, point, p2);
+		if(((feasableForComputation(p1) && feasableForComputation(p2)) || angle < Math.PI / 2)) {
+			return computeValue(point, p1, p2);
+		} // no update coming from this face is possible.
+		else if(!feasableForComputation(p1) || !feasableForComputation(p2)) {
+			PotentialPoint feasablePoint = feasableForComputation(p1) ? p1 : p2;
+			HalfEdge<? extends PotentialPoint> he = findPointInCone(halfEdge, p1, p2);
+			if(he == null) {
+				logger.warn("inconsistent update");
+				//return computeValue(point, p1, p2);
+				return feasablePoint.getPotential();
+			}
+			else {
+				logger.info("consistent update " + point.distance(he.getEnd()));
+				assert GeometryUtils.angle(feasablePoint, point, he.getEnd()) < Math.PI / 2;
+				return computeValue(point, feasablePoint, he.getEnd());
+			}
+		}
+		else { // both neighbours are computed but the angle is to large.
+			return point.getPotential();
 		}*/
+	}
+
+	private boolean feasableForComputation(final PotentialPoint p){
+		//return p.getPathFindingTag().frozen;
+		return p.getPathFindingTag() == PathFindingTag.Reachable || p.getPathFindingTag() == PathFindingTag.Reached;
+	}
+
+	private HalfEdge<? extends PotentialPoint> findIntersectionPoint(final HalfEdge<? extends PotentialPoint> halfEdge, final PotentialPoint acceptedPoint, final PotentialPoint unacceptedPoint) {
+		PotentialPoint point = halfEdge.getEnd();
+		VTriangle triangle = new VTriangle(new VPoint(point), new VPoint(acceptedPoint), new VPoint(unacceptedPoint));
 
-		// search another vertex in the acute cone
-		//if(GeometryUtils.angle(p1, point, p2) > Math.PI / 2) {
-		if(!p1.getPathFindingTag().frozen || !p2.getPathFindingTag().frozen) {
-			// find support vertex
+		// 1. construct the acute cone
+		VPoint direction = triangle.getIncenter().subtract(point);
+		double angle = Math.PI - GeometryUtils.angle(acceptedPoint, point, unacceptedPoint);
+		VPoint origin = new VPoint(point);
+		VCone cone = new VCone(origin, direction, angle);
+
+		HalfEdge<? extends PotentialPoint> minHe = null;
 
-			Optional<HalfEdge<? extends PotentialPoint>> optHe = findPointInCone(halfEdge, p1, p2);
+		//
+		/*Predicate<Face<? extends PotentialPoint>> pred = f -> {
+			List<HalfEdge<? extends PotentialPoint>> pointList = f.stream()
+				//.filter(he -> he.getEnd().getPathFindingTag().frozen)
+				.filter(he -> !he.getEnd().equals(acceptedPoint))
+				.filter(he -> !he.getEnd().equals(point))
+				.collect(Collectors.toList());
 
-			if(optHe.isPresent()) {
-				double pot1 = updatePoint(point, optHe.get().getEnd(), p1);
-				double pot2 = updatePoint(point, optHe.get().getEnd(), p2);
-				return Math.min(pot1, pot2);
+			for(HalfEdge<? extends PotentialPoint> he : pointList) {
+				VTriangle vTriangle = new VTriangle(new VPoint(he.getEnd()), new VPoint(point), new VPoint(acceptedPoint));
+				if(cone.intersect(vTriangle)) {
+					return true;
+				}
 			}
-			else {
-				return point.getPotential();
+			return false;
+		};*/
+		//
+
+
+
+		// 2. search for the nearest point inside the cone
+		FaceIterator<? extends PotentialPoint> faceIterator = new FaceIterator(halfEdge.getFace());
+		while (faceIterator.hasNext()) {
+			Face<? extends PotentialPoint> face = faceIterator.next();
+			List<HalfEdge<? extends PotentialPoint>> pointList = face.stream()
+					.filter(he -> feasableForComputation(he.getEnd()))
+					.filter(he -> !he.getEnd().equals(acceptedPoint))
+					.filter(he -> !he.getEnd().equals(point))
+					.collect(Collectors.toList());
+
+			for(HalfEdge<? extends PotentialPoint> he : pointList) {
+				if(cone.contains(new VPoint(he.getEnd()))) {
+					if(minHe == null || minHe.getEnd().getPotential() > he.getEnd().getPotential()) {
+						minHe = he;
+						return minHe;
+					}
+				}
 			}
 		}
-		else {
-			return updatePoint(point, p1, p2);
-		}
+
+		return minHe;
 	}
 
-	private Optional<HalfEdge<? extends PotentialPoint>> findPointInCone(final HalfEdge<? extends PotentialPoint> halfEdge, final PotentialPoint p1, final PotentialPoint p2) {
+
+	//TODO: refactoring!
+	private HalfEdge<? extends PotentialPoint> findPointInCone(final HalfEdge<? extends PotentialPoint> halfEdge, final PotentialPoint p1, final PotentialPoint p2) {
 		PotentialPoint point = halfEdge.getEnd();
 		VTriangle triangle = new VTriangle(new VPoint(point), new VPoint(p1), new VPoint(p2));
 
@@ -287,36 +379,63 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 		VCone cone = new VCone(origin, direction, angle);
 
 		// 2. search for the nearest point inside the cone
-		HalfEdge<? extends PotentialPoint> he = halfEdge.getPrevious().getTwin();
+		Set<Face<? extends PotentialPoint>> visitedFaces = new HashSet<>();
+		LinkedList<HalfEdge<? extends PotentialPoint>> pointList = new LinkedList<>();
+		pointList.add(halfEdge.getPrevious().getTwin().getNext());
+		visitedFaces.add(halfEdge.getPrevious().getTwin().getNext().getFace());
+
+		while (!pointList.isEmpty()) {
+			HalfEdge<? extends PotentialPoint> candidate = pointList.removeFirst();
+
+			// we can not search further since we reach the boundary.
+			if (!candidate.isBoundary()) {
+				if (feasableForComputation(candidate.getEnd()) && cone.contains(new VPoint(candidate.getEnd()))) {
+					return candidate;
+				} else if(cone.contains(new VPoint(candidate.getEnd()))) {
+					HalfEdge<? extends PotentialPoint> newCandidate = candidate.getTwin().getNext();
+					if (!visitedFaces.contains(newCandidate.getFace())) {
+						visitedFaces.add(newCandidate.getFace());
+						pointList.add(newCandidate);
+					}
 
-		while((!cone.contains(he.getNext().getEnd()) && !cone.contains(he.getPrevious().getEnd()))) {
+					newCandidate = candidate.getNext().getTwin().getNext();
+					if (!visitedFaces.contains(newCandidate.getFace())) {
+						visitedFaces.add(newCandidate.getFace());
+						pointList.add(newCandidate);
+					}
+				}
+				else {
+					VLine line1 = new VLine(new VPoint(candidate.getEnd()), new VPoint(candidate.getPrevious().getEnd()));
+					VLine line2 = new VLine(new VPoint(candidate.getEnd()), new VPoint(candidate.getNext().getEnd()));
+
+					if (cone.overlapLineSegment(line1)) {
+						HalfEdge<? extends PotentialPoint> newCandidate = candidate.getTwin().getNext();
+						if (!visitedFaces.contains(newCandidate.getFace())) {
+							visitedFaces.add(newCandidate.getFace());
+							pointList.add(newCandidate);
+						}
+					}
 
-			if(he.isBoundary()) {
-				return Optional.empty();
-			}
+					if (cone.overlapLineSegment(line2)) {
+						HalfEdge<? extends PotentialPoint> newCandidate = candidate.getNext().getTwin().getNext();
 
-			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();
+						if (!visitedFaces.contains(newCandidate.getFace())) {
+							visitedFaces.add(newCandidate.getFace());
+							pointList.add(newCandidate);
+						}
+					}
+				}
 			}
 			else {
-				logger.warn("could not recompute potential for " + point);
-				//findPointInCone(halfEdge, p1, p2);
-				return Optional.empty();
-				//findPointInCone(halfEdge, p1, p2);
-				//throw new IllegalStateException("no line overlap the acute cone!");
+				logger.warn("boundary reached!");
 			}
 		}
 
-		return Optional.of(he);
+		logger.warn("no virtual vertex was found");
+		return null;
 	}
 
-	private double updatePoint(final PotentialPoint point, final PotentialPoint p1, final PotentialPoint p2) {
+	private double computeValue(final PotentialPoint point, final PotentialPoint point1, final PotentialPoint point2) {
 
 		/*if ((Double.isInfinite(p1.getPotential()) && Double.isInfinite((p2.getPotential())))
 				|| (Double.isInfinite(p1.getPotential()) && Double.isInfinite(point.getPotential()))
@@ -333,37 +452,45 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements
 				(p1.getPathFindingTag() == PathFindingTag.Reached || p1.getPathFindingTag() == PathFindingTag.Reachable)
 						&& (p2.getPathFindingTag() == PathFindingTag.Reached || p2.getPathFindingTag() == PathFindingTag.Reachable))
 		{*/
-		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.
-			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);
-			}
+		PotentialPoint p1;
+		PotentialPoint p2;
+
+		// assuming T(B) > T(A)
+		if(point1.getPotential() > point2.getPotential()) {
+			p2 = point1;
+			p1 = point2;
 		}
 		else {
-			return point.getPotential();
+			p2 = point2;
+			p1 = point1;
+		}
+
+		double TA = p1.getPotential();
+		double TB = p2.getPotential();
+		double u = TB - TA;
+		double a = p1.distance(point);
+		double b = p2.