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