moved angle calc to util

brouter-util/src/main/java/btools/util/CheapAngleMeter.java

@@ -0,0 +1,55 @@
+/**
+ * Calculate the angle defined by 3 points
+ * (and deliver it's cosine on the fly)
+ */
+package btools.util;
+
+public final class CheapAngleMeter
+{
+  private double cosangle;
+
+  public double getCosAngle()
+  {
+    return cosangle;
+  }
+
+  public double calcAngle( int lon0, int lat0,  int lon1, int lat1, int lon2, int lat2 )
+  {
+    double[] lonlat2m = CheapRuler.getLonLatToMeterScales( lat1 );
+    double lon2m = lonlat2m[0];
+    double lat2m = lonlat2m[1];
+    double dx10 = (lon1 - lon0) * lon2m;
+    double dy10 = (lat1 - lat0) * lat2m;
+    double dx21 = (lon2 - lon1) * lon2m;
+    double dy21 = (lat2 - lat1) * lat2m;
+
+    double dd = Math.sqrt( (dx10*dx10 + dy10*dy10)*(dx21*dx21 + dy21*dy21) );
+    if ( dd == 0. ) { cosangle = 1.; return 0.; }
+    double sinp = (dy10*dx21 - dx10*dy21)/dd;
+    double cosp = (dy10*dy21 + dx10*dx21)/dd;
+    cosangle = cosp;
+
+    double offset = 0.;
+    double s2 = sinp*sinp;
+    if ( s2 > 0.5 )
+    {
+      if ( sinp > 0. )
+      {
+        offset = 90.;
+        sinp = -cosp;
+      }
+      else
+      {
+        offset = -90.;
+        sinp = cosp;
+      }
+      s2 = cosp*cosp;
+    }
+    else if ( cosp < 0. )
+    {
+      sinp = -sinp;
+      offset = sinp > 0. ? -180. : 180.;
+    }
+    return offset + sinp * ( 57.4539 + s2 * ( 9.57565 + s2 * ( 4.30904 + s2 * 2.56491 ) ) );
+  }
+}

brouter-util/src/test/java/btools/util/CheapAngleMeterTest.java

@@ -0,0 +1,118 @@
+package btools.util;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import org.junit.AfterClass;
+import org.junit.BeforeClass;
+import org.junit.Test;
+
+public class CheapAngleMeterTest {
+  static int toOsmLon(double lon) {
+    return (int)( ( lon + 180. ) / CheapRuler.ILATLNG_TO_LATLNG + 0.5);
+  }
+
+  static int toOsmLat(double lat) {
+    return (int)( ( lat +  90. ) / CheapRuler.ILATLNG_TO_LATLNG + 0.5);
+  }
+
+  @Test
+  public void testCalcAngle() {
+    CheapAngleMeter am = new CheapAngleMeter();
+    // Segment ends
+    int lon0, lat0, lon1, lat1, lon2, lat2;
+
+    lon0 = toOsmLon(2.317126);
+    lat0 = toOsmLat(48.817927);
+    lon1 = toOsmLon(2.317316);
+    lat1 = toOsmLat(48.817978);
+    lon2 = toOsmLon(2.317471);
+    lat2 = toOsmLat(48.818043);
+    assertEquals(
+      "Works for an angle between -pi/4 and pi/4",
+      -10.,
+      am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
+      0.05 * 10.
+    );
+
+    lon0 = toOsmLon(2.317020662874013);
+    lat0 = toOsmLat(48.81799440182911);
+    lon1 = toOsmLon(2.3169460585876327);
+    lat1 = toOsmLat(48.817812421536644);
+    lon2 = lon0;
+    lat2 = lat0;
+    assertEquals(
+      "Works for an angle between 3*pi/4 and 5*pi/4",
+      180.,
+      am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
+      0.05 * 180.
+    );
+
+    lon0 = toOsmLon(2.317112);
+    lat0 = toOsmLat(48.817802);
+    lon1 = toOsmLon(2.317632);
+    lat1 = toOsmLat(48.817944);
+    lon2 = toOsmLon(2.317673);
+    lat2 = toOsmLat(48.817799);
+    assertEquals(
+      "Works for an angle between -3*pi/4 and -pi/4",
+      100.,
+      am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
+      0.1 * 100.
+    );
+
+    lon0 = toOsmLon(2.317128);
+    lat0 = toOsmLat(48.818072);
+    lon1 = toOsmLon(2.317532);
+    lat1 = toOsmLat(48.818108);
+    lon2 = toOsmLon(2.317497);
+    lat2 = toOsmLat(48.818264);
+    assertEquals(
+      "Works for an angle between pi/4 and 3*pi/4",
+      -100.,
+      am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
+      0.1 * 100.
+    );
+  }
+
+  @Test
+  public void testCalcAngle2() {
+    CheapAngleMeter am = new CheapAngleMeter();
+    int lon1 =  8500000;
+    int lat1 = 49500000;
+    
+    double[] lonlat2m = CheapRuler.getLonLatToMeterScales( lat1 );
+    double lon2m = lonlat2m[0];
+    double lat2m = lonlat2m[1];
+
+    for ( double afrom = -175.; afrom < 180.; afrom += 10. )
+    {
+      double sf = Math.sin( afrom * Math.PI / 180. );
+      double cf = Math.cos( afrom * Math.PI / 180. );
+      
+      int lon0 = (int)(0.5+lon1 - cf*150./lon2m );
+      int lat0 = (int)(0.5+lat1 - sf*150./lat2m );
+    
+      for ( double ato = -177.; ato < 180.; ato += 10.  )
+      {
+        double st = Math.sin( ato * Math.PI / 180. );
+        double ct = Math.cos( ato * Math.PI / 180. );
+      
+        int lon2 = (int)(0.5+lon1 + ct*250./lon2m);
+        int lat2 = (int)(0.5+lat1 + st*250./lat2m);
+        
+        double a1 = afrom - ato;
+        if ( a1 > 180. ) a1 -= 360.;
+        if ( a1 < -180. ) a1 += 360.;
+        double a2 = am.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 );
+        double c1 = Math.cos( a1 * Math.PI / 180. );
+        double c2 = am.getCosAngle();
+
+        assertEquals( "angle mismatch for afrom=" + afrom + " ato=" + ato, a1, a2, 0.2 );        
+        assertEquals( "cosinus mismatch for afrom=" + afrom + " ato=" + ato, c1, c2, 0.001 );        
+      }
+    }
+  } 
+
+}