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;
  32. import org.orekit.utils.formatting.FastDoubleFormatter;
  33. import org.orekit.utils.formatting.FastLongFormatter;

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

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

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

  47.     /** Prefix for comment lines. */
  48.     private static final String COMMENT_LINE_PREFIX = "/* ";

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

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

  56.     /** Format for one 2 digits integer field. */
  57.     private static final FastLongFormatter TWO_DIGITS_INTEGER = new FastLongFormatter(2, false);

  59.     /** Format for one 3 digits integer field. */
  60.     private static final FastLongFormatter THREE_DIGITS_INTEGER = new FastLongFormatter(3, false);

  62.     /** Format for one 14.6 digits float field. */
  63.     private static final FastDoubleFormatter FOURTEEN_SIX_DIGITS_FLOAT = new FastDoubleFormatter(14, 6);

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

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

  71.     /** Destination of generated output. */
  72.     private final Appendable output;

  74.     /** Output name for error messages. */
  75.     private final String outputName;

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

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

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

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

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

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

  120.             for (final CoordinatesIterator iter : iterators) {

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

  133.                 // position
  134.                 writePosition(sp3.getHeader(), iter.id, coordinate);

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

  141.             }

  143.         }

  145.         output.append("EOF").
  146.                append(EOL);

  148.     }

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

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

  173.         final StringBuilder lineBuilder = new StringBuilder();

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

  182.         // clock
  183.         FOURTEEN_SIX_DIGITS_FLOAT.appendTo(lineBuilder,
  184.                                          SP3Utils.CLOCK_UNIT.fromSI(coordinate.getClockCorrection()));

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

  206.         // clock accuracy
  207.         lineBuilder.append(' ');
  208.         if (Double.isNaN(coordinate.getClockAccuracy())) {
  209.             lineBuilder.append(THREE_BLANKS);
  210.         } else {
  211.             THREE_DIGITS_INTEGER.appendTo(lineBuilder,
  212.                                           SP3Utils.indexAccuracy(SP3Utils.CLOCK_ACCURACY_UNIT, header.getClockBase(),
  213.                                                                  coordinate.getClockAccuracy()));
  214.         }

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

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

  227.     }

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

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

  246.         // clock rate
  247.         FOURTEEN_SIX_DIGITS_FLOAT.appendTo(lineBuilder, SP3Utils.CLOCK_RATE_UNIT.fromSI(coordinate.getClockRateChange()));

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

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

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

  281.     }

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

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

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

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

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

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

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

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

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

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

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

  429.     }

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

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

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

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

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

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

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

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

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

  475.         }

  477.     }

  479. }
Created with JaCoCo 0.8.12.202403310830