Commit 21db25d3 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

triangulation updates

parent f26b2c4a
......@@ -110,7 +110,8 @@
"treadCount" : 15,
"upwardDirection" : {
"x" : 1.0,
"y" : 0.0
"y" : 0.0,
"identifier" : -1
}
} ],
"targets" : [ {
......
......@@ -146,7 +146,8 @@
"treadCount" : 13,
"upwardDirection" : {
"x" : 0.0,
"y" : 1.0
"y" : 1.0,
"identifier" : -1
}
}, {
"shape" : {
......@@ -160,7 +161,8 @@
"treadCount" : 13,
"upwardDirection" : {
"x" : 0.0,
"y" : -1.0
"y" : -1.0,
"identifier" : -1
}
} ],
"targets" : [ {
......
......@@ -129,6 +129,5 @@ public class TrajectoryReader {
logger.warn("could not read trajectory file. The file format might not be compatible or it is missing.");
throw e;
}
}
}
......@@ -288,6 +288,13 @@ public interface IPolyConnectivity<P extends IPoint, V extends IVertex<P>, E ext
return GeometryUtils.isRightOf(p1, p2, x1, y1);
}
/**
* Tests if the line-segment edge intersects the line defined by p1 and p2.
* @param p1
* @param p2
* @param edge
* @return
*/
default boolean intersects(final IPoint p1, final IPoint p2, E edge) {
VPoint q1 = getMesh().toPoint(getMesh().getVertex(getMesh().getPrev(edge)));
VPoint q2 = getMesh().toPoint(getMesh().getVertex(edge));
......
......@@ -751,30 +751,17 @@ public interface ITriConnectivity<P extends IPoint, V extends IVertex<P>, E exte
* @param stopCondition
* @return
*/
default F straightWalk2D(final E startVertex, final VPoint direction, final Predicate<E> stopCondition) {
default F straightWalk2D(final E startVertex, final F face, final VPoint direction, final Predicate<E> stopCondition) {
E startEdge = getMesh().getPrev(startVertex);
E edge = getMesh().getPrev(startVertex);
VPoint p1 = getMesh().toPoint(startVertex);
VPoint p2 = p1.add(direction);
VPoint p3 = getMesh().toPoint(startEdge);
VPoint p4 = getMesh().toPoint(getMesh().getPrev(startEdge));
VPoint q = p1;
VPoint p = GeometryUtils.intersectionPoint(p1.getX(), p1.getY(), p2.getX(), p2.getY(), p3.getX(), p3.getY(), p4.getX(), p4.getY());
F face;
E edge;
if(isRightOf(p.getX(), p.getY(), startEdge)) {
edge = startEdge;
face = getMesh().getFace(edge);
}
else {
edge = getMesh().getTwin(startEdge);
face = getMesh().getFace(edge);
}
return straightWalk2D(q, p, face, edge, stopCondition);
assert intersects(p1, p1.add(direction), edge);
assert getMesh().getEdges(face).contains(startVertex);
// TODO: quick solution!
VPoint q = p1.add(direction.scalarMultiply(Double.MAX_VALUE * 0.5));
return straightWalk2D(p1, q, face, edge, e -> (stopCondition.test(e) || !isRightOf(q.x, q.y, e)));
}
default F straightWalk2D(final double x1, final double y1, final F startFace, final Predicate<E> stopCondition) {
......@@ -789,6 +776,19 @@ public interface ITriConnectivity<P extends IPoint, V extends IVertex<P>, E exte
return straightWalk2D(q, p, face, edge, stopCondition);
}
/**
* Walks along the line defined by q and p. The walking direction should be controlled by the stopCondition e.g.
* e -> !isRightOf(x1, y1, e) stops the walk if (x1, y1) is on the left side of each edge which is the case if the
* point is inside the reached face. The walk starts at the startFace and continues in the direction of line defined
* by q and p using the any edge which does not fulfill the stopCondition.
*
* @param q
* @param p
* @param startFace
* @param startEdge
* @param stopCondition
* @return
*/
default F straightWalk2D(final VPoint q, final VPoint p, final F startFace, final E startEdge, final Predicate<E> stopCondition) {
Optional<E> optEdge;
F face = startFace;
......
......@@ -15,9 +15,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)) {
/*if(p1.equals(p2)) {
throw new IllegalArgumentException(p1 + " is equal " + p2);
}
}*/
this.p1 = p1;
this.p2 = p2;
}
......
......@@ -31,6 +31,10 @@ public class CellGrid {
/** Number of points along y axis. */
protected final int numPointsY;
protected final double xMin;
protected final double yMin;
protected CellState[][] values;
/**
......@@ -38,10 +42,12 @@ public class CellGrid {
* point values are initialized with 'value'.
*/
public CellGrid(double width, double height, double resolution,
CellState value) {
CellState value, double xMin, double yMin) {
this.width = width;
this.height = height;
this.resolution = resolution;
this.xMin = xMin;
this.yMin = yMin;
/* 0.001 avoids that numPointsX/Y are too small due to numerical errors. */
numPointsX = (int) Math.floor(width / resolution + 0.001) + 1;
......@@ -52,6 +58,14 @@ public class CellGrid {
reset(value);
}
/**
* Creates an grid with the given width, height and resolution. All grid
* point values are initialized with 'value'.
*/
public CellGrid(double width, double height, double resolution, CellState value) {
this(width, height, resolution, value, 0, 0);
}
/**
* Creates a deep copy of the given grid.
*/
......@@ -62,6 +76,8 @@ public class CellGrid {
numPointsX = grid.numPointsX;
numPointsY = grid.numPointsY;
values = new CellState[numPointsX][numPointsY];
xMin = grid.xMin;
yMin = grid.yMin;
for (int row = 0; row < numPointsY; row++) {
for (int col = 0; col < numPointsX; col++) {
......@@ -136,7 +152,7 @@ public class CellGrid {
* Converts the matrix indices to coordinates.
*/
public VPoint pointToCoord(int pointX, int pointY) {
return new VPoint(pointX * resolution, pointY * resolution);
return new VPoint(xMin + pointX * resolution, yMin + pointY * resolution);
}
/**
......@@ -181,20 +197,20 @@ public class CellGrid {
* Returns the closest grid point (matrix index) to the given coordinates.
*/
public Point getNearestPoint(double x, double y) {
if (x < 0) {
x = 0;
if (x < xMin) {
x = xMin;
}
if (y < 0) {
y = 0;
if (y < yMin) {
y = yMin;
}
if (y > getHeight()) {
y = getHeight();
if (y > getHeight() + yMin) {
y = yMin+getHeight();
}
if (x > getWidth()) {
x = getWidth();
if (x > getWidth() + xMin) {
x = getWidth() + xMin;
}
return new Point((int) (x / resolution + 0.5),
(int) (y / resolution + 0.5));
return new Point((int) ((x - xMin) / resolution + 0.5),
(int) ((y - yMin) / resolution + 0.5));
}
/**
......@@ -202,19 +218,19 @@ public class CellGrid {
* towards origin.
*/
public Point getNearestPointTowardsOrigin(double x, double y) {
if (x < 0) {
x = 0;
}
if (y < 0) {
y = 0;
}
if (y > getHeight()) {
y = getHeight();
}
if (x > getWidth()) {
x = getWidth();
}
return new Point((int) (x / resolution), (int) (y / resolution));
if (x < xMin) {
x = xMin;
}
if (y < yMin) {
y = yMin;
}
if (y > getHeight() + yMin) {
y = yMin+getHeight();
}
if (x > getWidth() + xMin) {
x = getWidth() + xMin;
}
return new Point((int) ((x - xMin) / resolution), (int) ((y - yMin) / resolution));
}
/**
......@@ -286,6 +302,10 @@ public class CellGrid {
.flatMap(stream -> stream);
}
public double getMinX() { return xMin; }
public double getMinY() { return yMin; }
public boolean isValidPoint(Point point) {
Point p = (point);
......
......@@ -27,11 +27,11 @@ public abstract class AbstractGridEikonalSolver implements EikonalSolver {
double gridPotentials[];
double weightOfKnown[] = new double[1];
if (x >= potentialField.getWidth()) {
if (x >= potentialField.getWidth() + potentialField.getMinY()) {
incX = 0;
}
if (y >= potentialField.getHeight()) {
if (y >= potentialField.getHeight() + potentialField.getMinY()) {
incY = 0;
}
......
......@@ -14,6 +14,7 @@ import org.vadere.util.potential.CellGrid;
import org.vadere.util.potential.CellState;
import org.vadere.util.potential.PathFindingTag;
import org.vadere.util.potential.timecost.ITimeCostFunction;
import org.vadere.util.triangulation.adaptive.IDistanceFunction;
/**
* EikonalSolverFMM initializes a potential field on basis
......@@ -29,6 +30,8 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver {
protected CellGrid cellGrid;
protected List<Point> targetPoints;
protected List<VShape> targetShapes;
protected IDistanceFunction distFunc;
boolean isHighAccuracy = false;
/** only for logging */
......@@ -63,13 +66,41 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver {
}
}
/**
* Initializes the FM potential calculator with a time cost function F > 0.
*/
public EikonalSolverFMM(
final CellGrid potentialField,
final IDistanceFunction distFunc,
final boolean isHighAccuracy,
final ITimeCostFunction timeCostFunction,
final double unknownPenalty) {
super(potentialField, unknownPenalty);
this.cellGrid = potentialField;
this.targetPoints = cellGrid.pointStream().filter(p -> cellGrid.getValue(p).tag == PathFindingTag.Target)
.collect(Collectors.toList());
this.distFunc = distFunc;
this.isHighAccuracy = isHighAccuracy;
ComparatorPotentialFieldValue comparator = new ComparatorPotentialFieldValue(
potentialField);
this.narrowBand = new PriorityQueue<>(50, comparator);
this.timeCostFunction = timeCostFunction;
if (targetPoints.size() == 0) {
logger.error("PotentialFieldInitializerFastMarching::Run(): "
+ "Warning, no target points given. Target missing or grid resolution too low.");
return;
}
}
@Override
public void initialize() {
for (Point point : targetPoints) {
if(!targetShapes.isEmpty()) {
setTargetNeighborsDistances(point);
}
//if(!targetShapes.isEmpty()) {
setTargetNeighborsDistances(point);
//}
narrowBand.add(point);
}
......@@ -181,7 +212,7 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver {
cellGrid.setValue(
neighbor,
new CellState(minDistanceToTarget(cellGrid.pointToCoord(neighbor)),
new CellState(minDistanceToTarget(cellGrid.pointToCoord(neighbor)) / timeCostFunction.costAt(cellGrid.pointToCoord(neighbor)),
PathFindingTag.Reachable));
narrowBand.add(neighbor);
}
......@@ -195,13 +226,23 @@ public class EikonalSolverFMM extends AbstractGridEikonalSolver {
// computed starting there.
VPoint dp = new VPoint(cellGrid.getWidth() / (cellGrid.getNumPointsX() - 1) / 2.0,
cellGrid.getHeight() / (cellGrid.getNumPointsY() - 1) / 2.0);
for (VShape targetShape : targetShapes) {
// negative distances are possible when point is inside the target
tmp = Math.max(0, targetShape.distance(point.add(dp)));
if (tmp < minDistance) {
minDistance = tmp;
}
}
if(targetShapes != null && !targetShapes.isEmpty()) {
for (VShape targetShape : targetShapes) {
// negative distances are possible when point is inside the target
tmp = Math.max(0, targetShape.distance(point.add(dp)));
if (tmp < minDistance) {
minDistance = tmp;
}
}
}
else if(distFunc != null) {
tmp = Math.max(0, -distFunc.apply(point));
if (tmp < minDistance) {
minDistance = tmp;
}
}
return minDistance;
}
}
......@@ -129,7 +129,8 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
*/
public EikonalSolverFMMTriangulation(@NotNull final ITimeCostFunction timeCostFunction,
@NotNull final ITriangulation<P, V, E, F> triangulation,
@NotNull final Collection<V> targetVertices
@NotNull final Collection<V> targetVertices,
@NotNull final IDistanceFunction distFunc
) {
this.triangulation = triangulation;
this.calculationFinished = false;
......@@ -140,15 +141,27 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
for(V vertex : targetVertices) {
P potentialPoint = mesh.getPoint(vertex);
double distance = 0.0;
double distance = -distFunc.apply(potentialPoint);
if(potentialPoint.getPathFindingTag() != PathFindingTag.Undefined) {
narrowBand.remove(vertex);
}
potentialPoint.setPotential(Math.min(potentialPoint.getPotential(), distance * timeCostFunction.costAt(potentialPoint)));
potentialPoint.setPotential(Math.min(potentialPoint.getPotential(), distance / timeCostFunction.costAt(potentialPoint)));
potentialPoint.setPathFindingTag(PathFindingTag.Reached);
narrowBand.add(vertex);
for(V v : triangulation.getMesh().getAdjacentVertexIt(vertex)) {
if(!narrowBand.contains(v)) {
P potentialP = mesh.getPoint(v);
double dist = Math.max(-distFunc.apply(potentialP), 0);
logger.info(dist);
potentialP.setPotential(Math.min(potentialP.getPotential(), dist / timeCostFunction.costAt(potentialP)));
potentialP.setPathFindingTag(PathFindingTag.Reachable);
narrowBand.add(v);
}
}
}
}
......@@ -185,10 +198,10 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
double result = -1.0;
if(!optFace.isPresent()) {
logger.warn("no face found for coordinates (" + x + "," + y + ")");
//logger.warn("no face found for coordinates (" + x + "," + y + ")");
}
else if(optFace.isPresent() && triangulation.getMesh().isBoundary(optFace.get())) {
logger.warn("no triangle found for coordinates (" + x + "," + y + ")");
// logger.warn("no triangle found for coordinates (" + x + "," + y + ")");
}
else {
result = InterpolationUtil.barycentricInterpolation(mesh.getPoints(optFace.get()), x, y);
......@@ -267,6 +280,19 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
return potential;
}
public boolean isNonAcute(@NotNull final E edge) {
VPoint p1 = mesh.toPoint(mesh.getPrev(edge));
VPoint p2 = mesh.toPoint(edge);
VPoint p3 = mesh.toPoint(mesh.getNext(edge));
double angle1 = GeometryUtils.angle(p1, p2, p3);
// non-acute triangle
double rightAngle = Math.PI/2;
return angle1 > rightAngle + GeometryUtils.DOUBLE_EPS;
}
/**
* Updates a point given a triangle. The point can only be updated if the
* triangle triangleContains it and the other two points are in the frozen band.
......@@ -277,44 +303,47 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
private double computePotential(@NotNull final P point, @NotNull final F face) {
// check whether the triangle does contain useful data
List<E> edges = mesh.getEdges(face);
edges.removeIf(edge -> mesh.getPoint(edge).equals(point));
E halfEdge = edges.stream().filter(e -> mesh.getPoint(e).equals(point)).findAny().get();
edges.removeIf(edge -> mesh.getPoint(edge).equals(point));
assert edges.size() == 2;
P p1 = mesh.getPoint(edges.get(0));
P p2 = mesh.getPoint(edges.get(1));
if(feasableForComputation(p1) && feasableForComputation(p2)) {
if(!mesh.toTriangle(face).isNonAcute()) {
if(isFeasibleForComputation(p1) && isFeasibleForComputation(p2)) {
if(!isNonAcute(halfEdge)) {
return computePotential(point, p1, p2);
}
/*else {
logger.info("special case for non-acute triangle");
E halfEdge = mesh.getEdges(face).stream().filter(p -> mesh.getPoint(p).equals(point)).findAny().get();
Optional<P> optPoint = walkToFeasablePoint(halfEdge, face);
} // we only try to find a virtual vertex if both points are already frozen
else {
//logger.info("special case for non-acute triangle");
Optional<P> optPoint = walkToFeasiblePoint(halfEdge, face);
if(optPoint.isPresent()) {
P surrogatePoint = optPoint.get();
if(feasableForComputation(surrogatePoint)) {
if(isFeasibleForComputation(surrogatePoint)) {
//logger.info("feasible point found for " + point + " and " + face);
return Math.min(computePotential(point, surrogatePoint, p2), computePotential(point, p1, surrogatePoint));
}
else {
logger.info("no feasable point found for " + point + " and " + face);
logger.warn("no feasible point found for " + point + " and " + face);
}
}
else {
logger.info("no point found for " + point + " and " + face);
logger.warn("no point found for " + point + " and " + face);
}
}*/
}
}
return Double.MAX_VALUE;
}
private boolean feasableForComputation(final P p){
private boolean isFeasibleForComputation(final P p){
//return p.getPathFindingTag().frozen;
return p.getPathFindingTag() == PathFindingTag.Reachable || p.getPathFindingTag() == PathFindingTag.Reached;
}
private Optional<P> walkToFeasablePoint(@NotNull final E halfEdge, @NotNull final F face) {
private Optional<P> walkToFeasiblePoint(@NotNull final E halfEdge, @NotNull final F face) {
assert mesh.toTriangle(face).isNonAcute();
E next = mesh.getNext(halfEdge);
......@@ -324,12 +353,34 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
VPoint pNext = new VPoint(mesh.getVertex(next));
VPoint pPrev = new VPoint(mesh.getVertex(prev));
VPoint direction1 = pNext.subtract(p).rotate(+Math.PI/2);
VPoint direction2 = pPrev.subtract(p).rotate(-Math.PI/2);
//VPoint searchDirection = GeometryUtils.angleTo(p.add(direction1), p, p.add(direction2));
Predicate<V> feasableVertexPred = v -> GeometryUtils.isLeftOf(p, p.add(direction2), mesh.toPoint(v)) && GeometryUtils.isRightOf(p, p.add(direction1), mesh.toPoint(v));
//logger.info(p + ", " + pNext + ", " + pPrev);
//logger.info(direction1 + ", " + direction2);
Predicate<E> isPointInCone = e ->
{
VPoint point = mesh.toPoint(e);
return GeometryUtils.isLeftOf(p, p.add(direction2), point) &&
GeometryUtils.isRightOf(p, p.add(direction1), point);
};
Predicate<E> isEdgeInCone = e -> isPointInCone.test(e) || isPointInCone.test(mesh.getPrev(e));
F destination = triangulation.straightWalk2D(halfEdge, face, direction1, isEdgeInCone);
assert !destination.equals(face);
if(!mesh.isBoundary(destination)) {
return mesh.streamEdges(destination).filter(e -> isPointInCone.test(e)).map(v -> mesh.getPoint(v)).findAny();
}
else {
logger.warn("walked to boundary");
return Optional.empty();
}
/*Predicate<V> feasableVertexPred = v -> GeometryUtils.isLeftOf(p, p.add(direction2), mesh.toPoint(v)) && GeometryUtils.isRightOf(p, p.add(direction1), mesh.toPoint(v));
Predicate<E> feasableEdgePred = e -> feasableVertexPred.test(mesh.getVertex(halfEdge));
Predicate<E> stopCondition = e -> {
VPoint midPoint = mesh.toLine(e).midPoint();
......@@ -343,7 +394,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
}
else {
return Optional.empty();
}
}*/
}
private E findIntersectionPoint(final E halfEdge, final P acceptedPoint, final P unacceptedPoint) {
......@@ -382,7 +433,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
while (faceIterator.hasNext()) {
F face = faceIterator.next();
List<E> pointList = mesh.getEdges(face).stream()
.filter(he -> feasableForComputation(mesh.getPoint(he)))
.filter(he -> isFeasibleForComputation(mesh.getPoint(he)))
.filter(he -> !mesh.getVertex(he).equals(acceptedPoint))
.filter(he -> !mesh.getVertex(he).equals(point))
.collect(Collectors.toList());
......@@ -426,7 +477,7 @@ public class EikonalSolverFMMTriangulation<P extends PotentialPoint, V extends I
// we can not search further since we reach the boundary.
if (!mesh.isBoundary(candidate)) {
P vertex = mesh.getPoint(candidate);
if (feasableForComputation(vertex) && cone.contains(new VPoint(vertex))) {
if (isFeasibleForComputation(vertex) && cone.contains(new VPoint(vertex))) {
return candidate;
} else if(cone.contains(new VPoint(vertex))) {
E newCandidate = mesh.getNext(mesh.getTwin(candidate));
......
......@@ -129,14 +129,17 @@ public class PSMeshing implements IPSMeshing {