@Test
public void testMiss() throws OrekitException {
final CircularOrbit circ =
new CircularOrbit(
7178000.0,
0.5e-4,
-0.5e-4,
FastMath.toRadians(0.),
FastMath.toRadians(270.),
FastMath.toRadians(5.300),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
mu);
final LofOffset upsideDown =
new LofOffset(circ.getFrame(), LOFType.VVLH, RotationOrder.XYX, FastMath.PI, 0, 0);
final LofOffsetPointing pointing =
new LofOffsetPointing(earthSpheric, upsideDown, Vector3D.PLUS_K);
try {
pointing.getTargetPV(circ, date, circ.getFrame());
Assert.fail("an exception should have been thrown");
} catch (OrekitException oe) {
Assert.assertEquals(
OrekitMessages.ATTITUDE_POINTING_LAW_DOES_NOT_POINT_TO_GROUND, oe.getSpecifier());
}
}
/** check {@link GeodeticPoint#GeodeticPoint(double, double, double)} angle normalization. */
@Test
public void testGeodeticPointAngleNormalization() {
// action
GeodeticPoint point =
new GeodeticPoint(FastMath.toRadians(135), FastMath.toRadians(90 - 360), 0);
// verify
Assert.assertEquals(FastMath.toRadians(45), point.getLatitude(), 0);
Assert.assertEquals(FastMath.toRadians(-90), point.getLongitude(), 0);
Assert.assertEquals(0, Vector3D.distance(point.getEast(), Vector3D.PLUS_I), 1.0e-15);
Assert.assertEquals(
0,
Vector3D.distance(point.getNorth(), new Vector3D(0.50 * FastMath.PI, 0.25 * FastMath.PI)),
1.0e-15);
Assert.assertEquals(0, Vector3D.distance(point.getWest(), Vector3D.MINUS_I), 1.0e-15);
Assert.assertEquals(
0,
Vector3D.distance(point.getSouth(), new Vector3D(-0.50 * FastMath.PI, -0.25 * FastMath.PI)),
1.0e-15);
Assert.assertEquals(
0,
Vector3D.distance(point.getZenith(), new Vector3D(-0.50 * FastMath.PI, 0.25 * FastMath.PI)),
1.0e-15);
Assert.assertEquals(
0,
Vector3D.distance(point.getNadir(), new Vector3D(0.50 * FastMath.PI, -0.25 * FastMath.PI)),
1.0e-15);
}
private SphericalPolygonsSet buildSimpleZone(double[][] points) {
final S2Point[] vertices = new S2Point[points.length];
for (int i = 0; i < points.length; ++i) {
vertices[i] =
new S2Point(
FastMath.toRadians(points[i][1]), // points[i][1] is longitude
FastMath.toRadians(90.0 - points[i][0])); // points[i][0] is latitude
}
return new SphericalPolygonsSet(1.0e-10, vertices);
}
@Test
public void testSpin() throws OrekitException {
AbsoluteDate date =
new AbsoluteDate(
new DateComponents(1970, 01, 01),
new TimeComponents(3, 25, 45.6789),
TimeScalesFactory.getUTC());
KeplerianOrbit orbit =
new KeplerianOrbit(
7178000.0,
1.e-4,
FastMath.toRadians(50.),
FastMath.toRadians(10.),
FastMath.toRadians(20.),
FastMath.toRadians(30.),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
3.986004415e14);
final AttitudeProvider law =
new LofOffsetPointing(
earthSpheric,
new LofOffset(orbit.getFrame(), LOFType.VVLH, RotationOrder.XYX, 0.1, 0.2, 0.3),
Vector3D.PLUS_K);
Propagator propagator = new KeplerianPropagator(orbit, law);
double h = 0.01;
SpacecraftState sMinus = propagator.propagate(date.shiftedBy(-h));
SpacecraftState s0 = propagator.propagate(date);
SpacecraftState sPlus = propagator.propagate(date.shiftedBy(h));
// check spin is consistent with attitude evolution
double errorAngleMinus =
Rotation.distance(
sMinus.shiftedBy(h).getAttitude().getRotation(), s0.getAttitude().getRotation());
double evolutionAngleMinus =
Rotation.distance(sMinus.getAttitude().getRotation(), s0.getAttitude().getRotation());
Assert.assertEquals(0.0, errorAngleMinus, 1.0e-6 * evolutionAngleMinus);
double errorAnglePlus =
Rotation.distance(
s0.getAttitude().getRotation(), sPlus.shiftedBy(-h).getAttitude().getRotation());
double evolutionAnglePlus =
Rotation.distance(s0.getAttitude().getRotation(), sPlus.getAttitude().getRotation());
Assert.assertEquals(0.0, errorAnglePlus, 1.0e-6 * evolutionAnglePlus);
Vector3D spin0 = s0.getAttitude().getSpin();
Vector3D reference =
AngularCoordinates.estimateRate(
sMinus.getAttitude().getRotation(), sPlus.getAttitude().getRotation(), 2 * h);
Assert.assertTrue(spin0.getNorm() > 1.0e-3);
Assert.assertEquals(0.0, spin0.subtract(reference).getNorm(), 1.0e-10);
}
/** check {@link GeodeticPoint#toString()}. */
@Test
public void testToString() {
// setup
GeodeticPoint point = new GeodeticPoint(FastMath.toRadians(30), FastMath.toRadians(60), 90);
// action
String actual = point.toString();
// verify
Assert.assertEquals("{lat: 30 deg, lon: 60 deg, alt: 90}", actual);
}
@Test
public void testForward5DegreesStartEnabled() throws OrekitException {
doElevationTest(
FastMath.toRadians(5.0),
orbit.getDate().shiftedBy(12614.0),
orbit.getDate().shiftedBy(Constants.JULIAN_DAY),
6,
false);
}
@Test
public void testBackward0Degrees() throws OrekitException {
doElevationTest(
FastMath.toRadians(0.0),
orbit.getDate().shiftedBy(Constants.JULIAN_DAY),
orbit.getDate(),
8,
true);
}
@Test
public void testWMMWithHeightAboveMSL() throws Exception {
// test results for test values provided as part of the WMM2015 Report
// using height above MSL instead of height above ellipsoid
// the results have been obtained from the NOAA online calculator:
// http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
final double[][] testValues = {
// Date Alt Lat Lon X Y Z H F I D
// km deg deg nT nT nT nT nT deg deg
{2015.0, 100, 80, 0, 6314.2, -471.6, 52269.1, 6331.8, 52651.2, 83.093, -4.271},
{2015.0, 100, 0, 120, 37534.4, 364.3, -10773.1, 37536.2, 39051.6, -16.013, 0.556},
{2015.0, 100, -80, 240, 5613.2, 14791.9, -50379.6, 15821.1, 52805.4, -72.565, 69.219}
};
final Geoid geoid = new Geoid(potential, WGS84);
final double eps = 1e-1;
final double degreeEps = 1e-2;
for (int i = 0; i < testValues.length; i++) {
final AbsoluteDate date = new AbsoluteDate(2015, 1, 1, TimeScalesFactory.getUTC());
final GeoMagneticField model = GeoMagneticFieldFactory.getWMM(testValues[i][0]);
final double undulation =
geoid.getUndulation(
FastMath.toRadians(testValues[i][2]), FastMath.toRadians(testValues[i][3]), date);
final GeoMagneticElements result =
model.calculateField(
testValues[i][2], testValues[i][3], testValues[i][1] + undulation / 1000d);
// X
Assert.assertEquals(testValues[i][4], result.getFieldVector().getX(), eps);
// Y
Assert.assertEquals(testValues[i][5], result.getFieldVector().getY(), eps);
// Z
Assert.assertEquals(testValues[i][6], result.getFieldVector().getZ(), eps);
// H
Assert.assertEquals(testValues[i][7], result.getHorizontalIntensity(), eps);
// F
Assert.assertEquals(testValues[i][8], result.getTotalIntensity(), eps);
// inclination
Assert.assertEquals(testValues[i][9], result.getInclination(), degreeEps);
// declination
Assert.assertEquals(testValues[i][10], result.getDeclination(), degreeEps);
}
}
@Test
public void testSerialization()
throws IOException, ClassNotFoundException, NoSuchFieldException, IllegalAccessException,
OrekitException {
final double r = Constants.WGS84_EARTH_EQUATORIAL_RADIUS;
final BodyShape earth =
new OneAxisEllipsoid(
r,
Constants.WGS84_EARTH_FLATTENING,
FramesFactory.getITRF(IERSConventions.IERS_2010, true));
GeographicZoneDetector d =
new GeographicZoneDetector(20.0, 1.e-3, earth, buildFrance(), FastMath.toRadians(0.5))
.withMargin(FastMath.toRadians(0.75))
.withHandler(new ContinueOnEvent<GeographicZoneDetector>());
Assert.assertEquals(r, ((OneAxisEllipsoid) d.getBody()).getEquatorialRadius(), 1.0e-12);
Assert.assertEquals(0.75, FastMath.toDegrees(d.getMargin()), 1.0e-12);
Assert.assertEquals(5.6807e11, d.getZone().getSize() * r * r, 1.0e9);
Assert.assertEquals(4.0289e6, d.getZone().getBoundarySize() * r, 1.0e3);
ByteArrayOutputStream bos = new ByteArrayOutputStream();
ObjectOutputStream oos = new ObjectOutputStream(bos);
oos.writeObject(d);
Assert.assertTrue(bos.size() > 2100);
Assert.assertTrue(bos.size() < 2200);
ByteArrayInputStream bis = new ByteArrayInputStream(bos.toByteArray());
ObjectInputStream ois = new ObjectInputStream(bis);
GeographicZoneDetector deserialized = (GeographicZoneDetector) ois.readObject();
Assert.assertEquals(d.getZone().getSize(), deserialized.getZone().getSize(), 1.0e-3);
Assert.assertEquals(
d.getZone().getBoundarySize(), deserialized.getZone().getBoundarySize(), 1.0e-3);
Assert.assertEquals(d.getZone().getTolerance(), deserialized.getZone().getTolerance(), 1.0e-15);
Assert.assertEquals(d.getMaxCheckInterval(), deserialized.getMaxCheckInterval(), 1.0e-15);
Assert.assertEquals(d.getThreshold(), deserialized.getThreshold(), 1.0e-15);
Assert.assertEquals(d.getMaxIterationCount(), deserialized.getMaxIterationCount());
Assert.assertTrue(
new RegionFactory<Sphere2D>().difference(d.getZone(), deserialized.getZone()).isEmpty());
}
@Before
public void setUp() throws OrekitException {
Utils.setDataRoot("regular-data");
earth =
new OneAxisEllipsoid(
Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
FramesFactory.getITRF(IERSConventions.IERS_2010, true));
gp = new GeodeticPoint(FastMath.toRadians(51.0), FastMath.toRadians(66.6), 300.0);
final TimeScale utc = TimeScalesFactory.getUTC();
final Vector3D position = new Vector3D(-6142438.668, 3492467.56, -25767.257);
final Vector3D velocity = new Vector3D(505.848, 942.781, 7435.922);
final AbsoluteDate date = new AbsoluteDate(2003, 9, 16, utc);
orbit =
new EquinoctialOrbit(
new PVCoordinates(position, velocity),
FramesFactory.getEME2000(),
date,
Constants.EIGEN5C_EARTH_MU);
}
/** Test if both constructors are equivalent */
@Test
public void testLof() throws OrekitException {
// Satellite position
final CircularOrbit circ =
new CircularOrbit(
7178000.0,
0.5e-4,
-0.5e-4,
FastMath.toRadians(0.),
FastMath.toRadians(270.),
FastMath.toRadians(5.300),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
mu);
// Create lof aligned law
// ************************
final LofOffset lofLaw = new LofOffset(circ.getFrame(), LOFType.VVLH);
final LofOffsetPointing lofPointing =
new LofOffsetPointing(earthSpheric, lofLaw, Vector3D.PLUS_K);
final Rotation lofRot = lofPointing.getAttitude(circ, date, circ.getFrame()).getRotation();
// Compare to body center pointing law
// *************************************
final BodyCenterPointing centerLaw = new BodyCenterPointing(earthSpheric.getBodyFrame());
final Rotation centerRot = centerLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
final double angleBodyCenter = centerRot.applyInverseTo(lofRot).getAngle();
Assert.assertEquals(0., angleBodyCenter, Utils.epsilonAngle);
// Compare to nadir pointing law
// *******************************
final NadirPointing nadirLaw = new NadirPointing(earthSpheric);
final Rotation nadirRot = nadirLaw.getAttitude(circ, date, circ.getFrame()).getRotation();
final double angleNadir = nadirRot.applyInverseTo(lofRot).getAngle();
Assert.assertEquals(0., angleNadir, Utils.epsilonAngle);
}
@Override
public void processModel(CommandPlayer context, EnvironmentModel model) {
SeeBallInfo ball = model.getLastPercept().getLastSeenBall();
double agentAbsAngle = model.getAgentAbsAngleRadians();
double ballAngle = FastMath.toRadians(ball.getDirection());
double totalBallAngle = ballAngle + agentAbsAngle;
double goalAngle = model.getGoalAngle();
double angleBetweenBallAndGoal = totalBallAngle - goalAngle;
kickAtGoalAction.takeAction(context, model);
}
@Test
public void testFrance() throws OrekitException {
final BodyShape earth =
new OneAxisEllipsoid(
Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
Constants.WGS84_EARTH_FLATTENING,
FramesFactory.getITRF(IERSConventions.IERS_2010, true));
GeographicZoneDetector d =
new GeographicZoneDetector(20.0, 1.e-3, earth, buildFrance(), FastMath.toRadians(0.5))
.withHandler(new ContinueOnEvent<GeographicZoneDetector>());
Assert.assertEquals(20.0, d.getMaxCheckInterval(), 1.0e-15);
Assert.assertEquals(1.0e-3, d.getThreshold(), 1.0e-15);
Assert.assertEquals(0.5, FastMath.toDegrees(d.getMargin()), 1.0e-15);
Assert.assertEquals(AbstractDetector.DEFAULT_MAX_ITER, d.getMaxIterationCount());
final TimeScale utc = TimeScalesFactory.getUTC();
final Vector3D position = new Vector3D(-6142438.668, 3492467.56, -25767.257);
final Vector3D velocity = new Vector3D(505.848, 942.781, 7435.922);
final AbsoluteDate date = new AbsoluteDate(2003, 9, 16, utc);
final Orbit orbit =
new EquinoctialOrbit(
new PVCoordinates(position, velocity),
FramesFactory.getEME2000(),
date,
Constants.EIGEN5C_EARTH_MU);
Propagator propagator =
new EcksteinHechlerPropagator(
orbit,
Constants.EIGEN5C_EARTH_EQUATORIAL_RADIUS,
Constants.EIGEN5C_EARTH_MU,
Constants.EIGEN5C_EARTH_C20,
Constants.EIGEN5C_EARTH_C30,
Constants.EIGEN5C_EARTH_C40,
Constants.EIGEN5C_EARTH_C50,
Constants.EIGEN5C_EARTH_C60);
EventsLogger logger = new EventsLogger();
propagator.addEventDetector(logger.monitorDetector(d));
propagator.propagate(date.shiftedBy(10 * Constants.JULIAN_DAY));
Assert.assertEquals(26, logger.getLoggedEvents().size());
}
@Test
public void testJacobianIssue18() throws OrekitException {
// Body mu
final double mu = 3.9860047e14;
final double isp = 318;
final double mass = 2500;
final double a = 24396159;
final double e = 0.72831215;
final double i = FastMath.toRadians(7);
final double omega = FastMath.toRadians(180);
final double OMEGA = FastMath.toRadians(261);
final double lv = 0;
final double duration = 3653.99;
final double f = 420;
final double delta = FastMath.toRadians(-7.4978);
final double alpha = FastMath.toRadians(351);
final AttitudeProvider law =
new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
final AbsoluteDate initDate =
new AbsoluteDate(
new DateComponents(2004, 01, 01),
new TimeComponents(23, 30, 00.000),
TimeScalesFactory.getUTC());
final Orbit orbit =
new KeplerianOrbit(
a,
e,
i,
omega,
OMEGA,
lv,
PositionAngle.TRUE,
FramesFactory.getEME2000(),
initDate,
mu);
final SpacecraftState initialState =
new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
final AbsoluteDate fireDate =
new AbsoluteDate(
new DateComponents(2004, 01, 02),
new TimeComponents(04, 15, 34.080),
TimeScalesFactory.getUTC());
final ConstantThrustManeuver maneuver =
new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
double[] absTolerance = {0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001};
double[] relTolerance = {1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7};
AdaptiveStepsizeIntegrator integrator =
new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
integrator.setInitialStepSize(60);
final NumericalPropagator propagator = new NumericalPropagator(integrator);
propagator.setAttitudeProvider(law);
propagator.addForceModel(maneuver);
propagator.setOrbitType(OrbitType.CARTESIAN);
PartialDerivativesEquations PDE = new PartialDerivativesEquations("derivatives", propagator);
PDE.selectParamAndStep("thrust", Double.NaN);
Assert.assertEquals(3, PDE.getAvailableParameters().size());
Assert.assertEquals("central attraction coefficient", PDE.getAvailableParameters().get(0));
Assert.assertEquals("thrust", PDE.getAvailableParameters().get(1));
Assert.assertEquals("flow rate", PDE.getAvailableParameters().get(2));
propagator.setInitialState(PDE.setInitialJacobians(initialState, 7, 1));
final AbsoluteDate finalDate = fireDate.shiftedBy(3800);
final SpacecraftState finalorb = propagator.propagate(finalDate);
Assert.assertEquals(0, finalDate.durationFrom(finalorb.getDate()), 1.0e-11);
}
@Test
public void testBackward5DegreesStartEnabled() throws OrekitException {
doElevationTest(
FastMath.toRadians(5.0), orbit.getDate().shiftedBy(73112.0), orbit.getDate(), 6, true);
}
@Test
public void testWithOriginalTestsCases() throws OrekitException, ParseException {
Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
PVCoordinatesProvider sun = CelestialBodyFactory.getSun();
OneAxisEllipsoid earth = new OneAxisEllipsoid(6378136.460, 1.0 / 298.257222101, itrf);
SolarInputs97to05 in = SolarInputs97to05.getInstance();
earth.setAngularThreshold(1e-10);
DTM2000 atm = new DTM2000(in, sun, earth);
double roTestCase;
double tzTestCase;
double tinfTestCase;
double myRo;
// Inputs :
// alt=800.
// lat=40.
// day=185.
// hl=16.
// xlon=0.
// fm(1)=150.
// f(1) =fm(1)
// fm(2)=0.
// f(2)=0.
// akp(1)=0.
// akp(2)=0.
// akp(3)=0.
// akp(4)=0.
// Outputs :
roTestCase = 1.8710001353820e-17 * 1000;
tzTestCase = 1165.4839828984;
tinfTestCase = 1165.4919505608;
// Computation and results
myRo =
atm.getDensity(
185, 800 * 1000, 0, FastMath.toRadians(40), 16 * FastMath.PI / 12, 150, 150, 0, 0);
Assert.assertEquals(0, (roTestCase - myRo) / roTestCase, 1e-14);
Assert.assertEquals(0, (tzTestCase - atm.getT()) / tzTestCase, 1e-13);
Assert.assertEquals(0, (tinfTestCase - atm.getTinf()) / tinfTestCase, 1e-13);
// IDEM., day=275
roTestCase = 2.8524195214905e-17 * 1000;
tzTestCase = 1157.1872001392;
tinfTestCase = 1157.1933514185;
myRo =
atm.getDensity(
275, 800 * 1000, 0, FastMath.toRadians(40), 16 * FastMath.PI / 12, 150, 150, 0, 0);
Assert.assertEquals(0, (roTestCase - myRo) / roTestCase, 1e-14);
Assert.assertEquals(0, (tzTestCase - atm.getT()) / tzTestCase, 1e-13);
Assert.assertEquals(0, (tinfTestCase - atm.getTinf()) / tinfTestCase, 1e-13);
// IDEM., day=355
roTestCase = 1.7343324462212e-17 * 1000;
tzTestCase = 1033.0277846356;
tinfTestCase = 1033.0282703200;
myRo =
atm.getDensity(
355, 800 * 1000, 0, FastMath.toRadians(40), 16 * FastMath.PI / 12, 150, 150, 0, 0);
Assert.assertEquals(0, (roTestCase - myRo) / roTestCase, 2e-14);
Assert.assertEquals(0, (tzTestCase - atm.getT()) / tzTestCase, 1e-13);
Assert.assertEquals(0, (tinfTestCase - atm.getTinf()) / tinfTestCase, 1e-13);
// IDEM., day=85
roTestCase = 2.9983740796297e-17 * 1000;
tzTestCase = 1169.5405086196;
tinfTestCase = 1169.5485768345;
myRo =
atm.getDensity(
85, 800 * 1000, 0, FastMath.toRadians(40), 16 * FastMath.PI / 12, 150, 150, 0, 0);
Assert.assertEquals(0, (roTestCase - myRo) / roTestCase, 1e-14);
Assert.assertEquals(0, (tzTestCase - atm.getT()) / tzTestCase, 1e-13);
Assert.assertEquals(0, (tinfTestCase - atm.getTinf()) / tinfTestCase, 1e-13);
// alt=500.
// lat=-70. NB: the subroutine requires latitude in rad
// day=15.
// hl=16. NB: the subroutine requires local time in rad (0hr=0 rad)
// xlon=0.
// fm(1)=70.
// f(1) =fm(1)
// fm(2)=0.
// f(2)=0.
// akp(1)=0.
// akp(2)=0.
// akp(3)=0.
// akp(4)=0.
// ro= 1.3150282384722D-16
// tz= 793.65487014559
// tinf= 793.65549802348
// roTestCase = 1.3150282384722E-16;
// tzTestCase= 793.65487014559;
// tinfTestCase= 793.65549802348;
atm.getDensity(15, 500 * 1000, 0, FastMath.toRadians(-70), 16 * FastMath.PI / 12, 70, 70, 0, 0);
// IDEM., alt=800.
// ro= 1.9556768571305D-18
// tz= 793.65549797919
// tinf= 793.65549802348
atm.getDensity(15, 800 * 1000, 0, FastMath.toRadians(-70), 16 * FastMath.PI / 12, 70, 70, 0, 0);
}
/**
* Find a split on a particular edge. FIXME this appears to be copy-pasted from another method and
* only used for park and rides. Can we reuse some code here?
*/
public static Split findOnEdge(double lat, double lon, EdgeStore.Edge edge) {
// After this conversion, the entire geometric calculation is happening in fixed precision int
// degrees.
int fixedLat = VertexStore.floatingDegreesToFixed(lat);
int fixedLon = VertexStore.floatingDegreesToFixed(lon);
// We won't worry about the perpendicular walks yet.
// Just insert or find a vertex on the nearest road and return that vertex.
final double metersPerDegreeLat = 111111.111;
double cosLat =
FastMath.cos(FastMath.toRadians(lat)); // The projection factor, Earth is a "sphere"
// TODO copy paste code
// The split location currently being examined and the best one seen so far.
Split curr = new Split();
Split best = new Split();
curr.edge = edge.edgeIndex;
best.vertex0 = edge.getFromVertex();
best.vertex1 = edge.getToVertex();
double[] lengthBefore_fixedDeg = new double[1];
edge.forEachSegment(
(seg, fixedLat0, fixedLon0, fixedLat1, fixedLon1) -> {
// Find the fraction along the current segment
curr.seg = seg;
curr.frac =
GeometryUtils.segmentFraction(
fixedLon0, fixedLat0, fixedLon1, fixedLat1, fixedLon, fixedLat, cosLat);
// Project to get the closest point on the segment.
// Note: the fraction is scaleless, xScale is accounted for in the segmentFraction
// function.
curr.fixedLon = (int) (fixedLon0 + curr.frac * (fixedLon1 - fixedLon0));
curr.fixedLat = (int) (fixedLat0 + curr.frac * (fixedLat1 - fixedLat0));
double dx = (fixedLon1 - fixedLon0) * cosLat;
double dy = (fixedLat1 - fixedLat0);
double length = FastMath.sqrt(dx * dx + dy * dy);
curr.distance0_mm =
(int) ((lengthBefore_fixedDeg[0] + length * curr.frac) * metersPerDegreeLat * 1000);
lengthBefore_fixedDeg[0] += length;
curr.distSquared = (long) (dx * dx + dy * dy);
// Replace the best segment if we've found something closer.
if (curr.distSquared < best.distSquared) {
best.setFrom(curr);
}
}); // end loop over segments
int edgeLengthMm = edge.getLengthMm();
if (best.distance0_mm > edgeLengthMm) {
// rounding errors
best.distance0_mm = edgeLengthMm;
best.distance1_mm = 0;
} else {
best.distance1_mm = edgeLengthMm - best.distance0_mm;
}
return best;
}
/**
* Find a location on an existing street near the given point, without actually creating any
* vertices or edges.
*
* @return a new Split object, or null if no edge was found in range.
*/
public static Split find(
double lat,
double lon,
double searchRadiusMeters,
StreetLayer streetLayer,
StreetMode streetMode) {
// After this conversion, the entire geometric calculation is happening in fixed precision int
// degrees.
int fixedLat = VertexStore.floatingDegreesToFixed(lat);
int fixedLon = VertexStore.floatingDegreesToFixed(lon);
// We won't worry about the perpendicular walks yet.
// Just insert or find a vertex on the nearest road and return that vertex.
final double metersPerDegreeLat = 111111.111;
double cosLat =
FastMath.cos(FastMath.toRadians(lat)); // The projection factor, Earth is a "sphere"
// Use longs for radii and their square because squaring the fixed-point radius _will_ overflow
// a signed int32.
long radiusFixedLat =
VertexStore.floatingDegreesToFixed(searchRadiusMeters / metersPerDegreeLat);
long radiusFixedLon =
(int) (radiusFixedLat / cosLat); // Expand the X search space, don't shrink it.
Envelope envelope = new Envelope(fixedLon, fixedLon, fixedLat, fixedLat);
envelope.expandBy(radiusFixedLon, radiusFixedLat);
long squaredRadiusFixedLat = radiusFixedLat * radiusFixedLat;
EdgeStore.Edge edge = streetLayer.edgeStore.getCursor();
// Iterate over the set of forward (even) edges that may be near the given coordinate.
TIntCollection candidateEdges = streetLayer.findEdgesInEnvelope(envelope);
// The split location currently being examined and the best one seen so far.
Split curr = new Split();
Split best = new Split();
candidateEdges.forEach(
e -> {
curr.edge = e;
edge.seek(e);
// Skip Link edges those are links between transit stops/P+R/Bike share vertices and graph
// Without this origin or destination point can link to those edges because they have ALL
// permissions
// and route is never found since point is inaccessible because edges leading to it don't
// have required permission
if (edge.getFlag(EdgeStore.EdgeFlag.LINK)) return true;
// If an edge does not allow traversal with the specified mode, skip over it.
if (streetMode == StreetMode.WALK && !edge.getFlag(EdgeStore.EdgeFlag.ALLOWS_PEDESTRIAN))
return true;
if (streetMode == StreetMode.BICYCLE && !edge.getFlag(EdgeStore.EdgeFlag.ALLOWS_BIKE))
return true;
if (streetMode == StreetMode.CAR && !edge.getFlag(EdgeStore.EdgeFlag.ALLOWS_CAR))
return true;
// The distance to this edge is the distance to the closest segment of its geometry.
edge.forEachSegment(
(seg, fixedLat0, fixedLon0, fixedLat1, fixedLon1) -> {
// Find the fraction along the current segment
curr.seg = seg;
curr.frac =
GeometryUtils.segmentFraction(
fixedLon0, fixedLat0, fixedLon1, fixedLat1, fixedLon, fixedLat, cosLat);
// Project to get the closest point on the segment.
// Note: the fraction is scaleless, xScale is accounted for in the segmentFraction
// function.
curr.fixedLon = (int) (fixedLon0 + curr.frac * (fixedLon1 - fixedLon0));
curr.fixedLat = (int) (fixedLat0 + curr.frac * (fixedLat1 - fixedLat0));
// Find squared distance to edge (avoid taking root)
long dx = (long) ((curr.fixedLon - fixedLon) * cosLat);
long dy = (long) (curr.fixedLat - fixedLat);
curr.distSquared = dx * dx + dy * dy;
// Ignore segments that are too far away (filter false positives).
if (curr.distSquared < squaredRadiusFixedLat) {
if (curr.distSquared < best.distSquared) {
// Update the best segment if we've found something closer.
best.setFrom(curr);
} else if (curr.distSquared == best.distSquared && curr.edge < best.edge) {
// Break distance ties by favoring lower edge IDs. This makes destination
// linking
// deterministic where centroids are equidistant to edges (see issue #159).
best.setFrom(curr);
}
}
});
// The loop over the edges should continue.
return true;
});
if (best.edge < 0) {
// No edge found nearby.
return null;
}
// We found an edge. Iterate over its segments again, accumulating distances along its geometry.
// The distance calculations involve square roots so are deferred to happen here, only on the
// selected edge.
// TODO accumulate before/after geoms. Split point can be passed over since it's not an
// intermediate.
// The length is are stored in one-element array to dodge Java's "effectively final" BS.
edge.seek(best.edge);
best.vertex0 = edge.getFromVertex();
best.vertex1 = edge.getToVertex();
double[] lengthBefore_fixedDeg = new double[1];
edge.forEachSegment(
(seg, fLat0, fLon0, fLat1, fLon1) -> {
// Sum lengths only up to the split point.
// lengthAfter should be total length minus lengthBefore, which ensures splits do not
// change total lengths.
if (seg <= best.seg) {
double dx = (fLon1 - fLon0) * cosLat;
double dy = (fLat1 - fLat0);
double length = FastMath.sqrt(dx * dx + dy * dy);
if (seg == best.seg) {
length *= best.frac;
}
lengthBefore_fixedDeg[0] += length;
}
});
// Convert the fixed-precision degree measurements into (milli)meters
double lengthBefore_floatDeg =
VertexStore.fixedDegreesToFloating((int) lengthBefore_fixedDeg[0]);
best.distance0_mm = (int) (lengthBefore_floatDeg * metersPerDegreeLat * 1000);
// FIXME perhaps we should be using the sphericalDistanceLibrary here, or the other way around.
// The initial edge lengths are set using that library on OSM node coordinates, and they are
// slightly different.
// We are using a single cosLat value at the linking point, instead of a different value at each
// segment.
if (best.distance0_mm < 0) {
best.distance0_mm = 0;
LOG.error("Length of first street segment was not positive.");
}
if (best.distance0_mm > edge.getLengthMm()) {
// This mistake happens because the linear distance calculation we're using comes out longer
// than the
// spherical distance. The graph remains coherent because we force the two split edge lengths
// to add up
// to the original edge length.
LOG.debug(
"Length of first street segment was greater than the whole edge ({} > {}).",
best.distance0_mm,
edge.getLengthMm());
best.distance0_mm = edge.getLengthMm();
}
best.distance1_mm = edge.getLengthMm() - best.distance0_mm;
return best;
}
@Test
public void testIntersectionFromPoints() throws OrekitException {
AbsoluteDate date =
new AbsoluteDate(
new DateComponents(2008, 03, 21), TimeComponents.H12, TimeScalesFactory.getUTC());
Frame frame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
OneAxisEllipsoid earth = new OneAxisEllipsoid(6378136.460, 1 / 298.257222101, frame);
// Satellite on polar position
// ***************************
final double mu = 3.9860047e14;
CircularOrbit circ =
new CircularOrbit(
7178000.0,
0.5e-4,
0.,
FastMath.toRadians(90.),
FastMath.toRadians(60.),
FastMath.toRadians(90.),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
mu);
// Transform satellite position to position/velocity parameters in EME2000 and ITRF200B
PVCoordinates pvSatEME2000 = circ.getPVCoordinates();
PVCoordinates pvSatItrf =
frame.getTransformTo(FramesFactory.getEME2000(), date).transformPVCoordinates(pvSatEME2000);
Vector3D pSatItrf = pvSatItrf.getPosition();
// Test first visible surface points
GeodeticPoint geoPoint =
new GeodeticPoint(FastMath.toRadians(70.), FastMath.toRadians(60.), 0.);
Vector3D pointItrf = earth.transform(geoPoint);
Line line = new Line(pSatItrf, pointItrf, 1.0e-10);
GeodeticPoint geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
// Test second visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(65.), FastMath.toRadians(-120.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
// Test non visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(30.), FastMath.toRadians(60.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
// For polar satellite position, intersection point is at the same longitude but different
// latitude
Assert.assertEquals(1.04437199, geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(1.36198012, geoInter.getLatitude(), Utils.epsilonAngle);
// Satellite on equatorial position
// ********************************
circ =
new CircularOrbit(
7178000.0,
0.5e-4,
0.,
FastMath.toRadians(1.e-4),
FastMath.toRadians(0.),
FastMath.toRadians(0.),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
mu);
// Transform satellite position to position/velocity parameters in EME2000 and ITRF200B
pvSatEME2000 = circ.getPVCoordinates();
pvSatItrf =
frame.getTransformTo(FramesFactory.getEME2000(), date).transformPVCoordinates(pvSatEME2000);
pSatItrf = pvSatItrf.getPosition();
// Test first visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(5.), FastMath.toRadians(0.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
Assert.assertTrue(line.toSubSpace(pSatItrf).getX() < 0);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
// With the point opposite to satellite point along the line
GeodeticPoint geoInter2 =
earth.getIntersectionPoint(
line, line.toSpace(new Vector1D(-line.toSubSpace(pSatItrf).getX())), frame, date);
Assert.assertTrue(
FastMath.abs(geoInter.getLongitude() - geoInter2.getLongitude()) > FastMath.toRadians(0.1));
Assert.assertTrue(
FastMath.abs(geoInter.getLatitude() - geoInter2.getLatitude()) > FastMath.toRadians(0.1));
// Test second visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(-5.), FastMath.toRadians(0.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
// Test non visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(40.), FastMath.toRadians(0.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(-0.00768481, geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(0.32180410, geoInter.getLatitude(), Utils.epsilonAngle);
// Satellite on any position
// *************************
circ =
new CircularOrbit(
7178000.0,
0.5e-4,
0.,
FastMath.toRadians(50.),
FastMath.toRadians(0.),
FastMath.toRadians(90.),
PositionAngle.MEAN,
FramesFactory.getEME2000(),
date,
mu);
// Transform satellite position to position/velocity parameters in EME2000 and ITRF200B
pvSatEME2000 = circ.getPVCoordinates();
pvSatItrf =
frame.getTransformTo(FramesFactory.getEME2000(), date).transformPVCoordinates(pvSatEME2000);
pSatItrf = pvSatItrf.getPosition();
// Test first visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(40.), FastMath.toRadians(90.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
// Test second visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(60.), FastMath.toRadians(90.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(geoPoint.getLongitude(), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(geoPoint.getLatitude(), geoInter.getLatitude(), Utils.epsilonAngle);
// Test non visible surface points
geoPoint = new GeodeticPoint(FastMath.toRadians(0.), FastMath.toRadians(90.), 0.);
pointItrf = earth.transform(geoPoint);
line = new Line(pSatItrf, pointItrf, 1.0e-10);
geoInter = earth.getIntersectionPoint(line, pSatItrf, frame, date);
Assert.assertEquals(
FastMath.toRadians(89.5364061088196), geoInter.getLongitude(), Utils.epsilonAngle);
Assert.assertEquals(
FastMath.toRadians(35.555543683351125), geoInter.getLatitude(), Utils.epsilonAngle);
}
