1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.models.earth.ionosphere;
18
19 import java.io.BufferedReader;
20 import java.io.IOException;
21 import java.io.InputStream;
22 import java.io.InputStreamReader;
23 import java.nio.charset.StandardCharsets;
24 import java.util.Collections;
25 import java.util.List;
26 import java.util.regex.Pattern;
27
28 import org.hipparchus.Field;
29 import org.hipparchus.CalculusFieldElement;
30 import org.hipparchus.util.FastMath;
31 import org.hipparchus.util.FieldSinCos;
32 import org.hipparchus.util.MathArrays;
33 import org.hipparchus.util.SinCos;
34 import org.orekit.annotation.DefaultDataContext;
35 import org.orekit.bodies.BodyShape;
36 import org.orekit.bodies.FieldGeodeticPoint;
37 import org.orekit.bodies.GeodeticPoint;
38 import org.orekit.data.DataContext;
39 import org.orekit.errors.OrekitException;
40 import org.orekit.errors.OrekitMessages;
41 import org.orekit.frames.TopocentricFrame;
42 import org.orekit.propagation.FieldSpacecraftState;
43 import org.orekit.propagation.SpacecraftState;
44 import org.orekit.time.AbsoluteDate;
45 import org.orekit.time.DateComponents;
46 import org.orekit.time.DateTimeComponents;
47 import org.orekit.time.FieldAbsoluteDate;
48 import org.orekit.time.TimeScale;
49 import org.orekit.utils.ParameterDriver;
50 import org.orekit.utils.TimeStampedFieldPVCoordinates;
51 import org.orekit.utils.TimeStampedPVCoordinates;
52
53
54
55
56
57
58
59
60
61
62
63 public class NeQuickModel implements IonosphericModel {
64
65
66 private static final String NEQUICK_BASE = "/assets/org/orekit/nequick/";
67
68
69 private static final long serialVersionUID = 201928051L;
70
71
72 private static final Pattern SEPARATOR = Pattern.compile("\\s+");
73
74
75 private static final double RE = 6371200.0;
76
77
78 private static final double M_TO_KM = 0.001;
79
80
81 private static final double DENSITY_FACTOR = 1.0e11;
82
83
84 private static final double DELAY_FACTOR = 40.3e16;
85
86
87 private final double[] alpha;
88
89
90 private final double[][] stModip;
91
92
93 private int month;
94
95
96 private double[][][] f2;
97
98
99 private double[][][] fm3;
100
101
102 private final TimeScale utc;
103
104
105
106
107
108
109
110
111
112 @DefaultDataContext
113 public NeQuickModel(final double[] alpha) {
114 this(alpha, DataContext.getDefault().getTimeScales().getUTC());
115 }
116
117
118
119
120
121
122
123 public NeQuickModel(final double[] alpha,
124 final TimeScale utc) {
125
126 this.month = 0;
127 this.f2 = null;
128 this.fm3 = null;
129
130 final MODIPLoader parser = new MODIPLoader();
131 parser.loadMODIPGrid();
132 this.stModip = parser.getMODIPGrid();
133
134 this.alpha = alpha.clone();
135 this.utc = utc;
136 }
137
138 @Override
139 public double pathDelay(final SpacecraftState state, final TopocentricFrame baseFrame,
140 final double frequency, final double[] parameters) {
141
142 final GeodeticPoint recPoint = baseFrame.getPoint();
143
144 final AbsoluteDate date = state.getDate();
145
146
147 final BodyShape ellipsoid = baseFrame.getParentShape();
148
149 final TimeStampedPVCoordinates pv = state.getPVCoordinates(ellipsoid.getBodyFrame());
150 final GeodeticPoint satPoint = ellipsoid.transform(pv.getPosition(), ellipsoid.getBodyFrame(), state.getDate());
151
152
153 final double tec = stec(date, recPoint, satPoint);
154
155
156 final double factor = DELAY_FACTOR / (frequency * frequency);
157 return factor * tec;
158 }
159
160 @Override
161 public <T extends CalculusFieldElement<T>> T pathDelay(final FieldSpacecraftState<T> state, final TopocentricFrame baseFrame,
162 final double frequency, final T[] parameters) {
163
164 final FieldAbsoluteDate<T> date = state.getDate();
165
166 final FieldGeodeticPoint<T> recPoint = baseFrame.getPoint(date.getField());
167
168
169
170 final BodyShape ellipsoid = baseFrame.getParentShape();
171
172 final TimeStampedFieldPVCoordinates<T> pv = state.getPVCoordinates(ellipsoid.getBodyFrame());
173 final FieldGeodeticPoint<T> satPoint = ellipsoid.transform(pv.getPosition(), ellipsoid.getBodyFrame(), state.getDate());
174
175
176 final T tec = stec(date, recPoint, satPoint);
177
178
179 final double factor = DELAY_FACTOR / (frequency * frequency);
180 return tec.multiply(factor);
181 }
182
183 @Override
184 public List<ParameterDriver> getParametersDrivers() {
185 return Collections.emptyList();
186 }
187
188
189
190
191
192
193
194
195
196
197
198
199 public double stec(final AbsoluteDate date, final GeodeticPoint recP, final GeodeticPoint satP) {
200
201
202 final Ray ray = new Ray(recP, satP);
203
204
205 final DateTimeComponents dateTime = date.getComponents(utc);
206 loadsIfNeeded(dateTime.getDate());
207
208
209 final double h1 = recP.getAltitude();
210 final double tolerance;
211 if (h1 < 1000000.0) {
212 tolerance = 0.001;
213 } else {
214 tolerance = 0.01;
215 }
216
217
218 int n = 8;
219 final Segment seg1 = new Segment(n, ray);
220 double gn1 = stecIntegration(seg1, dateTime);
221 n *= 2;
222 final Segment seg2 = new Segment(n, ray);
223 double gn2 = stecIntegration(seg2, dateTime);
224
225 int count = 1;
226 while (FastMath.abs(gn2 - gn1) > tolerance * FastMath.abs(gn1) && count < 20) {
227 gn1 = gn2;
228 n *= 2;
229 final Segment seg = new Segment(n, ray);
230 gn2 = stecIntegration(seg, dateTime);
231 count += 1;
232 }
233
234
235 if (count == 20) {
236 throw new OrekitException(OrekitMessages.STEC_INTEGRATION_DID_NOT_CONVERGE);
237 }
238
239
240 return (gn2 + ((gn2 - gn1) / 15.0)) * 1.0e-16;
241 }
242
243
244
245
246
247
248
249
250
251
252
253
254
255 public <T extends CalculusFieldElement<T>> T stec(final FieldAbsoluteDate<T> date,
256 final FieldGeodeticPoint<T> recP,
257 final FieldGeodeticPoint<T> satP) {
258
259
260 final Field<T> field = date.getField();
261
262
263 final FieldRay<T> ray = new FieldRay<>(field, recP, satP);
264
265
266 final DateTimeComponents dateTime = date.getComponents(utc);
267 loadsIfNeeded(dateTime.getDate());
268
269
270 final T h1 = recP.getAltitude();
271 final double tolerance;
272 if (h1.getReal() < 1000000.0) {
273 tolerance = 0.001;
274 } else {
275 tolerance = 0.01;
276 }
277
278
279 int n = 8;
280 final FieldSegment<T> seg1 = new FieldSegment<>(field, n, ray);
281 T gn1 = stecIntegration(field, seg1, dateTime);
282 n *= 2;
283 final FieldSegment<T> seg2 = new FieldSegment<>(field, n, ray);
284 T gn2 = stecIntegration(field, seg2, dateTime);
285
286 int count = 1;
287 while (FastMath.abs(gn2.subtract(gn1)).getReal() > FastMath.abs(gn1).multiply(tolerance).getReal() && count < 20) {
288 gn1 = gn2;
289 n *= 2;
290 final FieldSegment<T> seg = new FieldSegment<>(field, n, ray);
291 gn2 = stecIntegration(field, seg, dateTime);
292 count += 1;
293 }
294
295
296 if (count == 20) {
297 throw new OrekitException(OrekitMessages.STEC_INTEGRATION_DID_NOT_CONVERGE);
298 }
299
300
301 return gn2.add(gn2.subtract(gn1).divide(15.0)).multiply(1.0e-16);
302 }
303
304
305
306
307
308
309
310 private double stecIntegration(final Segment seg, final DateTimeComponents dateTime) {
311
312 final double[] heightS = seg.getHeights();
313 final double[] latitudeS = seg.getLatitudes();
314 final double[] longitudeS = seg.getLongitudes();
315
316
317 double density = 0.0;
318 for (int i = 0; i < heightS.length; i++) {
319 final NeQuickParameters parameters = new NeQuickParameters(dateTime, f2, fm3,
320 latitudeS[i], longitudeS[i],
321 alpha, stModip);
322 density += electronDensity(heightS[i], parameters);
323 }
324
325 return 0.5 * seg.getInterval() * density;
326 }
327
328
329
330
331
332
333
334
335
336 private <T extends CalculusFieldElement<T>> T stecIntegration(final Field<T> field,
337 final FieldSegment<T> seg,
338 final DateTimeComponents dateTime) {
339
340 final T[] heightS = seg.getHeights();
341 final T[] latitudeS = seg.getLatitudes();
342 final T[] longitudeS = seg.getLongitudes();
343
344
345 T density = field.getZero();
346 for (int i = 0; i < heightS.length; i++) {
347 final FieldNeQuickParameters<T> parameters = new FieldNeQuickParameters<>(field, dateTime, f2, fm3,
348 latitudeS[i], longitudeS[i],
349 alpha, stModip);
350 density = density.add(electronDensity(field, heightS[i], parameters));
351 }
352
353 return seg.getInterval().multiply(density).multiply(0.5);
354 }
355
356
357
358
359
360
361
362 private double electronDensity(final double h, final NeQuickParameters parameters) {
363
364 final double hInKm = h * M_TO_KM;
365
366 final double n;
367 if (hInKm <= parameters.getHmF2()) {
368 n = bottomElectronDensity(hInKm, parameters);
369 } else {
370 n = topElectronDensity(hInKm, parameters);
371 }
372 return n;
373 }
374
375
376
377
378
379
380
381
382
383 private <T extends CalculusFieldElement<T>> T electronDensity(final Field<T> field,
384 final T h,
385 final FieldNeQuickParameters<T> parameters) {
386
387 final T hInKm = h.multiply(M_TO_KM);
388
389 final T n;
390 if (hInKm.getReal() <= parameters.getHmF2().getReal()) {
391 n = bottomElectronDensity(field, hInKm, parameters);
392 } else {
393 n = topElectronDensity(field, hInKm, parameters);
394 }
395 return n;
396 }
397
398
399
400
401
402
403
404 private double bottomElectronDensity(final double h, final NeQuickParameters parameters) {
405
406
407 final double be;
408 if (h > parameters.getHmE()) {
409 be = parameters.getBETop();
410 } else {
411 be = parameters.getBEBot();
412 }
413 final double bf1;
414 if (h > parameters.getHmF1()) {
415 bf1 = parameters.getB1Top();
416 } else {
417 bf1 = parameters.getB1Bot();
418 }
419 final double bf2 = parameters.getB2Bot();
420
421
422 final double[] ct = new double[] {
423 1.0 / bf2, 1.0 / bf1, 1.0 / be
424 };
425
426
427
428 final double hTemp = FastMath.max(100.0, h);
429 final double exp = clipExp(10.0 / (1.0 + FastMath.abs(hTemp - parameters.getHmF2())));
430 final double[] arguments = new double[3];
431 arguments[0] = (hTemp - parameters.getHmF2()) / bf2;
432 arguments[1] = ((hTemp - parameters.getHmF1()) / bf1) * exp;
433 arguments[2] = ((hTemp - parameters.getHmE()) / be) * exp;
434
435
436 final double[] s = new double[3];
437
438 final double[] ds = new double[3];
439
440
441 final double[] amplitudes = parameters.getLayerAmplitudes();
442
443
444 for (int i = 0; i < 3; i++) {
445 if (FastMath.abs(arguments[i]) > 25.0) {
446 s[i] = 0.0;
447 ds[i] = 0.0;
448 } else {
449 final double expA = clipExp(arguments[i]);
450 final double opExpA = 1.0 + expA;
451 s[i] = amplitudes[i] * (expA / (opExpA * opExpA));
452 ds[i] = ct[i] * ((1.0 - expA) / (1.0 + expA));
453 }
454 }
455
456
457 final double aNo = MathArrays.linearCombination(s[0], 1.0, s[1], 1.0, s[2], 1.0);
458 if (h >= 100) {
459 return aNo * DENSITY_FACTOR;
460 } else {
461
462 final double bc = 1.0 - 10.0 * (MathArrays.linearCombination(s[0], ds[0], s[1], ds[1], s[2], ds[2]) / aNo);
463 final double z = 0.1 * (h - 100.0);
464
465 return aNo * clipExp(1.0 - bc * z - clipExp(-z)) * DENSITY_FACTOR;
466 }
467 }
468
469
470
471
472
473
474
475
476
477 private <T extends CalculusFieldElement<T>> T bottomElectronDensity(final Field<T> field,
478 final T h,
479 final FieldNeQuickParameters<T> parameters) {
480
481
482 final T zero = field.getZero();
483 final T one = field.getOne();
484
485
486 final T be;
487 if (h.getReal() > parameters.getHmE().getReal()) {
488 be = parameters.getBETop();
489 } else {
490 be = parameters.getBEBot();
491 }
492 final T bf1;
493 if (h.getReal() > parameters.getHmF1().getReal()) {
494 bf1 = parameters.getB1Top();
495 } else {
496 bf1 = parameters.getB1Bot();
497 }
498 final T bf2 = parameters.getB2Bot();
499
500
501 final T[] ct = MathArrays.buildArray(field, 3);
502 ct[0] = bf2.reciprocal();
503 ct[1] = bf1.reciprocal();
504 ct[2] = be.reciprocal();
505
506
507
508 final T hTemp = FastMath.max(zero.add(100.0), h);
509 final T exp = clipExp(field, FastMath.abs(hTemp.subtract(parameters.getHmF2())).add(1.0).divide(10.0).reciprocal());
510 final T[] arguments = MathArrays.buildArray(field, 3);
511 arguments[0] = hTemp.subtract(parameters.getHmF2()).divide(bf2);
512 arguments[1] = hTemp.subtract(parameters.getHmF1()).divide(bf1).multiply(exp);
513 arguments[2] = hTemp.subtract(parameters.getHmE()).divide(be).multiply(exp);
514
515
516 final T[] s = MathArrays.buildArray(field, 3);
517
518 final T[] ds = MathArrays.buildArray(field, 3);
519
520
521 final T[] amplitudes = parameters.getLayerAmplitudes();
522
523
524 for (int i = 0; i < 3; i++) {
525 if (FastMath.abs(arguments[i]).getReal() > 25.0) {
526 s[i] = zero;
527 ds[i] = zero;
528 } else {
529 final T expA = clipExp(field, arguments[i]);
530 final T opExpA = expA.add(1.0);
531 s[i] = amplitudes[i].multiply(expA.divide(opExpA.multiply(opExpA)));
532 ds[i] = ct[i].multiply(expA.negate().add(1.0).divide(expA.add(1.0)));
533 }
534 }
535
536
537 final T aNo = one.linearCombination(s[0], one, s[1], one, s[2], one);
538 if (h.getReal() >= 100) {
539 return aNo.multiply(DENSITY_FACTOR);
540 } else {
541
542 final T bc = s[0].multiply(ds[0]).add(one.linearCombination(s[0], ds[0], s[1], ds[1], s[2], ds[2])).divide(aNo).multiply(10.0).negate().add(1.0);
543 final T z = h.subtract(100.0).multiply(0.1);
544
545 return aNo.multiply(clipExp(field, bc.multiply(z).add(clipExp(field, z.negate())).negate().add(1.0))).multiply(DENSITY_FACTOR);
546 }
547 }
548
549
550
551
552
553
554
555 private double topElectronDensity(final double h, final NeQuickParameters parameters) {
556
557
558 final double g = 0.125;
559 final double r = 100.0;
560
561
562 final double deltaH = h - parameters.getHmF2();
563 final double z = deltaH / (parameters.getH0() * (1.0 + (r * g * deltaH) / (r * parameters.getH0() + g * deltaH)));
564
565
566 final double ee = clipExp(z);
567
568
569 if (ee > 1.0e11) {
570 return (4.0 * parameters.getNmF2() / ee) * DENSITY_FACTOR;
571 } else {
572 final double opExpZ = 1.0 + ee;
573 return ((4.0 * parameters.getNmF2() * ee) / (opExpZ * opExpZ)) * DENSITY_FACTOR;
574 }
575 }
576
577
578
579
580
581
582
583
584
585 private <T extends CalculusFieldElement<T>> T topElectronDensity(final Field<T> field,
586 final T h,
587 final FieldNeQuickParameters<T> parameters) {
588
589
590 final double g = 0.125;
591 final double r = 100.0;
592
593
594 final T deltaH = h.subtract(parameters.getHmF2());
595 final T z = deltaH.divide(parameters.getH0().multiply(deltaH.multiply(r).multiply(g).divide(parameters.getH0().multiply(r).add(deltaH.multiply(g))).add(1.0)));
596
597
598 final T ee = clipExp(field, z);
599
600
601 if (ee.getReal() > 1.0e11) {
602 return parameters.getNmF2().multiply(4.0).divide(ee).multiply(DENSITY_FACTOR);
603 } else {
604 final T opExpZ = ee.add(field.getOne());
605 return parameters.getNmF2().multiply(4.0).multiply(ee).divide(opExpZ.multiply(opExpZ)).multiply(DENSITY_FACTOR);
606 }
607 }
608
609
610
611
612
613 private void loadsIfNeeded(final DateComponents date) {
614
615
616 final int currentMonth = date.getMonth();
617
618
619 if (currentMonth != month || f2 == null || fm3 == null) {
620 this.month = currentMonth;
621
622
623 final CCIRLoader loader = new CCIRLoader();
624 loader.loadCCIRCoefficients(date);
625
626
627 this.f2 = loader.getF2();
628 this.fm3 = loader.getFm3();
629 }
630 }
631
632
633
634
635
636
637
638
639
640
641 private double clipExp(final double power) {
642 if (power > 80.0) {
643 return 5.5406E34;
644 } else if (power < -80) {
645 return 1.8049E-35;
646 } else {
647 return FastMath.exp(power);
648 }
649 }
650
651
652
653
654
655
656
657
658
659
660
661
662 private <T extends CalculusFieldElement<T>> T clipExp(final Field<T> field, final T power) {
663 final T zero = field.getZero();
664 if (power.getReal() > 80.0) {
665 return zero.add(5.5406E34);
666 } else if (power.getReal() < -80) {
667 return zero.add(1.8049E-35);
668 } else {
669 return FastMath.exp(power);
670 }
671 }
672
673
674
675
676
677 private static InputStream getStream(final String name) {
678 return NeQuickModel.class.getResourceAsStream(name);
679 }
680
681
682
683
684
685
686
687
688
689
690
691
692 private static class MODIPLoader {
693
694
695 private static final String SUPPORTED_NAME = NEQUICK_BASE + "modip.txt";
696
697
698 private double[][] grid;
699
700
701
702
703 MODIPLoader() {
704 this.grid = null;
705 }
706
707
708
709
710 public double[][] getMODIPGrid() {
711 return grid.clone();
712 }
713
714
715
716
717 public void loadMODIPGrid() {
718 try (InputStream in = getStream(SUPPORTED_NAME)) {
719 loadData(in, SUPPORTED_NAME);
720 } catch (IOException e) {
721 throw new OrekitException(OrekitMessages.INTERNAL_ERROR, e);
722 }
723
724
725 if (grid == null) {
726 throw new OrekitException(OrekitMessages.MODIP_GRID_NOT_LOADED, SUPPORTED_NAME);
727 }
728 }
729
730
731
732
733
734
735
736 public void loadData(final InputStream input, final String name)
737 throws IOException {
738
739
740 final int size = 39;
741
742
743 final double[][] array = new double[size][size];
744
745
746 int lineNumber = 0;
747 String line = null;
748 try (InputStreamReader isr = new InputStreamReader(input, StandardCharsets.UTF_8);
749 BufferedReader br = new BufferedReader(isr)) {
750
751 for (line = br.readLine(); line != null; line = br.readLine()) {
752 ++lineNumber;
753 line = line.trim();
754
755
756 if (line.length() > 0) {
757 final String[] modip_line = SEPARATOR.split(line);
758 for (int column = 0; column < modip_line.length; column++) {
759 array[lineNumber - 1][column] = Double.valueOf(modip_line[column]);
760 }
761 }
762
763 }
764
765 } catch (NumberFormatException nfe) {
766 throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE,
767 lineNumber, name, line);
768 }
769
770
771 grid = array.clone();
772
773 }
774 }
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792 private static class CCIRLoader {
793
794
795 public static final String DEFAULT_SUPPORTED_NAME = "ccir**.asc";
796
797
798 private static final int NUMBER_F2_COEFFICIENTS = 1976;
799
800
801 private static final int ROWS = 2;
802
803
804 private static final int TOTAL_COLUMNS_F2 = 76;
805
806
807 private static final int TOTAL_COLUMNS_FM3 = 49;
808
809
810 private static final int DEPTH_F2 = 13;
811
812
813 private static final int DEPTH_FM3 = 9;
814
815
816 private String supportedName;
817
818
819 private double[][][] f2Loader;
820
821
822 private double[][][] fm3Loader;
823
824
825
826
827 CCIRLoader() {
828 this.supportedName = DEFAULT_SUPPORTED_NAME;
829 this.f2Loader = null;
830 this.fm3Loader = null;
831 }
832
833
834
835
836
837 public double[][][] getF2() {
838 return f2Loader.clone();
839 }
840
841
842
843
844
845 public double[][][] getFm3() {
846 return fm3Loader.clone();
847 }
848
849
850
851
852 public void loadCCIRCoefficients(final DateComponents dateComponents) {
853
854
855 final int currentMonth = dateComponents.getMonth();
856 this.supportedName = NEQUICK_BASE + String.format("ccir%02d.asc", currentMonth + 10);
857 try (InputStream in = getStream(supportedName)) {
858 loadData(in, supportedName);
859 } catch (IOException e) {
860 throw new OrekitException(OrekitMessages.INTERNAL_ERROR, e);
861 }
862
863 if (f2Loader == null || fm3Loader == null) {
864 throw new OrekitException(OrekitMessages.NEQUICK_F2_FM3_NOT_LOADED, supportedName);
865 }
866
867 }
868
869
870
871
872
873
874
875 public void loadData(final InputStream input, final String name)
876 throws IOException {
877
878
879 final double[][][] f2Temp = new double[ROWS][TOTAL_COLUMNS_F2][DEPTH_F2];
880 final double[][][] fm3Temp = new double[ROWS][TOTAL_COLUMNS_FM3][DEPTH_FM3];
881
882
883 int lineNumber = 0;
884 int index = 0;
885 int currentRowF2 = 0;
886 int currentColumnF2 = 0;
887 int currentDepthF2 = 0;
888 int currentRowFm3 = 0;
889 int currentColumnFm3 = 0;
890 int currentDepthFm3 = 0;
891 String line = null;
892
893 try (InputStreamReader isr = new InputStreamReader(input, StandardCharsets.UTF_8);
894 BufferedReader br = new BufferedReader(isr)) {
895
896 for (line = br.readLine(); line != null; line = br.readLine()) {
897 ++lineNumber;
898 line = line.trim();
899
900
901 if (line.length() > 0) {
902 final String[] ccir_line = SEPARATOR.split(line);
903 for (int i = 0; i < ccir_line.length; i++) {
904
905 if (index < NUMBER_F2_COEFFICIENTS) {
906
907 if (currentDepthF2 >= DEPTH_F2 && currentColumnF2 < (TOTAL_COLUMNS_F2 - 1)) {
908 currentDepthF2 = 0;
909 currentColumnF2++;
910 } else if (currentDepthF2 >= DEPTH_F2 && currentColumnF2 >= (TOTAL_COLUMNS_F2 - 1)) {
911 currentDepthF2 = 0;
912 currentColumnF2 = 0;
913 currentRowF2++;
914 }
915 f2Temp[currentRowF2][currentColumnF2][currentDepthF2++] = Double.valueOf(ccir_line[i]);
916 index++;
917 } else {
918
919 if (currentDepthFm3 >= DEPTH_FM3 && currentColumnFm3 < (TOTAL_COLUMNS_FM3 - 1)) {
920 currentDepthFm3 = 0;
921 currentColumnFm3++;
922 } else if (currentDepthFm3 >= DEPTH_FM3 && currentColumnFm3 >= (TOTAL_COLUMNS_FM3 - 1)) {
923 currentDepthFm3 = 0;
924 currentColumnFm3 = 0;
925 currentRowFm3++;
926 }
927 fm3Temp[currentRowFm3][currentColumnFm3][currentDepthFm3++] = Double.valueOf(ccir_line[i]);
928 index++;
929 }
930
931 }
932 }
933
934 }
935
936 } catch (NumberFormatException nfe) {
937 throw new OrekitException(OrekitMessages.UNABLE_TO_PARSE_LINE_IN_FILE,
938 lineNumber, name, line);
939 }
940
941 f2Loader = f2Temp.clone();
942 fm3Loader = fm3Temp.clone();
943
944 }
945
946 }
947
948
949
950
951
952 private static class Ray {
953
954
955 private static final double THRESHOLD = 1.0e-10;
956
957
958 private final double s1;
959
960
961 private final double s2;
962
963
964 private final double rp;
965
966
967 private final double latP;
968
969
970 private final double lonP;
971
972
973 private final SinCos scLatP;
974
975
976 private final double sinAzP;
977
978
979 private final double cosAzP;
980
981
982
983
984
985
986 Ray(final GeodeticPoint recP, final GeodeticPoint satP) {
987
988
989 final double r1 = RE + recP.getAltitude();
990 final double r2 = RE + satP.getAltitude();
991
992
993 final double lat1 = recP.getLatitude();
994 final double lat2 = satP.getLatitude();
995 final double lon1 = recP.getLongitude();
996 final double lon2 = satP.getLongitude();
997 final SinCos scLatSat = FastMath.sinCos(lat2);
998 final SinCos scLatRec = FastMath.sinCos(lat1);
999 final SinCos scLon21 = FastMath.sinCos(lon2 - lon1);
1000
1001
1002 final double cosD = scLatRec.sin() * scLatSat.sin() +
1003 scLatRec.cos() * scLatSat.cos() * scLon21.cos();
1004 final double sinD = FastMath.sqrt(1.0 - cosD * cosD);
1005 final double z = FastMath.atan2(sinD, cosD - (r1 / r2));
1006
1007
1008 this.rp = r1 * FastMath.sin(z);
1009
1010
1011 if (FastMath.abs(FastMath.abs(lat1) - 0.5 * FastMath.PI) < THRESHOLD) {
1012
1013
1014 if (lat1 < 0) {
1015 this.latP = -z;
1016 } else {
1017 this.latP = z;
1018 }
1019
1020
1021 if (z < 0) {
1022 this.lonP = lon2;
1023 } else {
1024 this.lonP = lon2 + FastMath.PI;
1025 }
1026
1027 } else {
1028
1029
1030 final double deltaP = 0.5 * FastMath.PI - z;
1031 final SinCos scDeltaP = FastMath.sinCos(deltaP);
1032 final double sinAz = scLon21.sin() * scLatSat.cos() / sinD;
1033 final double cosAz = (scLatSat.sin() - cosD * scLatRec.sin()) / (sinD * scLatRec.cos());
1034 final double sinLatP = scLatRec.sin() * scDeltaP.cos() - scLatRec.cos() * scDeltaP.sin() * cosAz;
1035 final double cosLatP = FastMath.sqrt(1.0 - sinLatP * sinLatP);
1036 this.latP = FastMath.atan2(sinLatP, cosLatP);
1037
1038
1039 final double sinLonP = -sinAz * scDeltaP.sin() / cosLatP;
1040 final double cosLonP = (scDeltaP.cos() - scLatRec.sin() * sinLatP) / (scLatRec.cos() * cosLatP);
1041 this.lonP = FastMath.atan2(sinLonP, cosLonP) + lon1;
1042
1043 }
1044
1045
1046 this.scLatP = FastMath.sinCos(latP);
1047
1048 final SinCos scLon = FastMath.sinCos(lon2 - lonP);
1049
1050 final double psi = greatCircleAngle(scLatSat, scLon);
1051 final SinCos scPsi = FastMath.sinCos(psi);
1052 if (FastMath.abs(FastMath.abs(latP) - 0.5 * FastMath.PI) < THRESHOLD) {
1053
1054 this.sinAzP = 0.0;
1055 if (latP < 0.0) {
1056 this.cosAzP = 1;
1057 } else {
1058 this.cosAzP = -1;
1059 }
1060 } else {
1061
1062 this.sinAzP = scLatSat.cos() * scLon.sin() / scPsi.sin();
1063 this.cosAzP = (scLatSat.sin() - scLatP.sin() * scPsi.cos()) / (scLatP.cos() * scPsi.sin());
1064 }
1065
1066
1067 this.s1 = FastMath.sqrt(r1 * r1 - rp * rp);
1068 this.s2 = FastMath.sqrt(r2 * r2 - rp * rp);
1069 }
1070
1071
1072
1073
1074
1075 public double getS1() {
1076 return s1;
1077 }
1078
1079
1080
1081
1082
1083 public double getS2() {
1084 return s2;
1085 }
1086
1087
1088
1089
1090
1091 public double getRadius() {
1092 return rp;
1093 }
1094
1095
1096
1097
1098
1099 public double getLatitude() {
1100 return latP;
1101 }
1102
1103
1104
1105
1106
1107 public double getLongitude() {
1108 return lonP;
1109 }
1110
1111
1112
1113
1114
1115 public double getSineAz() {
1116 return sinAzP;
1117 }
1118
1119
1120
1121
1122
1123 public double getCosineAz() {
1124 return cosAzP;
1125 }
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136 private double greatCircleAngle(final SinCos scLat, final SinCos scLon) {
1137 if (FastMath.abs(FastMath.abs(latP) - 0.5 * FastMath.PI) < THRESHOLD) {
1138 return FastMath.abs(FastMath.asin(scLat.sin()) - latP);
1139 } else {
1140 final double cosPhi = scLatP.sin() * scLat.sin() +
1141 scLatP.cos() * scLat.cos() * scLon.cos();
1142 final double sinPhi = FastMath.sqrt(1.0 - cosPhi * cosPhi);
1143 return FastMath.atan2(sinPhi, cosPhi);
1144 }
1145 }
1146 }
1147
1148
1149
1150
1151
1152 private static class FieldRay <T extends CalculusFieldElement<T>> {
1153
1154
1155 private static final double THRESHOLD = 1.0e-10;
1156
1157
1158 private final T s1;
1159
1160
1161 private final T s2;
1162
1163
1164 private final T rp;
1165
1166
1167 private final T latP;
1168
1169
1170 private final T lonP;
1171
1172
1173 private final FieldSinCos<T> scLatP;
1174
1175
1176 private final T sinAzP;
1177
1178
1179 private final T cosAzP;
1180
1181
1182
1183
1184
1185
1186
1187 FieldRay(final Field<T> field, final FieldGeodeticPoint<T> recP, final FieldGeodeticPoint<T> satP) {
1188
1189
1190 final T r1 = recP.getAltitude().add(RE);
1191 final T r2 = satP.getAltitude().add(RE);
1192
1193
1194 final T pi = r1.getPi();
1195 final T lat1 = recP.getLatitude();
1196 final T lat2 = satP.getLatitude();
1197 final T lon1 = recP.getLongitude();
1198 final T lon2 = satP.getLongitude();
1199 final FieldSinCos<T> scLatSat = FastMath.sinCos(lat2);
1200 final FieldSinCos<T> scLatRec = FastMath.sinCos(lat1);
1201
1202
1203 final T cosD = scLatRec.sin().multiply(scLatSat.sin()).
1204 add(scLatRec.cos().multiply(scLatSat.cos()).multiply(FastMath.cos(lon2.subtract(lon1))));
1205 final T sinD = FastMath.sqrt(cosD.multiply(cosD).negate().add(1.0));
1206 final T z = FastMath.atan2(sinD, cosD.subtract(r1.divide(r2)));
1207
1208
1209 this.rp = r1.multiply(FastMath.sin(z));
1210
1211
1212 if (FastMath.abs(FastMath.abs(lat1).subtract(pi.multiply(0.5)).getReal()) < THRESHOLD) {
1213
1214
1215 if (lat1.getReal() < 0) {
1216 this.latP = z.negate();
1217 } else {
1218 this.latP = z;
1219 }
1220
1221
1222 if (z.getReal() < 0) {
1223 this.lonP = lon2;
1224 } else {
1225 this.lonP = lon2.add(pi);
1226 }
1227
1228 } else {
1229
1230
1231 final T deltaP = z.negate().add(pi.multiply(0.5));
1232 final FieldSinCos<T> scDeltaP = FastMath.sinCos(deltaP);
1233 final T sinAz = FastMath.sin(lon2.subtract(lon1)).multiply(scLatSat.cos()).divide(sinD);
1234 final T cosAz = scLatSat.sin().subtract(cosD.multiply(scLatRec.sin())).divide(sinD.multiply(scLatRec.cos()));
1235 final T sinLatP = scLatRec.sin().multiply(scDeltaP.cos()).subtract(scLatRec.cos().multiply(scDeltaP.sin()).multiply(cosAz));
1236 final T cosLatP = FastMath.sqrt(sinLatP.multiply(sinLatP).negate().add(1.0));
1237 this.latP = FastMath.atan2(sinLatP, cosLatP);
1238
1239
1240 final T sinLonP = sinAz.negate().multiply(scDeltaP.sin()).divide(cosLatP);
1241 final T cosLonP = scDeltaP.cos().subtract(scLatRec.sin().multiply(sinLatP)).divide(scLatRec.cos().multiply(cosLatP));
1242 this.lonP = FastMath.atan2(sinLonP, cosLonP).add(lon1);
1243
1244 }
1245
1246
1247 this.scLatP = FastMath.sinCos(latP);
1248
1249 final FieldSinCos<T> scLon = FastMath.sinCos(lon2.subtract(lonP));
1250
1251 final T psi = greatCircleAngle(scLatSat, scLon);
1252 final FieldSinCos<T> scPsi = FastMath.sinCos(psi);
1253 if (FastMath.abs(FastMath.abs(latP).subtract(pi.multiply(0.5)).getReal()) < THRESHOLD) {
1254
1255 this.sinAzP = field.getZero();
1256 if (latP.getReal() < 0.0) {
1257 this.cosAzP = field.getOne();
1258 } else {
1259 this.cosAzP = field.getOne().negate();
1260 }
1261 } else {
1262
1263 this.sinAzP = scLatSat.cos().multiply(scLon.sin()).divide(scPsi.sin());
1264 this.cosAzP = scLatSat.sin().subtract(scLatP.sin().multiply(scPsi.cos())).divide(scLatP.cos().multiply(scPsi.sin()));
1265 }
1266
1267
1268 this.s1 = FastMath.sqrt(r1.multiply(r1).subtract(rp.multiply(rp)));
1269 this.s2 = FastMath.sqrt(r2.multiply(r2).subtract(rp.multiply(rp)));
1270 }
1271
1272
1273
1274
1275
1276 public T getS1() {
1277 return s1;
1278 }
1279
1280
1281
1282
1283
1284 public T getS2() {
1285 return s2;
1286 }
1287
1288
1289
1290
1291
1292 public T getRadius() {
1293 return rp;
1294 }
1295
1296
1297
1298
1299
1300 public T getLatitude() {
1301 return latP;
1302 }
1303
1304
1305
1306
1307
1308 public T getLongitude() {
1309 return lonP;
1310 }
1311
1312
1313
1314
1315
1316 public T getSineAz() {
1317 return sinAzP;
1318 }
1319
1320
1321
1322
1323
1324 public T getCosineAz() {
1325 return cosAzP;
1326 }
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337 private T greatCircleAngle(final FieldSinCos<T> scLat, final FieldSinCos<T> scLon) {
1338 if (FastMath.abs(FastMath.abs(latP).getReal() - 0.5 * FastMath.PI) < THRESHOLD) {
1339 return FastMath.abs(FastMath.asin(scLat.sin()).subtract(latP));
1340 } else {
1341 final T cosPhi = scLatP.sin().multiply(scLat.sin()).add(
1342 scLatP.cos().multiply(scLat.cos()).multiply(scLon.cos()));
1343 final T sinPhi = FastMath.sqrt(cosPhi.multiply(cosPhi).negate().add(1.0));
1344 return FastMath.atan2(sinPhi, cosPhi);
1345 }
1346 }
1347 }
1348
1349
1350 private static class Segment {
1351
1352
1353 private final double[] latitudes;
1354
1355
1356 private final double[] longitudes;
1357
1358
1359 private final double[] heights;
1360
1361
1362 private final double deltaN;
1363
1364
1365
1366
1367
1368
1369 Segment(final int n, final Ray ray) {
1370
1371 final double s1 = ray.getS1();
1372 final double s2 = ray.getS2();
1373
1374
1375 this.deltaN = (s2 - s1) / n;
1376
1377
1378 final double[] s = getSegments(n, s1, s2);
1379
1380
1381 final double rp = ray.getRadius();
1382 final SinCos scLatP = FastMath.sinCos(ray.getLatitude());
1383
1384
1385 final int size = s.length;
1386 heights = new double[size];
1387 latitudes = new double[size];
1388 longitudes = new double[size];
1389 for (int i = 0; i < size; i++) {
1390
1391 heights[i] = FastMath.sqrt(s[i] * s[i] + rp * rp) - RE;
1392
1393
1394 final double tanDs = s[i] / rp;
1395 final double cosDs = 1.0 / FastMath.sqrt(1.0 + tanDs * tanDs);
1396 final double sinDs = tanDs * cosDs;
1397
1398
1399 final double sinLatS = scLatP.sin() * cosDs + scLatP.cos() * sinDs * ray.getCosineAz();
1400 final double cosLatS = FastMath.sqrt(1.0 - sinLatS * sinLatS);
1401 latitudes[i] = FastMath.atan2(sinLatS, cosLatS);
1402
1403
1404 final double sinLonS = sinDs * ray.getSineAz() * scLatP.cos();
1405 final double cosLonS = cosDs - scLatP.sin() * sinLatS;
1406 longitudes[i] = FastMath.atan2(sinLonS, cosLonS) + ray.getLongitude();
1407 }
1408 }
1409
1410
1411
1412
1413
1414
1415
1416
1417 private double[] getSegments(final int n, final double s1, final double s2) {
1418
1419 final double g = 0.5773502691896 * deltaN;
1420
1421 final double y = s1 + (deltaN - g) * 0.5;
1422 final double[] segments = new double[2 * n];
1423 int index = 0;
1424 for (int i = 0; i < n; i++) {
1425
1426 segments[index] = y + i * deltaN;
1427 index++;
1428 segments[index] = y + i * deltaN + g;
1429 index++;
1430 }
1431 return segments;
1432 }
1433
1434
1435
1436
1437
1438 public double[] getLatitudes() {
1439 return latitudes;
1440 }
1441
1442
1443
1444
1445
1446 public double[] getLongitudes() {
1447 return longitudes;
1448 }
1449
1450
1451
1452
1453
1454 public double[] getHeights() {
1455 return heights;
1456 }
1457
1458
1459
1460
1461
1462 public double getInterval() {
1463 return deltaN;
1464 }
1465 }
1466
1467
1468 private static class FieldSegment <T extends CalculusFieldElement<T>> {
1469
1470
1471 private final T[] latitudes;
1472
1473
1474 private final T[] longitudes;
1475
1476
1477 private final T[] heights;
1478
1479
1480 private final T deltaN;
1481
1482
1483
1484
1485
1486
1487
1488 FieldSegment(final Field<T> field, final int n, final FieldRay<T> ray) {
1489
1490 final T s1 = ray.getS1();
1491 final T s2 = ray.getS2();
1492
1493
1494 this.deltaN = s2.subtract(s1).divide(n);
1495
1496
1497 final T[] s = getSegments(field, n, s1, s2);
1498
1499
1500 final T rp = ray.getRadius();
1501 final FieldSinCos<T> scLatP = FastMath.sinCos(ray.getLatitude());
1502
1503
1504 final int size = s.length;
1505 heights = MathArrays.buildArray(field, size);
1506 latitudes = MathArrays.buildArray(field, size);
1507 longitudes = MathArrays.buildArray(field, size);
1508 for (int i = 0; i < size; i++) {
1509
1510 heights[i] = FastMath.sqrt(s[i].multiply(s[i]).add(rp.multiply(rp))).subtract(RE);
1511
1512
1513 final T tanDs = s[i].divide(rp);
1514 final T cosDs = FastMath.sqrt(tanDs.multiply(tanDs).add(1.0)).reciprocal();
1515 final T sinDs = tanDs.multiply(cosDs);
1516
1517
1518 final T sinLatS = scLatP.sin().multiply(cosDs).add(scLatP.cos().multiply(sinDs).multiply(ray.getCosineAz()));
1519 final T cosLatS = FastMath.sqrt(sinLatS.multiply(sinLatS).negate().add(1.0));
1520 latitudes[i] = FastMath.atan2(sinLatS, cosLatS);
1521
1522
1523 final T sinLonS = sinDs.multiply(ray.getSineAz()).multiply(scLatP.cos());
1524 final T cosLonS = cosDs.subtract(scLatP.sin().multiply(sinLatS));
1525 longitudes[i] = FastMath.atan2(sinLonS, cosLonS).add(ray.getLongitude());
1526 }
1527 }
1528
1529
1530
1531
1532
1533
1534
1535
1536
1537 private T[] getSegments(final Field<T> field, final int n, final T s1, final T s2) {
1538
1539 final T g = deltaN.multiply(0.5773502691896);
1540
1541 final T y = s1.add(deltaN.subtract(g).multiply(0.5));
1542 final T[] segments = MathArrays.buildArray(field, 2 * n);
1543 int index = 0;
1544 for (int i = 0; i < n; i++) {
1545
1546 segments[index] = y.add(deltaN.multiply(i));
1547 index++;
1548 segments[index] = y.add(deltaN.multiply(i)).add(g);
1549 index++;
1550 }
1551 return segments;
1552 }
1553
1554
1555
1556
1557
1558 public T[] getLatitudes() {
1559 return latitudes;
1560 }
1561
1562
1563
1564
1565
1566 public T[] getLongitudes() {
1567 return longitudes;
1568 }
1569
1570
1571
1572
1573
1574 public T[] getHeights() {
1575 return heights;
1576 }
1577
1578
1579
1580
1581
1582 public T getInterval() {
1583 return deltaN;
1584 }
1585 }
1586
1587 }