Commit 8d94d9df by Benedikt Zoennchen

### 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; ... ...

implements EikonalSolver { ... ... @@ -41,7 +42,7 @@ public class EikonalSolverFMMTriangulation

implements private boolean calculationFinished; private PriorityQueue narrowBand; private final Collection targetAreas; private final PointConstructor

pointConstructor; private PointConstructor

pointConstructor; private Comparator pointComparator = (he1, he2) -> { if (he1.halfEdge.getEnd().getPotential() < he2.halfEdge.getEnd().getPotential()) { ... ... @@ -64,6 +65,35 @@ public class EikonalSolverFMMTriangulation

implements this.timeCostFunction = timeCostFunction; this.targetAreas = targetAreas; this.pointConstructor = pointConstructor; this.narrowBand = new PriorityQueue<>(pointComparator); } public EikonalSolverFMMTriangulation(final Collection targetPoints, final ITimeCostFunction timeCostFunction, final Triangulation

triangulation ) { this.triangulation = triangulation; this.calculationFinished = false; this.timeCostFunction = timeCostFunction; this.targetAreas = new ArrayList<>(); this.narrowBand = new PriorityQueue<>(pointComparator); for(IPoint point : targetPoints) { Face face = triangulation.locate(point); for(HalfEdge 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

implements @Override public void initialize() { narrowBand = new PriorityQueue<>(pointComparator); initializeTargetAreas(); /*for(IPoint point : targetPoints) { Face face = triangulation.locate(point); ... ... @@ -186,7 +215,6 @@ public class EikonalSolverFMMTriangulation

implements HalfEdge 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

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

implements while (it.hasNext()) { Face 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

implements * @param point * @param face */ private double updatePoint(final PotentialPoint point, final Face face) { private double computeValue(final PotentialPoint point, final Face face) { // check whether the triangle does contain useful data List points = face.getPoints(); HalfEdge halfEdge = face.stream().filter(p -> p.getEnd().equals(point)).findAny().get(); ... ... @@ -251,32 +277,98 @@ public class EikonalSolverFMMTriangulation

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 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 findIntersectionPoint(final HalfEdge 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 minHe = null; Optional> optHe = findPointInCone(halfEdge, p1, p2); // /*Predicate> pred = f -> { List> 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 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 faceIterator = new FaceIterator(halfEdge.getFace()); while (faceIterator.hasNext()) { Face face = faceIterator.next(); List> 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 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> findPointInCone(final HalfEdge halfEdge, final PotentialPoint p1, final PotentialPoint p2) { //TODO: refactoring! private HalfEdge findPointInCone(final HalfEdge 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

implements VCone cone = new VCone(origin, direction, angle); // 2. search for the nearest point inside the cone HalfEdge he = halfEdge.getPrevious().getTwin(); Set> visitedFaces = new HashSet<>(); LinkedList> pointList = new LinkedList<>(); pointList.add(halfEdge.getPrevious().getTwin().getNext()); visitedFaces.add(halfEdge.getPrevious().getTwin().getNext().getFace()); while (!pointList.isEmpty()) { HalfEdge 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 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 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 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

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.