diff --git a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/PathGeneratorTreeGrow.java b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/PathGeneratorTreeGrow.java index 07eabb38487..cb8ff2dde82 100644 --- a/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/PathGeneratorTreeGrow.java +++ b/java/com.sap.sailing.simulator/src/com/sap/sailing/simulator/impl/PathGeneratorTreeGrow.java @@ -11,6 +11,7 @@ import com.sap.sailing.domain.common.Bearing; import com.sap.sailing.domain.common.Distance; import com.sap.sailing.domain.common.Position; import com.sap.sailing.domain.common.TimePoint; +import com.sap.sailing.domain.common.impl.DegreeBearingImpl; import com.sap.sailing.domain.tracking.Wind; import com.sap.sailing.simulator.Path; import com.sap.sailing.simulator.PolarDiagram; @@ -62,6 +63,7 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { // generate step TimedPosition getStep(TimedPosition pos, long timeStep, long turnLoss, char prevDirection, char nextDirection) { + double offDeg = 5.0; WindFieldGenerator wf = simulationParameters.getWindField(); TimePoint curTime = pos.getTimePoint(); Position curPosition = pos.getPosition(); @@ -72,10 +74,17 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { // get beat-angle left and right Bearing travelBearing = null; + Bearing tmpBearing = null; if (nextDirection == 'L') { travelBearing = pd.optimalDirectionsUpwind()[0]; } else if (nextDirection == 'R') { travelBearing = pd.optimalDirectionsUpwind()[1]; + } else if (nextDirection == 'M') { + tmpBearing = pd.optimalDirectionsUpwind()[0]; + travelBearing = tmpBearing.add(new DegreeBearingImpl(+offDeg)); + } else if (nextDirection == 'S') { + tmpBearing = pd.optimalDirectionsUpwind()[1]; + travelBearing = tmpBearing.add(new DegreeBearingImpl(-offDeg)); } // determine beat-speed left and right @@ -92,31 +101,28 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { return new TimedPositionImpl(nextTime, travelSpeed.travelTo(curPosition, curTime, travelTime)); } - - // generate path candidates based on beat angles - List getPathCandsBeat(PathCand path, long timeStep, long turnLoss, Position posStart, Bearing bearVrt, double tgtHeight) { + PathCand getPathCand(PathCand path, char nextDirection, long timeStep, long turnLoss, Position posStart, Bearing bearVrt, double tgtHeight) { - // create path candidates - List result = new ArrayList(); + // calculate next path position (taking turn-loss into account) + TimedPosition pathPos = this.getStep(path.pos, timeStep, turnLoss, path.path.charAt(path.path.length()-1), nextDirection); - //TimedPosition pathPos = new TimedPositionImpl(leftTime, beatSpeedLft.travelTo(curPosition, curTime, leftTime)); - TimedPosition pathPos = this.getStep(path.pos, timeStep, turnLoss, path.path.charAt(path.path.length()-1), 'L'); + // calculate height-position with reference to race course Position posHeight = pathPos.getPosition().projectToLineThrough(posStart, bearVrt); - //System.out.println(""+pathPos.getPosition().getLatDeg()+", "+pathPos.getPosition().getLngDeg()); - //System.out.println(""+posStart.getLatDeg()+", "+posStart.getLngDeg()); + // calculate vertical distance as distance of height-position to start double vrtDist = Math.round(posHeight.getDistance(posStart).getMeters()*100.0)/100.0; - if (vrtDist > tgtHeight) { - // scale last step so that vrtDist ~ tgtHeight - Position prevPos = path.pos.getPosition(); - TimePoint prevTime = path.pos.getTimePoint(); - double heightFrac = (tgtHeight - path.vrt) / (vrtDist - path.vrt); - Position newPos = prevPos.translateGreatCircle(prevPos.getBearingGreatCircle(pathPos.getPosition()), prevPos.getDistance(pathPos.getPosition()).scale(heightFrac)); - TimePoint newTime = new MillisecondsTimePoint(Math.round(prevTime.asMillis() + (pathPos.getTimePoint().asMillis()-prevTime.asMillis())*heightFrac)); - pathPos = new TimedPositionImpl(newTime, newPos); - posHeight = pathPos.getPosition().projectToLineThrough(posStart, bearVrt); - } - + /*if (vrtDist > tgtHeight) { + // scale last step so that vrtDist ~ tgtHeight + Position prevPos = path.pos.getPosition(); + TimePoint prevTime = path.pos.getTimePoint(); + double heightFrac = (tgtHeight - path.vrt) / (vrtDist - path.vrt); + Position newPos = prevPos.translateGreatCircle(prevPos.getBearingGreatCircle(pathPos.getPosition()), prevPos.getDistance(pathPos.getPosition()).scale(heightFrac)); + TimePoint newTime = new MillisecondsTimePoint(Math.round(prevTime.asMillis() + (pathPos.getTimePoint().asMillis()-prevTime.asMillis())*heightFrac)); + pathPos = new TimedPositionImpl(newTime, newPos); + posHeight = pathPos.getPosition().projectToLineThrough(posStart, bearVrt); + }*/ + + // calculate horizontal side: left or right in reference to race course double posSide = 1; double posBear = posStart.getBearingGreatCircle(pathPos.getPosition()).getDegrees(); if ((posBear < 0.0)||(posBear > 180.0)) { @@ -124,44 +130,37 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { } else if ((posBear == 0.0)||(posBear == 180.0)) { posSide = 0; } + // calculate horizontal distance as distance of height-position to current position double hrzDist = Math.round(posSide*posHeight.getDistance(pathPos.getPosition()).getMeters()*100.0)/100.0; - //System.out.println(""+path.path+": "); - //System.out.println("vrtDistL:"+posHeight.getDistance(posStart).getCentralAngleRad()); - //System.out.println("hrzDistL:"+posSide*posHeight.getDistance(pathPos.getPosition()).getCentralAngleRad()); - String pathStr = path.path+"L"; - result.add(new PathCand(pathPos, vrtDist, hrzDist, pathStr)); - //System.out.println(""+posHeight.getLatDeg()+", "+posHeight.getLngDeg()+", "+pathStr); - - //pathPos = new TimedPositionImpl(rightTime, beatSpeedRght.travelTo(curPosition, curTime, rightTime)); - pathPos = this.getStep(path.pos, timeStep, turnLoss, path.path.charAt(path.path.length()-1), 'R'); - posHeight = pathPos.getPosition().projectToLineThrough(posStart, bearVrt); - //System.out.println(""+pathPos.getPosition().getLatDeg()+", "+pathPos.getPosition().getLngDeg()); - //System.out.println(""+posStart.getLatDeg()+", "+posStart.getLngDeg()); - vrtDist = Math.round(posHeight.getDistance(posStart).getMeters()*100.0)/100.0; - if (vrtDist > tgtHeight) { - // scale last step so that vrtDist ~ tgtHeight - Position prevPos = path.pos.getPosition(); - TimePoint prevTime = path.pos.getTimePoint(); - double heightFrac = (tgtHeight - path.vrt) / (vrtDist - path.vrt); - Position newPos = prevPos.translateGreatCircle(prevPos.getBearingGreatCircle(pathPos.getPosition()), prevPos.getDistance(pathPos.getPosition()).scale(heightFrac)); - TimePoint newTime = new MillisecondsTimePoint(Math.round(prevTime.asMillis() + (pathPos.getTimePoint().asMillis()-prevTime.asMillis())*heightFrac)); - pathPos = new TimedPositionImpl(newTime, newPos); - posHeight = pathPos.getPosition().projectToLineThrough(posStart, bearVrt); - } - posSide = 1; - posBear = posStart.getBearingGreatCircle(pathPos.getPosition()).getDegrees(); - if ((posBear < 0.0)||(posBear > 180.0)) { - posSide = -1; - } else if ((posBear == 0.0)||(posBear == 180.0)) { - posSide = 0; - } - hrzDist = Math.round(posSide*posHeight.getDistance(pathPos.getPosition()).getMeters()*100.0)/100.0; - //System.out.println("vrtDistR:"+posHeight.getDistance(posStart).getCentralAngleRad()); - //System.out.println("hrzDistR:"+posSide*posHeight.getDistance(pathPos.getPosition()).getCentralAngleRad()); - pathStr = path.path+"R"; - result.add(new PathCand(pathPos, vrtDist, hrzDist, pathStr)); - //System.out.println(""+posHeight.getLatDeg()+", "+posHeight.getLngDeg()+", "+pathStr); + // extend path-string by step-direction + String pathStr = path.path + nextDirection; + + return (new PathCand(pathPos, vrtDist, hrzDist, pathStr)); + } + + + // generate path candidates based on beat angles + List getPathCandsBeat(PathCand path, long timeStep, long turnLoss, Position posStart, Bearing bearVrt, double tgtHeight) { + + List result = new ArrayList(); + PathCand newPathCand; + + // step left + newPathCand = getPathCand(path, 'L', timeStep, turnLoss, posStart, bearVrt, tgtHeight); + result.add(newPathCand); + + // step wide left + newPathCand = getPathCand(path, 'M', timeStep, turnLoss, posStart, bearVrt, tgtHeight); + result.add(newPathCand); + + // step right + newPathCand = getPathCand(path, 'R', timeStep, turnLoss, posStart, bearVrt, tgtHeight); + result.add(newPathCand); + + // step wide right + newPathCand = getPathCand(path, 'S', timeStep, turnLoss, posStart, bearVrt, tgtHeight); + result.add(newPathCand); return result; } @@ -209,23 +208,37 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { tstPosition = this.getStep(new TimedPositionImpl(startTime, startPos), timeStep, turnLoss, '0', 'R'); double tstDist2 = startPos.getDistance(tstPosition.getPosition()).getMeters(); - double hrzBinSize = (tstDist1 + tstDist2)/4; // horizontal bin size in meters + double hrzBinSize = (tstDist1 + tstDist2)/3; // horizontal bin size in meters + System.out.println("Horizontal Bin Size: "+hrzBinSize); + double oobFact = 0.75; // out-of-bounds factor + //int hrzLeft = (int)Math.round(Math.floor( -distStartEndMeters / 2.0 / hrzBinSize / oobFact )); + //int hrzRight = (int)Math.round(Math.floor( distStartEndMeters / 2.0 / hrzBinSize / oobFact )); boolean reachedEnd = false; + int addSteps = 0; + int finalSteps = 0; // maximum number of additional steps after first target-path found //for(int count=0; count<10; count++) { - while (!reachedEnd) { + while ((!reachedEnd)||(addSteps newPathCands; List newPaths = new ArrayList(); for(PathCand curPath : allPaths) { - - List newPathCands = this.getPathCandsBeat(curPath, timeStep, turnLoss, startPos, bearVrt, distStartEndMeters); + + if ((curPath.vrt > distStartEndMeters)) { + continue; + } else { + newPathCands = this.getPathCandsBeat(curPath, timeStep, turnLoss, startPos, bearVrt, distStartEndMeters); + } for(PathCand newPath : newPathCands) { - //System.out.println(""+newPath.pos.getPosition().getLatDeg()+", "+newPath.pos.getPosition().getLngDeg()+", "+newPath.path); newPaths.add(newPath); if (newPath.hrz < hrzMin) { @@ -239,58 +252,82 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { int hrzLeft = (int)Math.round(Math.floor( (hrzMin + hrzBinSize/2.0) / hrzBinSize )); int hrzRight = (int)Math.round(Math.floor( (hrzMax + hrzBinSize/2.0) / hrzBinSize )); + //System.out.println("hrzLeft: "+hrzLeft+", hrzRight: "+hrzRight); // build map of best path per hrz-bin + int countPath = 0; int mapSize = hrzRight-hrzLeft+1; + //System.out.println("MapSize: "+mapSize); + int[] binIdx = new int[mapSize]; // TODO: init with zero? double[] binVal = new double[mapSize]; // TODO: init with zero? for(int curIdx=0; curIdx 0.75*distStartEndMeters) { + if (distFromMiddleMeters > oobFact*distStartEndMeters) { continue; // ignore curPath } + // increase path-counter + countPath++; + // determine map-key int curKey = (int)Math.round(Math.floor( (curPath.hrz + hrzBinSize/2.0) / hrzBinSize )) - hrzLeft; - + // check whether curPath is better then the ones looked at + if ((curKey>=0)&&(curKey < mapSize)) { if (curPath.vrt > binVal[curKey]) { binVal[curKey] = curPath.vrt; binIdx[curKey] = curIdx+1; } + } } - // take best from each hrz-bin and check if target is reached - allPaths = new ArrayList(); - for(int curKey=0; curKey0) { - PathCand curPath = newPaths.get(binIdx[curKey]-1); - allPaths.add(curPath); - if ((curPath.vrt > distStartEndMeters)) { - if ((Math.abs(curKey + hrzLeft) <= 3)) { - //System.out.println(""+curPath.path+": "+(curKey+hrzLeft)+", hrz:"+curPath.hrz+", bin:"+(Math.floor( (curPath.hrz + hrzBinSize/2.0) / hrzBinSize ))); - reachedEnd = true; - trgPaths.add(curPath); + // check if there are still paths inside regatta-area + if (countPath > 0) { + // take best from each horizontal-bin and check if target is reached + allPaths = new ArrayList(); + for (int curKey = 0; curKey < mapSize; curKey++) { + + if (binIdx[curKey] > 0) { + PathCand curPath = newPaths.get(binIdx[curKey] - 1); + allPaths.add(curPath); + + // debug output + /*if ((Math.abs(curKey + hrzLeft) <= 3)) { + System.out.println("" + curPath.path + ": " + (curKey + hrzLeft) + ", vrt:" + curPath.vrt + ", hrz:" + curPath.hrz + ", bin:" + (Math.floor((curPath.hrz + hrzBinSize / 2.0) / hrzBinSize))); + }*/ + + // terminate path-search if paths close enough to target are found + if ((curPath.vrt > distStartEndMeters)) { + if ((Math.abs(curKey + hrzLeft) <= 2)) { + reachedEnd = true; + trgPaths.add(curPath); // add path to list of target-paths + } } } } + } else { + // terminate path-search as no path inside regatta-area are left + reachedEnd = true; } } + // if no target-paths were found, return empty path if (trgPaths.size() == 0) { //trgPaths = allPaths; // TODO: only for testing; remove lateron return new PathImpl(path, wf); // return empty path } + // sort target-paths ascending by distance-to-target Collections.sort(trgPaths); // debug output for(PathCand curPath : trgPaths) { - System.out.print(""+curPath.path+": "+curPath.pos.getTimePoint().asMillis()); + System.out.print(""+curPath.path+": "+curPath.pos.getTimePoint().asMillis()+", "+curPath.pos.getPosition().getLatDeg()+", "+curPath.pos.getPosition().getLngDeg()+", "); System.out.println(" height:"+curPath.vrt+" of "+startPos.getDistance(endPos).getMeters()+", dist:"+curPath.pos.getPosition().getDistance(endPos)); } @@ -299,7 +336,7 @@ public class PathGeneratorTreeGrow extends PathGeneratorBase { // // generate intermediate steps - PathCand bstPath = trgPaths.get(0); + PathCand bstPath = trgPaths.get(0); // target-path ending closest to target TimedPositionWithSpeed curPosition = null; char curDirection = '0'; char prevDirection = '0';