From 68b7ddff5c962373e51ef1ccd642d407247e7d47 Mon Sep 17 00:00:00 2001 From: Arndt Brenschede Date: Fri, 19 Apr 2019 11:56:22 +0200 Subject: [PATCH] moved angle calc to util --- .../java/btools/router/KinematicPrePath.java | 2 +- .../src/main/java/btools/router/OsmPath.java | 4 +- .../java/btools/router/RoutingContext.java | 49 +---------------- .../java/btools/util/CheapAngleMeter.java | 55 +++++++++++++++++++ .../java/btools/util/CheapAngleMeterTest.java | 22 ++++---- 5 files changed, 71 insertions(+), 61 deletions(-) create mode 100644 brouter-util/src/main/java/btools/util/CheapAngleMeter.java rename brouter-core/src/test/java/btools/router/RoutingContextTest.java => brouter-util/src/test/java/btools/util/CheapAngleMeterTest.java (85%) diff --git a/brouter-core/src/main/java/btools/router/KinematicPrePath.java b/brouter-core/src/main/java/btools/router/KinematicPrePath.java index 13013ef..7d3efc2 100644 --- a/brouter-core/src/main/java/btools/router/KinematicPrePath.java +++ b/brouter-core/src/main/java/btools/router/KinematicPrePath.java @@ -51,7 +51,7 @@ final class KinematicPrePath extends OsmPrePath int dist = rc.calcDistance( lon1, lat1, lon2, lat2 ); - angle = rc.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 ); + angle = rc.anglemeter.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 ); priorityclassifier = (int)rc.expctxWay.getPriorityClassifier(); classifiermask = (int)rc.expctxWay.getClassifierMask(); } diff --git a/brouter-core/src/main/java/btools/router/OsmPath.java b/brouter-core/src/main/java/btools/router/OsmPath.java index 86daf97..86149b7 100644 --- a/brouter-core/src/main/java/btools/router/OsmPath.java +++ b/brouter-core/src/main/java/btools/router/OsmPath.java @@ -323,8 +323,8 @@ abstract class OsmPath implements OsmLinkHolder lat0 = lat1 - (lat2-lat1); } } - double angle = rc.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 ); - double cosangle = rc.getCosAngle(); + double angle = rc.anglemeter.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 ); + double cosangle = rc.anglemeter.getCosAngle(); // *** elevation stuff double delta_h = 0.; diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 73a0076..4ed12eb 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/brouter-core/src/main/java/btools/router/RoutingContext.java @@ -16,6 +16,7 @@ import btools.expressions.BExpressionContextNode; import btools.expressions.BExpressionContextWay; import btools.mapaccess.GeometryDecoder; import btools.mapaccess.OsmLink; +import btools.util.CheapAngleMeter; import btools.util.CheapRuler; public final class RoutingContext @@ -194,7 +195,8 @@ public final class RoutingContext public boolean startDirectionValid; public boolean forceUseStartDirection; - private double cosangle; + public CheapAngleMeter anglemeter = new CheapAngleMeter(); + public double nogoCost = 0.; public boolean isEndpoint = false; @@ -412,51 +414,6 @@ public final class RoutingContext return (int)(d + 1.0 ); } - 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 ) ) ); - } - public OsmPathModel pm; public OsmPrePath createPrePath( OsmPath origin, OsmLink link ) diff --git a/brouter-util/src/main/java/btools/util/CheapAngleMeter.java b/brouter-util/src/main/java/btools/util/CheapAngleMeter.java new file mode 100644 index 0000000..7fdad3b --- /dev/null +++ b/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 ) ) ); + } +} diff --git a/brouter-core/src/test/java/btools/router/RoutingContextTest.java b/brouter-util/src/test/java/btools/util/CheapAngleMeterTest.java similarity index 85% rename from brouter-core/src/test/java/btools/router/RoutingContextTest.java rename to brouter-util/src/test/java/btools/util/CheapAngleMeterTest.java index 86bd338..a086cc7 100644 --- a/brouter-core/src/test/java/btools/router/RoutingContextTest.java +++ b/brouter-util/src/test/java/btools/util/CheapAngleMeterTest.java @@ -1,4 +1,4 @@ -package btools.router; +package btools.util; import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertFalse; @@ -8,9 +8,7 @@ import org.junit.AfterClass; import org.junit.BeforeClass; import org.junit.Test; -import btools.util.CheapRuler; - -public class RoutingContextTest { +public class CheapAngleMeterTest { static int toOsmLon(double lon) { return (int)( ( lon + 180. ) / CheapRuler.ILATLNG_TO_LATLNG + 0.5); } @@ -21,7 +19,7 @@ public class RoutingContextTest { @Test public void testCalcAngle() { - RoutingContext rc = new RoutingContext(); + CheapAngleMeter am = new CheapAngleMeter(); // Segment ends int lon0, lat0, lon1, lat1, lon2, lat2; @@ -34,7 +32,7 @@ public class RoutingContextTest { assertEquals( "Works for an angle between -pi/4 and pi/4", -10., - rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), + am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), 0.05 * 10. ); @@ -47,7 +45,7 @@ public class RoutingContextTest { assertEquals( "Works for an angle between 3*pi/4 and 5*pi/4", 180., - rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), + am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), 0.05 * 180. ); @@ -60,7 +58,7 @@ public class RoutingContextTest { assertEquals( "Works for an angle between -3*pi/4 and -pi/4", 100., - rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), + am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), 0.1 * 100. ); @@ -73,14 +71,14 @@ public class RoutingContextTest { assertEquals( "Works for an angle between pi/4 and 3*pi/4", -100., - rc.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), + am.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2), 0.1 * 100. ); } @Test public void testCalcAngle2() { - RoutingContext rc = new RoutingContext(); + CheapAngleMeter am = new CheapAngleMeter(); int lon1 = 8500000; int lat1 = 49500000; @@ -107,9 +105,9 @@ public class RoutingContextTest { 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 a2 = am.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 ); double c1 = Math.cos( a1 * Math.PI / 180. ); - double c2 = rc.getCosAngle(); + 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 );