Merge pull request graphhopper#177 from JohannesPelzer/master

Added azimuth and direction to GPX <rtept> extension

Author: Peter

core/src/main/java/com/graphhopper/util/AngleCalc2D.java

@@ -27,13 +27,28 @@ public class AngleCalc2D
 {
 
     /**
-     * Return orientation of line relative to north. (North by coordinates, not magnetic north)
+     * Return orientation of line relative to east.
      * <p>
-     * @return Orientation in interval -pi to +pi where 0 is north and pi is south
+     * @return Orientation in interval -pi to +pi where 0 is east
+     * <p>
+     * @Deprecated because it seems to be nicer to align to north so try to use calcOrientationNorth
+     * instaead
      */
+    @Deprecated
     public double calcOrientation( double lat1, double lon1, double lat2, double lon2 )
     {
         return Math.atan2(lat2 - lat1, lon2 - lon1);
+        //return Math.atan2(lon2 - lon1, lat2 - lat1);
+    }
+
+    /**
+     * Return orientation of line relative to north. (North by coordinates, not magnetic north)
+     * <p>
+     * @return Orientation in interval -pi to +pi where 0 is north and pi is south
+     */
+    public double calcOrientationNorth( double lat1, double lon1, double lat2, double lon2 )
+    {
+        return Math.atan2(lon2 - lon1, lat2 - lat1);
     }
 
     /**
@@ -61,6 +76,25 @@ public double alignOrientation( double baseOrientation, double orientation )
         return resultOrientation;
     }
 
+    /**
+     * Calculate Azimuth for a line given by two coordinates. Direction in Degree where 0 is North,
+     * 90 is East, and 270 is West
+     * <p>
+     * @param lat1
+     * @param lon1
+     * @param lat2
+     * @param lon2
+     * @return
+     */
+    double calcAzimuth( double lat1, double lon1, double lat2, double lon2 )
+    {
+        double orientation = calcOrientationNorth(lat1, lon1, lat2, lon2);
+        double baseOrientation = calcOrientationNorth(2, 0, 1, 0);    // south
+        double alignedOrientation = alignOrientation(baseOrientation, orientation);
+
+        return Math.toDegrees(alignedOrientation);
+    }
+
     String azimuth2compassPoint( double azimuth )
     {
 

core/src/main/java/com/graphhopper/util/Instruction.java

@@ -18,8 +18,6 @@
 package com.graphhopper.util;
 
 import java.util.List;
-import java.util.Map;
-import java.util.HashMap;
 import java.text.DecimalFormat;
 
 public class Instruction
@@ -172,31 +170,68 @@ public String toString()
         return sb.toString();
     }
 
-    public Map<String, String> calcExtensions()
+    /**
+     * Return Direction/Compass point based on the first tracksegment of the instruction. If
+     * Instruction does not contain enough coordinate points, NULL will be returned.
+     * <p>
+     * @return
+     */
+    String getDirection( Instruction nextI )
     {
         AngleCalc2D ac = new AngleCalc2D();
+        double azimuth = calcAzimuth(nextI);
+        if (Double.compare(Double.NaN, azimuth) == 0)
+        {
+            return null;
+        }
+        String dir = ac.azimuth2compassPoint(azimuth);
+        return dir;
+    }
+
+    /**
+     * Return Azimuth based on the first tracksegment of the instruction. If Instruction does not
+     * contain enough coordinate points, NULL will be returned.
+     * <p>
+     * @return
+     */
+    String getAzimuth( Instruction nextI )
+    {
+        double az = calcAzimuth(nextI);
+        if (Double.compare(Double.NaN, az) == 0)
+        {
+            return null;
+        }
+
         DecimalFormat angleFormatter = new DecimalFormat("#");
+        return angleFormatter.format(az);
+
+    }
+
+    private double calcAzimuth( Instruction nextI )
+    {
+        double nextLat;
+        double nextLon;
+
+        if (points.getSize() >= 2)
+        {
+            nextLat = points.getLatitude(1);
+            nextLon = points.getLongitude(1);
+        } else if (points.getSize() == 1 && null != nextI)
+        {
+            nextLat = nextI.points.getLatitude(0);
+            nextLon = nextI.points.getLongitude(0);
+        } else
+        {
+            return Double.NaN;
+        }
+
+        double lat = points.getLatitude(0);
+        double lon = points.getLongitude(0);
+
+        AngleCalc2D ac = new AngleCalc2D();
 
-        // Add info for extensions
-        Map<String, String> extensions = new HashMap<String, String>();
-
-        // TODO instead of indication use angle which can be used here
-//        double distanceToNext = distanceCalc.calcDist(nextLat, nextLon, lat, lon);
-//        extensions.put("distance", angleFormatter.format(distanceToNext));
-//
-//        if (!(firstInstr && first))
-//        {
-//            // impossible to calculate an angle for first point of first instruction                
-//            double prevLat = first ? prevInstr.getLastLat() : points.getLatitude(i - 1);
-//            double prevLon = first ? prevInstr.getLastLon() : points.getLongitude(i - 1);
-//            double turnAngle = ac.calcTurnAngleDeg(prevLat, prevLon, lat, lon, nextLat, nextLon);
-//            extensions.put("turn-angle", angleFormatter.format(turnAngle));
-//        }
-//
-//        double azimuth = ac.calcAzimuthDeg(lat, lon, nextLat, nextLon);
-//        extensions.put("azimuth", angleFormatter.format(azimuth));
-//        extensions.put("direction", ac.azimuth2compassPoint(azimuth));
-        return extensions;
+        double azimuth = ac.calcAzimuth(lat, lon, nextLat, nextLon);
+        return azimuth;
     }
 
     void checkOne()

core/src/main/java/com/graphhopper/util/InstructionList.java

@@ -253,10 +253,20 @@ public String createGPX( String trackName, long startTimeMillis, String timeZone
         if (!isEmpty())
         {
             track.append("<rte>");
+            //Instruction prevI = null, middleI = null, nextI = null;
+            Instruction thisI = null, nextI;
+
             for (Instruction i : instructions)
             {
-                createExtensionsBlock(track, i);
+                nextI = i;
+
+                if (null != thisI)
+                {
+                    createRteptBlock(track, thisI, nextI);
+                }
+                thisI = nextI;
             }
+            createRteptBlock(track, thisI, null);
             track.append("</rte>");
         }
 
@@ -308,24 +318,31 @@ public String getLanguage()
         }
     };
 
