1 /* Copyright 2002-2016 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.bodies;
18
19 import java.io.Serializable;
20
21 import org.hipparchus.exception.MathRuntimeException;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.geometry.euclidean.twod.Vector2D;
24 import org.hipparchus.util.FastMath;
25 import org.hipparchus.util.MathArrays;
26 import org.hipparchus.util.Precision;
27 import org.orekit.errors.OrekitException;
28 import org.orekit.errors.OrekitMessages;
29 import org.orekit.frames.Frame;
30
31 /**
32 * Modeling of a general three-axes ellipsoid. <p>
33 * @since 7.0
34 * @author Luc Maisonobe
35 */
36 public class Ellipsoid implements Serializable {
37
38 /** Serializable UID. */
39 private static final long serialVersionUID = 20140924L;
40
41 /** Frame at the ellipsoid center, aligned with principal axes. */
42 private final Frame frame;
43
44 /** First semi-axis length. */
45 private final double a;
46
47 /** Second semi-axis length. */
48 private final double b;
49
50 /** Third semi-axis length. */
51 private final double c;
52
53 /** Simple constructor.
54 * @param frame at the ellipsoid center, aligned with principal axes
55 * @param a first semi-axis length
56 * @param b second semi-axis length
57 * @param c third semi-axis length
58 */
59 public Ellipsoid(final Frame frame, final double a, final double b, final double c) {
60 this.frame = frame;
61 this.a = a;
62 this.b = b;
63 this.c = c;
64 }
65
66 /** Get the length of the first semi-axis.
67 * @return length of the first semi-axis (m)
68 */
69 public double getA() {
70 return a;
71 }
72
73 /** Get the length of the second semi-axis.
74 * @return length of the second semi-axis (m)
75 */
76 public double getB() {
77 return b;
78 }
79
80 /** Get the length of the third semi-axis.
81 * @return length of the third semi-axis (m)
82 */
83 public double getC() {
84 return c;
85 }
86
87 /** Get the ellipsoid central frame.
88 * @return ellipsoid central frame
89 */
90 public Frame getFrame() {
91 return frame;
92 }
93
94 /** Check if a point is inside the ellipsoid.
95 * @param point point to check, in the ellipsoid frame
96 * @return true if the point is inside the ellipsoid
97 * (or exactly on ellipsoid surface)
98 * @since 7.1
99 */
100 public boolean isInside(final Vector3D point) {
101 final double scaledX = point.getX() / a;
102 final double scaledY = point.getY() / b;
103 final double scaledZ = point.getZ() / c;
104 return scaledX * scaledX + scaledY * scaledY + scaledZ * scaledZ <= 1.0;
105 }
106
107 /** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane.
108 * @param planePoint point belonging to the plane, in the ellipsoid frame
109 * @param planeNormal normal of the plane, in the ellipsoid frame
110 * @return plane section or null if there are no intersections
111 * @exception MathRuntimeException if the norm of planeNormal is null
112 */
113 public Ellipse getPlaneSection(final Vector3D planePoint, final Vector3D planeNormal)
114 throws MathRuntimeException {
115
116 // we define the points Q in the plane using two free variables τ and υ as:
117 // Q = P + τ u + υ v
118 // where u and v are two unit vectors belonging to the plane
119 // Q belongs to the 3D ellipsoid so:
120 // (xQ / a)² + (yQ / b)² + (zQ / c)² = 1
121 // combining both equations, we get:
122 // (xP² + 2 xP (τ xU + υ xV) + (τ xU + υ xV)²) / a²
123 // + (yP² + 2 yP (τ yU + υ yV) + (τ yU + υ yV)²) / b²
124 // + (zP² + 2 zP (τ zU + υ zV) + (τ zU + υ zV)²) / c²
125 // = 1
126 // which can be rewritten:
127 // α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
128 // with
129 // α = xU² / a² + yU² / b² + zU² / c² > 0
130 // β = xV² / a² + yV² / b² + zV² / c² > 0
131 // γ = xU xV / a² + yU yV / b² + zU zV / c²
132 // δ = xP xU / a² + yP yU / b² + zP zU / c²
133 // ε = xP xV / a² + yP yV / b² + zP zV / c²
134 // ζ = xP² / a² + yP² / b² + zP² / c² - 1
135 // this is the equation of a conic (here an ellipse)
136 // Of course, we note that if the point P belongs to the ellipsoid
137 // then ζ = 0 and the equation holds at point P since τ = 0 and υ = 0
138 final Vector3D u = planeNormal.orthogonal();
139 final Vector3D v = Vector3D.crossProduct(planeNormal, u).normalize();
140 final double xUOa = u.getX() / a;
141 final double yUOb = u.getY() / b;
142 final double zUOc = u.getZ() / c;
143 final double xVOa = v.getX() / a;
144 final double yVOb = v.getY() / b;
145 final double zVOc = v.getZ() / c;
146 final double xPOa = planePoint.getX() / a;
147 final double yPOb = planePoint.getY() / b;
148 final double zPOc = planePoint.getZ() / c;
149 final double alpha = xUOa * xUOa + yUOb * yUOb + zUOc * zUOc;
150 final double beta = xVOa * xVOa + yVOb * yVOb + zVOc * zVOc;
151 final double gamma = MathArrays.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc);
152 final double delta = MathArrays.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc);
153 final double epsilon = MathArrays.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc);
154 final double zeta = MathArrays.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, 1, -1);
155
156 // reduce the general equation α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
157 // to canonical form (λ/l)² + (μ/m)² = 1
158 // using a coordinates change
159 // τ = τC + λ cosθ - μ sinθ
160 // υ = υC + λ sinθ + μ cosθ
161 // or equivalently
162 // λ = (τ - τC) cosθ + (υ - υC) sinθ
163 // μ = - (τ - τC) sinθ + (υ - υC) cosθ
164 // τC and υC are the coordinates of the 2D ellipse center with respect to P
165 // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest)
166 // θ is the angle of the 2D ellipse axis corresponding to axis with length 2l
167
168 // choose θ in order to cancel the coupling term in λμ
169 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
170 // with C = 2[(β - α) cosθ sinθ + γ (cos²θ - sin²θ)]
171 // hence the term is cancelled when θ = arctan(t), with γ t² + (α - β) t - γ = 0
172 // As the solutions of the quadratic equation obey t₁t₂ = -1, they correspond to
173 // angles θ in quadrature to each other. Selecting one solution or the other simply
174 // exchanges the principal axes. As we don't care about which axis we want as the
175 // first one, we select an arbitrary solution
176 final double tanTheta;
177 if (FastMath.abs(gamma) < Precision.SAFE_MIN) {
178 tanTheta = 0.0;
179 } else {
180 final double bMA = beta - alpha;
181 tanTheta = (bMA >= 0) ?
182 (-2 * gamma / (bMA + FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))) :
183 (-2 * gamma / (bMA - FastMath.sqrt(bMA * bMA + 4 * gamma * gamma)));
184 }
185 final double tan2 = tanTheta * tanTheta;
186 final double cos2 = 1 / (1 + tan2);
187 final double sin2 = tan2 * cos2;
188 final double cosSin = tanTheta * cos2;
189 final double cos = FastMath.sqrt(cos2);
190 final double sin = tanTheta * cos;
191
192 // choose τC and υC in order to cancel the linear terms in λ and μ
193 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
194 // with D = 2[ (α τC + γ υC + δ) cosθ + (γ τC + β υC + ε) sinθ]
195 // E = 2[-(α τC + γ υC + δ) sinθ + (γ τC + β υC + ε) cosθ]
196 // θ can be eliminated by combining the equations
197 // D cosθ - E sinθ = 2[α τC + γ υC + δ]
198 // E cosθ + D sinθ = 2[γ τC + β υC + ε]
199 // hence the terms D and E are both cancelled (regardless of θ) when
200 // τC = (β δ - γ ε) / (γ² - α β)
201 // υC = (α ε - γ δ) / (γ² - α β)
202 final double denom = MathArrays.linearCombination(gamma, gamma, -alpha, beta);
203 final double tauC = MathArrays.linearCombination(beta, delta, -gamma, epsilon) / denom;
204 final double nuC = MathArrays.linearCombination(alpha, epsilon, -gamma, delta) / denom;
205
206 // compute l and m
207 // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
208 // with A = α cos²θ + β sin²θ + 2 γ cosθ sinθ
209 // B = α sin²θ + β cos²θ - 2 γ cosθ sinθ
210 // F = α τC² + β υC² + 2 γ τC υC + 2 δ τC + 2 ε υC + ζ
211 // hence we compute directly l = √(-F/A) and m = √(-F/B)
212 final double twogcs = 2 * gamma * cosSin;
213 final double bigA = alpha * cos2 + beta * sin2 + twogcs;
214 final double bigB = alpha * sin2 + beta * cos2 - twogcs;
215 final double bigF = (alpha * tauC + 2 * (gamma * nuC + delta)) * tauC +
216 (beta * nuC + 2 * epsilon) * nuC + zeta;
217 final double l = FastMath.sqrt(-bigF / bigA);
218 final double m = FastMath.sqrt(-bigF / bigB);
219 if (Double.isNaN(l + m)) {
220 // the plane does not intersect the ellipsoid
221 return null;
222 }
223
224 if (l > m) {
225 return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v),
226 new Vector3D( cos, u, sin, v),
227 new Vector3D(-sin, u, cos, v),
228 l, m, frame);
229 } else {
230 return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v),
231 new Vector3D(sin, u, -cos, v),
232 new Vector3D(cos, u, sin, v),
233 m, l, frame);
234 }
235
236 }
237
238 /** Find a point on ellipsoid limb, as seen by an external observer.
239 * @param observer observer position in ellipsoid frame
240 * @param outside point outside ellipsoid in ellipsoid frame, defining the phase around limb
241 * @return point on ellipsoid limb
242 * @exception OrekitException if the observer is inside the ellipsoid
243 * @exception MathRuntimeException if ellipsoid center, observer and outside
244 * points are aligned
245 * @since 7.1
246 */
247 public Vector3D pointOnLimb(final Vector3D observer, final Vector3D outside)
248 throws OrekitException, MathRuntimeException {
249
250 // there is no limb if we are inside the ellipsoid
251 if (isInside(observer)) {
252 throw new OrekitException(OrekitMessages.POINT_INSIDE_ELLIPSOID);
253 }
254 // cut the ellipsoid, to find an elliptical plane section
255 final Vector3D normal = Vector3D.crossProduct(observer, outside);
256 final Ellipse section = getPlaneSection(Vector3D.ZERO, normal);
257 final double a2 = section.getA() * section.getA();
258 final double b2 = section.getB() * section.getB();
259
260 // the point on limb is tangential to the ellipse
261 // if T(xt, yt) is an ellipse point at which the tangent is drawn
262 // if O(xo, yo) is a point outside of the ellipse belonging to the tangent at T,
263 // then the two following equations holds:
264 // a² yt² + b² xt² = a² b² (T belongs to the ellipse)
265 // a² yt yo + b² xt xo = a² b² (TP is tangent to the ellipse)
266 // using the second equation to eliminate yt from the first equation, we get
267 // b² (a² - xt xo)² + a² xt² yo² = a⁴ yo²
268 // (a² yo² + b² xo²) xt² - 2 a² b² xo xt + a⁴ (b² - yo²) = 0
269 // which can easily be solved for xt
270 final Vector2D observer2D = section.toPlane(observer);
271 final double xo = observer2D.getX();
272 final double yo = observer2D.getY();
273 final double xo2 = xo * xo;
274 final double yo2 = yo * yo;
275 final double alpha = a2 * yo2 + b2 * xo2;
276 final double beta = a2 * b2 * xo;
277 final double gamma = a2 * a2 * (b2 - yo2);
278 // we know there are two solutions as we already checked the point is outside ellipsoid
279 final double sqrt = FastMath.sqrt(beta * beta - alpha * gamma);
280 final double xt1;
281 final double xt2;
282 if (beta > 0) {
283 final double s = beta + sqrt;
284 xt1 = s / alpha;
285 xt2 = gamma / s;
286 } else {
287 final double s = beta - sqrt;
288 xt1 = gamma / s;
289 xt2 = s / alpha;
290 }
291
292 // we return the limb point in the direction of the outside point
293 final Vector3D t1 = section.toSpace(new Vector2D(xt1, b2 * (a2 - xt1 * xo) / (a2 * yo)));
294 final Vector3D t2 = section.toSpace(new Vector2D(xt2, b2 * (a2 - xt2 * xo) / (a2 * yo)));
295 return Vector3D.distance(t1, outside) <= Vector3D.distance(t2, outside) ? t1 : t2;
296
297 }
298
299 }