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.Gradient;
22 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.Rotation;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.MathArrays;
27 import org.orekit.errors.OrekitIllegalArgumentException;
28 import org.orekit.errors.OrekitMessages;
29 import org.orekit.frames.FieldTransform;
30 import org.orekit.frames.Frame;
31 import org.orekit.frames.Transform;
32 import org.orekit.time.AbsoluteDate;
33 import org.orekit.time.FieldAbsoluteDate;
34
35
36
37
38
39
40
41
42
43 public class CartesianAdjointInertialTerm extends AbstractCartesianAdjointEquationTerm {
44
45
46 private final Frame referenceInertialFrame;
47
48
49
50
51
52 public CartesianAdjointInertialTerm(final Frame referenceInertialFrame) {
53 this.referenceInertialFrame = referenceInertialFrame;
54 if (!referenceInertialFrame.isPseudoInertial()) {
55 throw new OrekitIllegalArgumentException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME_NOT_SUITABLE_AS_REFERENCE_FOR_INERTIAL_FORCES,
56 referenceInertialFrame.getName());
57 }
58 }
59
60
61
62
63
64 public Frame getReferenceInertialFrame() {
65 return referenceInertialFrame;
66 }
67
68
69 @Override
70 public double[] getRatesContribution(final AbsoluteDate date, final double[] stateVariables,
71 final double[] adjointVariables, final Frame frame) {
72 final double[] contribution = new double[adjointVariables.length];
73 final Gradient[] gradients = buildGradientCartesianVector(stateVariables);
74 final Transform transform = getReferenceInertialFrame().getTransformTo(frame, date);
75 final FieldTransform<Gradient> fieldTransform = new FieldTransform<>(gradients[0].getField(), transform);
76 final FieldVector3D<Gradient> acceleration = getFieldAcceleration(fieldTransform, gradients);
77 final double[] accelerationXgradient = acceleration.getX().getGradient();
78 final double[] accelerationYgradient = acceleration.getY().getGradient();
79 final double[] accelerationZgradient = acceleration.getZ().getGradient();
80 for (int i = 0; i < 6; i++) {
81 contribution[i] = -(accelerationXgradient[i] * adjointVariables[3] + accelerationYgradient[i] * adjointVariables[4] + accelerationZgradient[i] * adjointVariables[5]);
82 }
83 return contribution;
84 }
85
86
87 @Override
88 public <T extends CalculusFieldElement<T>> T[] getFieldRatesContribution(final FieldAbsoluteDate<T> date,
89 final T[] stateVariables,
90 final T[] adjointVariables, final Frame frame) {
91 final T[] contribution = MathArrays.buildArray(date.getField(), 6);
92 final FieldGradient<T>[] gradients = buildFieldGradientCartesianVector(stateVariables);
93 final FieldTransform<T> transform = getReferenceInertialFrame().getTransformTo(frame, date);
94 final FieldTransform<FieldGradient<T>> fieldTransform = new FieldTransform<>(gradients[0].getField(),
95 new Transform(date.toAbsoluteDate(), transform.getAngular().toAngularCoordinates()));
96 final FieldVector3D<FieldGradient<T>> acceleration = getFieldAcceleration(fieldTransform, gradients);
97 final T[] accelerationXgradient = acceleration.getX().getGradient();
98 final T[] accelerationYgradient = acceleration.getY().getGradient();
99 final T[] accelerationZgradient = acceleration.getZ().getGradient();
100 for (int i = 0; i < 6; i++) {
101 contribution[i] = (accelerationXgradient[i].multiply(adjointVariables[3])
102 .add(accelerationYgradient[i].multiply(adjointVariables[4])).add(accelerationZgradient[i].multiply(adjointVariables[5]))).negate();
103 }
104 return contribution;
105 }
106
107
108 @Override
109 protected Vector3D getAcceleration(final AbsoluteDate date, final double[] stateVariables,
110 final Frame frame) {
111 final Transform transform = getReferenceInertialFrame().getTransformTo(frame, date);
112 return getAcceleration(transform, stateVariables);
113 }
114
115
116
117
118
119
120
121 public Vector3D getAcceleration(final Transform inertialToPropagationFrame, final double[] stateVariables) {
122 final Vector3D a1 = inertialToPropagationFrame.getCartesian().getAcceleration();
123 final Rotation r1 = inertialToPropagationFrame.getAngular().getRotation();
124 final Vector3D o1 = inertialToPropagationFrame.getAngular().getRotationRate();
125 final Vector3D oDot1 = inertialToPropagationFrame.getAngular().getRotationAcceleration();
126
127 final Vector3D p2 = new Vector3D(stateVariables[0], stateVariables[1], stateVariables[2]);
128 final Vector3D v2 = new Vector3D(stateVariables[3], stateVariables[4], stateVariables[5]);
129
130 final Vector3D crossCrossP = Vector3D.crossProduct(o1, Vector3D.crossProduct(o1, p2));
131 final Vector3D crossV = Vector3D.crossProduct(o1, v2);
132 final Vector3D crossDotP = Vector3D.crossProduct(oDot1, p2);
133
134 return r1.applyTo(a1).subtract(new Vector3D(2, crossV, 1, crossCrossP, 1, crossDotP));
135 }
136
137
138 @Override
139 protected <T extends CalculusFieldElement<T>> FieldVector3D<T> getFieldAcceleration(final FieldAbsoluteDate<T> date,
140 final T[] stateVariables,
141 final Frame frame) {
142 final FieldTransform<T> transform = getReferenceInertialFrame().getTransformTo(frame, date);
143 return getFieldAcceleration(transform, stateVariables);
144 }
145
146
147
148
149
150
151
152
153 private <T extends CalculusFieldElement<T>> FieldVector3D<T> getFieldAcceleration(final FieldTransform<T> inertialToPropagationFrame,
154 final T[] stateVariables) {
155 final FieldVector3D<T> a1 = inertialToPropagationFrame.getCartesian().getAcceleration();
156 final FieldRotation<T> r1 = inertialToPropagationFrame.getAngular().getRotation();
157 final FieldVector3D<T> o1 = inertialToPropagationFrame.getAngular().getRotationRate();
158 final FieldVector3D<T> oDot1 = inertialToPropagationFrame.getAngular().getRotationAcceleration();
159
160 final FieldVector3D<T> p2 = new FieldVector3D<>(stateVariables[0], stateVariables[1], stateVariables[2]);
161 final FieldVector3D<T> v2 = new FieldVector3D<>(stateVariables[3], stateVariables[4], stateVariables[5]);
162
163 final FieldVector3D<T> crossCrossP = FieldVector3D.crossProduct(o1, FieldVector3D.crossProduct(o1, p2));
164 final FieldVector3D<T> crossV = FieldVector3D.crossProduct(o1, v2);
165 final FieldVector3D<T> crossDotP = FieldVector3D.crossProduct(oDot1, p2);
166
167 return r1.applyTo(a1).subtract(new FieldVector3D<>(2, crossV, 1, crossCrossP, 1, crossDotP));
168 }
169 }