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 @Override
45 public double[] getRatesContribution(final AbsoluteDate date, final double[] stateVariables,
46 final double[] adjointVariables, final Frame frame) {
47 final GradientField field = GradientField.getField(GRADIENT_DIMENSION);
48 final FieldAbsoluteDate<Gradient> fieldDate = new FieldAbsoluteDate<>(field, date);
49 final Gradient[] stateAsGradients = buildGradientCartesianVector(stateVariables);
50 final FieldVector3D<Gradient> acceleration = getFieldAcceleration(fieldDate, stateAsGradients, frame);
51 final double[] accelerationXgradient = acceleration.getX().getGradient();
52 final double[] accelerationYgradient = acceleration.getY().getGradient();
53 final double[] accelerationZgradient = acceleration.getZ().getGradient();
54 final double[] contribution = new double[adjointVariables.length];
55 for (int i = 0; i < 6; i++) {
56 contribution[i] = -(accelerationXgradient[i] * adjointVariables[3] + accelerationYgradient[i] * adjointVariables[4] + accelerationZgradient[i] * adjointVariables[5]);
57 }
58 return contribution;
59 }
60
61
62 @Override
63 public double getHamiltonianContribution(final AbsoluteDate date, final double[] stateVariables,
64 final double[] adjointVariables, final Frame frame) {
65 final Vector3D acceleration = getAcceleration(date, stateVariables, frame);
66 return acceleration.getX() * adjointVariables[3] + acceleration.getY() * adjointVariables[4] + acceleration.getZ() * adjointVariables[5];
67 }
68
69
70
71
72
73
74
75
76
77 protected abstract Vector3D getAcceleration(AbsoluteDate date, double[] stateVariables,
78 Frame frame);
79
80
81 @Override
82 public <T extends CalculusFieldElement<T>> T[] getFieldRatesContribution(final FieldAbsoluteDate<T> date,
83 final T[] stateVariables,
84 final T[] adjointVariables,
85 final Frame frame) {
86 final FieldGradientField<T> field = FieldGradientField.getField(date.getField(), GRADIENT_DIMENSION);
87 final FieldAbsoluteDate<FieldGradient<T>> fieldDate = new FieldAbsoluteDate<>(field, date.toAbsoluteDate());
88 final FieldGradient<T>[] gradients = buildFieldGradientCartesianVector(stateVariables);
89 final FieldVector3D<FieldGradient<T>> acceleration = getFieldAcceleration(fieldDate, gradients, frame);
90 final T[] contribution = MathArrays.buildArray(date.getField(), adjointVariables.length);
91 final T[] accelerationXgradient = acceleration.getX().getGradient();
92 final T[] accelerationYgradient = acceleration.getY().getGradient();
93 final T[] accelerationZgradient = acceleration.getZ().getGradient();
94 for (int i = 0; i < 6; i++) {
95 contribution[i] = (accelerationXgradient[i].multiply(adjointVariables[3])
96 .add(accelerationYgradient[i].multiply(adjointVariables[4])).add(accelerationZgradient[i].multiply(adjointVariables[5]))).negate();
97 }
98 return contribution;
99 }
100
101
102 @Override
103 public <T extends CalculusFieldElement<T>> T getFieldHamiltonianContribution(final FieldAbsoluteDate<T> date,
104 final T[] stateVariables,
105 final T[] adjointVariables,
106 final Frame frame) {
107 final FieldVector3D<T> acceleration = getFieldAcceleration(date, stateVariables, frame);
108 return acceleration.dotProduct(new FieldVector3D<>(adjointVariables[3], adjointVariables[4], adjointVariables[5]));
109 }
110
111
112
113
114
115
116
117
118
119
120 protected abstract <T extends CalculusFieldElement<T>> FieldVector3D<T> getFieldAcceleration(FieldAbsoluteDate<T> date,
121 T[] stateVariables,
122 Frame frame);
123
124
125
126
127
128
129 protected static Gradient[] buildGradientCartesianVector(final double[] stateVariables) {
130 final GradientField field = GradientField.getField(GRADIENT_DIMENSION);
131 final Gradient[] gradients = MathArrays.buildArray(field, GRADIENT_DIMENSION);
132 gradients[0] = Gradient.variable(GRADIENT_DIMENSION, 0, stateVariables[0]);
133 gradients[1] = Gradient.variable(GRADIENT_DIMENSION, 1, stateVariables[1]);
134 gradients[2] = Gradient.variable(GRADIENT_DIMENSION, 2, stateVariables[2]);
135 gradients[3] = Gradient.variable(GRADIENT_DIMENSION, 3, stateVariables[3]);
136 gradients[4] = Gradient.variable(GRADIENT_DIMENSION, 4, stateVariables[4]);
137 gradients[5] = Gradient.variable(GRADIENT_DIMENSION, 5, stateVariables[5]);
138 return gradients;
139 }
140
141
142
143
144
145
146
147 protected static <T extends CalculusFieldElement<T>> FieldGradient<T>[] buildFieldGradientCartesianVector(final T[] stateVariables) {
148 final FieldGradientField<T> field = FieldGradientField.getField(stateVariables[0].getField(), GRADIENT_DIMENSION);
149 final FieldGradient<T>[] gradients = MathArrays.buildArray(field, GRADIENT_DIMENSION);
150 gradients[0] = FieldGradient.variable(GRADIENT_DIMENSION, 0, stateVariables[0]);
151 gradients[1] = FieldGradient.variable(GRADIENT_DIMENSION, 1, stateVariables[1]);
152 gradients[2] = FieldGradient.variable(GRADIENT_DIMENSION, 2, stateVariables[2]);
153 gradients[3] = FieldGradient.variable(GRADIENT_DIMENSION, 3, stateVariables[3]);
154 gradients[4] = FieldGradient.variable(GRADIENT_DIMENSION, 4, stateVariables[4]);
155 gradients[5] = FieldGradient.variable(GRADIENT_DIMENSION, 5, stateVariables[5]);
156 return gradients;
157 }
158 }