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