@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());
   }
 }
Esempio n. 2
0
  /** 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);
  }
Esempio n. 5
0
  /** 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);
 }
Esempio n. 8
0
  @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);
  }
Esempio n. 11
0
  /** 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);
  }
Esempio n. 12
0
  @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());
  }
Esempio n. 14
0
  @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);
 }
Esempio n. 16
0
  @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);
  }
Esempio n. 17
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;
  }
Esempio n. 18
0
  /**
   * 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;
  }
Esempio n. 19
0
  @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);
  }