1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces.gravity;
18
19
20 import java.util.Collections;
21 import java.util.List;
22 import java.util.stream.Stream;
23
24 import org.hipparchus.CalculusFieldElement;
25 import org.hipparchus.Field;
26 import org.hipparchus.analysis.differentiation.DerivativeStructure;
27 import org.hipparchus.analysis.differentiation.Gradient;
28 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29 import org.hipparchus.geometry.euclidean.threed.SphericalCoordinates;
30 import org.hipparchus.geometry.euclidean.threed.Vector3D;
31 import org.hipparchus.linear.Array2DRowRealMatrix;
32 import org.hipparchus.linear.RealMatrix;
33 import org.hipparchus.util.FastMath;
34 import org.hipparchus.util.MathArrays;
35 import org.orekit.forces.AbstractForceModel;
36 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
37 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider.NormalizedSphericalHarmonics;
38 import org.orekit.forces.gravity.potential.TideSystem;
39 import org.orekit.forces.gravity.potential.TideSystemProvider;
40 import org.orekit.frames.Frame;
41 import org.orekit.frames.StaticTransform;
42 import org.orekit.propagation.FieldSpacecraftState;
43 import org.orekit.propagation.SpacecraftState;
44 import org.orekit.propagation.events.EventDetector;
45 import org.orekit.propagation.events.FieldEventDetector;
46 import org.orekit.time.AbsoluteDate;
47 import org.orekit.time.FieldAbsoluteDate;
48 import org.orekit.utils.FieldPVCoordinates;
49 import org.orekit.utils.ParameterDriver;
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78 public class HolmesFeatherstoneAttractionModel extends AbstractForceModel implements TideSystemProvider {
79
80
81
82
83
84 private static final int SCALING = 930;
85
86
87
88
89
90
91
92 private static final double MU_SCALE = FastMath.scalb(1.0, 32);
93
94
95 private final ParameterDriver gmParameterDriver;
96
97
98 private final NormalizedSphericalHarmonicsProvider provider;
99
100
101 private final Frame bodyFrame;
102
103
104 private final double[] gnmOj;
105
106
107 private final double[] hnmOj;
108
109
110 private final double[] enm;
111
112
113 private final double[] sectorial;
114
115
116
117
118
119
120 public HolmesFeatherstoneAttractionModel(final Frame centralBodyFrame,
121 final NormalizedSphericalHarmonicsProvider provider) {
122
123 gmParameterDriver = new ParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT,
124 provider.getMu(), MU_SCALE, 0.0, Double.POSITIVE_INFINITY);
125
126 this.provider = provider;
127 this.bodyFrame = centralBodyFrame;
128
129
130
131 final int degree = provider.getMaxDegree();
132 final int size = FastMath.max(0, degree * (degree + 1) / 2 - 1);
133 gnmOj = new double[size];
134 hnmOj = new double[size];
135 enm = new double[size];
136
137
138
139
140
141 int index = 0;
142 for (int m = degree; m >= 0; --m) {
143 final int j = (m == 0) ? 2 : 1;
144 for (int n = FastMath.max(2, m + 1); n <= degree; ++n) {
145 final double f = (n - m) * (n + m + 1);
146 gnmOj[index] = 2 * (m + 1) / FastMath.sqrt(j * f);
147 hnmOj[index] = FastMath.sqrt((n + m + 2) * (n - m - 1) / (j * f));
148 enm[index] = FastMath.sqrt(f / j);
149 ++index;
150 }
151 }
152
153
154 sectorial = new double[degree + 1];
155 sectorial[0] = FastMath.scalb(1.0, -SCALING);
156 if (degree > 0) {
157 sectorial[1] = FastMath.sqrt(3) * sectorial[0];
158 }
159 for (int m = 2; m < sectorial.length; ++m) {
160 sectorial[m] = FastMath.sqrt((2 * m + 1) / (2.0 * m)) * sectorial[m - 1];
161 }
162
163 }
164
165
166 @Override
167 public boolean dependsOnPositionOnly() {
168 return true;
169 }
170
171
172 public TideSystem getTideSystem() {
173 return provider.getTideSystem();
174 }
175
176
177
178
179 public double getMu() {
180 return gmParameterDriver.getValue();
181 }
182
183
184
185
186
187
188
189 public double value(final AbsoluteDate date, final Vector3D position,
190 final double mu) {
191 return mu / position.getNorm() + nonCentralPart(date, position, mu);
192 }
193
194
195
196
197
198
199
200 public double nonCentralPart(final AbsoluteDate date, final Vector3D position, final double mu) {
201
202 final int degree = provider.getMaxDegree();
203 final int order = provider.getMaxOrder();
204 final NormalizedSphericalHarmonics harmonics = provider.onDate(date);
205
206
207 double[] pnm0Plus2 = new double[degree + 1];
208 double[] pnm0Plus1 = new double[degree + 1];
209 double[] pnm0 = new double[degree + 1];
210
211
212 final double x = position.getX();
213 final double y = position.getY();
214 final double z = position.getZ();
215 final double x2 = x * x;
216 final double y2 = y * y;
217 final double z2 = z * z;
218 final double r2 = x2 + y2 + z2;
219 final double r = FastMath.sqrt (r2);
220 final double rho = FastMath.sqrt(x2 + y2);
221 final double t = z / r;
222 final double u = rho / r;
223 final double tOu = z / rho;
224
225
226 final double[] aOrN = createDistancePowersArray(provider.getAe() / r);
227
228
229 final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho);
230
231
232 int index = 0;
233 double value = 0;
234 for (int m = degree; m >= 0; --m) {
235
236
237 index = computeTesseral(m, degree, index, t, u, tOu,
238 pnm0Plus2, pnm0Plus1, null, pnm0, null, null);
239
240 if (m <= order) {
241
242
243
244 double sumDegreeS = 0;
245 double sumDegreeC = 0;
246 for (int n = FastMath.max(2, m); n <= degree; ++n) {
247 sumDegreeS += pnm0[n] * aOrN[n] * harmonics.getNormalizedSnm(n, m);
248 sumDegreeC += pnm0[n] * aOrN[n] * harmonics.getNormalizedCnm(n, m);
249 }
250
251
252 value = value * u + cosSinLambda[1][m] * sumDegreeS + cosSinLambda[0][m] * sumDegreeC;
253
254 }
255
256
257 final double[] tmp = pnm0Plus2;
258 pnm0Plus2 = pnm0Plus1;
259 pnm0Plus1 = pnm0;
260 pnm0 = tmp;
261
262 }
263
264
265 value = FastMath.scalb(value, SCALING);
266
267
268 return mu * value / r;
269
270 }
271
272
273
274
275
276
277
278 public double[] gradient(final AbsoluteDate date, final Vector3D position, final double mu) {
279
280 final int degree = provider.getMaxDegree();
281 final int order = provider.getMaxOrder();
282 final NormalizedSphericalHarmonics harmonics = provider.onDate(date);
283
284
285 double[] pnm0Plus2 = new double[degree + 1];
286 double[] pnm0Plus1 = new double[degree + 1];
287 double[] pnm0 = new double[degree + 1];
288 final double[] pnm1 = new double[degree + 1];
289
290
291 final double x = position.getX();
292 final double y = position.getY();
293 final double z = position.getZ();
294 final double x2 = x * x;
295 final double y2 = y * y;
296 final double z2 = z * z;
297 final double r2 = x2 + y2 + z2;
298 final double r = FastMath.sqrt (r2);
299 final double rho2 = x2 + y2;
300 final double rho = FastMath.sqrt(rho2);
301 final double t = z / r;
302 final double u = rho / r;
303 final double tOu = z / rho;
304
305
306 final double[] aOrN = createDistancePowersArray(provider.getAe() / r);
307
308
309 final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho);
310
311
312 int index = 0;
313 double value = 0;
314 final double[] gradient = new double[3];
315 for (int m = degree; m >= 0; --m) {
316
317
318 index = computeTesseral(m, degree, index, t, u, tOu,
319 pnm0Plus2, pnm0Plus1, null, pnm0, pnm1, null);
320
321 if (m <= order) {
322
323
324
325 double sumDegreeS = 0;
326 double sumDegreeC = 0;
327 double dSumDegreeSdR = 0;
328 double dSumDegreeCdR = 0;
329 double dSumDegreeSdTheta = 0;
330 double dSumDegreeCdTheta = 0;
331 for (int n = FastMath.max(2, m); n <= degree; ++n) {
332 final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m);
333 final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m);
334 final double nOr = n / r;
335 final double s0 = pnm0[n] * qSnm;
336 final double c0 = pnm0[n] * qCnm;
337 final double s1 = pnm1[n] * qSnm;
338 final double c1 = pnm1[n] * qCnm;
339 sumDegreeS += s0;
340 sumDegreeC += c0;
341 dSumDegreeSdR -= nOr * s0;
342 dSumDegreeCdR -= nOr * c0;
343 dSumDegreeSdTheta += s1;
344 dSumDegreeCdTheta += c1;
345 }
346
347
348
349
350
351 final double sML = cosSinLambda[1][m];
352 final double cML = cosSinLambda[0][m];
353 value = value * u + sML * sumDegreeS + cML * sumDegreeC;
354 gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
355 gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
356 gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
357
358 }
359
360
361 final double[] tmp = pnm0Plus2;
362 pnm0Plus2 = pnm0Plus1;
363 pnm0Plus1 = pnm0;
364 pnm0 = tmp;
365
366 }
367
368
369 value = FastMath.scalb(value, SCALING);
370 gradient[0] = FastMath.scalb(gradient[0], SCALING);
371 gradient[1] = FastMath.scalb(gradient[1], SCALING);
372 gradient[2] = FastMath.scalb(gradient[2], SCALING);
373
374
375 final double muOr = mu / r;
376 value *= muOr;
377 gradient[0] = muOr * gradient[0] - value / r;
378 gradient[1] *= muOr;
379 gradient[2] *= muOr;
380
381
382 return new SphericalCoordinates(position).toCartesianGradient(gradient);
383
384 }
385
386
387
388
389
390
391
392
393 public <T extends CalculusFieldElement<T>> T[] gradient(final FieldAbsoluteDate<T> date, final FieldVector3D<T> position,
394 final T mu) {
395
396 final int degree = provider.getMaxDegree();
397 final int order = provider.getMaxOrder();
398 final NormalizedSphericalHarmonics harmonics = provider.onDate(date.toAbsoluteDate());
399 final T zero = date.getField().getZero();
400
401 T[] pnm0Plus2 = MathArrays.buildArray(date.getField(), degree + 1);
402 T[] pnm0Plus1 = MathArrays.buildArray(date.getField(), degree + 1);
403 T[] pnm0 = MathArrays.buildArray(date.getField(), degree + 1);
404 final T[] pnm1 = MathArrays.buildArray(date.getField(), degree + 1);
405
406
407 final T x = position.getX();
408 final T y = position.getY();
409 final T z = position.getZ();
410 final T x2 = x.multiply(x);
411 final T y2 = y.multiply(y);
412 final T z2 = z.multiply(z);
413 final T r2 = x2.add(y2).add(z2);
414 final T r = r2.sqrt();
415 final T rho2 = x2.add(y2);
416 final T rho = rho2.sqrt();
417 final T t = z.divide(r);
418 final T u = rho.divide(r);
419 final T tOu = z.divide(rho);
420
421
422 final T[] aOrN = createDistancePowersArray(r.reciprocal().multiply(provider.getAe()));
423
424
425 final T[][] cosSinLambda = createCosSinArrays(rho.reciprocal().multiply(position.getX()), rho.reciprocal().multiply(position.getY()));
426
427 int index = 0;
428 T value = zero;
429 final T[] gradient = MathArrays.buildArray(zero.getField(), 3);
430 for (int m = degree; m >= 0; --m) {
431
432
433 index = computeTesseral(m, degree, index, t, u, tOu,
434 pnm0Plus2, pnm0Plus1, null, pnm0, pnm1, null);
435 if (m <= order) {
436
437
438
439 T sumDegreeS = zero;
440 T sumDegreeC = zero;
441 T dSumDegreeSdR = zero;
442 T dSumDegreeCdR = zero;
443 T dSumDegreeSdTheta = zero;
444 T dSumDegreeCdTheta = zero;
445 for (int n = FastMath.max(2, m); n <= degree; ++n) {
446 final T qSnm = aOrN[n].multiply(harmonics.getNormalizedSnm(n, m));
447 final T qCnm = aOrN[n].multiply(harmonics.getNormalizedCnm(n, m));
448 final T nOr = r.reciprocal().multiply(n);
449 final T s0 = pnm0[n].multiply(qSnm);
450 final T c0 = pnm0[n].multiply(qCnm);
451 final T s1 = pnm1[n].multiply(qSnm);
452 final T c1 = pnm1[n].multiply(qCnm);
453 sumDegreeS = sumDegreeS .add(s0);
454 sumDegreeC = sumDegreeC .add(c0);
455 dSumDegreeSdR = dSumDegreeSdR .subtract(nOr.multiply(s0));
456 dSumDegreeCdR = dSumDegreeCdR .subtract(nOr.multiply(c0));
457 dSumDegreeSdTheta = dSumDegreeSdTheta.add(s1);
458 dSumDegreeCdTheta = dSumDegreeCdTheta.add(c1);
459 }
460
461
462
463
464
465 final T sML = cosSinLambda[1][m];
466 final T cML = cosSinLambda[0][m];
467 value = value .multiply(u).add(sML.multiply(sumDegreeS )).add(cML.multiply(sumDegreeC));
468 gradient[0] = gradient[0].multiply(u).add(sML.multiply(dSumDegreeSdR)).add(cML.multiply(dSumDegreeCdR));
469 gradient[1] = gradient[1].multiply(u).add(cML.multiply(sumDegreeS).subtract(sML.multiply(sumDegreeC)).multiply(m));
470 gradient[2] = gradient[2].multiply(u).add(sML.multiply(dSumDegreeSdTheta)).add(cML.multiply(dSumDegreeCdTheta));
471 }
472
473 final T[] tmp = pnm0Plus2;
474 pnm0Plus2 = pnm0Plus1;
475 pnm0Plus1 = pnm0;
476 pnm0 = tmp;
477
478 }
479
480 value = value.scalb(SCALING);
481 gradient[0] = gradient[0].scalb(SCALING);
482 gradient[1] = gradient[1].scalb(SCALING);
483 gradient[2] = gradient[2].scalb(SCALING);
484
485
486 final T muOr = r.reciprocal().multiply(mu);
487 value = value.multiply(muOr);
488 gradient[0] = muOr.multiply(gradient[0]).subtract(value.divide(r));
489 gradient[1] = gradient[1].multiply(muOr);
490 gradient[2] = gradient[2].multiply(muOr);
491
492
493
494
495 final T rPos = position.getNorm();
496
497 final T xPos = position.getX();
498 final T yPos = position.getY();
499 final T zPos = position.getZ();
500 final T rho2Pos = x.multiply(x).add(y.multiply(y));
501 final T rhoPos = rho2.sqrt();
502 final T r2Pos = rho2.add(z.multiply(z));
503
504 final T[][] jacobianPos = MathArrays.buildArray(zero.getField(), 3, 3);
505
506
507 jacobianPos[0][0] = xPos.divide(rPos);
508 jacobianPos[0][1] = yPos.divide(rPos);
509 jacobianPos[0][2] = zPos.divide(rPos);
510
511
512 jacobianPos[1][0] = yPos.negate().divide(rho2Pos);
513 jacobianPos[1][1] = xPos.divide(rho2Pos);
514
515
516
517 jacobianPos[2][0] = xPos.multiply(zPos).divide(rhoPos.multiply(r2Pos));
518 jacobianPos[2][1] = yPos.multiply(zPos).divide(rhoPos.multiply(r2Pos));
519 jacobianPos[2][2] = rhoPos.negate().divide(r2Pos);
520 final T[] cartGradPos = MathArrays.buildArray(zero.getField(), 3);
521 cartGradPos[0] = gradient[0].multiply(jacobianPos[0][0]).add(gradient[1].multiply(jacobianPos[1][0])).add(gradient[2].multiply(jacobianPos[2][0]));
522 cartGradPos[1] = gradient[0].multiply(jacobianPos[0][1]).add(gradient[1].multiply(jacobianPos[1][1])).add(gradient[2].multiply(jacobianPos[2][1]));
523 cartGradPos[2] = gradient[0].multiply(jacobianPos[0][2]) .add(gradient[2].multiply(jacobianPos[2][2]));
524 return cartGradPos;
525
526 }
527
528
529
530
531
532
533
534 private GradientHessian gradientHessian(final AbsoluteDate date, final Vector3D position, final double mu) {
535
536 final int degree = provider.getMaxDegree();
537 final int order = provider.getMaxOrder();
538 final NormalizedSphericalHarmonics harmonics = provider.onDate(date);
539
540
541 double[] pnm0Plus2 = new double[degree + 1];
542 double[] pnm0Plus1 = new double[degree + 1];
543 double[] pnm0 = new double[degree + 1];
544 double[] pnm1Plus1 = new double[degree + 1];
545 double[] pnm1 = new double[degree + 1];
546 final double[] pnm2 = new double[degree + 1];
547
548
549 final double x = position.getX();
550 final double y = position.getY();
551 final double z = position.getZ();
552 final double x2 = x * x;
553 final double y2 = y * y;
554 final double z2 = z * z;
555 final double r2 = x2 + y2 + z2;
556 final double r = FastMath.sqrt (r2);
557 final double rho2 = x2 + y2;
558 final double rho = FastMath.sqrt(rho2);
559 final double t = z / r;
560 final double u = rho / r;
561 final double tOu = z / rho;
562
563
564 final double[] aOrN = createDistancePowersArray(provider.getAe() / r);
565
566
567 final double[][] cosSinLambda = createCosSinArrays(position.getX() / rho, position.getY() / rho);
568
569
570 int index = 0;
571 double value = 0;
572 final double[] gradient = new double[3];
573 final double[][] hessian = new double[3][3];
574 for (int m = degree; m >= 0; --m) {
575
576
577 index = computeTesseral(m, degree, index, t, u, tOu,
578 pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2);
579
580 if (m <= order) {
581
582
583
584 double sumDegreeS = 0;
585 double sumDegreeC = 0;
586 double dSumDegreeSdR = 0;
587 double dSumDegreeCdR = 0;
588 double dSumDegreeSdTheta = 0;
589 double dSumDegreeCdTheta = 0;
590 double d2SumDegreeSdRdR = 0;
591 double d2SumDegreeSdRdTheta = 0;
592 double d2SumDegreeSdThetadTheta = 0;
593 double d2SumDegreeCdRdR = 0;
594 double d2SumDegreeCdRdTheta = 0;
595 double d2SumDegreeCdThetadTheta = 0;
596 for (int n = FastMath.max(2, m); n <= degree; ++n) {
597 final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m);
598 final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m);
599 final double nOr = n / r;
600 final double nnP1Or2 = nOr * (n + 1) / r;
601 final double s0 = pnm0[n] * qSnm;
602 final double c0 = pnm0[n] * qCnm;
603 final double s1 = pnm1[n] * qSnm;
604 final double c1 = pnm1[n] * qCnm;
605 final double s2 = pnm2[n] * qSnm;
606 final double c2 = pnm2[n] * qCnm;
607 sumDegreeS += s0;
608 sumDegreeC += c0;
609 dSumDegreeSdR -= nOr * s0;
610 dSumDegreeCdR -= nOr * c0;
611 dSumDegreeSdTheta += s1;
612 dSumDegreeCdTheta += c1;
613 d2SumDegreeSdRdR += nnP1Or2 * s0;
614 d2SumDegreeSdRdTheta -= nOr * s1;
615 d2SumDegreeSdThetadTheta += s2;
616 d2SumDegreeCdRdR += nnP1Or2 * c0;
617 d2SumDegreeCdRdTheta -= nOr * c1;
618 d2SumDegreeCdThetadTheta += c2;
619 }
620
621
622 final double sML = cosSinLambda[1][m];
623 final double cML = cosSinLambda[0][m];
624 value = value * u + sML * sumDegreeS + cML * sumDegreeC;
625 gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
626 gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
627 gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
628 hessian[0][0] = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR;
629 hessian[1][0] = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR);
630 hessian[2][0] = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta;
631 hessian[1][1] = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC);
632 hessian[2][1] = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta);
633 hessian[2][2] = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta;
634
635 }
636
637
638 final double[] tmp0 = pnm0Plus2;
639 pnm0Plus2 = pnm0Plus1;
640 pnm0Plus1 = pnm0;
641 pnm0 = tmp0;
642 final double[] tmp1 = pnm1Plus1;
643 pnm1Plus1 = pnm1;
644 pnm1 = tmp1;
645
646 }
647
648
649 value = FastMath.scalb(value, SCALING);
650 for (int i = 0; i < 3; ++i) {
651 gradient[i] = FastMath.scalb(gradient[i], SCALING);
652 for (int j = 0; j <= i; ++j) {
653 hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING);
654 }
655 }
656
657
658
659 final double muOr = mu / r;
660 value *= muOr;
661 gradient[0] = muOr * gradient[0] - value / r;
662 gradient[1] *= muOr;
663 gradient[2] *= muOr;
664 hessian[0][0] = muOr * hessian[0][0] - 2 * gradient[0] / r;
665 hessian[1][0] = muOr * hessian[1][0] - gradient[1] / r;
666 hessian[2][0] = muOr * hessian[2][0] - gradient[2] / r;
667 hessian[1][1] *= muOr;
668 hessian[2][1] *= muOr;
669 hessian[2][2] *= muOr;
670
671
672 final SphericalCoordinates sc = new SphericalCoordinates(position);
673 return new GradientHessian(sc.toCartesianGradient(gradient),
674 sc.toCartesianHessian(hessian, gradient));
675
676
677 }
678
679
680 private static class GradientHessian {
681
682
683 private final double[] gradient;
684
685
686 private final double[][] hessian;
687
688
689
690
691
692
693
694
695 GradientHessian(final double[] gradient, final double[][] hessian) {
696 this.gradient = gradient;
697 this.hessian = hessian;
698 }
699
700
701
702
703 public double[] getGradient() {
704 return gradient;
705 }
706
707
708
709
710 public double[][] getHessian() {
711 return hessian;
712 }
713
714 }
715
716
717
718
719
720 private double[] createDistancePowersArray(final double aOr) {
721
722
723 final double[] aOrN = new double[provider.getMaxDegree() + 1];
724 aOrN[0] = 1;
725 if (provider.getMaxDegree() > 0) {
726 aOrN[1] = aOr;
727 }
728
729
730 for (int n = 2; n < aOrN.length; ++n) {
731 final int p = n / 2;
732 final int q = n - p;
733 aOrN[n] = aOrN[p] * aOrN[q];
734 }
735
736 return aOrN;
737
738 }
739
740
741
742
743
744 private <T extends CalculusFieldElement<T>> T[] createDistancePowersArray(final T aOr) {
745
746
747 final T[] aOrN = MathArrays.buildArray(aOr.getField(), provider.getMaxDegree() + 1);
748 aOrN[0] = aOr.getField().getOne();
749 if (provider.getMaxDegree() > 0) {
750 aOrN[1] = aOr;
751 }
752
753
754 for (int n = 2; n < aOrN.length; ++n) {
755 final int p = n / 2;
756 final int q = n - p;
757 aOrN[n] = aOrN[p].multiply(aOrN[q]);
758 }
759
760 return aOrN;
761
762 }
763
764
765
766
767
768
769
770 private double[][] createCosSinArrays(final double cosLambda, final double sinLambda) {
771
772
773 final double[][] cosSin = new double[2][provider.getMaxOrder() + 1];
774 cosSin[0][0] = 1;
775 cosSin[1][0] = 0;
776 if (provider.getMaxOrder() > 0) {
777 cosSin[0][1] = cosLambda;
778 cosSin[1][1] = sinLambda;
779
780
781 for (int m = 2; m < cosSin[0].length; ++m) {
782
783
784
785
786
787
788 final int p = m / 2;
789 final int q = m - p;
790
791 cosSin[0][m] = cosSin[0][p] * cosSin[0][q] - cosSin[1][p] * cosSin[1][q];
792 cosSin[1][m] = cosSin[1][p] * cosSin[0][q] + cosSin[0][p] * cosSin[1][q];
793 }
794 }
795
796 return cosSin;
797
798 }
799
800
801
802
803
804
805
806
807 private <T extends CalculusFieldElement<T>> T[][] createCosSinArrays(final T cosLambda, final T sinLambda) {
808
809 final T one = cosLambda.getField().getOne();
810 final T zero = cosLambda.getField().getZero();
811
812 final T[][] cosSin = MathArrays.buildArray(one.getField(), 2, provider.getMaxOrder() + 1);
813 cosSin[0][0] = one;
814 cosSin[1][0] = zero;
815 if (provider.getMaxOrder() > 0) {
816 cosSin[0][1] = cosLambda;
817 cosSin[1][1] = sinLambda;
818
819
820 for (int m = 2; m < cosSin[0].length; ++m) {
821
822
823
824
825
826
827 final int p = m / 2;
828 final int q = m - p;
829
830 cosSin[0][m] = cosSin[0][p].multiply(cosSin[0][q]).subtract(cosSin[1][p].multiply(cosSin[1][q]));
831 cosSin[1][m] = cosSin[1][p].multiply(cosSin[0][q]).add(cosSin[0][p].multiply(cosSin[1][q]));
832
833 }
834 }
835
836 return cosSin;
837
838 }
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861 private int computeTesseral(final int m, final int degree, final int index,
862 final double t, final double u, final double tOu,
863 final double[] pnm0Plus2, final double[] pnm0Plus1, final double[] pnm1Plus1,
864 final double[] pnm0, final double[] pnm1, final double[] pnm2) {
865
866 final double u2 = u * u;
867
868
869 int n = FastMath.max(2, m);
870 if (n == m) {
871 pnm0[n] = sectorial[n];
872 ++n;
873 }
874
875
876 int localIndex = index;
877 while (n <= degree) {
878
879
880 pnm0[n] = gnmOj[localIndex] * t * pnm0Plus1[n] - hnmOj[localIndex] * u2 * pnm0Plus2[n];
881
882 ++localIndex;
883 ++n;
884
885 }
886
887 if (pnm1 != null) {
888
889
890 n = FastMath.max(2, m);
891 if (n == m) {
892 pnm1[n] = m * tOu * pnm0[n];
893 ++n;
894 }
895
896
897 localIndex = index;
898 while (n <= degree) {
899
900
901 pnm1[n] = m * tOu * pnm0[n] - enm[localIndex] * u * pnm0Plus1[n];
902
903 ++localIndex;
904 ++n;
905
906 }
907
908 if (pnm2 != null) {
909
910
911 n = FastMath.max(2, m);
912 if (n == m) {
913 pnm2[n] = m * (tOu * pnm1[n] - pnm0[n] / u2);
914 ++n;
915 }
916
917
918 localIndex = index;
919 while (n <= degree) {
920
921
922 pnm2[n] = m * (tOu * pnm1[n] - pnm0[n] / u2) - enm[localIndex] * u * pnm1Plus1[n];
923
924 ++localIndex;
925 ++n;
926
927 }
928
929 }
930
931 }
932
933 return localIndex;
934
935 }
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959 private <T extends CalculusFieldElement<T>> int computeTesseral(final int m, final int degree, final int index,
960 final T t, final T u, final T tOu,
961 final T[] pnm0Plus2, final T[] pnm0Plus1, final T[] pnm1Plus1,
962 final T[] pnm0, final T[] pnm1, final T[] pnm2) {
963
964 final T u2 = u.multiply(u);
965 final T zero = u.getField().getZero();
966
967 int n = FastMath.max(2, m);
968 if (n == m) {
969 pnm0[n] = zero.add(sectorial[n]);
970 ++n;
971 }
972
973
974 int localIndex = index;
975 while (n <= degree) {
976
977
978 pnm0[n] = t.multiply(gnmOj[localIndex]).multiply(pnm0Plus1[n]).subtract(u2.multiply(pnm0Plus2[n]).multiply(hnmOj[localIndex]));
979 ++localIndex;
980 ++n;
981
982 }
983 if (pnm1 != null) {
984
985
986 n = FastMath.max(2, m);
987 if (n == m) {
988 pnm1[n] = tOu.multiply(m).multiply(pnm0[n]);
989 ++n;
990 }
991
992
993 localIndex = index;
994 while (n <= degree) {
995
996
997 pnm1[n] = tOu.multiply(m).multiply(pnm0[n]).subtract(u.multiply(enm[localIndex]).multiply(pnm0Plus1[n]));
998
999 ++localIndex;
1000 ++n;
1001
1002 }
1003
1004 if (pnm2 != null) {
1005
1006
1007 n = FastMath.max(2, m);
1008 if (n == m) {
1009 pnm2[n] = tOu.multiply(pnm1[n]).subtract(pnm0[n].divide(u2)).multiply(m);
1010 ++n;
1011 }
1012
1013
1014 localIndex = index;
1015 while (n <= degree) {
1016
1017
1018 pnm2[n] = tOu.multiply(pnm1[n]).subtract(pnm0[n].divide(u2)).multiply(m).subtract(u.multiply(pnm1Plus1[n]).multiply(enm[localIndex]));
1019 ++localIndex;
1020 ++n;
1021
1022 }
1023
1024 }
1025
1026 }
1027 return localIndex;
1028
1029 }
1030
1031
1032 @Override
1033 public Vector3D acceleration(final SpacecraftState s, final double[] parameters) {
1034
1035 final double mu = parameters[0];
1036
1037
1038 final AbsoluteDate date = s.getDate();
1039 final StaticTransform fromBodyFrame =
1040 bodyFrame.getStaticTransformTo(s.getFrame(), date);
1041 final StaticTransform toBodyFrame = fromBodyFrame.getInverse();
1042 final Vector3D position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition());
1043
1044
1045 return fromBodyFrame.transformVector(new Vector3D(gradient(date, position, mu)));
1046
1047 }
1048
1049
1050 public <T extends CalculusFieldElement<T>> FieldVector3D<T> acceleration(final FieldSpacecraftState<T> s,
1051 final T[] parameters) {
1052
1053 final T mu = parameters[0];
1054
1055
1056 if (isGradientStateDerivative(s)) {
1057 @SuppressWarnings("unchecked")
1058 final FieldVector3D<Gradient> p = (FieldVector3D<Gradient>) s.getPVCoordinates().getPosition();
1059 @SuppressWarnings("unchecked")
1060 final FieldVector3D<T> a = (FieldVector3D<T>) accelerationWrtState(s.getDate().toAbsoluteDate(),
1061 s.getFrame(), p,
1062 (Gradient) mu);
1063 return a;
1064 } else if (isDSStateDerivative(s)) {
1065 @SuppressWarnings("unchecked")
1066 final FieldVector3D<DerivativeStructure> p = (FieldVector3D<DerivativeStructure>) s.getPVCoordinates().getPosition();
1067 @SuppressWarnings("unchecked")
1068 final FieldVector3D<T> a = (FieldVector3D<T>) accelerationWrtState(s.getDate().toAbsoluteDate(),
1069 s.getFrame(), p,
1070 (DerivativeStructure) mu);
1071 return a;
1072 }
1073
1074
1075 final FieldAbsoluteDate<T> date = s.getDate();
1076 final StaticTransform fromBodyFrame =
1077 bodyFrame.getStaticTransformTo(s.getFrame(), date.toAbsoluteDate());
1078 final StaticTransform toBodyFrame = fromBodyFrame.getInverse();
1079 final FieldVector3D<T> position = toBodyFrame.transformPosition(s.getPVCoordinates().getPosition());
1080
1081
1082 return fromBodyFrame.transformVector(new FieldVector3D<>(gradient(date, position, mu)));
1083
1084 }
1085
1086
1087 public Stream<EventDetector> getEventsDetectors() {
1088 return Stream.empty();
1089 }
1090
1091 @Override
1092
1093 public <T extends CalculusFieldElement<T>> Stream<FieldEventDetector<T>> getFieldEventsDetectors(final Field<T> field) {
1094 return Stream.empty();
1095 }
1096
1097
1098
1099
1100
1101
1102
1103 private <T extends CalculusFieldElement<T>> boolean isDSStateDerivative(final FieldSpacecraftState<T> state) {
1104 try {
1105 final DerivativeStructure dsMass = (DerivativeStructure) state.getMass();
1106 final int o = dsMass.getOrder();
1107 final int p = dsMass.getFreeParameters();
1108 if (o != 1 || p < 3) {
1109 return false;
1110 }
1111 @SuppressWarnings("unchecked")
1112 final FieldPVCoordinates<DerivativeStructure> pv = (FieldPVCoordinates<DerivativeStructure>) state.getPVCoordinates();
1113 return isVariable(pv.getPosition().getX(), 0) &&
1114 isVariable(pv.getPosition().getY(), 1) &&
1115 isVariable(pv.getPosition().getZ(), 2);
1116 } catch (ClassCastException cce) {
1117 return false;
1118 }
1119 }
1120
1121
1122
1123
1124
1125
1126
1127 private <T extends CalculusFieldElement<T>> boolean isGradientStateDerivative(final FieldSpacecraftState<T> state) {
1128 try {
1129 final Gradient gMass = (Gradient) state.getMass();
1130 final int p = gMass.getFreeParameters();
1131 if (p < 3) {
1132 return false;
1133 }
1134 @SuppressWarnings("unchecked")
1135 final FieldPVCoordinates<Gradient> pv = (FieldPVCoordinates<Gradient>) state.getPVCoordinates();
1136 return isVariable(pv.getPosition().getX(), 0) &&
1137 isVariable(pv.getPosition().getY(), 1) &&
1138 isVariable(pv.getPosition().getZ(), 2);
1139 } catch (ClassCastException cce) {
1140 return false;
1141 }
1142 }
1143
1144
1145
1146
1147
1148
1149
1150 private boolean isVariable(final DerivativeStructure ds, final int index) {
1151 final double[] derivatives = ds.getAllDerivatives();
1152 boolean check = true;
1153 for (int i = 1; i < derivatives.length; ++i) {
1154 check &= derivatives[i] == ((index + 1 == i) ? 1.0 : 0.0);
1155 }
1156 return check;
1157 }
1158
1159
1160
1161
1162
1163
1164
1165 private boolean isVariable(final Gradient g, final int index) {
1166 final double[] derivatives = g.getGradient();
1167 boolean check = true;
1168 for (int i = 0; i < derivatives.length; ++i) {
1169 check &= derivatives[i] == ((index == i) ? 1.0 : 0.0);
1170 }
1171 return check;
1172 }
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201 private FieldVector3D<DerivativeStructure> accelerationWrtState(final AbsoluteDate date, final Frame frame,
1202 final FieldVector3D<DerivativeStructure> position,
1203 final DerivativeStructure mu) {
1204
1205
1206 final int freeParameters = mu.getFreeParameters();
1207
1208
1209 final StaticTransform fromBodyFrame =
1210 bodyFrame.getStaticTransformTo(frame, date);
1211 final StaticTransform toBodyFrame = fromBodyFrame.getInverse();
1212 final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
1213
1214
1215 final GradientHessian gh = gradientHessian(date, positionBody, mu.getReal());
1216
1217
1218 final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
1219
1220
1221 final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
1222 final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
1223 final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
1224
1225
1226 final double[] derivatives = new double[freeParameters + 1];
1227 final DerivativeStructure[] accDer = new DerivativeStructure[3];
1228 for (int i = 0; i < 3; ++i) {
1229
1230
1231 derivatives[0] = gInertial[i];
1232
1233
1234 derivatives[1] = hInertial.getEntry(i, 0);
1235 derivatives[2] = hInertial.getEntry(i, 1);
1236 derivatives[3] = hInertial.getEntry(i, 2);
1237
1238
1239 if (derivatives.length > 4 && isVariable(mu, 3)) {
1240 derivatives[4] = gInertial[i] / mu.getReal();
1241 }
1242
1243 accDer[i] = position.getX().getFactory().build(derivatives);
1244
1245 }
1246
1247 return new FieldVector3D<>(accDer);
1248
1249 }
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278 private FieldVector3D<Gradient> accelerationWrtState(final AbsoluteDate date, final Frame frame,
1279 final FieldVector3D<Gradient> position,
1280 final Gradient mu) {
1281
1282
1283 final int freeParameters = mu.getFreeParameters();
1284
1285
1286 final StaticTransform fromBodyFrame = bodyFrame.getStaticTransformTo(frame, date);
1287 final StaticTransform toBodyFrame = fromBodyFrame.getInverse();
1288 final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
1289
1290
1291 final GradientHessian gh = gradientHessian(date, positionBody, mu.getReal());
1292
1293
1294 final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
1295
1296
1297 final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
1298 final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
1299 final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
1300
1301
1302 final double[] derivatives = new double[freeParameters];
1303 final Gradient[] accDer = new Gradient[3];
1304 for (int i = 0; i < 3; ++i) {
1305
1306
1307 derivatives[0] = hInertial.getEntry(i, 0);
1308 derivatives[1] = hInertial.getEntry(i, 1);
1309 derivatives[2] = hInertial.getEntry(i, 2);
1310
1311
1312 if (derivatives.length > 3 && isVariable(mu, 3)) {
1313 derivatives[3] = gInertial[i] / mu.getReal();
1314 }
1315
1316 accDer[i] = new Gradient(gInertial[i], derivatives);
1317
1318 }
1319
1320 return new FieldVector3D<>(accDer);
1321
1322 }
1323
1324
1325 public List<ParameterDriver> getParametersDrivers() {
1326 return Collections.singletonList(gmParameterDriver);
1327 }
1328
1329 }