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.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 PVCoordinates PLUS_J =
55 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);
56
57
58 private static final PVCoordinates PLUS_K =
59 new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO);
60
61
62 private final Frame inertialFrame;
63
64
65 private final Frame bodyFrame;
66
67
68
69
70
71
72
73 protected GroundPointing(final Frame inertialFrame, final Frame bodyFrame) {
74 if (!inertialFrame.isPseudoInertial()) {
75 throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME,
76 inertialFrame.getName());
77 }
78 this.inertialFrame = inertialFrame;
79 this.bodyFrame = bodyFrame;
80 }
81
82
83
84
85 public Frame getBodyFrame() {
86 return bodyFrame;
87 }
88
89
90
91
92
93
94
95
96
97
98
99
100
101 public abstract TimeStampedPVCoordinates getTargetPV(PVCoordinatesProvider pvProv,
102 AbsoluteDate date, Frame frame);
103
104
105
106
107
108
109
110
111
112
113 public abstract <T extends CalculusFieldElement<T>> TimeStampedFieldPVCoordinates<T> getTargetPV(FieldPVCoordinatesProvider<T> pvProv,
114 FieldAbsoluteDate<T> date,
115 Frame frame);
116
117
118 public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date,
119 final Frame frame) {
120
121
122 final PVCoordinates pva = pvProv.getPVCoordinates(date, inertialFrame);
123 final TimeStampedPVCoordinates delta =
124 new TimeStampedPVCoordinates(date, pva, getTargetPV(pvProv, date, inertialFrame));
125
126
127 if (delta.getPosition().getNorm() == 0.0) {
128 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
129 }
130
131
132
133
134 final Vector3D p = pva.getPosition();
135 final Vector3D v = pva.getVelocity();
136 final Vector3D a = pva.getAcceleration();
137 final double r2 = p.getNormSq();
138 final double r = FastMath.sqrt(r2);
139 final Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
140 final PVCoordinates velocity = new PVCoordinates(v, a, keplerianJerk);
141
142 final PVCoordinates los = delta.normalize();
143 final PVCoordinates normal = PVCoordinates.crossProduct(delta, velocity).normalize();
144
145 AngularCoordinates ac = new AngularCoordinates(los, normal, PLUS_K, PLUS_J, 1.0e-9);
146
147 if (frame != inertialFrame) {
148
149 ac = ac.addOffset(frame.getTransformTo(inertialFrame, date).getAngular());
150 }
151
152
153 return new Attitude(date, frame, ac);
154
155 }
156
157
158 public <T extends CalculusFieldElement<T>>FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
159 final FieldAbsoluteDate<T> date,
160 final Frame frame) {
161
162
163 final FieldPVCoordinates<T> pva = pvProv.getPVCoordinates(date, inertialFrame);
164 final TimeStampedFieldPVCoordinates<T> delta =
165 new TimeStampedFieldPVCoordinates<>(date, pva, getTargetPV(pvProv, date, inertialFrame));
166
167
168 if (delta.getPosition().getNorm().getReal() == 0.0) {
169 throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
170 }
171
172
173
174
175 final FieldVector3D<T> p = pva.getPosition();
176 final FieldVector3D<T> v = pva.getVelocity();
177 final FieldVector3D<T> a = pva.getAcceleration();
178 final T r2 = p.getNormSq();
179 final T r = r2.sqrt();
180 final FieldVector3D<T> keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-3).divide(r2), a, a.getNorm().divide(r).multiply(-1), v);
181 final FieldPVCoordinates<T> velocity = new FieldPVCoordinates<>(v, a, keplerianJerk);
182
183
184 final FieldPVCoordinates<T> los = delta.normalize();
185
186 final FieldPVCoordinates<T> normal = (delta.crossProduct(velocity)).normalize();
187
188 final FieldVector3D<T> zero = FieldVector3D.getZero(r.getField());
189 final FieldVector3D<T> plusK = FieldVector3D.getPlusK(r.getField());
190 final FieldVector3D<T> plusJ = FieldVector3D.getPlusJ(r.getField());
191 FieldAngularCoordinates<T> ac =
192 new FieldAngularCoordinates<>(los, normal,
193 new FieldPVCoordinates<>(plusK, zero, zero),
194 new FieldPVCoordinates<>(plusJ, zero, zero),
195 1.0e-6);
196
197 if (frame != inertialFrame) {
198
199 ac = ac.addOffset(new FieldAngularCoordinates<>(r.getField(),
200 frame.getTransformTo(inertialFrame, date.toAbsoluteDate()).getAngular()));
201 }
202
203
204 return new FieldAttitude<>(date, frame, ac);
205
206 }
207
208 }