From 02711866b792c2da059b2b8cf20e22446f32b894 Mon Sep 17 00:00:00 2001 From: Alexandru Dimofte Date: Thu, 7 Jun 2012 20:34:18 +0300 Subject: [PATCH 1/2] added createDjikstra() method --- .../simulator/impl/SailingSimulatorImpl.java | 226 +++++++----------- 1 file changed, 92 insertions(+), 134 deletions(-) diff --git a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java index c31a2cd6a27..5134c3c41b9 100644 --- a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java +++ b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java @@ -1,7 +1,12 @@ package com.sap.sailing.simulator.impl; import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collections; +import java.util.HashMap; +import java.util.LinkedList; import java.util.List; +import java.util.Map; import com.sap.sailing.domain.base.SpeedWithBearing; import com.sap.sailing.domain.base.impl.MeterDistance; @@ -124,157 +129,110 @@ public class SailingSimulatorImpl implements SailingSimulator { return new PathImpl(lst); } - private Path createForwardDynamic() { + private Path createDjikstra() { + //retrieve simulation parameters Boundary boundary = simulationParameters.getBoundaries(); - WindField wf = simulationParameters.getWindField(); - PolarDiagram pd = simulationParameters.getBoatPolarDiagram(); + WindField windField = simulationParameters.getWindField(); + PolarDiagram polarDiagram = simulationParameters.getBoatPolarDiagram(); Position start = simulationParameters.getCourse().get(0); Position end = simulationParameters.getCourse().get(1); TimePoint startTime = new MillisecondsTimePoint(0); - List lst = new ArrayList(); - - Position currentPosition = start; - TimePoint currentTime = startTime; + //the solution path + LinkedList lst = new LinkedList(); + + //initiate grid int gridv = 10; // number of vertical grid steps int gridh = 30; // number of horizontal grid steps Position[][] sailGrid = boundary.extractGrid(gridh, gridv); + //create adjacency graph including start and end + Map> graph = new HashMap>(); + graph.put(start, Arrays.asList(sailGrid[0])); + for(int i = 0; i < gridv-1; i++) { + for(Position p : sailGrid[i]) { + graph.put(p, Arrays.asList(sailGrid[i+1])); + } + } + for(Position p: sailGrid[gridv-1]) { + graph.put(p, Arrays.asList(end)); + } + //create backwards adjacency graph, required to reconstruct the optimal path + Map> backGraph = new HashMap>(); + backGraph.put(end, Arrays.asList(sailGrid[gridv-1])); + for(int i = gridv-2; i>0; i--) { + for(Position p: sailGrid[i]) { + backGraph.put(p, Arrays.asList(sailGrid[i-1])); + } + } + for(Position p : sailGrid[0]) { + backGraph.put(p, Arrays.asList(start)); + } + //create tentative distance matrix + Map tentativeDistances = new HashMap(); + for(Position p : graph.keySet()) { + tentativeDistances.put(p, Double.POSITIVE_INFINITY); + } + tentativeDistances.put(start, 0.0); - /* - stretch.x <- 0.5 ## stretch-factor for horizontal grid + //create set of unvisited nodes + List unvisited = new ArrayList(graph.keySet()); - grid.x <- (-grid.h:grid.h)/grid.h*stretch.x - grid.y <- (0:grid.v)/grid.v + //set the initial node as current + Position currentPosition = start; + TimePoint currentTime = startTime; - sail.grid <- expand.grid(grid.x,grid.y) - */ + //search loop + while(unvisited.contains(end)) { + + TimedPosition currentTimedPosition = new TimedPositionImpl(currentTime, currentPosition); + SpeedWithBearing currentWind = windField.getWind(currentTimedPosition); + polarDiagram.setWind(currentWind); + + List unvisitedNeighbours = graph.get(currentPosition); + unvisitedNeighbours.retainAll(unvisited); + for(Position p : unvisitedNeighbours) { + Bearing bearingToP = currentPosition.getBearingGreatCircle(p); + Distance distanceToP = currentPosition.getDistance(p); + Speed speedToP = polarDiagram.getSpeedAtBearing(bearingToP); + //multiplied by 1000 to have milliseconds + Double timeToP = 1000 * (distanceToP.getMeters() / speedToP.getMetersPerSecond()); + Double tentativeDistanceToP = currentTime.asMillis() + timeToP; + if (tentativeDistanceToP < tentativeDistances.get(p)) { + tentativeDistances.put(p, tentativeDistanceToP); + } + } + unvisited.remove(currentPosition); + + Map unvisitedTentativeDistances = new HashMap(tentativeDistances); + unvisitedTentativeDistances.entrySet().retainAll(unvisited); + Double minTentativeDistance = Collections.min(unvisitedTentativeDistances.values()); + for (Position p : unvisitedTentativeDistances.keySet()) { + if( unvisitedTentativeDistances.get(p) == minTentativeDistance ) { + currentPosition = p; + currentTime = new MillisecondsTimePoint(minTentativeDistance.longValue()); + break; + } + } + + } - - /*for(idxv=1; idxv<=hridv; idxv++) { - - paths.new <- vector("list",2*grid.h+1) - duras.new <- vector("list",2*grid.h+1) - alldur <- NULL - - if (idx.v==1) { - range1 <- 0 - } else { - range1 <- grid.h - } - - if (idx.v==grid.v) { - range2 <- 0 - } else { - range2 <- grid.h - } - - for(idx.h2 in -range2:range2) { - - alldur <- NULL - - for(idx.h1 in -range1:range1) { - - duration.h1h2 <- duration(idx.h1,idx.h2,idx.v) - - if (duration.h1h2 == Inf) { - next - } - - if (idx.v==1) { - dur <- data.frame(duration=signif(duration.h1h2,digits=sig.digits),hidx=idx.h1+range1+1,pidx=1) - if (is.null(alldur)) { - alldur <- dur - } else { - alldur <- rbind(alldur,dur) - } - - } else { - - for(pidx in 1:length(paths[[idx.h1+range1+1]])) { - - curdur = signif(duration.h1h2 + duras[[idx.h1+range1+1]][pidx],digits=sig.digits) - - dur <- data.frame(duration=curdur,hidx=idx.h1+range1+1,pidx=pidx) - if (is.null(alldur)) { - alldur <- dur - } else { - alldur <- rbind(alldur,dur) - } - - } - - } - - } - - if (idx.v>1) { - - if (idx.v != grid.v) { - - idx.min <- alldur[which(alldur[,"duration"] == min(alldur[,"duration"])),] - - jdx <- 0 - for (pidx in 1:nrow(idx.min)) { - jdx <- jdx+1 - paths.new[[idx.h2+grid.h+1]][[jdx]] <- c(paths[[idx.min[pidx,"hidx"]]][[idx.min[pidx,"pidx"]]],idx.h2) - duras.new[[idx.h2+grid.h+1]][[jdx]] <- idx.min[pidx,"duration"] - } - - } else { - - for(ddx in 1:nrow(alldur)) { - - tcks <- tacks(c(paths[[alldur[ddx,"hidx"]]][[alldur[ddx,"pidx"]]],0)) - alldur[ddx,"duration"] <- tcks*tack.cost + alldur[ddx,"duration"] - - } - - dur.idx <- order(alldur[,"duration"]) - srtdur <- alldur[dur.idx,] - - jdx <- 1 - kdx <- 1 - p.grps <- length(unique(alldur[,"duration"])) - paths.new <- vector("list",p.grps) - paths.new[[kdx]][[jdx]] <- c(paths[[srtdur[1,"hidx"]]][[srtdur[1,"pidx"]]],0) - duras.new <- vector("list",p.grps) - duras.new[[kdx]] <- srtdur[1,"duration"] - - for (pidx in 2:nrow(alldur)) { - - if (srtdur[pidx,"duration"]==srtdur[pidx-1,"duration"]) { - jdx <- jdx + 1 - } else { - jdx <- 1 - kdx <- kdx + 1 - duras.new[[kdx]] <- srtdur[pidx,"duration"] - } - paths.new[[kdx]][[jdx]] <- c(paths[[srtdur[pidx,"hidx"]]][[srtdur[pidx,"pidx"]]],0) - - } - - } - - } else { - for (idx in -grid.h:grid.h) { - paths.new[[idx+grid.h+1]][[1]] <- c(0,idx) - dur.eval <- duration(0,idx,1) - duras.new[[idx+grid.h+1]][[1]] <- dur.eval[1] - } - } - - } - - paths <- paths.new - duras <- duras.new + //reconstruct the optimal path by going from start to end + currentPosition = end; + Position nextPosition = end; + while(nextPosition!=start) { + List currentPredecessors = backGraph.get(currentPosition); + + currentTime = new MillisecondsTimePoint(tentativeDistances.get(currentPosition).longValue()); + TimedPosition currentTimedPosition = new TimedPositionImpl(currentTime, end); + SpeedWithBearing currentWind = windField.getWind(currentTimedPosition); + polarDiagram.setWind(currentWind); + SpeedWithBearing currentSpeed; + //TimedPositionWithSpeed = new TimedPositionWithSpeedImpl(currentTime, currentPosition, ); } - - */ - return new PathImpl(lst); } From 51464df683ab7b970df14821ed375fd53c51107c Mon Sep 17 00:00:00 2001 From: Alexandru Dimofte Date: Fri, 8 Jun 2012 13:45:00 +0300 Subject: [PATCH 2/2] fixed createDjikstra(); added getAllPaths() --- .../sailing/simulator/test/SimulatorTest.java | 42 ++++++--- .../sailing/simulator/SailingSimulator.java | 3 + .../simulator/impl/SailingSimulatorImpl.java | 86 ++++++++++++------- 3 files changed, 90 insertions(+), 41 deletions(-) diff --git a/java/com.sap.sailing.simulator.test/src/com/sap/sailing/simulator/test/SimulatorTest.java b/java/com.sap.sailing.simulator.test/src/com/sap/sailing/simulator/test/SimulatorTest.java index ac18a8bd58c..66154c941da 100644 --- a/java/com.sap.sailing.simulator.test/src/com/sap/sailing/simulator/test/SimulatorTest.java +++ b/java/com.sap.sailing.simulator.test/src/com/sap/sailing/simulator/test/SimulatorTest.java @@ -56,7 +56,7 @@ public class SimulatorTest { SailingSimulatorImpl sailingSim = new SailingSimulatorImpl(param); Path path = sailingSim.getOptimumPath(); - logger.info("Path with " + path.getPathPoints().size() + "points"); + //logger.info("Path with " + path.getPathPoints().size() + "points"); assertEquals("Number of path points", 2612, path.getPathPoints().size()); /* * for(TimedPositionWithSpeed p : path.getPathPoints()) { logger.info("Position: " + p.getPosition() + " Wind: " @@ -88,15 +88,15 @@ public class SimulatorTest { SailingSimulator solver = new SailingSimulatorImpl(sp); Path pth = solver.getOptimumPath(); - logger.info("Path with " + pth.getPathPoints().size() + "points"); - for (TimedPositionWithSpeed p : pth.getPathPoints()) { + //logger.info("Path with " + pth.getPathPoints().size() + "points"); + /* for (TimedPositionWithSpeed p : pth.getPathPoints()) { // the null in the Wind output is the timestamp - this Wind is time-invariant! System.out.println("Position: " + p.getPosition() + " Wind: " + wf.getWind(new TimedPositionWithSpeedSimple(p.getPosition()))); System.out.println("Position: " + p.getPosition() + " Wind: " + wf.getWind(pth.getPositionAtTime(p.getTimePoint()))); // Wind wind = wf.getWind(pth.getPositionAtTime(p.getTimePoint())); - } + }*/ assertEquals("Number of path points", 37, pth.getPathPoints().size()); } @@ -132,12 +132,12 @@ public class SimulatorTest { SailingSimulator solver = new SailingSimulatorImpl(sp); Path pth = solver.getOptimumPath(); - logger.info("Path with " + pth.getPathPoints().size() + " points"); - for (TimedPositionWithSpeed p : pth.getPathPoints()) { + //logger.info("Path with " + pth.getPathPoints().size() + " points"); + /* for (TimedPositionWithSpeed p : pth.getPathPoints()) { System.out.println("Position: " + p.getPosition() + " Wind: " + wf.getWind(pth.getPositionAtTime(p.getTimePoint()))); // Wind wind = wf.getWind(pth.getPositionAtTime(p.getTimePoint())); - } + }*/ assertEquals("Number of path points", 37, pth.getPathPoints().size()); } @@ -174,16 +174,38 @@ public class SimulatorTest { SailingSimulator solver = new SailingSimulatorImpl(sp); Path pth = solver.getOptimumPath(); - logger.info("Path with " + pth.getPathPoints().size() + " points"); - for (TimedPositionWithSpeed p : pth.getPathPoints()) { + //logger.info("Path with " + pth.getPathPoints().size() + " points"); + /*for (TimedPositionWithSpeed p : pth.getPathPoints()) { System.out.println("Position: " + p.getPosition() + " Wind: " + wf.getWind(pth.getPositionAtTime(p.getTimePoint()))); // Wind wind = wf.getWind(pth.getPositionAtTime(p.getTimePoint())); - } + }*/ assertEquals("Number of path points", 47, pth.getPathPoints().size()); } + @Test + public void testSailingSimulatorDjikstra() { + + Position start = new DegreePosition(48.401856, -140.001526); + Position end = new DegreePosition(49.143987, -139.987783); + //System.out.println(start.getDistance(end).getKilometers()); + + List course = new LinkedList(); + course.add(start); + course.add(end); + PolarDiagram pd = new PolarDiagramImpl(0); + RectangularBoundary bd = new RectangularBoundary(start, end, 0.1); + KilometersPerHourSpeedImpl kmhrSpeed = new KilometersPerHourSpeedImpl(7.2); + WindControlParameters windParameters = new WindControlParameters(kmhrSpeed.getKnots(), 45); + WindField wf = new WindFieldImpl(bd, windParameters); + SimulationParameters param = new SimulationParametersImpl(course, pd, wf); + + SailingSimulatorImpl sailingSim = new SailingSimulatorImpl(param); + Path path = sailingSim.getAllPaths().get("Djikstra"); + System.out.println(path.getPathPoints().size()); + assertEquals("none", 1, 1); + } } diff --git a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/SailingSimulator.java b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/SailingSimulator.java index 0c8634db307..4873966d545 100644 --- a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/SailingSimulator.java +++ b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/SailingSimulator.java @@ -1,10 +1,13 @@ package com.sap.sailing.simulator; +import java.util.Map; + public interface SailingSimulator { void setSimulationParameters(SimulationParameters params); SimulationParameters getSimulationParameters(); Path getOptimumPath(); + Map getAllPaths(); } diff --git a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java index 5134c3c41b9..7ab08d36c0d 100644 --- a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java +++ b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/SailingSimulatorImpl.java @@ -154,14 +154,14 @@ public class SailingSimulatorImpl implements SailingSimulator { graph.put(p, Arrays.asList(sailGrid[i+1])); } } - for(Position p: sailGrid[gridv-1]) { + for(Position p : sailGrid[gridv-1]) { graph.put(p, Arrays.asList(end)); } //create backwards adjacency graph, required to reconstruct the optimal path Map> backGraph = new HashMap>(); backGraph.put(end, Arrays.asList(sailGrid[gridv-1])); - for(int i = gridv-2; i>0; i--) { + for(int i = gridv-1; i>0; i--) { for(Position p: sailGrid[i]) { backGraph.put(p, Arrays.asList(sailGrid[i-1])); } @@ -171,69 +171,93 @@ public class SailingSimulatorImpl implements SailingSimulator { } //create tentative distance matrix - Map tentativeDistances = new HashMap(); + Map tentativeDistances = new HashMap(); for(Position p : graph.keySet()) { - tentativeDistances.put(p, Double.POSITIVE_INFINITY); + tentativeDistances.put(p, Long.MAX_VALUE); } - tentativeDistances.put(start, 0.0); + tentativeDistances.put(start, 0L); + tentativeDistances.put(end, Long.MAX_VALUE); //create set of unvisited nodes - List unvisited = new ArrayList(graph.keySet()); + List unvisited = new ArrayList(graph.keySet()); + unvisited.add(end); //set the initial node as current Position currentPosition = start; TimePoint currentTime = startTime; //search loop - while(unvisited.contains(end)) { - + //ends when the end is visited + while(currentPosition != end) { + //set the polar diagram to the wind at the current position and time TimedPosition currentTimedPosition = new TimedPositionImpl(currentTime, currentPosition); SpeedWithBearing currentWind = windField.getWind(currentTimedPosition); polarDiagram.setWind(currentWind); - List unvisitedNeighbours = graph.get(currentPosition); + //compute the tentative distance to all the unvisited neighbours of the current node + //and replace it in the matrix if is smaller than the previous one + List unvisitedNeighbours = new LinkedList(graph.get(currentPosition)); unvisitedNeighbours.retainAll(unvisited); for(Position p : unvisitedNeighbours) { Bearing bearingToP = currentPosition.getBearingGreatCircle(p); Distance distanceToP = currentPosition.getDistance(p); Speed speedToP = polarDiagram.getSpeedAtBearing(bearingToP); //multiplied by 1000 to have milliseconds - Double timeToP = 1000 * (distanceToP.getMeters() / speedToP.getMetersPerSecond()); - Double tentativeDistanceToP = currentTime.asMillis() + timeToP; + Long timeToP = (long) (1000 * (distanceToP.getMeters() / speedToP.getMetersPerSecond())); + Long tentativeDistanceToP = currentTime.asMillis() + timeToP; if (tentativeDistanceToP < tentativeDistances.get(p)) { tentativeDistances.put(p, tentativeDistanceToP); } } + + //mark current node as visited unvisited.remove(currentPosition); - Map unvisitedTentativeDistances = new HashMap(tentativeDistances); - unvisitedTentativeDistances.entrySet().retainAll(unvisited); - Double minTentativeDistance = Collections.min(unvisitedTentativeDistances.values()); - for (Position p : unvisitedTentativeDistances.keySet()) { - if( unvisitedTentativeDistances.get(p) == minTentativeDistance ) { + //select the unvisited node with the smallest tentative distance + //and set it as current + Long minTentativeDistance = Long.MAX_VALUE; + for (Position p : unvisited) { + if( tentativeDistances.get(p) < minTentativeDistance ) { currentPosition = p; - currentTime = new MillisecondsTimePoint(minTentativeDistance.longValue()); - break; + currentTime = new MillisecondsTimePoint(minTentativeDistance); } - } - } + } + } + //I need to add the end point to the distances matrix + tentativeDistances.put(end,currentTime.asMillis()); + + //at this point currentPosition = end + //currentTime = total duration of the course //reconstruct the optimal path by going from start to end - currentPosition = end; - Position nextPosition = end; - while(nextPosition!=start) { + while(currentPosition != start) { + TimedPositionWithSpeed currentTimedPositionWithSpeed = new TimedPositionWithSpeedImpl(currentTime, currentPosition, null ); + lst.addFirst(currentTimedPositionWithSpeed); List currentPredecessors = backGraph.get(currentPosition); - - currentTime = new MillisecondsTimePoint(tentativeDistances.get(currentPosition).longValue()); - TimedPosition currentTimedPosition = new TimedPositionImpl(currentTime, end); - SpeedWithBearing currentWind = windField.getWind(currentTimedPosition); - polarDiagram.setWind(currentWind); - SpeedWithBearing currentSpeed; - //TimedPositionWithSpeed = new TimedPositionWithSpeedImpl(currentTime, currentPosition, ); - + Long minTime = Long.MAX_VALUE; + for(Position p : currentPredecessors) { + if(tentativeDistances.get(p) < minTime) { + minTime = tentativeDistances.get(p); + currentPosition = p; + currentTime = new MillisecondsTimePoint(minTime); + } + } } + //I need to add the first point to the path + lst.addFirst(new TimedPositionWithSpeedImpl(startTime, start, null)); + + return new PathImpl(lst); } + + @Override + public Map getAllPaths() { + Map allPaths = new HashMap(); + allPaths.put("Dummy", createDummy()); + allPaths.put("Heuristic", createHeuristic()); + allPaths.put("Djikstra", createDjikstra()); + return allPaths; + } }