1 /* Copyright 2013-2017 CS Systèmes d'Information
2 * Licensed to CS Systèmes d'Information (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.rugged.utils;
18
19 import org.hipparchus.geometry.euclidean.threed.Line;
20 import org.hipparchus.geometry.euclidean.threed.Vector3D;
21 import org.hipparchus.util.FastMath;
22 import org.hipparchus.util.MathArrays;
23 import org.orekit.bodies.GeodeticPoint;
24 import org.orekit.bodies.OneAxisEllipsoid;
25 import org.orekit.errors.OrekitException;
26 import org.orekit.frames.Frame;
27 import org.orekit.rugged.errors.DumpManager;
28 import org.orekit.rugged.errors.RuggedException;
29 import org.orekit.rugged.errors.RuggedMessages;
30 import org.orekit.time.AbsoluteDate;
31
32 /** Transform provider from Spacecraft frame to observed body frame.
33 * @author Luc Maisonobe
34 */
35 public class ExtendedEllipsoid extends OneAxisEllipsoid {
36
37 /** Serializable UID. */
38 private static final long serialVersionUID = 20140312L;
39
40 /** Convergence threshold for {@link #pointAtAltitude(Vector3D, Vector3D, double)}. */
41 private static final double ALTITUDE_CONVERGENCE = 1.0e-3;
42
43 /** Equatorial radius power 2. */
44 private final double a2;
45
46 /** Polar radius power 2. */
47 private final double b2;
48
49 /** Simple constructor.
50 * @param ae equatorial radius (m)
51 * @param f the flattening (f = (a-b)/a)
52 * @param bodyFrame body frame related to body shape
53 * @see org.orekit.frames.FramesFactory#getITRF(org.orekit.utils.IERSConventions, boolean)
54 */
55 public ExtendedEllipsoid(final double ae, final double f, final Frame bodyFrame) {
56 super(ae, f, bodyFrame);
57 a2 = ae * ae;
58 final double b = ae * (1.0 - f);
59 b2 = b * b;
60 }
61
62 /** {@inheritDoc} */
63 @Override
64 public Vector3D transform(final GeodeticPoint point) {
65 DumpManager.dumpEllipsoid(this);
66 return super.transform(point);
67 }
68
69 /** {@inheritDoc} */
70 @Override
71 public GeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date)
72 throws OrekitException {
73 DumpManager.dumpEllipsoid(this);
74 return super.transform(point, frame, date);
75 }
76
77 /** Get point at some latitude along a pixel line of sight.
78 * @param position cell position (in body frame) (m)
79 * @param los pixel line-of-sight, not necessarily normalized (in body frame)
80 * @param latitude latitude with respect to ellipsoid (rad)
81 * @param closeReference reference point used to select the closest solution
82 * when there are two points at the desired latitude along the line, it should
83 * be close to los surface intersection (m)
84 * @return point at latitude (m)
85 * @exception RuggedException if no such point exists
86 */
87 public Vector3D pointAtLatitude(final Vector3D position, final Vector3D los,
88 final double latitude, final Vector3D closeReference)
89 throws RuggedException {
90
91 DumpManager.dumpEllipsoid(this);
92
93 // find apex of iso-latitude cone, somewhere along polar axis
94 final double sinPhi = FastMath.sin(latitude);
95 final double sinPhi2 = sinPhi * sinPhi;
96 final double e2 = getFlattening() * (2 - getFlattening());
97 final double apexZ = -getA() * e2 * sinPhi / FastMath.sqrt(1 - e2 * sinPhi2);
98
99 // quadratic equation representing line intersection with iso-latitude cone
100 // a k² + 2 b k + c = 0
101 // when line of sight is almost along an iso-latitude generatrix, the quadratic
102 // equation above may become unsolvable due to numerical noise (we get catastrophic
103 // cancellation when computing b * b - a * c). So we set up the model in two steps,
104 // first searching k₀ such that position + k₀ los is close to closeReference, and
105 // then using position + k₀ los as the new initial position, which should be in
106 // the neighborhood of the solution
107 final double cosPhi = FastMath.cos(latitude);
108 final double cosPhi2 = cosPhi * cosPhi;
109 final double k0 = Vector3D.dotProduct(closeReference.subtract(position), los) / los.getNormSq();
110
111 final Vector3D delta = new Vector3D(MathArrays.linearCombination(1, position.getX(), k0, los.getX()),
112 MathArrays.linearCombination(1, position.getY(), k0, los.getY()),
113 MathArrays.linearCombination(1, position.getZ(), k0, los.getZ(), -1.0, apexZ));
114 final double a = MathArrays.linearCombination(+sinPhi2, los.getX() * los.getX() + los.getY() * los.getY(),
115 -cosPhi2, los.getZ() * los.getZ());
116 final double b = MathArrays.linearCombination(+sinPhi2, MathArrays.linearCombination(delta.getX(), los.getX(),
117 delta.getY(), los.getY()),
118 -cosPhi2, delta.getZ() * los.getZ());
119 final double c = MathArrays.linearCombination(+sinPhi2, delta.getX() * delta.getX() + delta.getY() * delta.getY(),
120 -cosPhi2, delta.getZ() * delta.getZ());
121
122 // find the two intersections along the line
123 if (b * b < a * c) {
124 throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE,
125 FastMath.toDegrees(latitude));
126 }
127 final double s = FastMath.sqrt(MathArrays.linearCombination(b, b, -a, c));
128 final double k1 = (b > 0) ? -(s + b) / a : c / (s - b);
129 final double k2 = c / (a * k1);
130
131 // the quadratic equation has two solutions
132 final boolean k1IsOK = (delta.getZ() + k1 * los.getZ()) * latitude >= 0;
133 final boolean k2IsOK = (delta.getZ() + k2 * los.getZ()) * latitude >= 0;
134 final double selectedK;
135 if (k1IsOK) {
136 if (k2IsOK) {
137 // both solutions are in the good nappe,
138 // select the one closest to the specified reference
139 final double kRef = Vector3D.dotProduct(los, closeReference.subtract(position)) /
140 los.getNormSq() - k0;
141 selectedK = FastMath.abs(k1 - kRef) <= FastMath.abs(k2 - kRef) ? k1 : k2;
142 } else {
143 // only k1 is in the good nappe
144 selectedK = k1;
145 }
146 } else {
147 if (k2IsOK) {
148 // only k2 is in the good nappe
149 selectedK = k2;
150 } else {
151 // both solutions are in the wrong nappe,
152 // there are no solutions
153 throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LATITUDE,
154 FastMath.toDegrees(latitude));
155 }
156 }
157
158 // compute point
159 return new Vector3D(1, position, k0 + selectedK, los);
160
161 }
162
163 /** Get point at some longitude along a pixel line of sight.
164 * @param position cell position (in body frame) (m)
165 * @param los pixel line-of-sight, not necessarily normalized (in body frame)
166 * @param longitude longitude with respect to ellipsoid (rad)
167 * @return point at longitude (m)
168 * @exception RuggedException if no such point exists
169 */
170 public Vector3D pointAtLongitude(final Vector3D position, final Vector3D los, final double longitude)
171 throws RuggedException {
172
173 DumpManager.dumpEllipsoid(this);
174
175 // normal to meridian
176 final Vector3D normal = new Vector3D(-FastMath.sin(longitude), FastMath.cos(longitude), 0);
177 final double d = Vector3D.dotProduct(los, normal);
178 if (FastMath.abs(d) < 1.0e-12) {
179 throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_LONGITUDE,
180 FastMath.toDegrees(longitude));
181 }
182
183 // compute point
184 return new Vector3D(1, position, -Vector3D.dotProduct(position, normal) / d, los);
185
186 }
187
188 /** Get point on ground along a pixel line of sight.
189 * @param position cell position (in body frame) (m)
190 * @param los pixel line-of-sight, not necessarily normalized (in body frame)
191 * @param centralLongitude reference longitude lc such that the point longitude will
192 * be normalized between lc-π and lc+π (rad)
193 * @return point on ground
194 * @exception RuggedException if no such point exists (typically line-of-sight missing body)
195 */
196 public NormalizedGeodeticPoint pointOnGround(final Vector3D position, final Vector3D los,
197 final double centralLongitude)
198 throws RuggedException {
199 try {
200 DumpManager.dumpEllipsoid(this);
201 final GeodeticPoint gp =
202 getIntersectionPoint(new Line(position, new Vector3D(1, position, 1e6, los), 1.0e-12),
203 position, getBodyFrame(), null);
204 if (gp == null) {
205 throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_DOES_NOT_REACH_GROUND);
206 }
207 return new NormalizedGeodeticPoint(gp.getLatitude(), gp.getLongitude(), gp.getAltitude(),
208 centralLongitude);
209 } catch (OrekitException oe) {
210 throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
211 }
212 }
213
214 /** Get point at some altitude along a pixel line of sight.
215 * @param position cell position (in body frame) (m)
216 * @param los pixel line-of-sight, not necessarily normalized (in body frame)
217 * @param altitude altitude with respect to ellipsoid (m)
218 * @return point at altitude (m)
219 * @exception RuggedException if no such point exists (typically too negative altitude)
220 */
221 public Vector3D pointAtAltitude(final Vector3D position, final Vector3D los, final double altitude)
222 throws RuggedException {
223 try {
224
225 DumpManager.dumpEllipsoid(this);
226
227 // point on line closest to origin
228 final double los2 = los.getNormSq();
229 final double dot = Vector3D.dotProduct(position, los);
230 final double k0 = -dot / los2;
231 final Vector3D close0 = new Vector3D(1, position, k0, los);
232
233 // very rough guess: if body is spherical, the desired point on line
234 // is at distance ae + altitude from origin
235 final double r = getEquatorialRadius() + altitude;
236 final double delta2 = r * r - close0.getNormSq();
237 if (delta2 < 0) {
238 throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_ALTITUDE, altitude);
239 }
240 final double deltaK = FastMath.sqrt(delta2 / los2);
241 final double k1 = k0 + deltaK;
242 final double k2 = k0 - deltaK;
243 double k = (FastMath.abs(k1) <= FastMath.abs(k2)) ? k1 : k2;
244
245 // this loop generally converges in 3 iterations
246 for (int i = 0; i < 100; ++i) {
247
248 final Vector3D point = new Vector3D(1, position, k, los);
249 final GeodeticPoint gpK = transform(point, getBodyFrame(), null);
250 final double deltaH = altitude - gpK.getAltitude();
251 if (FastMath.abs(deltaH) <= ALTITUDE_CONVERGENCE) {
252 return point;
253 }
254
255 // improve the offset using linear ratio between
256 // altitude variation and displacement along line-of-sight
257 k += deltaH / Vector3D.dotProduct(gpK.getZenith(), los);
258
259 }
260
261 // this should never happen
262 throw new RuggedException(RuggedMessages.LINE_OF_SIGHT_NEVER_CROSSES_ALTITUDE, altitude);
263
264 } catch (OrekitException oe) {
265 // this should never happen
266 throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
267 }
268 }
269
270 /** Convert a line-of-sight from Cartesian to topocentric.
271 * @param point geodetic point on the line-of-sight
272 * @param los line-of-sight, not necessarily normalized (in body frame and Cartesian coordinates)
273 * @return line-of-sight in topocentric frame (East, North, Zenith) of the point,
274 * scaled to match radians in the horizontal plane and meters along the vertical axis
275 */
276 public Vector3D convertLos(final GeodeticPoint point, final Vector3D los) {
277
278 // Cartesian coordinates of the topocentric frame origin
279 final Vector3D p3D = transform(point);
280
281 // local radius of curvature in the East-West direction (parallel)
282 final double r = FastMath.hypot(p3D.getX(), p3D.getY());
283
284 // local radius of curvature in the North-South direction (meridian)
285 final double b2r = b2 * r;
286 final double b4r2 = b2r * b2r;
287 final double a2z = a2 * p3D.getZ();
288 final double a4z2 = a2z * a2z;
289 final double q = a4z2 + b4r2;
290 final double rho = q * FastMath.sqrt(q) / (b2 * a4z2 + a2 * b4r2);
291
292 final double norm = los.getNorm();
293 return new Vector3D(Vector3D.dotProduct(los, point.getEast()) / (norm * r),
294 Vector3D.dotProduct(los, point.getNorth()) / (norm * rho),
295 Vector3D.dotProduct(los, point.getZenith()) / norm);
296
297 }
298
299 /** Convert a line-of-sight from Cartesian to topocentric.
300 * @param primary reference point on the line-of-sight (in body frame and Cartesian coordinates)
301 * @param secondary secondary point on the line-of-sight, only used to define a direction
302 * with respect to the primary point (in body frame and Cartesian coordinates)
303 * @return line-of-sight in topocentric frame (East, North, Zenith) of the point,
304 * scaled to match radians in the horizontal plane and meters along the vertical axis
305 * @exception RuggedException if points cannot be converted to geodetic coordinates
306 */
307 public Vector3D convertLos(final Vector3D primary, final Vector3D secondary)
308 throws RuggedException {
309 try {
310
311 // switch to geodetic coordinates using primary point as reference
312 final GeodeticPoint point = transform(primary, getBodyFrame(), null);
313 final Vector3D los = secondary.subtract(primary);
314
315 // convert line of sight
316 return convertLos(point, los);
317
318 } catch (OrekitException oe) {
319 throw new RuggedException(oe, oe.getSpecifier(), oe.getParts());
320 }
321 }
322
323 /** Transform a cartesian point to a surface-relative point.
324 * @param point cartesian point (m)
325 * @param frame frame in which cartesian point is expressed
326 * @param date date of the computation (used for frames conversions)
327 * @param centralLongitude reference longitude lc such that the point longitude will
328 * be normalized between lc-π and lc+π (rad)
329 * @return point at the same location but as a surface-relative point
330 * @exception OrekitException if point cannot be converted to body frame
331 */
332 public NormalizedGeodeticPoint transform(final Vector3D point, final Frame frame, final AbsoluteDate date,
333 final double centralLongitude)
334 throws OrekitException {
335 final GeodeticPoint gp = transform(point, frame, date);
336 return new NormalizedGeodeticPoint(gp.getLatitude(), gp.getLongitude(), gp.getAltitude(),
337 centralLongitude);
338 }
339
340 }