1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.data;
18
19 import java.util.HashMap;
20 import java.util.Map;
21
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.util.MathArrays;
24 import org.hipparchus.util.MathUtils;
25 import org.hipparchus.util.MathUtils.SumAndResidual;
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41 public class PoissonSeries {
42
43
44 private final PolynomialNutation polynomial;
45
46
47 private final Map<Long, SeriesTerm> series;
48
49
50
51
52
53 public PoissonSeries(final PolynomialNutation polynomial, final Map<Long, SeriesTerm> series) {
54 this.polynomial = polynomial;
55 this.series = series;
56 }
57
58
59
60
61 public PolynomialNutation getPolynomial() {
62 return polynomial;
63 }
64
65
66
67
68 public int getNonPolynomialSize() {
69 return series.size();
70 }
71
72
73
74
75
76 public double value(final BodiesElements elements) {
77
78
79 final double p = polynomial.value(elements.getTC());
80
81
82 double npHigh = 0;
83 double npLow = 0;
84 for (final Map.Entry<Long, SeriesTerm> entry : series.entrySet()) {
85 final double v = entry.getValue().value(elements)[0];
86
87 final SumAndResidual sumAndResidual = MathUtils.twoSum(npHigh, v);
88 npHigh = sumAndResidual.getSum();
89 npLow += sumAndResidual.getResidual();
90 }
91
92
93 return p + (npHigh + npLow);
94
95 }
96
97
98
99
100
101
102 public <T extends CalculusFieldElement<T>> T value(final FieldBodiesElements<T> elements) {
103
104
105 final T tc = elements.getTC();
106 final T p = polynomial.value(tc);
107
108
109 T sum = tc.getField().getZero();
110 for (final Map.Entry<Long, SeriesTerm> entry : series.entrySet()) {
111 sum = sum.add(entry.getValue().value(elements)[0]);
112 }
113
114
115 return p.add(sum);
116
117 }
118
119
120
121
122
123 public interface CompiledSeries {
124
125
126
127
128
129 double[] value(BodiesElements elements);
130
131
132
133
134
135 double[] derivative(BodiesElements elements);
136
137
138
139
140
141
142 <S extends CalculusFieldElement<S>> S[] value(FieldBodiesElements<S> elements);
143
144
145
146
147
148
149 <S extends CalculusFieldElement<S>> S[] derivative(FieldBodiesElements<S> elements);
150
151 }
152
153
154
155
156
157
158 @SafeVarargs
159 public static CompiledSeries compile(final PoissonSeries... poissonSeries) {
160
161
162 final PolynomialNutation[] polynomials = new PolynomialNutation[poissonSeries.length];
163 for (int i = 0; i < polynomials.length; ++i) {
164 polynomials[i] = poissonSeries[i].polynomial;
165 }
166
167
168 final Map<Long, SeriesTerm> joinedMap = new HashMap<Long, SeriesTerm>();
169 for (final PoissonSeries ps : poissonSeries) {
170 for (Map.Entry<Long, SeriesTerm> entry : ps.series.entrySet()) {
171 final long key = entry.getKey();
172 if (!joinedMap.containsKey(key)) {
173
174
175 final int[] m = NutationCodec.decode(key);
176
177
178 final SeriesTerm term =
179 SeriesTerm.buildTerm(m[0],
180 m[1], m[2], m[3], m[4], m[5],
181 m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13], m[14]);
182 term.add(poissonSeries.length - 1, -1, Double.NaN, Double.NaN);
183
184
185 joinedMap.put(key, term);
186
187 }
188 }
189 }
190
191
192
193 for (int i = 0; i < poissonSeries.length; ++i) {
194 for (final Map.Entry<Long, SeriesTerm> entry : poissonSeries[i].series.entrySet()) {
195 final SeriesTerm singleTerm = entry.getValue();
196 final SeriesTerm joinedTerm = joinedMap.get(entry.getKey());
197 for (int degree = 0; degree <= singleTerm.getDegree(0); ++degree) {
198 joinedTerm.add(i, degree,
199 singleTerm.getSinCoeff(0, degree),
200 singleTerm.getCosCoeff(0, degree));
201 }
202 }
203 }
204
205
206 final SeriesTerm[] joinedTerms = new SeriesTerm[joinedMap.size()];
207 int index = 0;
208 for (final Map.Entry<Long, SeriesTerm> entry : joinedMap.entrySet()) {
209 joinedTerms[index++] = entry.getValue();
210 }
211
212 return new CompiledSeries() {
213
214
215 @Override
216 public double[] value(final BodiesElements elements) {
217
218
219 final double[] npHigh = new double[polynomials.length];
220 final double[] npLow = new double[polynomials.length];
221 for (final SeriesTerm term : joinedTerms) {
222 final double[] termValue = term.value(elements);
223 for (int i = 0; i < termValue.length; ++i) {
224
225 final SumAndResidual sumAndResidual = MathUtils.twoSum(npHigh[i], termValue[i]);
226 npHigh[i] = sumAndResidual.getSum();
227 npLow[i] += sumAndResidual.getResidual();
228 }
229 }
230
231
232 for (int i = 0; i < npHigh.length; ++i) {
233 npHigh[i] += npLow[i] + polynomials[i].value(elements.getTC());
234 }
235 return npHigh;
236
237 }
238
239
240 @Override
241 public double[] derivative(final BodiesElements elements) {
242
243
244 final double[] v = new double[polynomials.length];
245 for (final SeriesTerm term : joinedTerms) {
246 final double[] termDerivative = term.derivative(elements);
247 for (int i = 0; i < termDerivative.length; ++i) {
248 v[i] += termDerivative[i];
249 }
250 }
251
252
253 for (int i = 0; i < v.length; ++i) {
254 v[i] += polynomials[i].derivative(elements.getTC());
255 }
256 return v;
257
258 }
259
260
261 @Override
262 public <S extends CalculusFieldElement<S>> S[] value(final FieldBodiesElements<S> elements) {
263
264
265 final S[] v = MathArrays.buildArray(elements.getTC().getField(), polynomials.length);
266 for (final SeriesTerm term : joinedTerms) {
267 final S[] termValue = term.value(elements);
268 for (int i = 0; i < termValue.length; ++i) {
269 v[i] = v[i].add(termValue[i]);
270 }
271 }
272
273
274 final S tc = elements.getTC();
275 for (int i = 0; i < v.length; ++i) {
276 v[i] = v[i].add(polynomials[i].value(tc));
277 }
278 return v;
279
280 }
281
282
283 @Override
284 public <S extends CalculusFieldElement<S>> S[] derivative(final FieldBodiesElements<S> elements) {
285
286
287 final S[] v = MathArrays.buildArray(elements.getTC().getField(), polynomials.length);
288 for (final SeriesTerm term : joinedTerms) {
289 final S[] termDerivative = term.derivative(elements);
290 for (int i = 0; i < termDerivative.length; ++i) {
291 v[i] = v[i].add(termDerivative[i]);
292 }
293 }
294
295
296 final S tc = elements.getTC();
297 for (int i = 0; i < v.length; ++i) {
298 v[i] = v[i].add(polynomials[i].derivative(tc));
299 }
300 return v;
301
302 }
303
304 };
305
306 }
307
308 }