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