-    private String createExtensionsBlock( StringBuilder sbEx, Instruction instruction )
+    private void createRteptBlock( StringBuilder output, Instruction instruction, Instruction nextI )
     {
-        sbEx.append("<rtept lat=\"").append(InstructionList.round(instruction.getFirstLat(), 6)).
+        output.append("<rtept lat=\"").append(InstructionList.round(instruction.getFirstLat(), 6)).
                 append("\" lon=\"").append(InstructionList.round(instruction.getFirstLon(), 6)).append("\">");
 
         if (!instruction.getName().isEmpty())
-            sbEx.append("<desc>").append(getTurnDescription(instruction, NO_TRANSLATE)).append("</desc>");
+            output.append("<desc>").append(getTurnDescription(instruction, NO_TRANSLATE)).append("</desc>");
 
-        sbEx.append("<extensions>");
+        output.append("<extensions>");
 
-        sbEx.append("<distance>").append((int) instruction.getDistance()).append("</distance>");
-        sbEx.append("<time>").append(instruction.getTime()).append("</time>");
+        output.append("<distance>").append((int) instruction.getDistance()).append("</distance>");
+        output.append("<time>").append(instruction.getTime()).append("</time>");
 
-        // sbEx.append("<direction>").append(instruction.getDirection()).append("</direction>");
-        // sbEx.append("<azimuth>").append(instruction.getAzimutz()).append("</azimuth>");
-        sbEx.append("</extensions>");
-        sbEx.append("</rtept>");
-        return sbEx.toString();
+        String direction = instruction.getDirection(nextI);
+        if (null != direction)
+        {
+            output.append("<direction>").append(direction).append("</direction>");
+        }
+        String azimuth = instruction.getAzimuth(nextI);
+        if (null != azimuth)
+        {
+            output.append("<azimuth>").append(azimuth).append("</azimuth>");
+        }
+        output.append("</extensions>");
+        output.append("</rtept>");
     }
 
     public static String getWayName( String name, int paveType, int wayType, TranslationMap.Translation tr )

core/src/test/java/com/graphhopper/util/AngleCalcTest.java

@@ -37,6 +37,15 @@ public void testOrientation()
         assertEquals(-135.0, Math.toDegrees(ac.calcOrientation(0, 0, -10, -10)), 0.0001);
     }
 
