calcAngle back to clockwise positive + fine-tuning

brouter-core/src/main/java/btools/router/RoutingContext.java

@@ -419,46 +419,42 @@ public final class RoutingContext
   public double calcAngle( int lon0, int lat0,  int lon1, int lat1, int lon2, int lat2 )
   {
     double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( lat1 );
-    double dy10 = (lat1 - lat0) * lonlat2m[1];
+    double lon2m = lonlat2m[0];
-    double dx10 = (lon1 - lon0) * lonlat2m[0];
+    double lat2m = lonlat2m[1];
-    double dy21 = (lat2 - lat1) * lonlat2m[1];
+    double dx10 = (lon1 - lon0) * lon2m;
-    double dx21 = (lon2 - lon1) * lonlat2m[0];
+    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 = (dx10*dy21 - dy10*dx21)/dd;
+    double sinp = (dy10*dx21 - dx10*dy21)/dd;
     double cosp = (dy10*dy21 + dx10*dx21)/dd;
     cosangle = cosp;
 
-    double p;
+    double offset = 0.;
-    if ( sinp > -0.7071 && sinp < 0.7071 )
+    double s2 = sinp*sinp;
+    double c2 = cosp*cosp;
+    if ( c2 < s2 )
     {
-      p = asin( sinp );
+      if ( sinp > 0. )
-      if ( cosp < 0. )
       {
-        p = 180. - p;
+        offset = 90.;
-      }
+        sinp = -cosp;
       }
       else
       {
-      p = 90. - asin( cosp );
+        offset = -90.;
-      if ( sinp < 0. )
+        sinp = cosp;
+      }
+      s2 = c2;
+    }
+    else if ( cosp < 0. )
     {
-        p = - p;
+      sinp = -sinp;
+      offset = sinp > 0. ? -180. : 180.;
     }
-    }
+    return offset + sinp * ( 57.4539 + s2 * ( 9.57565 + s2 * ( 4.30904 + s2 * 2.56491 ) ) );
-    if ( p > 180. )
-    {
-      p -= 360.;
-    }
-    return p;
-  }
-
-  private static double asin( double x )
-  {
-    double x2 = x*x;
-    double x4 = x2*x2;
-    return x * ( 57.4539 + 9.57565 * x2 + 4.30904 * x4 + 2.56491 * x2*x4 );
   }
 
   public OsmPathModel pm;

brouter-core/src/test/java/btools/router/RoutingContextTest.java

@@ -33,7 +33,7 @@ public class RoutingContextTest {
     lat2 = toOsmLat(48.818043);
     assertEquals(
       "Works for an angle between -pi/4 and pi/4",
-      10.,
+      -10.,
       rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
       0.05 * 10.
     );
@@ -59,7 +59,7 @@ public class RoutingContextTest {
     lat2 = toOsmLat(48.817799);
     assertEquals(
       "Works for an angle between -3*pi/4 and -pi/4",
-      -100.,
+      100.,
       rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
       0.1 * 100.
     );
@@ -72,9 +72,49 @@ public class RoutingContextTest {
     lat2 = toOsmLat(48.818264);
     assertEquals(
       "Works for an angle between pi/4 and 3*pi/4",
-      100.,
+      -100.,
       rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2),
       0.1 * 100.
     );
   }
+
+  @Test
+  public void testCalcAngle2() {
+    RoutingContext rc = new RoutingContext();
+    int lon1 =  8500000;
+    int lat1 = 49500000;
+    
+    double[] lonlat2m = CheapRulerSingleton.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 = rc.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 );
+        double c1 = Math.cos( a1 * Math.PI / 180. );
+        double c2 = rc.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 );        
+      }
+    }
+  } 
+
 }