SP3Writer.java

  1. /* Copyright 2022-2025 Luc Maisonobe
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.files.sp3;

  19. import java.io.IOException;
  20. import java.util.Iterator;
  21. import java.util.List;
  22. import java.util.Locale;
  23. import java.util.Map;

  25. import org.hipparchus.util.FastMath;
  26. import org.orekit.gnss.TimeSystem;
  27. import org.orekit.time.AbsoluteDate;
  28. import org.orekit.time.DateTimeComponents;
  29. import org.orekit.time.TimeScale;
  30. import org.orekit.time.TimeScales;
  31. import org.orekit.utils.CartesianDerivativesFilter;

  33. /** Writer for SP3 file.
  34.  * @author Luc Maisonobe
  35.  * @since 12.0
  36.  */
  37. public class SP3Writer {

  39.     /** End Of Line. */
  40.     private static final String EOL = System.lineSeparator();

  42.     /** Prefix for accuracy lines. */
  43.     private static final String ACCURACY_LINE_PREFIX = "++       ";

  45.     /** Prefix for comment lines. */
  46.     private static final String COMMENT_LINE_PREFIX = "/* ";

  48.     /** Format for accuracy base lines. */
  49.     private static final String ACCURACY_BASE_FORMAT = "%%f %10.7f %12.9f %14.11f %18.15f%n";

  51.     /** Constant additional parameters lines. */
  52.     private static final String ADDITIONAL_PARAMETERS_LINE = "%i    0    0    0    0      0      0      0      0         0";

  54.     /** Format for one 2 digits integer field. */
  55.     private static final String TWO_DIGITS_INTEGER = "%2d";

  57.     /** Format for one 3 digits integer field. */
  58.     private static final String THREE_DIGITS_INTEGER = "%3d";

  60.     /** Format for one 14.6 digits float field. */
  61.     private static final String FOURTEEN_SIX_DIGITS_FLOAT = "%14.6f";

  63.     /** Format for three blanks field. */
  64.     private static final String THREE_BLANKS = "   ";

  66.     /** Time system default line. */
  67.     private static final String TIME_SYSTEM_DEFAULT = "%c cc cc ccc ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc";

  69.     /** Destination of generated output. */
  70.     private final Appendable output;

  72.     /** Output name for error messages. */
  73.     private final String outputName;

  75.     /** Set of time scales used for parsing dates. */
  76.     private final TimeScales timeScales;

  78.     /** Simple constructor.
  79.      * @param output destination of generated output
  80.      * @param outputName output name for error messages
  81.      * @param timeScales set of time scales used for parsing dates
  82.      */
  83.     public SP3Writer(final Appendable output, final String outputName, final TimeScales timeScales) {
  84.         this.output     = output;
  85.         this.outputName = outputName;
  86.         this.timeScales = timeScales;
  87.     }

  89.     /** Write a SP3 file.
  90.      * @param sp3 SP3 file to write
  91.      * @exception IOException if an I/O error occurs.
  92.      */
  93.     public void write(final SP3 sp3)
  94.         throws IOException {
  95.         sp3.validate(false, outputName);
  96.         writeHeader(sp3.getHeader());

  98.         // set up iterators for all satellites
  99.         final CoordinatesIterator[] iterators = new CoordinatesIterator[sp3.getSatelliteCount()];
  100.         int k = 0;
  101.         for (final Map.Entry<String, SP3Ephemeris> entry : sp3.getSatellites().entrySet()) {
  102.             iterators[k++] = new CoordinatesIterator(entry.getValue());
  103.         }

  105.         final TimeScale timeScale  = sp3.getHeader().getTimeSystem().getTimeScale(timeScales);
  106.         for (AbsoluteDate date = earliest(iterators); !date.equals(AbsoluteDate.FUTURE_INFINITY); date = earliest(iterators)) {

  108.             // epoch
  109.             final DateTimeComponents dtc = date.getComponents(timeScale).roundIfNeeded(60, 8);
  110.             output.append(String.format(Locale.US, "*  %4d %2d %2d %2d %2d %11.8f%n",
  111.                                         dtc.getDate().getYear(),
  112.                                         dtc.getDate().getMonth(),
  113.                                         dtc.getDate().getDay(),
  114.                                         dtc.getTime().getHour(),
  115.                                         dtc.getTime().getMinute(),
  116.                                         dtc.getTime().getSecond()));

  118.             for (final CoordinatesIterator iter : iterators) {

  120.                 final SP3Coordinate coordinate;
  121.                 if (iter.pending != null &&
  122.                     FastMath.abs(iter.pending.getDate().durationFrom(date)) <= 0.001 * sp3.getHeader().getEpochInterval()) {
  123.                     // the pending coordinate date matches current epoch
  124.                     coordinate = iter.pending;
  125.                     iter.advance();
  126.                 } else {
  127.                     // the pending coordinate  does not match current epoch
  128.                     coordinate = SP3Coordinate.DUMMY;
  129.                 }

  131.                 // position
  132.                 writePosition(sp3.getHeader(), iter.id, coordinate);

  134.                 if (sp3.getHeader().getFilter() != CartesianDerivativesFilter.USE_P) {
  135.                     // velocity
  136.                     writeVelocity(sp3.getHeader(), iter.id, coordinate);
  137.                 }

  139.             }

  141.         }

  143.         output.append("EOF").
  144.                append(EOL);

  146.     }

  148.     /** Find earliest date in ephemerides.
  149.      * @param iterators ephemerides iterators
  150.      * @return earliest date in iterators
  151.      */
  152.     private AbsoluteDate earliest(final CoordinatesIterator[] iterators) {
  153.         AbsoluteDate date = AbsoluteDate.FUTURE_INFINITY;
  154.         for (final CoordinatesIterator iter : iterators) {
  155.             if (iter.pending != null && iter.pending.getDate().isBefore(date)) {
  156.                 date = iter.pending.getDate();
  157.             }
  158.         }
  159.         return date;
  160.     }

  162.     /** Write position.
  163.      * @param header file header
  164.      * @param satId satellite id
  165.      * @param coordinate coordinate
  166.      * @exception IOException if an I/O error occurs.
  167.      */
  168.     private void writePosition(final SP3Header header, final String satId, final SP3Coordinate coordinate)
  169.         throws IOException {

  171.         final StringBuilder lineBuilder = new StringBuilder();

  173.         // position
  174.         lineBuilder.append(String.format(Locale.US, "P%3s%14.6f%14.6f%14.6f",
  175.                                          satId,
  176.                                          SP3Utils.POSITION_UNIT.fromSI(coordinate.getPosition().getX()),
  177.                                          SP3Utils.POSITION_UNIT.fromSI(coordinate.getPosition().getY()),
  178.                                          SP3Utils.POSITION_UNIT.fromSI(coordinate.getPosition().getZ())));

  180.         // clock
  181.         lineBuilder.append(String.format(Locale.US, FOURTEEN_SIX_DIGITS_FLOAT,
  182.                                          SP3Utils.CLOCK_UNIT.fromSI(coordinate.getClockCorrection())));

  184.         // position accuracy
  185.         if (coordinate.getPositionAccuracy() == null) {
  186.             lineBuilder.append(THREE_BLANKS).
  187.                         append(THREE_BLANKS).
  188.                         append(THREE_BLANKS);
  189.         } else {
  190.             lineBuilder.append(' ');
  191.             lineBuilder.append(String.format(Locale.US, TWO_DIGITS_INTEGER,
  192.                                              SP3Utils.indexAccuracy(SP3Utils.POSITION_ACCURACY_UNIT, header.getPosVelBase(),
  193.                                                                     coordinate.getPositionAccuracy().getX())));
  194.             lineBuilder.append(' ');
  195.             lineBuilder.append(String.format(Locale.US, TWO_DIGITS_INTEGER,
  196.                                              SP3Utils.indexAccuracy(SP3Utils.POSITION_ACCURACY_UNIT, header.getPosVelBase(),
  197.                                                                     coordinate.getPositionAccuracy().getY())));
  198.             lineBuilder.append(' ');
  199.             lineBuilder.append(String.format(Locale.US, TWO_DIGITS_INTEGER,
  200.                                              SP3Utils.indexAccuracy(SP3Utils.POSITION_ACCURACY_UNIT, header.getPosVelBase(),
  201.                                                                     coordinate.getPositionAccuracy().getZ())));
  202.         }

  204.         // clock accuracy
  205.         lineBuilder.append(' ');
  206.         if (Double.isNaN(coordinate.getClockAccuracy())) {
  207.             lineBuilder.append(THREE_BLANKS);
  208.         } else {
  209.             lineBuilder.append(String.format(Locale.US, THREE_DIGITS_INTEGER,
  210.                                              SP3Utils.indexAccuracy(SP3Utils.CLOCK_ACCURACY_UNIT, header.getClockBase(),
  211.                                                                     coordinate.getClockAccuracy())));
  212.         }

  214.         // events
  215.         lineBuilder.append(' ');
  216.         lineBuilder.append(coordinate.hasClockEvent()         ? 'E' : ' ');
  217.         lineBuilder.append(coordinate.hasClockPrediction()    ? 'P' : ' ');
  218.         lineBuilder.append(' ');
  219.         lineBuilder.append(' ');
  220.         lineBuilder.append(coordinate.hasOrbitManeuverEvent() ? 'M' : ' ');
  221.         lineBuilder.append(coordinate.hasOrbitPrediction()    ? 'P' : ' ');

  223.         output.append(lineBuilder.toString().trim()).append(EOL);

  225.     }

  227.     /** Write velocity.
  228.      * @param header file header
  229.      * @param satId satellite id
  230.      * @param coordinate coordinate
  231.      * @exception IOException if an I/O error occurs.
  232.      */
  233.     private void writeVelocity(final SP3Header header, final String satId, final SP3Coordinate coordinate)
  234.         throws IOException {

  236.         final StringBuilder lineBuilder = new StringBuilder();
  237.          // velocity
  238.         lineBuilder.append(String.format(Locale.US, "V%3s%14.6f%14.6f%14.6f",
  239.                                          satId,
  240.                                          SP3Utils.VELOCITY_UNIT.fromSI(coordinate.getVelocity().getX()),
  241.                                          SP3Utils.VELOCITY_UNIT.fromSI(coordinate.getVelocity().getY()),
  242.                                          SP3Utils.VELOCITY_UNIT.fromSI(coordinate.getVelocity().getZ())));

  244.         // clock rate
  245.         lineBuilder.append(String.format(Locale.US, FOURTEEN_SIX_DIGITS_FLOAT,
  246.                                          SP3Utils.CLOCK_RATE_UNIT.fromSI(coordinate.getClockRateChange())));

  248.         // velocity accuracy
  249.         if (coordinate.getVelocityAccuracy() == null) {
  250.             lineBuilder.append(THREE_BLANKS).
  251.                         append(THREE_BLANKS).
  252.                         append(THREE_BLANKS);
  253.         } else {
  254.             lineBuilder.append(' ');
  255.             lineBuilder.append(String.format(Locale.US, TWO_DIGITS_INTEGER,
  256.                                              SP3Utils.indexAccuracy(SP3Utils.VELOCITY_ACCURACY_UNIT, header.getPosVelBase(),
  257.                                                                     coordinate.getVelocityAccuracy().getX())));
  258.             lineBuilder.append(' ');
  259.             lineBuilder.append(String.format(Locale.US, TWO_DIGITS_INTEGER,
  260.                                              SP3Utils.indexAccuracy(SP3Utils.VELOCITY_ACCURACY_UNIT, header.getPosVelBase(),
  261.                                                                     coordinate.getVelocityAccuracy().getY())));
  262.             lineBuilder.append(' ');
  263.             lineBuilder.append(String.format(Locale.US, TWO_DIGITS_INTEGER,
  264.                                              SP3Utils.indexAccuracy(SP3Utils.VELOCITY_ACCURACY_UNIT, header.getPosVelBase(),
  265.                                                                     coordinate.getVelocityAccuracy().getZ())));
  266.         }

  268.         // clock rate accuracy
  269.         lineBuilder.append(' ');
  270.         if (Double.isNaN(coordinate.getClockRateAccuracy())) {
  271.             lineBuilder.append(THREE_BLANKS);
  272.         } else {
  273.             lineBuilder.append(String.format(Locale.US, THREE_DIGITS_INTEGER,
  274.                                              SP3Utils.indexAccuracy(SP3Utils.CLOCK_RATE_ACCURACY_UNIT, header.getClockBase(),
  275.                                                                     coordinate.getClockRateAccuracy())));
  276.         }

  278.         output.append(lineBuilder.toString().trim()).append(EOL);

  280.     }

  282.     /** Write header.
  283.      * @param header SP3 header to write
  284.      * @exception IOException if an I/O error occurs.
  285.      */
  286.     private void writeHeader(final SP3Header header)
  287.         throws IOException {
  288.         final TimeScale timeScale = header.getTimeSystem().getTimeScale(timeScales);
  289.         final DateTimeComponents dtc = header.getEpoch().getComponents(timeScale).roundIfNeeded(60, 8);
  290.         final StringBuilder dataUsedBuilder = new StringBuilder();
  291.         for (final DataUsed du : header.getDataUsed()) {
  292.             if (dataUsedBuilder.length() > 0) {
  293.                 dataUsedBuilder.append('+');
  294.             }
  295.             dataUsedBuilder.append(du.getKey());
  296.         }
  297.         final String dataUsed = dataUsedBuilder.length() <= 5 ?
  298.                                 dataUsedBuilder.toString() :
  299.                                 DataUsed.MIXED.getKey();

  301.         // header first line: version, epoch...
  302.         output.append(String.format(Locale.US, "#%c%c%4d %2d %2d %2d %2d %11.8f %7d %5s %5s %3s %4s%n",
  303.                                     header.getVersion(),
  304.                                     header.getFilter() == CartesianDerivativesFilter.USE_P ? 'P' : 'V',
  305.                                     dtc.getDate().getYear(),
  306.                                     dtc.getDate().getMonth(),
  307.                                     dtc.getDate().getDay(),
  308.                                     dtc.getTime().getHour(),
  309.                                     dtc.getTime().getMinute(),
  310.                                     dtc.getTime().getSecond(),
  311.                                     header.getNumberOfEpochs(),
  312.                                     dataUsed,
  313.                                     header.getCoordinateSystem(),
  314.                                     header.getOrbitTypeKey(),
  315.                                     header.getAgency()));

  317.         // header second line : dates
  318.         output.append(String.format(Locale.US, "## %4d %15.8f %14.8f %5d %15.13f%n",
  319.                                     header.getGpsWeek(),
  320.                                     header.getSecondsOfWeek(),
  321.                                     header.getEpochInterval(),
  322.                                     header.getModifiedJulianDay(),
  323.                                     header.getDayFraction()));

  325.         // list of satellites
  326.         final List<String> satellites = header.getSatIds();
  327.         output.append(String.format(Locale.US, "+  %3d   ", satellites.size()));
  328.         int lines  = 0;
  329.         int column = 9;
  330.         int remaining = satellites.size();
  331.         for (final String satId : satellites) {
  332.             output.append(String.format(Locale.US, "%3s", satId));
  333.             --remaining;
  334.             column += 3;
  335.             if (column >= 60 && remaining > 0) {
  336.                 // finish line
  337.                 output.append(EOL);
  338.                 ++lines;

  340.                 // start new line
  341.                 output.append("+        ");
  342.                 column = 9;
  343.             }
  344.         }
  345.         while (column < 60) {
  346.             output.append(' ').
  347.                    append(' ').
  348.                    append('0');
  349.             column += 3;
  350.         }
  351.         output.append(EOL);
  352.         ++lines;
  353.         while (lines++ < 5) {
  354.             // write extra lines to have at least 85 satellites
  355.             output.append("+          0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0").
  356.                    append(EOL);
  357.         }

  359.         // general accuracy
  360.         output.append(ACCURACY_LINE_PREFIX);
  361.         lines  = 0;
  362.         column = 9;
  363.         remaining = satellites.size();
  364.         for (final String satId : satellites) {
  365.             final double accuracy    = header.getAccuracy(satId);
  366.             final int    accuracyExp = SP3Utils.indexAccuracy(SP3Utils.POSITION_ACCURACY_UNIT, SP3Utils.POS_VEL_BASE_ACCURACY, accuracy);
  367.             output.append(String.format(Locale.US, THREE_DIGITS_INTEGER, accuracyExp));
  368.             --remaining;
  369.             column += 3;
  370.             if (column >= 60 && remaining > 0) {
  371.                 // finish line
  372.                 output.append(EOL);
  373.                 ++lines;

  375.                 // start new line
  376.                 output.append(ACCURACY_LINE_PREFIX);
  377.                 column = 9;
  378.             }
  379.         }
  380.         while (column < 60) {
  381.             output.append(' ').
  382.                    append(' ').
  383.                    append('0');
  384.             column += 3;
  385.         }
  386.         output.append(EOL);
  387.         ++lines;
  388.         while (lines++ < 5) {
  389.             // write extra lines to have at least 85 satellites
  390.             output.append("++         0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0").
  391.                    append(EOL);
  392.         }

  394.         // type
  395.         if (header.getVersion() == 'a') {
  396.             output.append(TIME_SYSTEM_DEFAULT).append(EOL);
  397.         } else {
  398.             final TimeSystem ts = header.getTimeSystem().getKey() == null ?
  399.                                   TimeSystem.UTC :
  400.                                   header.getTimeSystem();
  401.             output.append(String.format(Locale.US, "%%c %1s  cc %3s ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc%n",
  402.                                         header.getType().getKey(), ts.getKey()));
  403.         }
  404.         output.append(TIME_SYSTEM_DEFAULT).append(EOL);

  406.         // entries accuracy
  407.         output.append(String.format(Locale.US, ACCURACY_BASE_FORMAT,
  408.                                     header.getPosVelBase(), header.getClockBase(), 0.0, 0.0));
  409.         output.append(String.format(Locale.US, ACCURACY_BASE_FORMAT,
  410.                                     0.0, 0.0, 0.0, 0.0));

  412.         // additional parameters
  413.         output.append(ADDITIONAL_PARAMETERS_LINE).append(EOL);
  414.         output.append(ADDITIONAL_PARAMETERS_LINE).append(EOL);

  416.         // comments
  417.         int count = 0;
  418.         for (final String comment : header.getComments()) {
  419.             ++count;
  420.             output.append(COMMENT_LINE_PREFIX).append(comment).append(EOL);
  421.         }
  422.         while (count < 4) {
  423.             // add dummy comments to get at least the four comments specified for versions a, b and c
  424.             ++count;
  425.             output.append(COMMENT_LINE_PREFIX).append(EOL);
  426.         }

  428.     }

  430.     /** Iterator for coordinates. */
  431.     private static class CoordinatesIterator {

  433.         /** Satellite ID. */
  434.         private final String id;

  436.         /** Iterator over segments. */
  437.         private Iterator<SP3Segment> segmentsIterator;

  439.         /** Iterator over coordinates. */
  440.         private Iterator<SP3Coordinate> coordinatesIterator;

  442.         /** Pending coordinate. */
  443.         private SP3Coordinate pending;

  445.         /** Simple constructor.
  446.          * @param ephemeris underlying ephemeris
  447.          */
  448.         CoordinatesIterator(final SP3Ephemeris ephemeris) {
  449.             this.id                  = ephemeris.getId();
  450.             this.segmentsIterator    = ephemeris.getSegments().iterator();
  451.             this.coordinatesIterator = null;
  452.             advance();
  453.         }

  455.         /** Advance to next coordinates.
  456.          */
  457.         private void advance() {

  459.             while (coordinatesIterator == null || !coordinatesIterator.hasNext()) {
  460.                 // we have exhausted previous segment
  461.                 if (segmentsIterator != null && segmentsIterator.hasNext()) {
  462.                     coordinatesIterator = segmentsIterator.next().getCoordinates().iterator();
  463.                 } else {
  464.                     // we have exhausted the ephemeris
  465.                     segmentsIterator = null;
  466.                     pending          = null;
  467.                     return;
  468.                 }
  469.             }

  471.             // retrieve the next entry
  472.             pending = coordinatesIterator.next();

  474.         }

  476.     }

  478. }
Created with JaCoCo 0.8.12.202403310830