Commit 385c9537 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

fixing the problem with non-acute triangles for the FMM

parent dea61af2
......@@ -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();
}
}
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;
}
}
......@@ -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);
......
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();
}
}
}
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();