Commit 8d94d9df authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

before changing initial point generation of DSMesh

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