Commit f46538bd authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

before change of the fmm update scheme

parent 550befe2
......@@ -129,7 +129,9 @@ public class HalfEdge<P extends IPoint> implements Iterable<HalfEdge<P>> {
public Iterator<Face<P>> incidentFaceIterator() { return new NeighbourFaceIterator(); }
public List<Face<P>> getIncidentFaces() {
return IteratorUtils.toList(incidentFaceIterator());
}
public List<HalfEdge<P>> getIncidentPoints() {
List<HalfEdge<P>> incidentPoints = new ArrayList<>();
......@@ -158,12 +160,7 @@ public class HalfEdge<P extends IPoint> implements Iterable<HalfEdge<P>> {
private class NeighbourFaceIterator implements Iterator<Face<P>> {
private NeighbourIterator neighbourIterator = new NeighbourIterator();
private NeighbourFaceIterator() {
// such that no duplicated faces returned
if(neighbourIterator.hasNext()) {
neighbourIterator.next();
}
}
private NeighbourFaceIterator() {}
@Override
public boolean hasNext() {
......
......@@ -11,6 +11,6 @@ public interface Triangulation<P extends IPoint> extends Iterable<Face<P>> {
Face<P> locate(final IPoint point);
Stream<Face<P>> streamFaces();
Set<Face<P>> getFaces();
void insert(final P point);
HalfEdge<P> insert(final P point);
void remove(final P point);
}
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;
import org.vadere.util.geometry.data.HalfEdge;
import org.vadere.util.geometry.data.Triangulation;
......@@ -9,25 +12,36 @@ 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.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.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.Iterator;
import java.util.List;
import java.util.Optional;
import java.util.PriorityQueue;
public class EikonalSolverFMMTriangulation implements EikonalSolver {
public class EikonalSolverFMMTriangulation<P extends PotentialPoint> implements EikonalSolver {
private static Logger logger = LogManager.getLogger(EikonalSolverFMMTriangulation.class);
private ITimeCostFunction timeCostFunction;
private Triangulation<? extends PotentialPoint> triangulation;
private Triangulation<P> triangulation;
private boolean calculationFinished;
private PriorityQueue<FFMHalfEdge> narrowBand;
private final Collection<IPoint> targetPoints;
private final Collection<VRectangle> targetAreas;
private final PointConstructor<P> pointConstructor;
private Comparator<FFMHalfEdge> pointComparator = (he1, he2) -> {
if (he1.halfEdge.getEnd().getPotential() < he2.halfEdge.getEnd().getPotential()) {
......@@ -40,20 +54,68 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
}
};
public EikonalSolverFMMTriangulation(final Collection<IPoint> targetPoints,
final ITimeCostFunction timeCostFunction,
final Triangulation<? extends PotentialPoint> triangulation) {
public EikonalSolverFMMTriangulation(final Collection<VRectangle> targetAreas,
final ITimeCostFunction timeCostFunction,
final Triangulation<P> triangulation,
final PointConstructor<P> pointConstructor
) {
this.triangulation = triangulation;
this.calculationFinished = false;
this.timeCostFunction = timeCostFunction;
this.targetPoints = targetPoints;
this.targetAreas = targetAreas;
this.pointConstructor = pointConstructor;
}
private void initializeTargetAreas() {
for(VRectangle rectangle : targetAreas) {
VPoint topLeft = new VPoint(rectangle.getX(), rectangle.getY());
VPoint bottomLeft = new VPoint(rectangle.getX(), rectangle.getMaxY());
VPoint bottomRight = new VPoint(rectangle.getMaxX(), rectangle.getMaxY());
VPoint topRight = new VPoint(rectangle.getMaxX(), rectangle.getY());
LineIterator lineIterator1 = new LineIterator(new VLine(topLeft, topRight), 1.0);
LineIterator lineIterator2 = new LineIterator(new VLine(topLeft, bottomLeft), 1.0);
LineIterator lineIterator3 = new LineIterator(new VLine(bottomLeft, bottomRight), 1.0);
LineIterator lineIterator4 = new LineIterator(new VLine(topRight, bottomRight), 1.0);
List<LineIterator> lineIterators = Arrays.asList(lineIterator1, lineIterator2, lineIterator3, lineIterator4);
for(LineIterator lineIterator : lineIterators) {
while (lineIterator.hasNext()) {
IPoint next = lineIterator.next();
P potentialPoint = pointConstructor.create(next.getX(), next.getY());
potentialPoint.setPathFindingTag(PathFindingTag.Reached);
potentialPoint.setPotential(0.0);
HalfEdge<P> halfEdge = triangulation.insert(potentialPoint);
if(halfEdge != null && halfEdge.getEnd().equals(potentialPoint)) {
narrowBand.add(new FFMHalfEdge(halfEdge));
}
else {
logger.warn("did not found inserted edge!");
}
}
}
for(Face<P> face : triangulation) {
for(HalfEdge<P> potentialPoint : face) {
for(VRectangle targetRect : targetAreas) {
if(targetRect.contains(potentialPoint.getEnd())) {
potentialPoint.getEnd().setPotential(0.0);
potentialPoint.getEnd().setPathFindingTag(PathFindingTag.Reached);
narrowBand.add(new FFMHalfEdge(potentialPoint));
}
}
}
}
}
}
@Override
public void initialize() {
this.narrowBand = new PriorityQueue<>(pointComparator);
for(IPoint point : targetPoints) {
narrowBand = new PriorityQueue<>(pointComparator);
initializeTargetAreas();
/*for(IPoint point : targetPoints) {
Face<? extends PotentialPoint> face = triangulation.locate(point);
for(HalfEdge<? extends PotentialPoint> halfEdge : face) {
......@@ -68,7 +130,7 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
potentialPoint.setPathFindingTag(PathFindingTag.Reached);
narrowBand.add(new FFMHalfEdge(halfEdge));
}
}
}*/
calculate();
}
......@@ -76,7 +138,14 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
@Override
public double getValue(double x, double y) {
Face<? extends PotentialPoint> triangle = triangulation.locate(new VPoint(x, y));
return InterpolationUtil.barycentricInterpolation(triangle, x, y);
if(triangle == null) {
logger.warn("no triangle found for coordinates (" + x + "," + y + ")");
}
else {
return InterpolationUtil.barycentricInterpolation(triangle, x, y);
}
return Double.MAX_VALUE;
}
......@@ -93,8 +162,8 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
// 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));
//double potential = recalculatePoint(ffmHalfEdge.halfEdge);
//ffmHalfEdge.halfEdge.getEnd().setPotential(Math.min(ffmHalfEdge.halfEdge.getEnd().getPotential(), potential));
// add narrow points
setNeighborDistances(ffmHalfEdge.halfEdge);
}
......@@ -117,10 +186,18 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
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));
// if not, it was not possible to compute a valid potential. TODO?
if(potential < neighbour.getEnd().getPotential()) {
neighbour.getEnd().setPotential(potential);
neighbour.getEnd().setPathFindingTag(PathFindingTag.Reachable);
narrowBand.add(new FFMHalfEdge(neighbour));
}
else {
logger.warn("could not set neighbour vertex" + neighbour + "," + neighbour.getFace().isBorder());
potential = recalculatePoint(neighbour);
potential = recalculatePoint(neighbour);
}
}
else if(neighbour.getEnd().getPathFindingTag() == PathFindingTag.Reachable) {
//double potential = neighbour.getEnd().getPotential();
......@@ -174,61 +251,89 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
PotentialPoint p1 = points.get(0);
PotentialPoint p2 = points.get(1);
// 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!");
}
}
/*if(!p1.getPathFindingTag().frozen && !p2.getPathFindingTag().frozen) {
return Double.MAX_VALUE;
}*/
he = cone.contains(he.getNext().getEnd()) ? he.getNext() : he.getPrevious();
// 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
double pot1 = updatePoint(point, he.getEnd(), p1);
double pot2 = updatePoint(point, he.getEnd(), p2);
return Math.min(pot1, pot2);
Optional<HalfEdge<? extends PotentialPoint>> optHe = findPointInCone(halfEdge, p1, p2);
if(optHe.isPresent()) {
double pot1 = updatePoint(point, optHe.get().getEnd(), p1);
double pot2 = updatePoint(point, optHe.get().getEnd(), p2);
return Math.min(pot1, pot2);
}
else {
return point.getPotential();
}
}
else {
return updatePoint(point, p1, p2);
}
}
private Optional<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));
// 1. construct the acute cone
VPoint direction = triangle.getIncenter().subtract(point);
double angle = Math.PI - GeometryUtils.angle(p1, point, p2);
VPoint origin = new VPoint(point);
VCone cone = new VCone(origin, 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()))) {
if(he.isBoundary()) {
return Optional.empty();
}
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 {
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!");
}
}
return Optional.of(he);
}
private double updatePoint(final PotentialPoint point, final PotentialPoint p1, final PotentialPoint p2) {
if ((Double.isInfinite(p1.getPotential()) && Double.isInfinite((p2.getPotential())))
/*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 point.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)
{
/*if(
(p1.getPathFindingTag() == PathFindingTag.Reached || p1.getPathFindingTag() == PathFindingTag.Reachable)
&& (p2.getPathFindingTag() == PathFindingTag.Reached || p2.getPathFindingTag() == PathFindingTag.Reachable))
{*/
if(p1.getPathFindingTag().frozen && p2.getPathFindingTag().frozen) {
// see: Sethian, Level Set Methods and Fast Marching Methods, page
// 124.
double u = p2.getPotential() - p1.getX();
......@@ -257,9 +362,9 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
return Math.min(b * F + TA, c * F + TB);
}
}
/*else {
return halfEdge.getEnd().getPotential();
}*/
else {
return point.getPotential();
}
}
/**
......@@ -272,13 +377,23 @@ public class EikonalSolverFMMTriangulation implements EikonalSolver {
* returns Double.MIN_VALUE
*/
private double solveQuadratic(double a, double b, double c) {
double det = b * b - 4 * a * c;
List<Double> solutions = MathUtil.solveQuadratic(a, b, c);
double result = Double.MIN_VALUE;
if (solutions.size() == 2) {
result = Math.max(solutions.get(0), solutions.get(1));
} else if (solutions.size() == 1) {
result = solutions.get(0);
}
return result;
/*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));
/ (2 * a));*/
}
/**
......
......@@ -28,7 +28,6 @@ import java.util.List;
import java.util.Optional;
import java.util.Random;
import java.util.Set;
import java.util.Spliterator;
import java.util.stream.Collectors;
import java.util.stream.Stream;
import java.util.stream.StreamSupport;
......@@ -40,6 +39,8 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
private Face<P> face;
private final Set<P> points;
private final PointConstructor<P> pointConstructor;
// TODO: use symbolic for the super-triangle points instead of real points!
private P p0;
private P p1;
private P p2;
......@@ -50,7 +51,6 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
private DAG<DAGElement<P>> dag;
private final HashMap<Face<P>, DAG<DAGElement<P>>> map;
private int count;
private double eps = 0.0000001;
private Face<P> superTriangle;
private Face<P> borderFace;
......@@ -62,7 +62,6 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
this.points = points;
this.pointConstructor = pointConstructor;
this.map = new HashMap<>();
this.count = 0;
P p_max = points.parallelStream().reduce(pointConstructor.create(Double.MIN_VALUE, Double.MIN_VALUE), (a, b) -> pointConstructor.create(Math.max(a.getX(), b.getX()), Math.max(a.getY(), b.getY())));
P p_min = points.parallelStream().reduce(pointConstructor.create(Double.MIN_VALUE, Double.MIN_VALUE), (a, b) -> pointConstructor.create(Math.min(a.getX(), b.getX()), Math.min(a.getY(), b.getY())));
init(p_max, p_min);
......@@ -77,12 +76,9 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
this.points = new HashSet<P>();
this.pointConstructor = pointConstructor;
this.map = new HashMap<>();
this.count = 0;
P p_max = pointConstructor.create(minX + width, minY + height);
P p_min = pointConstructor.create(minX, minY);
init(p_max, p_min);
}
private void init(final P p_max, final P p_min) {
......@@ -105,33 +101,48 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
this.dag = new DAG<>(new DAGElement<>(superTriangle, Triple.of(p0, p1, p2)));
this.map.put(superTriangle, dag);
this.count = 0;
this.face = null;
this.finalized = false;
}
@Override
public void insert(P point) {
public void compute() {
// 1. insert points
for(P p : points) {
insert(p);
}
// 2. remove super triangle
finalize();
}
@Override
public HalfEdge<P> insert(P point) {
Collection<DAG<DAGElement<P>>> leafs = locatePoint(point, true);
assert leafs.size() == 2 ||leafs.size() == 1 ||leafs.size() == 0;
count++;
if(leafs.size() == 0) {
log.warn(point);
}
assert leafs.size() == 2 || leafs.size() == 1 || leafs.size() > 0;
// point is inside a triangle
if(leafs.size() == 1) {
log.info("splitTriangle");
splitTriangleDB(point, leafs.stream().findAny().get());
//log.info("splitTriangle");
return splitTriangleDB(point, leafs.stream().findAny().get());
} // point lies on an edge of 2 triangles
else if(leafs.size() == 2) {
Iterator<DAG<DAGElement<P>>> it = leafs.iterator();
log.info("splitEdge");
splitEdgeDB(point, findTwins(it.next().getElement().getFace(), it.next().getElement().getFace()));
//log.info("splitEdge");
return splitEdgeDB(point, findTwins(it.next().getElement().getFace(), it.next().getElement().getFace()));
}
else if(leafs.size() == 0) {
// problem due numerical calculation.
log.warn("numerical error!");
log.warn("no triangle was found! Maybe " + point + " lies outside of the triangulation." );
return null;
}
else {
log.warn("ignore insertion point, since this point already exists!");
return leafs.iterator().next().getElement().getFace().stream().filter(he -> he.getEnd().equals(point)).findAny().get();
}
}
......@@ -139,21 +150,24 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
* Removes the super triangle from the mesh data structure.
*/
public void finalize() {
// we have to use other halfedges than he1 and he2 since they might be deleted
// if we deleteBoundaryFace he0!
List<Face<P>> faces1 = IteratorUtils.toList(he0.incidentFaceIterator());
List<Face<P>> faces2 = IteratorUtils.toList(he1.incidentFaceIterator());
List<Face<P>> faces3 = IteratorUtils.toList(he2.incidentFaceIterator());
if(!finalized) {
// we have to use other halfedges than he1 and he2 since they might be deleted
// if we deleteBoundaryFace he0!
List<Face<P>> faces1 = IteratorUtils.toList(he0.incidentFaceIterator());
List<Face<P>> faces2 = IteratorUtils.toList(he1.incidentFaceIterator());
List<Face<P>> faces3 = IteratorUtils.toList(he2.incidentFaceIterator());
faces1.forEach(f -> deleteBoundaryFace(f));
faces1.removeIf(f -> f.isBorder());
faces1.forEach(f -> deleteBoundaryFace(f));
faces2.removeIf(f -> f.isDestroyed());
faces2.forEach(f -> deleteBoundaryFace(f));
faces2.removeIf(f -> f.isDestroyed() || f.isBorder());
faces2.forEach(f -> deleteBoundaryFace(f));
faces3.removeIf(f -> f.isDestroyed());
faces3.forEach(f -> deleteBoundaryFace(f));
faces3.removeIf(f -> f.isDestroyed() ||f.isBorder());
faces3.forEach(f -> deleteBoundaryFace(f));
finalized = true;
finalized = true;
}
}
public boolean isDeletionOk(final Face<P> face) {
......@@ -206,9 +220,12 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
toF.getTwin().getFace().setEdge(nB);
this.face = toF.getTwin().getFace();
// release memory
toF.destroy();
toB.destroy();
}
else {
HalfEdge<P> boundaryHe = boundaryEdges.get(0);
......@@ -221,6 +238,7 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
boundaryHe.getPrevious().setFace(boundaryHe.getTwin().getFace());
prec.getFace().setEdge(prec);
this.face = prec.getFace();
// release memory
boundaryHe.destroy();
}
......@@ -229,18 +247,9 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
map.remove(face);
}
@Override
public void compute() {
// 2. insert points
for(P p : points) {
insert(p);
count++;
}
}
@Override
public Face<P> locate(final IPoint point) {
Optional<DAG<DAGElement<P>>> optDag = locatePoint(point, false).stream().findAny();
Optional<DAG<DAGElement<P>>> optDag = locatePoint(point, false).stream().findAny();
if(optDag.isPresent()) {
return optDag.get().getElement().getFace();
}
......@@ -332,7 +341,7 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
while(!nodesToVisit.isEmpty()) {
DAG<DAGElement<P>> currentNode = nodesToVisit.removeLast();
if(currentNode.getElement().getTriangle().isPartOf(point, eps)) {
if(currentNode.isLeaf()) {
if(currentNode.isLeaf() && !currentNode.getElement().getFace().isDestroyed()) {
leafs.add(currentNode);
// if we are not interested in insertion we just want to find one triangle.
......@@ -356,7 +365,7 @@ public class DelaunayTriangulation<P extends IPoint> implements Triangulation<P>
* @param p the split point