1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.attitudes;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Rotation;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
26 import org.hipparchus.util.FastMath;
27 import org.orekit.errors.OrekitException;
28 import org.orekit.errors.OrekitMessages;
29 import org.orekit.frames.Frame;
30 import org.orekit.time.AbsoluteDate;
31 import org.orekit.time.FieldAbsoluteDate;
32 import org.orekit.utils.AngularCoordinates;
33 import org.orekit.utils.FieldAngularCoordinates;
34 import org.orekit.utils.FieldPVCoordinates;
35 import org.orekit.utils.FieldPVCoordinatesProvider;
36 import org.orekit.utils.PVCoordinates;
37 import org.orekit.utils.PVCoordinatesProvider;
38 import org.orekit.utils.TimeStampedFieldPVCoordinates;
39 import org.orekit.utils.TimeStampedPVCoordinates;
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55 public abstract class GroundPointing implements AttitudeProvider {
56
57
58 private static final PVCoordinates PLUS_J =
59 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);
60
61
62 private static final PVCoordinates PLUS_K =
63 new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO);
64
65
66 private final Frame inertialFrame;
67
68
69 private final Frame bodyFrame;
70
71
72
73
74
75
76
77 protected GroundPointing(final Frame inertialFrame, final Frame bodyFrame) {
78 if (!inertialFrame.isPseudoInertial()) {
79 throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME,
80 inertialFrame.getName());
81 }
82 this.inertialFrame = inertialFrame;
83 this.bodyFrame = bodyFrame;
84 }
85
86
87
88
89 public Frame getBodyFrame() {
90 return bodyFrame;
91 }
92
93
94
95
96
97
98
99
100 protected abstract TimeStampedPVCoordinates getTargetPV(PVCoordinatesProvider pvProv, AbsoluteDate date, Frame frame);
101
102
103
104
105
106
107
108
109
110
111 protected abstract <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(FieldPVCoordinatesProvider<T> pvProv,
112 FieldAbsoluteDate<T> date,
113 Frame frame);
114
115
116
117
118
119
120
121
122 protected Vector3D getTargetPosition(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) {
123 return getTargetPV(pvProv, date, frame).getPosition();
124 }
125
126
127
128
129
130
131
132
133
134 protected <T extends CalculusFieldElement<T>> FieldVector3D<T> getTargetPosition(final FieldPVCoordinatesProvider<T> pvProv,
135 final FieldAbsoluteDate<T> date,
136 final Frame frame) {
137 return getTargetPV(pvProv, date, frame).getPosition();
138 }
139
140
141 @Override
142 public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date,
143 final Frame frame) {
144
145
146 final PVCoordinates pva = pvProv.getPVCoordinates(date, inertialFrame);
147 final TimeStampedPVCoordinates delta =
148 new TimeStampedPVCoordinates(date, pva, getTargetPV(pvProv, date, inertialFrame));
149
150
151 if (delta.getPosition().getNorm() == 0.0) {
152 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
153 }
154
155
156
157
158 final Vector3D p = pva.getPosition();
159 final Vector3D v = pva.getVelocity();
160 final Vector3D a = pva.getAcceleration();
161 final double r2 = p.getNormSq();
162 final double r = FastMath.sqrt(r2);
163 final Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
164 final PVCoordinates velocity = new PVCoordinates(v, a, keplerianJerk);
165
166 final PVCoordinates los = delta.normalize();
167 final PVCoordinates normal = PVCoordinates.crossProduct(delta, velocity).normalize();
168
169 final AngularCoordinates ac = new AngularCoordinates(los, normal, PLUS_K, PLUS_J, 1.0e-9);
170 final Attitude attitude = new Attitude(date, inertialFrame, ac);
171 return attitude.withReferenceFrame(frame);
172 }
173
174
175 @Override
176 public <T extends CalculusFieldElement<T>>FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
177 final FieldAbsoluteDate<T> date,
178 final Frame frame) {
179
180
181 final FieldPVCoordinates<T> pva = pvProv.getPVCoordinates(date, inertialFrame);
182 final TimeStampedFieldPVCoordinates<T> delta =
183 new TimeStampedFieldPVCoordinates<>(date, pva, getTargetPV(pvProv, date, inertialFrame));
184
185
186 if (delta.getPosition().getNorm().getReal() == 0.0) {
187 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
188 }
189
190
191
192
193 final FieldVector3D<T> p = pva.getPosition();
194 final FieldVector3D<T> v = pva.getVelocity();
195 final FieldVector3D<T> a = pva.getAcceleration();
196 final T r2 = p.getNormSq();
197 final T r = r2.sqrt();
198 final FieldVector3D<T> keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-3).divide(r2), a, a.getNorm().divide(r).multiply(-1), v);
199 final FieldPVCoordinates<T> velocity = new FieldPVCoordinates<>(v, a, keplerianJerk);
200
201
202 final FieldPVCoordinates<T> los = delta.normalize();
203
204 final FieldPVCoordinates<T> normal = (delta.crossProduct(velocity)).normalize();
205
206 final FieldVector3D<T> zero = FieldVector3D.getZero(r.getField());
207 final FieldVector3D<T> plusK = FieldVector3D.getPlusK(r.getField());
208 final FieldVector3D<T> plusJ = FieldVector3D.getPlusJ(r.getField());
209 final FieldAngularCoordinates<T> ac =
210 new FieldAngularCoordinates<>(los, normal,
211 new FieldPVCoordinates<>(plusK, zero, zero),
212 new FieldPVCoordinates<>(plusJ, zero, zero),
213 1.0e-6);
214 final FieldAttitude<T> attitude = new FieldAttitude<>(date, inertialFrame, ac);
215 return attitude.withReferenceFrame(frame);
216 }
217
218
219 @Override
220 public Rotation getAttitudeRotation(final PVCoordinatesProvider pvProv, final AbsoluteDate date,
221 final Frame frame) {
222
223
224 final PVCoordinates pva = pvProv.getPVCoordinates(date, inertialFrame);
225 final Vector3D targetPosition = getTargetPosition(pvProv, date, inertialFrame);
226 final Vector3D deltaPosition = targetPosition.subtract(pva.getPosition());
227
228
229 if (deltaPosition.getNorm() == 0.0) {
230 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
231 }
232
233 final Vector3D los = deltaPosition.normalize();
234 final Vector3D normal = Vector3D.crossProduct(los, pva.getVelocity()).normalize();
235
236 final Rotation rotation = new Rotation(los, normal, PLUS_K.getPosition(), PLUS_J.getPosition());
237
238 if (frame != inertialFrame) {
239
240 return rotation.compose(frame.getStaticTransformTo(inertialFrame, date).getRotation(),
241 RotationConvention.VECTOR_OPERATOR);
242 }
243
244 return rotation;
245
246 }
247
248
249
250 @Override
251 public <T extends CalculusFieldElement<T>> FieldRotation<T> getAttitudeRotation(final FieldPVCoordinatesProvider<T> pvProv,
252 final FieldAbsoluteDate<T> date,
253 final Frame frame) {
254
255 final FieldPVCoordinates<T> pva = pvProv.getPVCoordinates(date, inertialFrame);
256 final FieldVector3D<T> targetPosition = getTargetPosition(pvProv, date, inertialFrame);
257 final FieldVector3D<T> deltaPosition = targetPosition.subtract(pva.getPosition());
258
259
260 if (deltaPosition.getNorm().getReal() == 0.0) {
261 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
262 }
263
264 final FieldVector3D<T> los = deltaPosition.normalize();
265 final FieldVector3D<T> normal = FieldVector3D.crossProduct(los, pva.getVelocity()).normalize();
266
267 final Field<T> field = date.getField();
268 final FieldRotation<T> rotation = new FieldRotation<>(los, normal,
269 new FieldVector3D<>(field, PLUS_K.getPosition()), new FieldVector3D<>(field, PLUS_J.getPosition()));
270
271 if (frame != inertialFrame) {
272
273 return rotation.compose(frame.getStaticTransformTo(inertialFrame, date).getRotation(),
274 RotationConvention.VECTOR_OPERATOR);
275 }
276
277 return rotation;
278
279 }
280
281 }