ben/brouter
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions 3

moved angle calc to util

Browse source
This commit is contained in:
Arndt Brenschede 2019-04-19 11:56:22 +02:00
parent 9e64eb39c4
commit 68b7ddff5c
5 changed files with 71 additions and 61 deletions
Download patch file Download diff file Expand all files Collapse all files

2
brouter-core/src/main/java/btools/router/KinematicPrePath.java
View file

@ -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();
}

4
brouter-core/src/main/java/btools/router/OsmPath.java
View file

@ -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.;

49
brouter-core/src/main/java/btools/router/RoutingContext.java
View file

@ -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 )

120
brouter-core/src/test/java/btools/router/RoutingContextTest.java
View file

@ -1,120 +0,0 @@
package btools.router;
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;
import btools.util.CheapRuler;
public class RoutingContextTest {
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() {
RoutingContext rc = new RoutingContext();
// 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.,
rc.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.,
rc.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.,
rc.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.,
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 = 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 = 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 );
}
}
}
}