Ellipsoid.java

  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. import java.io.Serializable;

  19. import org.apache.commons.math3.exception.MathArithmeticException;
  20. import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  21. import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
  22. import org.apache.commons.math3.util.FastMath;
  23. import org.apache.commons.math3.util.MathArrays;
  24. import org.apache.commons.math3.util.Precision;
  25. import org.orekit.errors.OrekitException;
  26. import org.orekit.errors.OrekitMessages;
  27. import org.orekit.frames.Frame;

  28. /**
  29.  * Modeling of a general three-axes ellipsoid. <p>
  30.  * @since 7.0
  31.  * @author Luc Maisonobe
  32.  */
  33. public class Ellipsoid implements Serializable {

  34.     /** Serializable UID. */
  35.     private static final long serialVersionUID = 20140924L;

  36.     /** Frame at the ellipsoid center, aligned with principal axes. */
  37.     private final Frame frame;

  38.     /** First semi-axis length. */
  39.     private final double a;

  40.     /** Second semi-axis length. */
  41.     private final double b;

  42.     /** Third semi-axis length. */
  43.     private final double c;

  44.     /** Simple constructor.
  45.      * @param frame at the ellipsoid center, aligned with principal axes
  46.      * @param a first semi-axis length
  47.      * @param b second semi-axis length
  48.      * @param c third semi-axis length
  49.      */
  50.     public Ellipsoid(final Frame frame, final double a, final double b, final double c) {
  51.         this.frame = frame;
  52.         this.a     = a;
  53.         this.b     = b;
  54.         this.c     = c;
  55.     }

  56.     /** Get the length of the first semi-axis.
  57.      * @return length of the first semi-axis (m)
  58.      */
  59.     public double getA() {
  60.         return a;
  61.     }

  62.     /** Get the length of the second semi-axis.
  63.      * @return length of the second semi-axis (m)
  64.      */
  65.     public double getB() {
  66.         return b;
  67.     }

  68.     /** Get the length of the third semi-axis.
  69.      * @return length of the third semi-axis (m)
  70.      */
  71.     public double getC() {
  72.         return c;
  73.     }

  74.     /** Get the ellipsoid central frame.
  75.      * @return ellipsoid central frame
  76.      */
  77.     public Frame getFrame() {
  78.         return frame;
  79.     }

  80.     /** Check if a point is inside the ellipsoid.
  81.      * @param point point to check, in the ellipsoid frame
  82.      * @return true if the point is inside the ellipsoid
  83.      * (or exactly on ellipsoid surface)
  84.      * @since 7.1
  85.      */
  86.     public boolean isInside(final Vector3D point) {
  87.         final double scaledX = point.getX() / a;
  88.         final double scaledY = point.getY() / b;
  89.         final double scaledZ = point.getZ() / c;
  90.         return scaledX * scaledX + scaledY * scaledY + scaledZ * scaledZ <= 1.0;
  91.     }

  92.     /** Compute the 2D ellipse at the intersection of the 3D ellipsoid and a plane.
  93.      * @param planePoint point belonging to the plane, in the ellipsoid frame
  94.      * @param planeNormal normal of the plane, in the ellipsoid frame
  95.      * @return plane section or null if there are no intersections
  96.      */
  97.     public Ellipse getPlaneSection(final Vector3D planePoint, final Vector3D planeNormal) {

  98.         // we define the points Q in the plane using two free variables τ and υ as:
  99.         // Q = P + τ u + υ v
  100.         // where u and v are two unit vectors belonging to the plane
  101.         // Q belongs to the 3D ellipsoid so:
  102.         // (xQ / a)² + (yQ / b)² + (zQ / c)² = 1
  103.         // combining both equations, we get:
  104.         //   (xP² + 2 xP (τ xU + υ xV) + (τ xU + υ xV)²) / a²
  105.         // + (yP² + 2 yP (τ yU + υ yV) + (τ yU + υ yV)²) / b²
  106.         // + (zP² + 2 zP (τ zU + υ zV) + (τ zU + υ zV)²) / c²
  107.         // = 1
  108.         // which can be rewritten:
  109.         // α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
  110.         // with
  111.         // α =  xU²  / a² +  yU²  / b² +  zU²  / c² > 0
  112.         // β =  xV²  / a² +  yV²  / b² +  zV²  / c² > 0
  113.         // γ = xU xV / a² + yU yV / b² + zU zV / c²
  114.         // δ = xP xU / a² + yP yU / b² + zP zU / c²
  115.         // ε = xP xV / a² + yP yV / b² + zP zV / c²
  116.         // ζ =  xP²  / a² +  yP²  / b² +  zP²  / c² - 1
  117.         // this is the equation of a conic (here an ellipse)
  118.         // Of course, we note that if the point P belongs to the ellipsoid
  119.         // then ζ = 0 and the equation holds at point P since τ = 0 and υ = 0
  120.         final Vector3D u     = planeNormal.orthogonal();
  121.         final Vector3D v     = Vector3D.crossProduct(planeNormal, u).normalize();
  122.         final double xUOa    = u.getX() / a;
  123.         final double yUOb    = u.getY() / b;
  124.         final double zUOc    = u.getZ() / c;
  125.         final double xVOa    = v.getX() / a;
  126.         final double yVOb    = v.getY() / b;
  127.         final double zVOc    = v.getZ() / c;
  128.         final double xPOa    = planePoint.getX() / a;
  129.         final double yPOb    = planePoint.getY() / b;
  130.         final double zPOc    = planePoint.getZ() / c;
  131.         final double alpha   = xUOa * xUOa + yUOb * yUOb + zUOc * zUOc;
  132.         final double beta    = xVOa * xVOa + yVOb * yVOb + zVOc * zVOc;
  133.         final double gamma   = MathArrays.linearCombination(xUOa, xVOa, yUOb, yVOb, zUOc, zVOc);
  134.         final double delta   = MathArrays.linearCombination(xPOa, xUOa, yPOb, yUOb, zPOc, zUOc);
  135.         final double epsilon = MathArrays.linearCombination(xPOa, xVOa, yPOb, yVOb, zPOc, zVOc);
  136.         final double zeta    = MathArrays.linearCombination(xPOa, xPOa, yPOb, yPOb, zPOc, zPOc, 1, -1);

  137.         // reduce the general equation α τ² + β υ² + 2 γ τυ + 2 δ τ + 2 ε υ + ζ = 0
  138.         // to canonical form (λ/l)² + (μ/m)² = 1
  139.         // using a coordinates change
  140.         //       τ = τC + λ cosθ - μ sinθ
  141.         //       υ = υC + λ sinθ + μ cosθ
  142.         // or equivalently
  143.         //       λ =   (τ - τC) cosθ + (υ - υC) sinθ
  144.         //       μ = - (τ - τC) sinθ + (υ - υC) cosθ
  145.         // τC and υC are the coordinates of the 2D ellipse center with respect to P
  146.         // 2l and 2m and are the axes lengths (major or minor depending on which one is greatest)
  147.         // θ is the angle of the 2D ellipse axis corresponding to axis with length 2l

  148.         // choose θ in order to cancel the coupling term in λμ
  149.         // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
  150.         // with C = 2[(β - α) cosθ sinθ + γ (cos²θ - sin²θ)]
  151.         // hence the term is cancelled when θ = arctan(t), with γ t² + (α - β) t - γ = 0
  152.         // As the solutions of the quadratic equation obey t₁t₂ = -1, they correspond to
  153.         // angles θ in quadrature to each other. Selecting one solution or the other simply
  154.         // exchanges the principal axes. As we don't care about which axis we want as the
  155.         // first one, we select an arbitrary solution
  156.         final double tanTheta;
  157.         if (FastMath.abs(gamma) < Precision.SAFE_MIN) {
  158.             tanTheta = 0.0;
  159.         } else {
  160.             final double bMA = beta - alpha;
  161.             tanTheta = (bMA >= 0) ?
  162.                        (-2 * gamma / (bMA + FastMath.sqrt(bMA * bMA + 4 * gamma * gamma))) :
  163.                        (-2 * gamma / (bMA - FastMath.sqrt(bMA * bMA + 4 * gamma * gamma)));
  164.         }
  165.         final double tan2   = tanTheta * tanTheta;
  166.         final double cos2   = 1 / (1 + tan2);
  167.         final double sin2   = tan2 * cos2;
  168.         final double cosSin = tanTheta * cos2;
  169.         final double cos    = FastMath.sqrt(cos2);
  170.         final double sin    = tanTheta * cos;

  171.         // choose τC and υC in order to cancel the linear terms in λ and μ
  172.         // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
  173.         // with D = 2[ (α τC + γ υC + δ) cosθ + (γ τC + β υC + ε) sinθ]
  174.         //      E = 2[-(α τC + γ υC + δ) sinθ + (γ τC + β υC + ε) cosθ]
  175.         // θ can be eliminated by combining the equations
  176.         // D cosθ - E sinθ = 2[α τC + γ υC + δ]
  177.         // E cosθ + D sinθ = 2[γ τC + β υC + ε]
  178.         // hence the terms D and E are both cancelled (regardless of θ) when
  179.         //     τC = (β δ - γ ε) / (γ² - α β)
  180.         //     υC = (α ε - γ δ) / (γ² - α β)
  181.         final double denom = MathArrays.linearCombination(gamma, gamma,   -alpha, beta);
  182.         final double tauC  = MathArrays.linearCombination(beta,  delta,   -gamma, epsilon) / denom;
  183.         final double nuC   = MathArrays.linearCombination(alpha, epsilon, -gamma, delta)   / denom;

  184.         // compute l and m
  185.         // expanding the general equation, we get: A λ² + B μ² + C λμ + D λ + E μ + F = 0
  186.         // with A = α cos²θ + β sin²θ + 2 γ cosθ sinθ
  187.         //      B = α sin²θ + β cos²θ - 2 γ cosθ sinθ
  188.         //      F = α τC² + β υC² + 2 γ τC υC + 2 δ τC + 2 ε υC + ζ
  189.         // hence we compute directly l = √(-F/A) and m = √(-F/B)
  190.         final double twogcs = 2 * gamma * cosSin;
  191.         final double bigA   = alpha * cos2 + beta * sin2 + twogcs;
  192.         final double bigB   = alpha * sin2 + beta * cos2 - twogcs;
  193.         final double bigF   = (alpha * tauC + 2 * (gamma * nuC + delta)) * tauC +
  194.                               (beta * nuC + 2 * epsilon) * nuC + zeta;
  195.         final double l      = FastMath.sqrt(-bigF / bigA);
  196.         final double m      = FastMath.sqrt(-bigF / bigB);
  197.         if (Double.isNaN(l + m)) {
  198.             // the plane does not intersect the ellipsoid
  199.             return null;
  200.         }

  201.         if (l > m) {
  202.             return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v),
  203.                                new Vector3D( cos, u, sin, v),
  204.                                new Vector3D(-sin, u, cos, v),
  205.                                l, m, frame);
  206.         } else {
  207.             return new Ellipse(new Vector3D(1, planePoint, tauC, u, nuC, v),
  208.                                new Vector3D(sin, u, -cos, v),
  209.                                new Vector3D(cos, u,  sin, v),
  210.                                m, l, frame);
  211.         }

  212.     }

  213.     /** Find a point on ellipsoid limb, as seen by an external observer.
  214.      * @param observer observer position in ellipsoid frame
  215.      * @param outside point outside ellipsoid in ellipsoid frame, defining the phase around limb
  216.      * @return point on ellipsoid limb
  217.      * @exception OrekitException if the observer is inside the ellipsoid
  218.      * @exception MathArithmeticException if ellipsoid center, observer and outside
  219.      * points are aligned
  220.      * @since 7.1
  221.      */
  222.     public Vector3D pointOnLimb(final Vector3D observer, final Vector3D outside)
  223.         throws OrekitException, MathArithmeticException {

  224.         // there is no limb if we are inside the ellipsoid
  225.         if (isInside(observer)) {
  226.             throw new OrekitException(OrekitMessages.POINT_INSIDE_ELLIPSOID);
  227.         }
  228.         // cut the ellipsoid, to find an elliptical plane section
  229.         final Vector3D normal  = Vector3D.crossProduct(observer, outside);
  230.         final Ellipse  section = getPlaneSection(Vector3D.ZERO, normal);
  231.         final double   a2      = section.getA() * section.getA();
  232.         final double   b2      = section.getB() * section.getB();

  233.         // the point on limb is tangential to the ellipse
  234.         // if T(xt, yt) is an ellipse point at which the tangent is drawn
  235.         // if O(xo, yo) is a point outside of the ellipse belonging to the tangent at T,
  236.         // then the two following equations holds:
  237.         //  a² yt²   + b² xt²   = a² b²  (T belongs to the ellipse)
  238.         //  a² yt yo + b² xt xo = a² b²  (TP is tangent to the ellipse)
  239.         // using the second equation to eliminate yt from the first equation, we get
  240.         // b² (a² - xt xo)² + a² xt² yo² = a⁴ yo²
  241.         // (a² yo² + b² xo²) xt² - 2 a² b² xo xt + a⁴ (b² - yo²) = 0
  242.         // which can easily be solved for xt
  243.         final Vector2D observer2D = section.toPlane(observer);
  244.         final double   xo         = observer2D.getX();
  245.         final double   yo         = observer2D.getY();
  246.         final double   xo2        = xo * xo;
  247.         final double   yo2        = yo * yo;
  248.         final double   alpha      = a2 * yo2 + b2 * xo2;
  249.         final double   beta       = a2 * b2 * xo;
  250.         final double   gamma      = a2 * a2 * (b2 - yo2);
  251.         // we know there are two solutions as we already checked the point is outside ellipsoid
  252.         final double   sqrt       = FastMath.sqrt(beta * beta - alpha * gamma);
  253.         final double   xt1;
  254.         final double   xt2;
  255.         if (beta > 0) {
  256.             final double s = beta + sqrt;
  257.             xt1 = s / alpha;
  258.             xt2 = gamma / s;
  259.         } else {
  260.             final double s = beta - sqrt;
  261.             xt1 = gamma / s;
  262.             xt2 = s / alpha;
  263.         }

  264.         // we return the limb point in the direction of the outside point
  265.         final Vector3D t1 = section.toSpace(new Vector2D(xt1, b2 * (a2 - xt1 * xo) / (a2 * yo)));
  266.         final Vector3D t2 = section.toSpace(new Vector2D(xt2, b2 * (a2 - xt2 * xo) / (a2 * yo)));
  267.         return Vector3D.distance(t1, outside) <= Vector3D.distance(t2, outside) ? t1 : t2;

  268.     }

  269. }