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