1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.files.sp3;
18
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;
24
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
33
34
35
36
37 public class SP3Writer {
38
39
40 private static final String EOL = System.lineSeparator();
41
42
43 private static final String ACCURACY_LINE_PREFIX = "++ ";
44
45
46 private static final String COMMENT_LINE_PREFIX = "/* ";
47
48
49 private static final String ACCURACY_BASE_FORMAT = "%%f %10.7f %12.9f %14.11f %18.15f%n";
50
51
52 private static final String ADDITIONAL_PARAMETERS_LINE = "%i 0 0 0 0 0 0 0 0 0";
53
54
55 private static final String TWO_DIGITS_INTEGER = "%2d";
56
57
58 private static final String THREE_DIGITS_INTEGER = "%3d";
59
60
61 private static final String FOURTEEN_SIX_DIGITS_FLOAT = "%14.6f";
62
63
64 private static final String THREE_BLANKS = " ";
65
66
67 private static final String TIME_SYSTEM_DEFAULT = "%c cc cc ccc ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc";
68
69
70 private final Appendable output;
71
72
73 private final String outputName;
74
75
76 private final TimeScales timeScales;
77
78
79
80
81
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 }
88
89
90
91
92
93 public void write(final SP3 sp3)
94 throws IOException {
95 sp3.validate(false, outputName);
96 writeHeader(sp3.getHeader());
97
98
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 }
104
105 final TimeScale timeScale = sp3.getHeader().getTimeSystem().getTimeScale(timeScales);
106 for (AbsoluteDate date = earliest(iterators); !date.equals(AbsoluteDate.FUTURE_INFINITY); date = earliest(iterators)) {
107
108
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()));
117
118 for (final CoordinatesIterator iter : iterators) {
119
120 final SP3Coordinate coordinate;
121 if (iter.pending != null &&
122 FastMath.abs(iter.pending.getDate().durationFrom(date)) <= 0.001 * sp3.getHeader().getEpochInterval()) {
123
124 coordinate = iter.pending;
125 iter.advance();
126 } else {
127
128 coordinate = SP3Coordinate.DUMMY;
129 }
130
131
132 writePosition(sp3.getHeader(), iter.id, coordinate);
133
134 if (sp3.getHeader().getFilter() != CartesianDerivativesFilter.USE_P) {
135
136 writeVelocity(sp3.getHeader(), iter.id, coordinate);
137 }
138
139 }
140
141 }
142
143 output.append("EOF").
144 append(EOL);
145
146 }
147
148
149
150
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 }
161
162
163
164
165
166
167
168 private void writePosition(final SP3Header header, final String satId, final SP3Coordinate coordinate)
169 throws IOException {
170
171 final StringBuilder lineBuilder = new StringBuilder();
172
173
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())));
179
180
181 lineBuilder.append(String.format(Locale.US, FOURTEEN_SIX_DIGITS_FLOAT,
182 SP3Utils.CLOCK_UNIT.fromSI(coordinate.getClockCorrection())));
183
184
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 }
203
204
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 }
213
214
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' : ' ');
222
223 output.append(lineBuilder.toString().trim()).append(EOL);
224
225 }
226
227
228
229
230
231
232
233 private void writeVelocity(final SP3Header header, final String satId, final SP3Coordinate coordinate)
234 throws IOException {
235
236 final StringBuilder lineBuilder = new StringBuilder();
237
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())));
243
244
245 lineBuilder.append(String.format(Locale.US, FOURTEEN_SIX_DIGITS_FLOAT,
246 SP3Utils.CLOCK_RATE_UNIT.fromSI(coordinate.getClockRateChange())));
247
248
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 }
267
268
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 }
277
278 output.append(lineBuilder.toString().trim()).append(EOL);
279
280 }
281
282
283
284
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();
300
301
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()));
316
317
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()));
324
325
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
337 output.append(EOL);
338 ++lines;
339
340
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
355 output.append("+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0").
356 append(EOL);
357 }
358
359
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
372 output.append(EOL);
373 ++lines;
374
375
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
390 output.append("++ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0").
391 append(EOL);
392 }
393
394
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);
405
406
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));
411
412
413 output.append(ADDITIONAL_PARAMETERS_LINE).append(EOL);
414 output.append(ADDITIONAL_PARAMETERS_LINE).append(EOL);
415
416
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
424 ++count;
425 output.append(COMMENT_LINE_PREFIX).append(EOL);
426 }
427
428 }
429
430
431 private static class CoordinatesIterator {
432
433
434 private final String id;
435
436
437 private Iterator<SP3Segment> segmentsIterator;
438
439
440 private Iterator<SP3Coordinate> coordinatesIterator;
441
442
443 private SP3Coordinate pending;
444
445
446
447
448 CoordinatesIterator(final SP3Ephemeris ephemeris) {
449 this.id = ephemeris.getId();
450 this.segmentsIterator = ephemeris.getSegments().iterator();
451 this.coordinatesIterator = null;
452 advance();
453 }
454
455
456
457 private void advance() {
458
459 while (coordinatesIterator == null || !coordinatesIterator.hasNext()) {
460
461 if (segmentsIterator != null && segmentsIterator.hasNext()) {
462 coordinatesIterator = segmentsIterator.next().getCoordinates().iterator();
463 } else {
464
465 segmentsIterator = null;
466 pending = null;
467 return;
468 }
469 }
470
471
472 pending = coordinatesIterator.next();
473
474 }
475
476 }
477
478 }