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

implement the eikonal solver for acute triangulation

parent 55079e75
......@@ -9,6 +9,7 @@ import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.scenario.Agent;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VRectangle;
import org.vadere.util.potential.timecost.ITimeCostFunction;
......@@ -67,13 +68,13 @@ public class TimeCostDensityVoronoi implements ITimeCostFunction {
this.voronoiDiagram = new VoronoiDiagram(new RectangleLimits(0, 0,
floor.getBounds().getWidth(), floor.getBounds().getHeight()));
this.measurementAreaRadius = 1.5;
this.voronoiAreas = new HashMap<Integer, Double>();
this.voronoiAreas = new HashMap<>();
this.scaleFactor = attributesPedestrian.getRadius() * 2
* attributesPedestrian.getRadius() * 2 * Math.sqrt(3) * 0.5;
}
@Override
public double costAt(VPoint p) {
public double costAt(final IPoint p) {
double density = 0.0;
int numberPedestriansInPolygon = 0;
double pedestrianSpaceSum = 0;
......@@ -83,8 +84,8 @@ public class TimeCostDensityVoronoi implements ITimeCostFunction {
List<org.vadere.util.voronoi.Face> faces = null;
// the area
VRectangle measurementArea = new VRectangle(p.x - measurementAreaRadius
/ 2, p.y - measurementAreaRadius / 2, measurementAreaRadius,
VRectangle measurementArea = new VRectangle(p.getX() - measurementAreaRadius
/ 2, p.getY() - measurementAreaRadius / 2, measurementAreaRadius,
measurementAreaRadius);
// voronoiDiagram.computeVoronoiDiagram( pedBodies );
......
......@@ -3,6 +3,7 @@ package org.vadere.simulator.models.potential.timeCostFunction;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.vadere.simulator.models.density.IGaussianFilter;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.potential.timecost.ITimeCostFunction;
......@@ -57,9 +58,9 @@ public class TimeCostObstacleDensity implements ITimeCostFunction {
}
@Override
public double costAt(VPoint p) {
public double costAt(final IPoint p) {
double obstacleDensity = 0.0;
obstacleDensity = obstacleImageFilter.getFilteredValue(p.x, p.y);
obstacleDensity = obstacleImageFilter.getFilteredValue(p.getX(), p.getY());
if (obstacleDensity > highest) {
highest = obstacleDensity;
// logger.info("obstacle density: " + obstacleDensity);
......
......@@ -4,6 +4,7 @@ import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.vadere.simulator.models.density.IGaussianFilter;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.potential.timecost.ITimeCostFunction;
......@@ -45,12 +46,9 @@ public class TimeCostPedestrianDensity implements ITimeCostFunction {
}
@Override
public double costAt(final VPoint p) {
double cost = 0.0;
public double costAt(final IPoint p) {
long ms = System.currentTimeMillis();
cost = gaussianCalculator.getFilteredValue(p.x, p.y);
double cost = gaussianCalculator.getFilteredValue(p.getX(), p.getY());
if (heighestDensity < cost) {
heighestDensity = cost;
......
......@@ -8,6 +8,7 @@ import org.vadere.state.attributes.models.AttributesTimeCost;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.potential.timecost.ITimeCostFunction;
......@@ -79,7 +80,7 @@ public class TimeCostPedestrianDensityIteration implements ITimeCostFunction {
}
@Override
public double costAt(VPoint p) {
public double costAt(final IPoint p) {
long ms = System.currentTimeMillis();
double cost = calculatePedestrianDensity(new VPoint(p.getX(), p.getY()));
......@@ -106,12 +107,12 @@ public class TimeCostPedestrianDensityIteration implements ITimeCostFunction {
return true;
}
private double calculatePedestrianDensity(final VPoint position) {
private double calculatePedestrianDensity(final IPoint position) {
double densitySum = 0.0;
double radius = 4;
Collection<Pedestrian> pedestrianBodies = floor
.getSpatialMap(Pedestrian.class).getObjects(position, radius);
.getSpatialMap(Pedestrian.class).getObjects(new VPoint(position), radius);
if (!pedestrianBodies.isEmpty()) {
for (Pedestrian pedestrianBody : pedestrianBodies) {
......
......@@ -4,6 +4,7 @@ import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.vadere.simulator.models.density.IGaussianFilter;
import org.vadere.state.attributes.models.AttributesTimeCost;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.potential.timecost.ITimeCostFunction;
......@@ -52,11 +53,11 @@ public class TimeCostPedestrianDensityQueuing implements ITimeCostFunction {
}
@Override
public double costAt(final VPoint p) {
public double costAt(final IPoint p) {
long ms = System.currentTimeMillis();
double cost = queueWidhtFactor
* gaussianCalculator.getFilteredValue(p.x, p.y);
* gaussianCalculator.getFilteredValue(p.getX(), p.getY());
runtime += System.currentTimeMillis() - ms;
......
......@@ -22,6 +22,7 @@ import java.util.HashSet;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import java.util.Optional;
import java.util.Random;
import java.util.Set;
import java.util.stream.Collectors;
......@@ -80,8 +81,19 @@ public class BowyerWatson<P extends IPoint> implements Triangulation<P> {
}
@Override
public Face<P> locate(P point) {
return locatePoint(point).stream().findAny().get().getElement().getFace();
public Face<P> locate(final IPoint point) {
Optional<DAG<DAGElement<P>>> optDag = locatePoint(point).stream().findAny();
if(optDag.isPresent()) {
return optDag.get().getElement().getFace();
}
else {
return null;
}
}
@Override
public Face<P> locate(final double x, double y) {
return locate(new VPoint(x, y));
}
@Override
......@@ -153,7 +165,7 @@ public class BowyerWatson<P extends IPoint> implements Triangulation<P> {
return new VTriangle(new VPoint(p1.getX(), p1.getY()), new VPoint(p2.getX(), p2.getY()), new VPoint(p3.getX(), p3.getY()));
}
private Set<DAG<DAGElement<P>>> locatePoint(final P point) {
private Set<DAG<DAGElement<P>>> locatePoint(final IPoint point) {
Set<DAG<DAGElement<P>>> leafs = new HashSet<>();
LinkedList<DAG<DAGElement<P>>> nodesToVisit = new LinkedList<>();
......@@ -230,7 +242,7 @@ public class BowyerWatson<P extends IPoint> implements Triangulation<P> {
pointOnEdge(edges.get(2), p);
throw new IllegalArgumentException(p + " lies on no edge!");
}
HalfEdge<P> zx = xz.getTwin().get();
HalfEdge<P> zx = xz.getTwin();
HalfEdge<P> xw = zx.getNext();
HalfEdge<P> wz = xw.getNext();
......@@ -406,9 +418,9 @@ public class BowyerWatson<P extends IPoint> implements Triangulation<P> {
*/
private boolean isIllegalEdge(P p, HalfEdge<P> edge){
if(edge.hasTwin()) {
P x = edge.getTwin().get().getEnd();
P y = edge.getTwin().get().getNext().getEnd();
P z = edge.getTwin().get().getNext().getNext().getEnd();
P x = edge.getTwin().getEnd();
P y = edge.getTwin().getNext().getEnd();
P z = edge.getTwin().getNext().getNext().getEnd();
VTriangle triangle = new VTriangle(new VPoint(x.getX(), x.getY()), new VPoint(y.getX(), y.getY()), new VPoint(z.getX(), z.getY()));
return triangle.isInCircumscribedCycle(p);
}
......@@ -431,7 +443,7 @@ public class BowyerWatson<P extends IPoint> implements Triangulation<P> {
HalfEdge<P> pz = xp.getNext();
Face<P> f2 = zx.getFace();
HalfEdge<P> xz = zx.getTwin().get();
HalfEdge<P> xz = zx.getTwin();
HalfEdge<P> zy = xz.getNext();
HalfEdge<P> yx = zy.getNext();
......
......@@ -130,12 +130,7 @@ public class PointLocation<P extends VPoint> {
return Optional.of(edge.getFace());
}
else {
if(edge.getTwin().isPresent()) {
return Optional.of(edge.getTwin().get().getFace());
}
else {
return Optional.empty();
}
}
return Optional.of(edge.getTwin().getFace());
}
}
}
......@@ -72,13 +72,13 @@ public class HalfEdge<P extends IPoint> {
return previous;
}
public Optional<HalfEdge<P>> getTwin() {
return Optional.ofNullable(twin);
public HalfEdge<P> getTwin() {
return twin;
}
public void setTwin(final @NotNull HalfEdge twin) {
this.twin = twin;
if(!twin.getTwin().isPresent() || twin.getTwin().get() != this) {
if(twin.getTwin() != this) {
twin.setTwin(this);
}
}
......@@ -113,23 +113,51 @@ public class HalfEdge<P extends IPoint> {
return new NeighbourIterator();
}
public Iterator<Face<P>> inciedentFaceIterator() { return new NeighbourFaceIterator(); }
/**
* This iterator assumes that the this edge is completely surrounded by faces.
*/
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();
}
}
@Override
public boolean hasNext() {
return neighbourIterator.hasNext();
}
@Override
public Face<P> next() {
return neighbourIterator.next().getFace();
}
}
/**
* This iterator assumes that the this edge is completely surrounded by faces.
*/
private class NeighbourIterator implements Iterator<HalfEdge<P>> {
private HalfEdge<P> current = HalfEdge.this;
private HalfEdge<P> current = HalfEdge.this.getNext();
private boolean first = true;
@Override
public boolean hasNext() {
return current.getTwin().isPresent() && (first || current != HalfEdge.this);
return (first || current != HalfEdge.this.getNext());
}
@Override
public HalfEdge<P> next() {
current = current.getTwin().get().getNext();
return current;
HalfEdge<P> result = current;
current = result.getTwin().getNext();
return result;
}
}
}
......@@ -6,9 +6,10 @@ import java.util.Set;
import java.util.stream.Stream;
public interface Triangulation<P extends IPoint> {
Face<P> locate(P point);
Face<P> locate(final double x, final double y);
Face<P> locate(final IPoint point);
Stream<Face<P>> streamFaces();
Set<Face<P>> getFaces();
void insert(P point);
void remove(P point);
void insert(final P point);
void remove(final P point);
}
package org.vadere.util.geometry.shapes;
public interface DPoint extends IPoint {
double getData();
void setData(final double date);
}
......@@ -21,6 +21,10 @@ public class VPoint implements Cloneable, IPoint {
public VPoint() {}
public VPoint(final IPoint point) {
this(point.getX(), point.getY());
}
public VPoint(double x, double y) {
this.x = x;
this.y = y;
......
......@@ -193,8 +193,7 @@ public class VTriangle extends VPolygon {
}
public boolean hasBoundaryPoint(final DataPoint point) {
return this.p1.equals(point) || this.p2.equals(point)
|| this.p3.equals(point);
return this.p1.equals(point) || this.p2.equals(point) || this.p3.equals(point);
}
public VLine[] getLines() {
......
package org.vadere.util.math;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.util.geometry.data.Face;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VTriangle;
import org.vadere.util.potential.CellGrid;
import org.vadere.util.potential.calculators.PotentialPoint;
import java.util.List;
/**
* Interpolation utilities not covered by java.lang.Math
......@@ -8,6 +16,25 @@ import org.vadere.util.potential.CellGrid;
*/
public class InterpolationUtil {
public static double barycentricInterpolation(final Face<PotentialPoint> triangle, final double x, final double y){
List<PotentialPoint> points = triangle.getPoints();
assert points.size() == 3;
PotentialPoint p1 = points.get(0);
PotentialPoint p2 = points.get(1);
PotentialPoint p3 = points.get(2);
VTriangle vtriangle = triangle.toTriangle();
double totalArea = vtriangle.getArea();
VPoint point = new VPoint(x, y);
double percentP1 = totalArea / (new VTriangle(new VPoint(p2), new VPoint(p3), point).getArea());
double percentP2 = totalArea / (new VTriangle(new VPoint(p1), new VPoint(p3), point).getArea());
double percentP3 = totalArea / (new VTriangle(new VPoint(p1), new VPoint(p2), point).getArea());
double value = percentP1 * p1.getPotential() + percentP2 * p2.getPotential() + percentP3 * p3.getPotential();
return value;
}
/**
* Computes bilinear interpolation of z = f(x,y) with z1 to z4 being the
* given z values at the edges of a rectangle in the xy-plane. z1 refers to
......
......@@ -2,6 +2,7 @@ package org.vadere.util.potential.calculators;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.math.InterpolationUtil;
import org.vadere.util.math.MathUtil;
import org.vadere.util.potential.CellGrid;
import java.awt.*;
......@@ -74,6 +75,10 @@ public abstract class AbstractGridEikonalSolver implements EikonalSolver {
return potentialField;
}
public boolean isValidPoint(Point point) {
return potentialField.isValidPoint(point);
}
private double[] getGridPotentials(final List<Point> points) {
double[] result = new double[points.size()];
......@@ -83,4 +88,199 @@ public abstract class AbstractGridEikonalSolver implements EikonalSolver {
return result;
}
protected double computeGodunovDifference(final Point point, final CellGrid cellGrid, final Direction direction) {
VPoint position = cellGrid.pointToCoord(point.x, point.y);
double cost = getTimeCostFunction().costAt(new VPoint(position.x, position.y));
double speed = (1.0 / cellGrid.getResolution()) / cost; // = F/cost
double a = 0;
double b = 0;
double distance = 1.0 / speed;
double c = -1.0 / (speed * speed);
double result = Double.MAX_VALUE;
Point xPoint;
Point yPoint;
Point xhPoint;
Point yhPoint;
switch (direction) {
case UP_LEFT:
xPoint = new Point(point.x - 1, point.y);
yPoint = new Point(point.x, point.y + 1);
xhPoint = new Point(point.x - 2, point.y);
yhPoint = new Point(point.x, point.y + 2);
break;
case UP_RIGHT:
xPoint = new Point(point.x + 1, point.y);
yPoint = new Point(point.x, point.y + 1);
xhPoint = new Point(point.x + 2, point.y);
yhPoint = new Point(point.x, point.y + 2);
break;
case BOTTOM_LEFT:
xPoint = new Point(point.x - 1, point.y);
yPoint = new Point(point.x, point.y - 1);
xhPoint = new Point(point.x - 2, point.y);
yhPoint = new Point(point.x, point.y - 2);
break;
case BOTTOM_RIGHT:
xPoint = new Point(point.x + 1, point.y);
yPoint = new Point(point.x, point.y - 1);
xhPoint = new Point(point.x + 2, point.y);
yhPoint = new Point(point.x, point.y - 2);
break;
default: {
if (isValidPoint(new Point(point.x + 1, point.y)) &&
(!isValidPoint(new Point(point.x - 1, point.y))
|| (cellGrid.getValue(new Point(point.x + 1, point.y)).potential < cellGrid
.getValue(new Point(point.x - 1, point.y)).potential))) {
xPoint = new Point(point.x + 1, point.y);
xhPoint = new Point(point.x + 2, point.y);
} else {
xPoint = new Point(point.x - 1, point.y);
xhPoint = new Point(point.x - 2, point.y);
}
if (isValidPoint(new Point(point.x, point.y + 1)) &&
(!isValidPoint(new Point(point.x, point.y - 1))
|| (cellGrid.getValue(new Point(point.x, point.y + 1)).potential < cellGrid
.getValue(new Point(point.x, point.y - 1)).potential))) {
yPoint = new Point(point.x, point.y + 1);
yhPoint = new Point(point.x, point.y + 2);
} else {
yPoint = new Point(point.x, point.y - 1);
yhPoint = new Point(point.x, point.y - 2);
}
}
}
double xVal = Double.MAX_VALUE;
if (isValidPoint(xPoint)) {
xVal = cellGrid.getValue(xPoint).potential;
if (xVal != Double.MAX_VALUE) {
a += 1.0;
b -= 2 * xVal;
c += Math.pow(xVal, 2);
}
}
double yVal = Double.MAX_VALUE;
if (isValidPoint(yPoint)) {
yVal = cellGrid.getValue(yPoint).potential;
if (yVal != Double.MAX_VALUE) {
a += 1.0;
b -= 2 * yVal;
c += Math.pow(yVal, 2);
}
}
if ((xVal != Double.MAX_VALUE ^ yVal != Double.MAX_VALUE) || Math.abs(xVal - yVal) >= distance) {
result = Math.min(xVal, yVal) + distance;
} else if ((xVal == Double.MAX_VALUE && yVal == Double.MAX_VALUE)) {
// logger.warn("no solution possible");
} else {
if (isHighAccuracy()) {
if (isValidPoint(xhPoint) && cellGrid.getValue(xhPoint).potential < xVal) {
double tp = (1.0 / 3.0) * (4.0 * xVal - cellGrid.getValue(xhPoint).potential);
double factor = 9.0 / 4.0;
a += factor;
b -= 2.0 * 9.0 / 4.0 * tp;
c += factor * Math.pow(tp, 2);
}
if (isValidPoint(yhPoint) && cellGrid.getValue(yhPoint).potential < yVal) {
double tp = (1.0 / 3.0) * (4.0 * yVal - cellGrid.getValue(yhPoint).potential);
double factor = 9.0 / 4.0;
a += factor;
b -= 2.0 * factor * tp;
c += factor * Math.pow(tp, 2);
}
}
java.util.List<Double> solutions = MathUtil.solveQuadratic(a, b, c);
int numberOfSolutions = solutions.size();
if (numberOfSolutions == 2) {
result = Math.max(solutions.get(0), solutions.get(1));
} else if (numberOfSolutions == 1) {
result = solutions.get(0);
}
}
return result;
}
protected double computeGodunovDifference(final Point point, final CellGrid cellGrid) {
double result = Double.MAX_VALUE;
// enables cost fields with cost != 1
VPoint position = cellGrid.pointToCoord(point.x, point.y);
double cost = getTimeCostFunction().costAt(new VPoint(position.x, position.y));
double speed = (1.0 / cellGrid.getResolution()) / cost; // = F/cost
double coeff0 = -1.0 / (speed * speed); // = - (F/cost)^2
double coeff1 = 0;
double coeff2 = 0;
java.util.List<Point> neighbors = MathUtil.getRelativeNeumannNeighborhood();
for (int j = 0; j < 2; j++) {
double val1 = Double.MAX_VALUE;
double val2 = Double.MAX_VALUE;
for (int i = 0; i < 2; i++) {
Point pni = new Point(point.x + neighbors.get(2 * j + i).x,
point.y + neighbors.get(2 * j + i).y);
Point pni2 = new Point(
point.x + neighbors.get(2 * j + i).x * 2, point.y
+ neighbors.get(2 * j + i).y * 2);
if (isValidPoint(pni) && cellGrid.getValue(pni).tag.frozen) {
double val1n = cellGrid.getValue(pni).potential;
if (val1n < val1) {
val1 = val1n;
if (isValidPoint(pni2)) {
double val2n = cellGrid.getValue(pni2).potential;
if (cellGrid.getValue(pni2).tag.frozen