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.RealFieldElement;
20 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.util.FastMath;
23 import org.orekit.errors.OrekitException;
24 import org.orekit.errors.OrekitMessages;
25 import org.orekit.frames.Frame;
26 import org.orekit.time.AbsoluteDate;
27 import org.orekit.time.FieldAbsoluteDate;
28 import org.orekit.utils.AngularCoordinates;
29 import org.orekit.utils.FieldAngularCoordinates;
30 import org.orekit.utils.FieldPVCoordinates;
31 import org.orekit.utils.FieldPVCoordinatesProvider;
32 import org.orekit.utils.PVCoordinates;
33 import org.orekit.utils.PVCoordinatesProvider;
34 import org.orekit.utils.TimeStampedFieldPVCoordinates;
35 import org.orekit.utils.TimeStampedPVCoordinates;
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51 public abstract class GroundPointing implements AttitudeProvider {
52
53
54 private static final long serialVersionUID = 20150529L;
55
56
57 private static final PVCoordinates PLUS_J =
58 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);
59
60
61 private static final PVCoordinates PLUS_K =
62 new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO);
63
64
65 private final Frame inertialFrame;
66
67
68 private final Frame bodyFrame;
69
70
71
72
73
74
75
76
77 protected GroundPointing(final Frame inertialFrame, final Frame bodyFrame)
78 throws OrekitException {
79 if (!inertialFrame.isPseudoInertial()) {
80 throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME,
81 inertialFrame.getName());
82 }
83 this.inertialFrame = inertialFrame;
84 this.bodyFrame = bodyFrame;
85 }
86
87
88
89
90 public Frame getBodyFrame() {
91 return bodyFrame;
92 }
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108 public abstract TimeStampedPVCoordinates getTargetPV(PVCoordinatesProvider pvProv,
109 AbsoluteDate date, Frame frame)
110 throws OrekitException;
111
112
113
114
115
116
117
118
119
120
121
122
123 public abstract <T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(FieldPVCoordinatesProvider<T> pvProv,
124 FieldAbsoluteDate<T> date,
125 Frame frame)
126 throws OrekitException;
127
128
129 public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date,
130 final Frame frame)
131 throws OrekitException {
132
133
134 final PVCoordinates pva = pvProv.getPVCoordinates(date, inertialFrame);
135 final TimeStampedPVCoordinates delta =
136 new TimeStampedPVCoordinates(date, pva, getTargetPV(pvProv, date, inertialFrame));
137
138
139 if (delta.getPosition().getNorm() == 0.0) {
140 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
141 }
142
143
144
145
146 final Vector3D p = pva.getPosition();
147 final Vector3D v = pva.getVelocity();
148 final Vector3D a = pva.getAcceleration();
149 final double r2 = p.getNormSq();
150 final double r = FastMath.sqrt(r2);
151 final Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
152 final PVCoordinates velocity = new PVCoordinates(v, a, keplerianJerk);
153
154 final PVCoordinates los = delta.normalize();
155 final PVCoordinates normal = PVCoordinates.crossProduct(delta, velocity).normalize();
156
157 AngularCoordinates ac = new AngularCoordinates(los, normal, PLUS_K, PLUS_J, 1.0e-9);
158
159 if (frame != inertialFrame) {
160
161 ac = ac.addOffset(frame.getTransformTo(inertialFrame, date).getAngular());
162 }
163
164
165 return new Attitude(date, frame, ac);
166
167 }
168
169
170 public <T extends RealFieldElement<T>>FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
171 final FieldAbsoluteDate<T> date,
172 final Frame frame)
173 throws OrekitException {
174
175
176 final FieldPVCoordinates<T> pva = pvProv.getPVCoordinates(date, inertialFrame);
177 final TimeStampedFieldPVCoordinates<T> delta =
178 new TimeStampedFieldPVCoordinates<>(date, pva, getTargetPV(pvProv, date, inertialFrame));
179
180
181 if (delta.getPosition().getNorm().getReal() == 0.0) {
182 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
183 }
184
185
186
187
188 final FieldVector3D<T> p = pva.getPosition();
189 final FieldVector3D<T> v = pva.getVelocity();
190 final FieldVector3D<T> a = pva.getAcceleration();
191 final T r2 = p.getNormSq();
192 final T r = r2.sqrt();
193 final FieldVector3D<T> keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-3).divide(r2), a, a.getNorm().divide(r).multiply(-1), v);
194 final FieldPVCoordinates<T> velocity = new FieldPVCoordinates<>(v, a, keplerianJerk);
195
196
197 final FieldPVCoordinates<T> los = delta.normalize();
198
199 final FieldPVCoordinates<T> normal = (delta.crossProduct(velocity)).normalize();
200
201 final FieldVector3D<T> zero = FieldVector3D.getZero(r.getField());
202 final FieldVector3D<T> plusK = FieldVector3D.getPlusK(r.getField());
203 final FieldVector3D<T> plusJ = FieldVector3D.getPlusJ(r.getField());
204 FieldAngularCoordinates<T> ac =
205 new FieldAngularCoordinates<>(los, normal,
206 new FieldPVCoordinates<>(plusK, zero, zero),
207 new FieldPVCoordinates<>(plusJ, zero, zero),
208 1.0e-6);
209
210 if (frame != inertialFrame) {
211
212 ac = ac.addOffset(new FieldAngularCoordinates<>(r.getField(),
213 frame.getTransformTo(inertialFrame, date.toAbsoluteDate()).getAngular()));
214 }
215
216
217 return new FieldAttitude<>(date, frame, ac);
218
219 }
220
221 }