1 /* Copyright 2002-2024 CS GROUP
2 * Licensed to CS GROUP (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.bodies;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.CalculusFieldElement;
22 import org.hipparchus.exception.MathRuntimeException;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.geometry.euclidean.twod.FieldVector2D;
26 import org.hipparchus.geometry.euclidean.twod.Vector2D;
27 import org.hipparchus.util.FastMath;
28 import org.hipparchus.util.MathArrays;
29 import org.hipparchus.util.Precision;
30 import org.orekit.errors.OrekitException;
31 import org.orekit.errors.OrekitMessages;
32 import org.orekit.frames.Frame;
33
34 /**
35 * Modeling of a general three-axes ellipsoid.
36 * @since 7.0
37 * @author Luc Maisonobe
38 */
39 public class Ellipsoid implements Serializable {
40
41 /** Serializable UID. */
42 private static final long serialVersionUID = 20140924L;
43
44 /** Frame at the ellipsoid center, aligned with principal axes. */
45 private final Frame frame;
46
47 /** First semi-axis length. */
48 private final double a;
49
50 /** Second semi-axis length. */
51 private final double b;
52
53 /** Third semi-axis length. */
54 private final double c;
55
56 /** Simple constructor.
57 * @param frame at the ellipsoid center, aligned with principal axes
58 * @param a first semi-axis length
59 * @param b second semi-axis length
60 * @param c third semi-axis length
61 */
62 public Ellipsoid(final Frame frame, final double a, final double b, final double c) {
63 this.frame = frame;
64 this.a = a;
65 this.b = b;
66 this.c = c;
67 }
68
69 /** Get the length of the first semi-axis.
70 * @return length of the first semi-axis (m)
71 */
72 public double getA() {
73 return a;
74 }
75
76 /** Get the length of the second semi-axis.
77 * @return length of the second semi-axis (m)
78 */
79 public double getB() {
80 return b;
81 }
82
83 /** Get the length of the third semi-axis.
84 * @return length of the third semi-axis (m)
85 */
86 public double getC() {
87 return c;
88 }
89
90 /** Get the ellipsoid central frame.
91 * @return ellipsoid central frame
92 */
93 public Frame getFrame() {
94 return frame;
95 }
96
97 /** Check if a point is inside the ellipsoid.
98 * @param point point to check, in the ellipsoid frame
99 * @return true if the point is inside the ellipsoid
100 * (or exactly on ellipsoid surface)
101 * @since 7.1
102 */
103 public boolean isInside(final Vector3D point) {
104 final double scaledX = point.getX() / a;
105 final double scaledY = point.getY() / b;
106 final double scaledZ = point.getZ() / c;
107 return scaledX * scaledX + scaledY * scaledY + scaledZ * scaledZ <= 1.0;
108 }
109
110 /** Check if a point is inside the ellipsoid.
111 * @param point point to check, in the ellipsoid frame
112 * @return true if the point is inside the ellipsoid
113 * (or exactly on ellipsoid surface)
114 * @param <T> the type of the field elements
115 * @since 12.0
116 */
117 public <T extends CalculusFieldElement<T>> boolean isInside(final FieldVector3D<T> point) {
118 final T scaledX = point.getX().divide(a);
119 final T scaledY = point.getY().divide(b);
120 final T scaledZ = point.getZ().divide(c);
121 final T d2 = scaledX.multiply(scaledX).add(scaledY.multiply(scaledY)).add(scaledZ.multiply(scaledZ));
122 return d2.subtract(1.0).getReal() <= 0.0;
123 }
124
125 /** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane.
126 * @param planePoint point belonging to the plane, in the ellipsoid frame
127 * @param planeNormal normal of the plane, in the ellipsoid frame
128 * @return plane section or null if there are no intersections
129 * @exception MathRuntimeException if the norm of planeNormal is null
130 */
131 public Ellipse getPlaneSection(final Vector3D planePoint, final Vector3D planeNormal)
132 throws MathRuntimeException {
133
134 // we define the points Q in the plane using two free variables τ and υ as:
135 // Q = P + τ u + υ v
136 // where u and v are two unit vectors belonging to the plane
137 // Q belongs to the 3D ellipsoid so:
138 // (xQ / a)² + (yQ / b)² + (zQ / c)² = 1
139 // combining both equations, we get:
140 // (xP² + 2 xP (τ xU + υ xV) + (τ xU + υ xV)²) / a²
141 // + (yP² + 2 yP (τ yU + υ yV) + (τ yU + υ yV)²) / b²
142 // + (zP² + 2 zP (τ zU + υ zV) + (τ zU + υ zV)²) / c²
143 // = 1
144 // which can be rewritten:
145 // α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
146 // with
147 // α = xU² / a² + yU² / b² + zU² / c² > 0
148 // β = xV² / a² + yV² / b² + zV² / c² > 0
149 // γ = xU xV / a² + yU yV / b² + zU zV / c²
150 // δ = xP xU / a² + yP yU / b² + zP zU / c²
151 // ε = xP xV / a² + yP yV / b² + zP zV / c²
152 // ζ = xP² / a² + yP² / b² + zP² / c² - 1
153 // this is the equation of a conic (here an ellipse)
154 // Of course, we note that if the point P belongs to the ellipsoid
155 // then ζ = 0 and the equation holds at point P since τ = 0 and υ = 0
156 final Vector3D u = planeNormal.orthogonal();
157 final Vector3D v = Vector3D.crossProduct(planeNormal, u).normalize();
158 final double xUOa = u.getX() / a;
159 final double yUOb = u.getY() / b;
160 final double zUOc = u.getZ() / c;
161 final double xVOa = v.getX() / a;
162 final double yVOb = v.getY() / b;
163 final double zVOc = v.getZ() / c;
164 final double xPOa = planePoint.getX() / a;
165 final double yPOb = planePoint.getY() / b;
166 final double zPOc = planePoint.getZ() / c;
167 final double alpha = xUOa * xUOa + yUOb * yUOb + zUOc * zUOc;
168 final double beta = xVOa * xVOa + yVOb * yVOb + zVOc * zVOc;
169 final double gamma = MathArrays.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc);
170 final double delta = MathArrays.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc);
171 final double epsilon = MathArrays.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc);
172 final double zeta = MathArrays.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, 1, -1);
173
174 // reduce the general equation α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
175 // to canonical form (λ/l)² + (μ/m)² = 1
176 // using a coordinates change
177 // τ = τC + λ cosθ - μ sinθ
178 // υ = υC + λ sinθ + μ cosθ
179 // or equivalently
180 // λ = (τ - τC) cosθ + (υ - υC) sinθ
181 // μ = - (τ - τC) sinθ + (υ - υC) cosθ
182 // τC and υC are the coordinates of the 2D ellipse center with respect to P
183 // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest)
184 // θ is the angle of the 2D ellipse axis corresponding to axis with length 2l
185
186 // choose θ in order to cancel the coupling term in λμ
187 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
188 // with C = 2[(β - α) cosθ sinθ + γ (cos²θ - sin²θ)]
189 // hence the term is cancelled when θ = arctan(t), with γ t² + (α - β) t - γ = 0
190 // As the solutions of the quadratic equation obey t₁t₂ = -1, they correspond to
191 // angles θ in quadrature to each other. Selecting one solution or the other simply
192 // exchanges the principal axes. As we don't care about which axis we want as the
193 // first one, we select an arbitrary solution
194 final double tanTheta;
195 if (FastMath.abs(gamma) < Precision.SAFE_MIN) {
196 tanTheta = 0.0;
197 } else {
198 final double bMA = beta - alpha;
199 tanTheta = (bMA >= 0) ?
200 (-2 * gamma / (bMA + FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))) :
201 (-2 * gamma / (bMA - FastMath.sqrt(bMA * bMA + 4 * gamma * gamma)));
202 }
203 final double tan2 = tanTheta * tanTheta;
204 final double cos2 = 1 / (1 + tan2);
205 final double sin2 = tan2 * cos2;
206 final double cosSin = tanTheta * cos2;
207 final double cos = FastMath.sqrt(cos2);
208 final double sin = tanTheta * cos;
209
210 // choose τC and υC in order to cancel the linear terms in λ and μ
211 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
212 // with D = 2[ (α τC + γ υC + δ) cosθ + (γ τC + β υC + ε) sinθ]
213 // E = 2[-(α τC + γ υC + δ) sinθ + (γ τC + β υC + ε) cosθ]
214 // θ can be eliminated by combining the equations
215 // D cosθ - E sinθ = 2[α τC + γ υC + δ]
216 // E cosθ + D sinθ = 2[γ τC + β υC + ε]
217 // hence the terms D and E are both cancelled (regardless of θ) when
218 // τC = (β δ - γ ε) / (γ² - α β)
219 // υC = (α ε - γ δ) / (γ² - α β)
220 final double denom = MathArrays.linearCombination(gamma, gamma, -alpha, beta);
221 final double tauC = MathArrays.linearCombination(beta, delta, -gamma, epsilon) / denom;
222 final double nuC = MathArrays.linearCombination(alpha, epsilon, -gamma, delta) / denom;
223
224 // compute l and m
225 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
226 // with A = α cos²θ + β sin²θ + 2 γ cosθ sinθ
227 // B = α sin²θ + β cos²θ - 2 γ cosθ sinθ
228 // F = α τC² + β υC² + 2 γ τC υC + 2 δ τC + 2 ε υC + ζ
229 // hence we compute directly l = √(-F/A) and m = √(-F/B)
230 final double twogcs = 2 * gamma * cosSin;
231 final double bigA = alpha * cos2 + beta * sin2 + twogcs;
232 final double bigB = alpha * sin2 + beta * cos2 - twogcs;
233 final double bigF = (alpha * tauC + 2 * (gamma * nuC + delta)) * tauC +
234 (beta * nuC + 2 * epsilon) * nuC + zeta;
235 final double l = FastMath.sqrt(-bigF / bigA);
236 final double m = FastMath.sqrt(-bigF / bigB);
237 if (Double.isNaN(l + m)) {
238 // the plane does not intersect the ellipsoid
239 return null;
240 }
241
242 if (l > m) {
243 return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v),
244 new Vector3D( cos, u, sin, v),
245 new Vector3D(-sin, u, cos, v),
246 l, m, frame);
247 } else {
248 return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v),
249 new Vector3D(sin, u, -cos, v),
250 new Vector3D(cos, u, sin, v),
251 m, l, frame);
252 }
253
254 }
255
256 /** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane.
257 * @param planePoint point belonging to the plane, in the ellipsoid frame
258 * @param planeNormal normal of the plane, in the ellipsoid frame
259 * @return plane section or null if there are no intersections
260 * @exception MathRuntimeException if the norm of planeNormal is null
261 * @param <T> the type of the field elements
262 * @since 12.0
263 */
264 public <T extends CalculusFieldElement<T>> FieldEllipse<T> getPlaneSection(final FieldVector3D<T> planePoint, final FieldVector3D<T> planeNormal)
265 throws MathRuntimeException {
266
267 final T zero = planePoint.getX().getField().getZero();
268 final T one = planePoint.getX().getField().getOne();
269
270 // we define the points Q in the plane using two free variables τ and υ as:
271 // Q = P + τ u + υ v
272 // where u and v are two unit vectors belonging to the plane
273 // Q belongs to the 3D ellipsoid so:
274 // (xQ / a)² + (yQ / b)² + (zQ / c)² = 1
275 // combining both equations, we get:
276 // (xP² + 2 xP (τ xU + υ xV) + (τ xU + υ xV)²) / a²
277 // + (yP² + 2 yP (τ yU + υ yV) + (τ yU + υ yV)²) / b²
278 // + (zP² + 2 zP (τ zU + υ zV) + (τ zU + υ zV)²) / c²
279 // = 1
280 // which can be rewritten:
281 // α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
282 // with
283 // α = xU² / a² + yU² / b² + zU² / c² > 0
284 // β = xV² / a² + yV² / b² + zV² / c² > 0
285 // γ = xU xV / a² + yU yV / b² + zU zV / c²
286 // δ = xP xU / a² + yP yU / b² + zP zU / c²
287 // ε = xP xV / a² + yP yV / b² + zP zV / c²
288 // ζ = xP² / a² + yP² / b² + zP² / c² - 1
289 // this is the equation of a conic (here an ellipse)
290 // Of course, we note that if the point P belongs to the ellipsoid
291 // then ζ = 0 and the equation holds at point P since τ = 0 and υ = 0
292 final FieldVector3D<T> u = planeNormal.orthogonal();
293 final FieldVector3D<T> v = FieldVector3D.crossProduct(planeNormal, u).normalize();
294 final T xUOa = u.getX().divide(a);
295 final T yUOb = u.getY().divide(b);
296 final T zUOc = u.getZ().divide(c);
297 final T xVOa = v.getX().divide(a);
298 final T yVOb = v.getY().divide(b);
299 final T zVOc = v.getZ().divide(c);
300 final T xPOa = planePoint.getX().divide(a);
301 final T yPOb = planePoint.getY().divide(b);
302 final T zPOc = planePoint.getZ().divide(c);
303 final T alpha = xUOa.multiply(xUOa).add(yUOb.multiply(yUOb)).add(zUOc.multiply(zUOc));
304 final T beta = xVOa.multiply(xVOa).add(yVOb.multiply(yVOb)).add(zVOc.multiply(zVOc));
305 final T gamma = alpha.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc);
306 final T delta = alpha.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc);
307 final T epsilon = alpha.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc);
308 final T zeta = alpha.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, one, one.negate());
309
310 // reduce the general equation α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
311 // to canonical form (λ/l)² + (μ/m)² = 1
312 // using a coordinates change
313 // τ = τC + λ cosθ - μ sinθ
314 // υ = υC + λ sinθ + μ cosθ
315 // or equivalently
316 // λ = (τ - τC) cosθ + (υ - υC) sinθ
317 // μ = - (τ - τC) sinθ + (υ - υC) cosθ
318 // τC and υC are the coordinates of the 2D ellipse center with respect to P
319 // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest)
320 // θ is the angle of the 2D ellipse axis corresponding to axis with length 2l
321
322 // choose θ in order to cancel the coupling term in λμ
323 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
324 // with C = 2[(β - α) cosθ sinθ + γ (cos²θ - sin²θ)]
325 // hence the term is cancelled when θ = arctan(t), with γ t² + (α - β) t - γ = 0
326 // As the solutions of the quadratic equation obey t₁t₂ = -1, they correspond to
327 // angles θ in quadrature to each other. Selecting one solution or the other simply
328 // exchanges the principal axes. As we don't care about which axis we want as the
329 // first one, we select an arbitrary solution
330 final T tanTheta;
331 if (FastMath.abs(gamma.getReal()) < Precision.SAFE_MIN) {
332 tanTheta = zero;
333 } else {
334 final T bMA = beta.subtract(alpha);
335 tanTheta = (bMA.getReal() >= 0) ?
336 gamma.multiply(-2).divide(bMA.add(FastMath.sqrt(bMA.multiply(bMA).add(gamma.multiply(gamma).multiply(4))))) :
337 gamma.multiply(-2).divide(bMA.subtract(FastMath.sqrt(bMA.multiply(bMA).add(gamma.multiply(gamma).multiply(4)))));
338 }
339 final T tan2 = tanTheta.multiply(tanTheta);
340 final T cos2 = tan2.add(1).reciprocal();
341 final T sin2 = tan2.multiply(cos2);
342 final T cosSin = tanTheta.multiply(cos2);
343 final T cos = FastMath.sqrt(cos2);
344 final T sin = tanTheta.multiply(cos);
345
346 // choose τC and υC in order to cancel the linear terms in λ and μ
347 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
348 // with D = 2[ (α τC + γ υC + δ) cosθ + (γ τC + β υC + ε) sinθ]
349 // E = 2[-(α τC + γ υC + δ) sinθ + (γ τC + β υC + ε) cosθ]
350 // θ can be eliminated by combining the equations
351 // D cosθ - E sinθ = 2[α τC + γ υC + δ]
352 // E cosθ + D sinθ = 2[γ τC + β υC + ε]
353 // hence the terms D and E are both cancelled (regardless of θ) when
354 // τC = (β δ - γ ε) / (γ² - α β)
355 // υC = (α ε - γ δ) / (γ² - α β)
356 final T invDenom = gamma.linearCombination(gamma, gamma, alpha.negate(), beta).reciprocal();
357 final T tauC = gamma.linearCombination(beta, delta, gamma.negate(), epsilon).multiply(invDenom);
358 final T nuC = gamma.linearCombination(alpha, epsilon, gamma.negate(), delta).multiply(invDenom);
359
360 // compute l and m
361 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
362 // with A = α cos²θ + β sin²θ + 2 γ cosθ sinθ
363 // B = α sin²θ + β cos²θ - 2 γ cosθ sinθ
364 // F = α τC² + β υC² + 2 γ τC υC + 2 δ τC + 2 ε υC + ζ
365 // hence we compute directly l = √(-F/A) and m = √(-F/B)
366 final T twogcs = gamma.multiply(cosSin).multiply(2);
367 final T bigA = alpha.multiply(cos2).add(beta.multiply(sin2)).add(twogcs);
368 final T bigB = alpha.multiply(sin2).add(beta.multiply(cos2)).subtract(twogcs);
369 final T bigFN = alpha.multiply(tauC).add(gamma.multiply(nuC).add(delta).multiply(2)).multiply(tauC).
370 add(beta.multiply(nuC).add(epsilon.multiply(2)).multiply(nuC)).
371 add(zeta).
372 negate();
373 final T l = FastMath.sqrt(bigFN.divide(bigA));
374 final T m = FastMath.sqrt(bigFN.divide(bigB));
375 if (l.add(m).isNaN()) {
376 // the plane does not intersect the ellipsoid
377 return null;
378 }
379
380 if (l.subtract(m).getReal() > 0) {
381 return new FieldEllipse<>(new FieldVector3D<>(tauC.getField().getOne(), planePoint, tauC, u, nuC, v),
382 new FieldVector3D<>(cos, u, sin, v),
383 new FieldVector3D<>(sin.negate(), u, cos, v),
384 l, m, frame);
385 } else {
386 return new FieldEllipse<>(new FieldVector3D<>(tauC.getField().getOne(), planePoint, tauC, u, nuC, v),
387 new FieldVector3D<>(sin, u, cos.negate(), v),
388 new FieldVector3D<>(cos, u, sin, v),
389 m, l, frame);
390 }
391
392 }
393
394 /** Find a point on ellipsoid limb, as seen by an external observer.
395 * @param observer observer position in ellipsoid frame
396 * @param outside point outside ellipsoid in ellipsoid frame, defining the phase around limb
397 * @return point on ellipsoid limb
398 * @exception MathRuntimeException if ellipsoid center, observer and outside
399 * points are aligned
400 * @since 7.1
401 */
402 public Vector3D pointOnLimb(final Vector3D observer, final Vector3D outside)
403 throws MathRuntimeException {
404
405 // There is no limb if we are inside the ellipsoid
406 if (isInside(observer)) {
407 throw new OrekitException(OrekitMessages.POINT_INSIDE_ELLIPSOID);
408 }
409 // Cut the ellipsoid, to find an elliptical plane section
410 final Vector3D normal = Vector3D.crossProduct(observer, outside);
411 final Ellipse section = getPlaneSection(Vector3D.ZERO, normal);
412
413 // the point on limb is tangential to the ellipse
414 // if T(xt, yt) is an ellipse point at which the tangent is drawn
415 // if O(xo, yo) is a point outside of the ellipse belonging to the tangent at T,
416 // then the two following equations holds:
417 // (1) a² yt² + b² xt² = a² b² (T belongs to the ellipse)
418 // (2) a² yt yo + b² xt xo = a² b² (TP is tangent to the ellipse)
419 // using the second equation to eliminate yt from the first equation, we get
420 // b² (a² - xt xo)² + a² xt² yo² = a⁴ yo²
421 // (3) (a² yo² + b² xo²) xt² - 2 a² b² xo xt + a⁴ (b² - yo²) = 0
422 // which can easily be solved for xt
423
424 // To avoid numerical errors, the x and y coordinates in the ellipse plane are normalized using:
425 // x' = x / a and y' = y / b
426 //
427 // This gives:
428 // (1) y't² + x't² = 1
429 // (2) y't y'o + x't x'o = 1
430 //
431 // And finally:
432 // (3) (x'o² + y'o²) x't² - 2 x't x'o + 1 - y'o² = 0
433 //
434 // Solving for x't, we get the reduced discriminant:
435 // delta' = beta'² - alpha' * gamma'
436 //
437 // With:
438 // beta' = x'o
439 // alpha' = x'o² + y'o²
440 // gamma' = 1 - y'o²
441 //
442 // Simplifying to cancel a term of x'o².
443 // delta' = y'o² (x'o² + y'o² - 1) = y'o² (alpha' - 1)
444 //
445 // After solving for xt1, xt2 using (3) the values are substituted into (2) to
446 // compute yt1, yt2. Then terms of x'o may be canceled from the expressions for
447 // yt1 and yt2. Additionally a point discontinuity is removed at y'o=0 from both
448 // yt1 and yt2.
449 //
450 // y't1 = (y'o - x'o d) / (x'o² + y'o²)
451 // y't2 = (x'o y'o + d) / (x + sqrt(delta'))
452 //
453 // where:
454 // d = sign(y'o) sqrt(alpha' - 1)
455
456 // Get the point in ellipse plane frame (2D)
457 final Vector2D observer2D = section.toPlane(observer);
458
459 // Normalize and compute intermediary terms
460 final double ap = section.getA();
461 final double bp = section.getB();
462 final double xpo = observer2D.getX() / ap;
463 final double ypo = observer2D.getY() / bp;
464 final double xpo2 = xpo * xpo;
465 final double ypo2 = ypo * ypo;
466 final double alphap = ypo2 + xpo2;
467 final double gammap = 1. - ypo2;
468
469 // Compute the roots
470 // We know there are two solutions as we already checked the point is outside ellipsoid
471 final double sqrt = FastMath.sqrt(alphap - 1);
472 final double sqrtp = FastMath.abs(ypo) * sqrt;
473 final double sqrtSigned = FastMath.copySign(sqrt, ypo);
474
475 // Compute the roots (ordered by value)
476 final double xpt1;
477 final double xpt2;
478 final double ypt1;
479 final double ypt2;
480 if (xpo > 0) {
481 final double s = xpo + sqrtp;
482 // xpt1 = (beta' + sqrt(delta')) / alpha' (with beta' = x'o)
483 xpt1 = s / alphap;
484 // x't2 = gamma' / (beta' + sqrt(delta')) since x't1 * x't2 = gamma' / alpha'
485 xpt2 = gammap / s;
486 // Get the corresponding values of y't
487 ypt1 = (ypo - xpo * sqrtSigned) / alphap;
488 ypt2 = (xpo * ypo + sqrtSigned) / s;
489 } else {
490 final double s = xpo - sqrtp;
491 // x't1 and x't2 are reverted compared to previous solution
492 xpt1 = gammap / s;
493 xpt2 = s / alphap;
494 // Get the corresponding values of y't
495 ypt2 = (ypo + xpo * sqrtSigned) / alphap;
496 ypt1 = (xpo * ypo - sqrtSigned) / s;
497 }
498
499 // De-normalize and express the two solutions in 3D
500 final Vector3D tp1 = section.toSpace(new Vector2D(ap * xpt1, bp * ypt1));
501 final Vector3D tp2 = section.toSpace(new Vector2D(ap * xpt2, bp * ypt2));
502
503 // Return the limb point in the direction of the outside point
504 return Vector3D.distance(tp1, outside) <= Vector3D.distance(tp2, outside) ? tp1 : tp2;
505
506 }
507
508 /** Find a point on ellipsoid limb, as seen by an external observer.
509 * @param observer observer position in ellipsoid frame
510 * @param outside point outside ellipsoid in ellipsoid frame, defining the phase around limb
511 * @return point on ellipsoid limb
512 * @exception MathRuntimeException if ellipsoid center, observer and outside
513 * points are aligned
514 * @param <T> the type of the field elements
515 * @since 12.0
516 */
517 public <T extends CalculusFieldElement<T>> FieldVector3D<T> pointOnLimb(final FieldVector3D<T> observer, final FieldVector3D<T> outside)
518 throws MathRuntimeException {
519
520 // There is no limb if we are inside the ellipsoid
521 if (isInside(observer)) {
522 throw new OrekitException(OrekitMessages.POINT_INSIDE_ELLIPSOID);
523 }
524 // Cut the ellipsoid, to find an elliptical plane section
525 final FieldVector3D<T> normal = FieldVector3D.crossProduct(observer, outside);
526 final FieldEllipse<T> section = getPlaneSection(FieldVector3D.getZero(observer.getX().getField()), normal);
527
528 // the point on limb is tangential to the ellipse
529 // if T(xt, yt) is an ellipse point at which the tangent is drawn
530 // if O(xo, yo) is a point outside of the ellipse belonging to the tangent at T,
531 // then the two following equations holds:
532 // (1) a² yt² + b² xt² = a² b² (T belongs to the ellipse)
533 // (2) a² yt yo + b² xt xo = a² b² (TP is tangent to the ellipse)
534 // using the second equation to eliminate yt from the first equation, we get
535 // b² (a² - xt xo)² + a² xt² yo² = a⁴ yo²
536 // (3) (a² yo² + b² xo²) xt² - 2 a² b² xo xt + a⁴ (b² - yo²) = 0
537 // which can easily be solved for xt
538
539 // To avoid numerical errors, the x and y coordinates in the ellipse plane are normalized using:
540 // x' = x / a and y' = y / b
541 //
542 // This gives:
543 // (1) y't² + x't² = 1
544 // (2) y't y'o + x't x'o = 1
545 //
546 // And finally:
547 // (3) (x'o² + y'o²) x't² - 2 x't x'o + 1 - y'o² = 0
548 //
549 // Solving for x't, we get the reduced discriminant:
550 // delta' = beta'² - alpha' * gamma'
551 //
552 // With:
553 // beta' = x'o
554 // alpha' = x'o² + y'o²
555 // gamma' = 1 - y'o²
556 //
557 // Simplifying to cancel a term of x'o².
558 // delta' = y'o² (x'o² + y'o² - 1) = y'o² (alpha' - 1)
559 //
560 // After solving for xt1, xt2 using (3) the values are substituted into (2) to
561 // compute yt1, yt2. Then terms of x'o may be canceled from the expressions for
562 // yt1 and yt2. Additionally a point discontinuity is removed at y'o=0 from both
563 // yt1 and yt2.
564 //
565 // y't1 = (y'o - x'o d) / (x'o² + y'o²)
566 // y't2 = (x'o y'o + d) / (x + sqrt(delta'))
567 //
568 // where:
569 // d = sign(y'o) sqrt(alpha' - 1)
570
571 // Get the point in ellipse plane frame (2D)
572 final FieldVector2D<T> observer2D = section.toPlane(observer);
573
574 // Normalize and compute intermediary terms
575 final T ap = section.getA();
576 final T bp = section.getB();
577 final T xpo = observer2D.getX().divide(ap);
578 final T ypo = observer2D.getY().divide(bp);
579 final T xpo2 = xpo.multiply(xpo);
580 final T ypo2 = ypo.multiply(ypo);
581 final T alphap = ypo2.add(xpo2);
582 final T gammap = ypo2.negate().add(1);
583
584 // Compute the roots
585 // We know there are two solutions as we already checked the point is outside ellipsoid
586 final T sqrt = FastMath.sqrt(alphap.subtract(1));
587 final T sqrtp = FastMath.abs(ypo).multiply(sqrt);
588 final T sqrtSigned = FastMath.copySign(sqrt, ypo);
589
590 // Compute the roots (ordered by value)
591 final T xpt1;
592 final T xpt2;
593 final T ypt1;
594 final T ypt2;
595 if (xpo.getReal() > 0) {
596 final T s = xpo.add(sqrtp);
597 // xpt1 = (beta' + sqrt(delta')) / alpha' (with beta' = x'o)
598 xpt1 = s.divide(alphap);
599 // x't2 = gamma' / (beta' + sqrt(delta')) since x't1 * x't2 = gamma' / alpha'
600 xpt2 = gammap.divide(s);
601 // Get the corresponding values of y't
602 ypt1 = ypo.subtract(xpo.multiply(sqrtSigned)).divide(alphap);
603 ypt2 = xpo.multiply(ypo).add(sqrtSigned).divide(s);
604 } else {
605 final T s = xpo.subtract(sqrtp);
606 // x't1 and x't2 are reverted compared to previous solution
607 xpt1 = gammap.divide(s);
608 xpt2 = s.divide(alphap);
609 // Get the corresponding values of y't
610 ypt2 = ypo.add(xpo.multiply(sqrtSigned)).divide(alphap);
611 ypt1 = xpo.multiply(ypo).subtract(sqrtSigned).divide(s);
612 }
613
614 // De-normalize and express the two solutions in 3D
615 final FieldVector3D<T> tp1 = section.toSpace(new FieldVector2D<>(ap.multiply(xpt1), bp.multiply(ypt1)));
616 final FieldVector3D<T> tp2 = section.toSpace(new FieldVector2D<>(ap.multiply(xpt2), bp.multiply(ypt2)));
617
618 // Return the limb point in the direction of the outside point
619 return FieldVector3D.distance(tp1, outside).subtract(FieldVector3D.distance(tp2, outside)).getReal() <= 0 ? tp1 : tp2;
620
621 }
622
623 }