+    @Test
+    public void testOrientationNorth()
+    {
+        assertEquals(0.0, Math.toDegrees(ac.calcOrientationNorth(0, 0, 10, 0)), 0.0001);
+        assertEquals(45.0, Math.toDegrees(ac.calcOrientationNorth(0, 0, 10, 10)), 0.0001);
+        assertEquals(90.0, Math.toDegrees(ac.calcOrientationNorth(0, 0, 0, 10)), 0.0001);
+        assertEquals(-135.0, Math.toDegrees(ac.calcOrientationNorth(0, 0, -10, -10)), 0.0001);
+    }
+
     @Test
     public void testAlignOrientation()
     {
@@ -53,6 +62,14 @@ public void testCombined()
         assertEquals(146.458, Math.toDegrees(ac.alignOrientation(0, orientation)), 0.001);
     }
 
+    @Test
+    public void testCalcAzimuth()
+    {
+        assertEquals(90.0, ac.calcAzimuth(0, 0, 0, 10), 0.0001);
+        assertEquals(180.0, ac.calcAzimuth(0, 0, -10, 0), 0.0001);
+        assertEquals(270.0, ac.calcAzimuth(0, 0, 0, -10), 0.0001);
+    }
+
     @Test
     public void testAzimuthCompassPoint()
     {

core/src/test/java/com/graphhopper/util/InstructionListTest.java

@@ -310,6 +310,7 @@ public void testInstructionsWithTimeAndPlace()
         assertEquals(9.9, wayList.get(3).getFirstLon(), 1e-3);
 
         String gpxStr = wayList.createGPX("test", 0, "GMT+1");
+
         assertTrue(gpxStr, gpxStr.contains("<trkpt lat=\"15.0\" lon=\"10.0\"><time>1970-01-01T01:00:00+01:00</time>"));
         assertTrue(gpxStr, gpxStr.contains("<extensions>") && gpxStr.contains("</extensions>"));
         assertTrue(gpxStr, gpxStr.contains("<rtept lat=\"15.1\" lon=\"10.0\">"));

core/src/test/java/com/graphhopper/util/InstructionTest.java

@@ -0,0 +1,77 @@
+/*
+ *  Licensed to GraphHopper and Peter Karich under one or more contributor
+ *  license agreements. See the NOTICE file distributed with this work for
+ *  additional information regarding copyright ownership.
+ *
+ *  GraphHopper licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
+ *  compliance with the License. You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+package com.graphhopper.util;
+
+import org.junit.Test;
+import static org.junit.Assert.*;
+
+/**
+ *
+ * @author Johannes Pelzer
+ */
+public class InstructionTest
+{   
+    @Test
+    public void testGetAzimuthAndGetDirection() {
+        PointList pl = new PointList();
+        pl.add(49.942, 11.584);
+        pl.add(49.942, 11.582);
+        Instruction i1 = new Instruction(Instruction.CONTINUE_ON_STREET, "temp", 0, 0, pl).setDistance(240).setTime(15000);
+        
+        assertEquals("270", i1.getAzimuth(null));
+        assertEquals("W", i1.getDirection(null));
+
+        
+        PointList p2 = new PointList();
+        p2.add(49.942, 11.580);
+        p2.add(49.944, 11.582);
+        Instruction i2 = new Instruction(Instruction.CONTINUE_ON_STREET, "temp", 0, 0, p2).setDistance(240).setTime(15000);
+        
+        assertEquals("45", i2.getAzimuth(null));
+        assertEquals("NE", i2.getDirection(null));
+        
+        
+        PointList p3 = new PointList();
+        p3.add(49.942, 11.580);
+        p3.add(49.944, 11.580);
+        Instruction i3 = new Instruction(Instruction.CONTINUE_ON_STREET, "temp", 0, 0, p3).setDistance(240).setTime(15000);
+        
+        
+        assertEquals("0", i3.getAzimuth(null));
+        assertEquals("N", i3.getDirection(null));
+        
+        PointList p4 = new PointList();
+        p4.add(49.940, 11.580);
+        p4.add(49.920, 11.586);
+        Instruction i4 = new Instruction(Instruction.CONTINUE_ON_STREET, "temp", 0, 0, p4).setDistance(240).setTime(15000);
+        
+        
+        
+        assertEquals("S", i4.getDirection(null));
+ 
+        PointList p5 = new PointList();
+        p5.add(49.940, 11.580);
+        Instruction i5 = new Instruction(Instruction.CONTINUE_ON_STREET, "temp", 0, 0, p5).setDistance(240).setTime(15000);
+        
+        assertEquals(null, i5.getAzimuth(null));
+        assertEquals(null, i5.getDirection(null));
+    }
+    
+    
+    
+}