diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 2a35ef4..0bb6d76 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/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 dx10 = (lon1 - lon0) * lonlat2m[0]; - double dy21 = (lat2 - lat1) * lonlat2m[1]; - double dx21 = (lon2 - lon1) * lonlat2m[0]; + 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 = (dx10*dy21 - dy10*dx21)/dd; + double sinp = (dy10*dx21 - dx10*dy21)/dd; double cosp = (dy10*dy21 + dx10*dx21)/dd; cosangle = cosp; - double p; - if ( sinp > -0.7071 && sinp < 0.7071 ) + double offset = 0.; + double s2 = sinp*sinp; + double c2 = cosp*cosp; + if ( c2 < s2 ) { - p = asin( sinp ); - if ( cosp < 0. ) + if ( sinp > 0. ) { - p = 180. - p; + offset = 90.; + sinp = -cosp; } - } - else - { - p = 90. - asin( cosp ); - if ( sinp < 0. ) + else { - p = - p; + offset = -90.; + sinp = cosp; } + s2 = c2; } - if ( p > 180. ) + else if ( cosp < 0. ) { - p -= 360.; + sinp = -sinp; + offset = sinp > 0. ? -180. : 180.; } - 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 ); + return offset + sinp * ( 57.4539 + s2 * ( 9.57565 + s2 * ( 4.30904 + s2 * 2.56491 ) ) ); } public OsmPathModel pm; diff --git a/brouter-core/src/test/java/btools/router/RoutingContextTest.java b/brouter-core/src/test/java/btools/router/RoutingContextTest.java index f1338f5..3c667ed 100644 --- a/brouter-core/src/test/java/btools/router/RoutingContextTest.java +++ b/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 ); + } + } + } + }