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