1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.control.indirect.adjoint;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.analysis.differentiation.FieldGradient;
21 import org.hipparchus.analysis.differentiation.FieldGradientField;
22 import org.hipparchus.analysis.differentiation.Gradient;
23 import org.hipparchus.analysis.differentiation.GradientField;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.MathArrays;
27 import org.orekit.frames.Frame;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.time.FieldAbsoluteDate;
30
31
32
33
34
35
36
37
38 public abstract class AbstractCartesianAdjointEquationTerm implements CartesianAdjointEquationTerm {
39
40
41 private static final int GRADIENT_DIMENSION = 6;
42
43
44 protected AbstractCartesianAdjointEquationTerm() {
45
46 }
47
48
49 @Override
50 public double[] getRatesContribution(final AbsoluteDate date, final double[] stateVariables,
51 final double[] adjointVariables, final Frame frame) {
52 final GradientField field = GradientField.getField(GRADIENT_DIMENSION);
53 final FieldAbsoluteDate<Gradient> fieldDate = new FieldAbsoluteDate<>(field, date);
54 final Gradient[] stateAsGradients = buildGradientCartesianVector(stateVariables);
55 final FieldVector3D<Gradient> acceleration = getFieldAcceleration(fieldDate, stateAsGradients, frame);
56 final double[] accelerationXgradient = acceleration.getX().getGradient();
57 final double[] accelerationYgradient = acceleration.getY().getGradient();
58 final double[] accelerationZgradient = acceleration.getZ().getGradient();
59 final double[] contribution = new double[adjointVariables.length];
60 for (int i = 0; i < 6; i++) {
61 contribution[i] = -(accelerationXgradient[i] * adjointVariables[3] + accelerationYgradient[i] * adjointVariables[4] + accelerationZgradient[i] * adjointVariables[5]);
62 }
63 return contribution;
64 }
65
66
67 @Override
68 public double getHamiltonianContribution(final AbsoluteDate date, final double[] stateVariables,
69 final double[] adjointVariables, final Frame frame) {
70 final Vector3D acceleration = getAcceleration(date, stateVariables, frame);
71 return acceleration.getX() * adjointVariables[3] + acceleration.getY() * adjointVariables[4] + acceleration.getZ() * adjointVariables[5];
72 }
73
74
75
76
77
78
79
80
81
82 protected abstract Vector3D getAcceleration(AbsoluteDate date, double[] stateVariables,
83 Frame frame);
84
85
86 @Override
87 public <T extends CalculusFieldElement<T>> T[] getFieldRatesContribution(final FieldAbsoluteDate<T> date,
88 final T[] stateVariables,
89 final T[] adjointVariables,
90 final Frame frame) {
91 final FieldGradientField<T> field = FieldGradientField.getField(date.getField(), GRADIENT_DIMENSION);
92 final FieldAbsoluteDate<FieldGradient<T>> fieldDate = new FieldAbsoluteDate<>(field, date.toAbsoluteDate());
93 final FieldGradient<T>[] gradients = buildFieldGradientCartesianVector(stateVariables);
94 final FieldVector3D<FieldGradient<T>> acceleration = getFieldAcceleration(fieldDate, gradients, frame);
95 final T[] contribution = MathArrays.buildArray(date.getField(), adjointVariables.length);
96 final T[] accelerationXgradient = acceleration.getX().getGradient();
97 final T[] accelerationYgradient = acceleration.getY().getGradient();
98 final T[] accelerationZgradient = acceleration.getZ().getGradient();
99 for (int i = 0; i < 6; i++) {
100 contribution[i] = (accelerationXgradient[i].multiply(adjointVariables[3])
101 .add(accelerationYgradient[i].multiply(adjointVariables[4])).add(accelerationZgradient[i].multiply(adjointVariables[5]))).negate();
102 }
103 return contribution;
104 }
105
106
107 @Override
108 public <T extends CalculusFieldElement<T>> T getFieldHamiltonianContribution(final FieldAbsoluteDate<T> date,
109 final T[] stateVariables,
110 final T[] adjointVariables,
111 final Frame frame) {
112 final FieldVector3D<T> acceleration = getFieldAcceleration(date, stateVariables, frame);
113 return acceleration.dotProduct(new FieldVector3D<>(adjointVariables[3], adjointVariables[4], adjointVariables[5]));
114 }
115
116
117
118
119
120
121
122
123
124
125 protected abstract <T extends CalculusFieldElement<T>> FieldVector3D<T> getFieldAcceleration(FieldAbsoluteDate<T> date,
126 T[] stateVariables,
127 Frame frame);
128
129
130
131
132
133
134 protected static Gradient[] buildGradientCartesianVector(final double[] stateVariables) {
135 final GradientField field = GradientField.getField(GRADIENT_DIMENSION);
136 final Gradient[] gradients = MathArrays.buildArray(field, GRADIENT_DIMENSION);
137 gradients[0] = Gradient.variable(GRADIENT_DIMENSION, 0, stateVariables[0]);
138 gradients[1] = Gradient.variable(GRADIENT_DIMENSION, 1, stateVariables[1]);
139 gradients[2] = Gradient.variable(GRADIENT_DIMENSION, 2, stateVariables[2]);
140 gradients[3] = Gradient.variable(GRADIENT_DIMENSION, 3, stateVariables[3]);
141 gradients[4] = Gradient.variable(GRADIENT_DIMENSION, 4, stateVariables[4]);
142 gradients[5] = Gradient.variable(GRADIENT_DIMENSION, 5, stateVariables[5]);
143 return gradients;
144 }
145
146
147
148
149
150
151
152 protected static <T extends CalculusFieldElement<T>> FieldGradient<T>[] buildFieldGradientCartesianVector(final T[] stateVariables) {
153 final FieldGradientField<T> field = FieldGradientField.getField(stateVariables[0].getField(), GRADIENT_DIMENSION);
154 final FieldGradient<T>[] gradients = MathArrays.buildArray(field, GRADIENT_DIMENSION);
155 gradients[0] = FieldGradient.variable(GRADIENT_DIMENSION, 0, stateVariables[0]);
156 gradients[1] = FieldGradient.variable(GRADIENT_DIMENSION, 1, stateVariables[1]);
157 gradients[2] = FieldGradient.variable(GRADIENT_DIMENSION, 2, stateVariables[2]);
158 gradients[3] = FieldGradient.variable(GRADIENT_DIMENSION, 3, stateVariables[3]);
159 gradients[4] = FieldGradient.variable(GRADIENT_DIMENSION, 4, stateVariables[4]);
160 gradients[5] = FieldGradient.variable(GRADIENT_DIMENSION, 5, stateVariables[5]);
161 return gradients;
162 }
163 }