EllipticalFieldOfView.java

  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.geometry.fov;

  19. import org.hipparchus.CalculusFieldElement;
  20. import org.hipparchus.analysis.differentiation.DSFactory;
  21. import org.hipparchus.analysis.differentiation.DerivativeStructure;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  24. import org.hipparchus.util.FastMath;
  25. import org.hipparchus.util.SinCos;
  26. import org.orekit.propagation.events.VisibilityTrigger;

  28. /** Class representing a spacecraft sensor Field Of View with elliptical shape.
  29.  * <p>
  30.  * Without loss of generality, one can assume that with a suitable rotation
  31.  * the ellipse center is along the Z<sub>ell</sub> axis and the ellipse principal axes
  32.  * are along the X<sub>ell</sub> and Y<sub>ell</sub> axes. The first defining
  33.  * elements for an ellipse are these canonical axes. This class allows specifying
  34.  * them by giving directly the Z<sub>ell</sub> axis as the {@code center} of
  35.  * the ellipse, and giving a {@code primaryMeridian} vector in the (+X<sub>ell</sub>,
  36.  * Z<sub>ell</sub>) half-plane. It is allowed to have {@code primaryMeridian} not
  37.  * orthogonal to {@code center} as orthogonality will be fixed internally (i.e
  38.  * {@code primaryMeridian} may be different from X<sub>ell</sub>).
  39.  * </p>
  40.  * <p>
  41.  * We can define angular coordinates \((\alpha, \beta)\) as dihedra angles around the
  42.  * +Y<sub>ell</sub> and -X<sub>ell</sub> axes respectively to specify points on the
  43.  * unit sphere. The corresponding Cartesian coordinates will be
  44.  * \[P_{\alpha,\beta}\left(\begin{gather*}
  45.  *   \frac{\sin\alpha\cos\beta}{\sqrt{1-\sin^2\alpha\sin^2\beta}}\\
  46.  *   \frac{\cos\alpha\sin\beta}{\sqrt{1-\sin^2\alpha\sin^2\beta}}\\
  47.  *   \frac{\cos\alpha\cos\beta}{\sqrt{1-\sin^2\alpha\sin^2\beta}}
  48.  * \end{gather*}\right)\]
  49.  * which shows that angle \(\beta=0\) corresponds to the (X<sub>ell</sub>, Z<sub>ell</sub>)
  50.  * plane and that angle \(\alpha=0\) corresponds to the (Y<sub>ell</sub>, Z<sub>ell</sub>)
  51.  * plane. Note that at least one of the angles must be different from \(\pm\frac{\pi}{2}\),
  52.  * which means that the expression above is singular for points in the (X<sub>ell</sub>,
  53.  * Y<sub>ell</sub>) plane.
  54.  * </p>
  55.  * <p>
  56.  * The size of the ellipse is defined by its half aperture angles \(\lambda\) along the
  57.  * X<sub>ell</sub> axis and \(\mu\) along the Y<sub>ell</sub> axis.
  58.  * For points belonging to the ellipse, we always have \(-\lambda \le \alpha \le +\lambda\)
  59.  * and \(-\mu \le \beta \le +\mu\), equalities being reached at the end of principal axes.
  60.  * An ellipse defined on the sphere is not a planar ellipse because the four endpoints
  61.  * \((\alpha=\pm\lambda, \beta=0)\) and \((\alpha=0, \beta=\pm\mu)\) are not coplanar
  62.  * when \(\lambda\neq\mu\).
  63.  * </p>
  64.  * <p>
  65.  * We define an ellipse on the sphere as the locus of points \(P\) such that the sum of
  66.  * their angular distance to two foci \(F_+\) and \(F_-\) is constant, all points being on
  67.  * the sphere. The relationship between the foci and the two half aperture angles \(\lambda\)
  68.  * and \(\mu\) is:
  69.  * \[\lambda \ge \mu \Rightarrow F_\pm\left(\begin{gather*}
  70.  *   \pm\sin\delta\\
  71.  *   0\\
  72.  *   \cos\delta
  73.  * \end{gather*}\right)
  74.  * \quad\text{with}\quad
  75.  * \cos\delta = \frac{\cos\lambda}{\cos\mu}\]
  76.  * </p>
  77.  * <p>
  78.  * and
  79.  * \[\mu \ge \lambda \Rightarrow F_\pm\left(\begin{gather*}
  80.  *   0\\
  81.  *   \pm\sin\delta\\
  82.  *   \cos\delta
  83.  * \end{gather*}\right)
  84.  * \quad\text{with}\quad
  85.  * \cos\delta = \frac{\cos\mu}{\cos\lambda}\]
  86.  * </p>
  87.  * <p>
  88.  * It can be shown that the previous definition is equivalent to define first a regular
  89.  * planar ellipse drawn on a plane \(z = z_0\) (\(z_0\) being an arbitrary strictly positive
  90.  * number, \(z_0=1\) being the simplest choice) with semi major axis \(a=z_0\tan\lambda\)
  91.  * and semi minor axis \(b=z_0\tan\mu\) and then to project it onto the sphere using a
  92.  * central projection:
  93.  * \[\left\{\begin{align*}
  94.  * \left(\frac{x}{z_0\tan\lambda}\right)^2 + \left(\frac{y}{z_0\tan\mu}\right)^2 &amp;= \left(\frac{z}{z_0}\right)^2\\
  95.  * x^2 + y^2 + z^2 &amp;= 1
  96.  * \end{align*}\right.\]
  97.  * </p>
  98.  * <p>
  99.  * Simplifying first equation by \(z_0\) and eliminating \(z^2\) in it using the second equation gives:
  100.  * \[\left\{\begin{align*}
  101.  * \left(\frac{x}{\sin\lambda}\right)^2 + \left(\frac{y}{\sin\mu}\right)^2 &amp;= 1\\
  102.  * x^2 + y^2 + z^2 &amp;= 1
  103.  * \end{align*}\right.\]
  104.  * which shows that the previous definition is also equivalent to define first a
  105.  * dimensionless planar ellipse on the \((x, y)\) plane and to project it onto the sphere
  106.  * using a projection along \(z\).
  107.  * </p>
  108.  * <p>
  109.  * Note however that despite the ellipse on the sphere can be computed as a projection
  110.  * of an ellipse on the \((x, y)\) plane, the foci of one ellipse are not the projection of the
  111.  * foci of the other ellipse. The foci on the plane are closer to each other by a factor
  112.  * \(\cos\mu\) than the projection of the foci \(F_+\) and \(F_-\)).
  113.  * </p>
  114.  * @author Luc Maisonobe
  115.  * @since 10.1
  116.  */
  117. public class EllipticalFieldOfView extends SmoothFieldOfView {

  119.     /** Factory for derivatives. */
  120.     private static final DSFactory FACTORY = new DSFactory(1, 3);

  122.     /** FOV half aperture angle for spreading along X (i.e. rotation around +Y). */
  123.     private final double halfApertureAlongX;

  125.     /** FOV half aperture angle for spreading along Y (i.e. rotation around -X). */
  126.     private final double halfApertureAlongY;

  128.     /** tan(halfApertureAlongX). */
  129.     private final double   tanX;

  131.     /** tan(halfApertureAlongX). */
  132.     private final double   tanY;

  134.     /** Unit vector along major axis. */
  135.     private final Vector3D u;

  137.     /** First focus. */
  138.     private final Vector3D focus1;

  140.     /** Second focus. */
  141.     private final Vector3D focus2;

  143.     /** Cross product of foci. */
  144.     private final Vector3D crossF1F2;

  146.     /** Dot product of foci. */
  147.     private final double dotF1F2;

  149.     /** Half angle between foci. */
  150.     private final double gamma;

  152.     /** Scaling factor for normalizing ellipse points. */
  153.     private final double d;

  155.     /** Angular semi major axis. */
  156.     private double a;

  158.     /** Build a new instance.
  159.      * <p>
  160.      * Using a suitable rotation, an elliptical Field Of View can be oriented such
  161.      * that the ellipse center is along the Z<sub>ell</sub> axis, one of its principal
  162.      * axes is in the (X<sub>ell</sub>, Z<sub>ell</sub>) plane and the other principal
  163.      * axis is in the (Y<sub>ell</sub>, Z<sub>ell</sub>) plane. Beware that the ellipse
  164.      * principal axis that spreads along the Y<sub>ell</sub> direction corresponds to a
  165.      * rotation around -X<sub>ell</sub> axis and that the ellipse principal axis that
  166.      * spreads along the X<sub>ell</sub> direction corresponds to a rotation around
  167.      * +Y<sub>ell</sub> axis. The naming convention used here is that the angles are
  168.      * named after the spreading axis.
  169.      * </p>
  170.      * @param center direction of the FOV center (i.e. Z<sub>ell</sub>),
  171.      * in spacecraft frame
  172.      * @param primaryMeridian vector defining the (+X<sub>ell</sub>, Z<sub>ell</sub>)
  173.      * half-plane (it is allowed to have {@code primaryMeridian} not orthogonal to
  174.      * {@code center} as orthogonality will be fixed internally)
  175.      * @param halfApertureAlongX FOV half aperture angle defining the ellipse spreading
  176.      * along X<sub>ell</sub> (i.e. it corresponds to a rotation around +Y<sub>ell</sub>)
  177.      * @param halfApertureAlongY FOV half aperture angle defining the ellipse spreading
  178.      * along Y<sub>ell</sub> (i.e. it corresponds to a rotation around -X<sub>ell</sub>)
  179.      * @param margin angular margin to apply to the zone (if positive,
  180.      * the Field Of View will consider points slightly outside of the
  181.      * zone are still visible)
  182.      */
  183.     public EllipticalFieldOfView(final Vector3D center, final Vector3D primaryMeridian,
  184.                                  final double halfApertureAlongX, final double halfApertureAlongY,
  185.                                  final double margin) {

  187.         super(center, primaryMeridian, margin);

  189.         final Vector3D v;
  190.         final double b;
  191.         if (halfApertureAlongX >= halfApertureAlongY) {
  192.             u = getX();
  193.             v = getY();
  194.             a = halfApertureAlongX;
  195.             b = halfApertureAlongY;
  196.         } else {
  197.             u = getY();
  198.             v = getX().negate();
  199.             a = halfApertureAlongY;
  200.             b = halfApertureAlongX;
  201.         }

  203.         final double cos = FastMath.cos(a) / FastMath.cos(b);
  204.         final double sin = FastMath.sqrt(1 - cos * cos);

  206.         this.halfApertureAlongX = halfApertureAlongX;
  207.         this.halfApertureAlongY = halfApertureAlongY;
  208.         this.tanX               = FastMath.tan(halfApertureAlongX);
  209.         this.tanY               = FastMath.tan(halfApertureAlongY);
  210.         this.focus1             = new Vector3D(+sin, u, cos, getZ());
  211.         this.focus2             = new Vector3D(-sin, u, cos, getZ());
  212.         this.crossF1F2          = new Vector3D(-2 * sin * cos, v);
  213.         this.dotF1F2            = 2 * cos * cos - 1;
  214.         this.gamma              = FastMath.acos(cos);
  215.         this.d                  = 1.0 / (1 - dotF1F2 * dotF1F2);

  217.     }

  219.     /** get the FOV half aperture angle for spreading along X<sub>ell</sub> (i.e. rotation around +Y<sub>ell</sub>).
  220.      * @return FOV half aperture angle for spreading along X<sub>ell</sub> (i.e. rotation around +Y<sub>ell</sub>
  221.      */
  222.     public double getHalfApertureAlongX() {
  223.         return halfApertureAlongX;
  224.     }

  226.     /** get the FOV half aperture angle for spreading along Y<sub>ell</sub> (i.e. rotation around -X<sub>ell</sub>).
  227.      * @return FOV half aperture angle for spreading along Y<sub>ell</sub> (i.e. rotation around -X<sub>ell</sub>)
  228.      */
  229.     public double getHalfApertureAlongY() {
  230.         return halfApertureAlongY;
  231.     }

  233.     /** Get first focus in spacecraft frame.
  234.      * @return first focus in spacecraft frame
  235.      */
  236.     public Vector3D getFocus1() {
  237.         return focus1;
  238.     }

  240.     /** Get second focus in spacecraft frame.
  241.      * @return second focus in spacecraft frame
  242.      */
  243.     public Vector3D getFocus2() {
  244.         return focus2;
  245.     }

  247.     /** {@inheritDoc} */
  248.     @Override
  249.     public double offsetFromBoundary(final Vector3D lineOfSight, final double angularRadius,
  250.                                      final VisibilityTrigger trigger) {

  252.         final double  margin          = getMargin();
  253.         final double  correctedRadius = trigger.radiusCorrection(angularRadius);
  254.         final double  deadBand        = margin + angularRadius;

  256.         // for faster computation, we start using only the surrounding cap, to filter out
  257.         // far away points (which correspond to most of the points if the Field Of View is small)
  258.         final double crudeDistance = Vector3D.angle(getZ(), lineOfSight) - a;
  259.         if (crudeDistance > deadBand + 0.01) {
  260.             // we know we are strictly outside of the zone,
  261.             // use the crude distance to compute the (positive) return value
  262.             return crudeDistance + correctedRadius - margin;
  263.         }

  265.         // we are close, we need to compute carefully the exact offset;
  266.         // we project the point to the closest zone boundary
  267.         final double   d1      = Vector3D.angle(lineOfSight, focus1);
  268.         final double   d2      = Vector3D.angle(lineOfSight, focus2);
  269.         final Vector3D closest = projectToBoundary(lineOfSight, d1, d2);

  271.         // compute raw offset as an accurate signed angle
  272.         final double rawOffset = FastMath.copySign(Vector3D.angle(lineOfSight, closest),
  273.                                                    d1 + d2 - 2 * a);

  275.         return rawOffset + correctedRadius - getMargin();

  277.     }

  279.     /** {@inheritDoc} */
  280.     @Override
  281.     public Vector3D projectToBoundary(final Vector3D lineOfSight) {
  282.         final double d1 = Vector3D.angle(lineOfSight, focus1);
  283.         final double d2 = Vector3D.angle(lineOfSight, focus2);
  284.         return projectToBoundary(lineOfSight, d1, d2);
  285.     }

  287.     /** Find the direction on Field Of View Boundary closest to a line of sight.
  288.      * @param lineOfSight line of sight from the center of the Field Of View support
  289.      * unit sphere to the target in spacecraft frame
  290.      * @param d1 distance to first focus
  291.      * @param d2 distance to second focus
  292.      * @return direction on Field Of View Boundary closest to a line of sight
  293.      */
  294.     private Vector3D projectToBoundary(final Vector3D lineOfSight, final double d1, final double d2) {

  296.         final Vector3D los  = lineOfSight.normalize();
  297.         final double   side = Vector3D.dotProduct(los, crossF1F2);
  298.         if (FastMath.abs(side) < 1.0e-12) {
  299.             // the line of sight is almost along the major axis
  300.             return directionAt(Vector3D.dotProduct(los, u) > 0 ? 0.0 : FastMath.PI);
  301.         }

  303.         // find an initial point on ellipse, that approximates closest point
  304.         final double offset0 = 0.5 * (d1 - d2);
  305.         double minOffset = -gamma;
  306.         double maxOffset = +gamma;

  308.         // find closest ellipse point
  309.         DerivativeStructure offset = FACTORY.variable(0, offset0);
  310.         for (int i = 0; i < 100; i++) { // this loop usually converges in 1-4 iterations

  312.             // distance function we want to minimize
  313.             final FieldVector3D<DerivativeStructure> pn = directionAt(offset.add(a), offset.subtract(a).negate(), side);
  314.             final DerivativeStructure                yn = FieldVector3D.angle(pn, los);
  315.             if (yn.getValue() < 1.0e-12) {
  316.                 // the query point is almost on the ellipse boundary
  317.                 break;
  318.             }

  320.             // Halley's iteration on the derivative (since we want the minimum of the distance function)
  321.             final double f0 = yn.getPartialDerivative(1);
  322.             final double f1 = yn.getPartialDerivative(2);
  323.             final double f2 = yn.getPartialDerivative(3);
  324.             double dx = -2 * f0 * f1 / (2 * f1 * f1 - f0 * f2);
  325.             if (dx * f0 > 0) {
  326.                 // the Halley's iteration is going towards maximum, not minimum
  327.                 // try to go past inflection point
  328.                 dx = -1.5 * f2 / f1;
  329.             }

  331.             // manage bounds
  332.             if (dx < 0) {
  333.                 maxOffset = offset.getValue();
  334.                 if (offset.getValue() + dx <= minOffset) {
  335.                     // we overshoot limit, fall back to bisection
  336.                     dx = 0.5 * (minOffset - offset.getValue());
  337.                 }
  338.             } else {
  339.                 minOffset = offset.getValue();
  340.                 if (offset.getValue() + dx >= maxOffset) {
  341.                     // we overshoot limit, fall back to bisection
  342.                     dx = 0.5 * (maxOffset - offset.getValue());
  343.                 }
  344.             }

  346.             // apply offset change
  347.             offset = offset.add(dx);

  349.             // check convergence
  350.             if (FastMath.abs(dx) < 1.0e-12) {
  351.                 break;
  352.             }

  354.         }

  356.         return directionAt(a + offset.getReal(), a - offset.getReal(), side);

  358.     }

  360.     /** {@inheritDoc} */
  361.     @Override
  362.     protected Vector3D directionAt(final double angle) {
  363.         final SinCos   sce  = FastMath.sinCos(angle);
  364.         final Vector3D dEll = new Vector3D(tanX * sce.cos(), tanY * sce.sin(), 1.0).normalize();
  365.         return new Vector3D(dEll.getX(), getX(), dEll.getY(), getY(), dEll.getZ(), getZ());
  366.     }

  368.     /** Get a direction from distances to foci.
  369.      * <p>
  370.      * if {@code d1} + {@code d2} = 2 max({@link #getHalfApertureAlongX()}, {@link #getHalfApertureAlongY()}),
  371.      * then the point is on the ellipse boundary
  372.      * </p>
  373.      * @param d1 distance to focus 1
  374.      * @param d2 distance to focus 2
  375.      * @param sign sign of the ellipse point with respect to F1 ^ F2
  376.      * @return direction
  377.      */
  378.     private Vector3D directionAt(final double d1, final double d2, final double sign) {
  379.         final double cos1 = FastMath.cos(d1);
  380.         final double cos2 = FastMath.cos(d2);
  381.         final double a1   = (cos1 - cos2 * dotF1F2) * d;
  382.         final double a2   = (cos2 - cos1 * dotF1F2) * d;
  383.         final double ac   = FastMath.sqrt((1 - (a1 * a1 + 2 * a1 * a2 * dotF1F2 + a2 * a2)) * d);
  384.         return new Vector3D(a1, focus1, a2, focus2, FastMath.copySign(ac, sign), crossF1F2);
  385.     }

  387.     /** Get a direction from distances to foci.
  388.      * <p>
  389.      * if {@code d1} + {@code d2} = 2 max({@link #getHalfApertureAlongX()}, {@link #getHalfApertureAlongY()}),
  390.      * then the point is on the ellipse boundary
  391.      * </p>
  392.      * @param d1 distance to focus 1
  393.      * @param d2 distance to focus 2
  394.      * @param sign sign of the ellipse point with respect to F1 ^ F2
  395.      * @param <T> type of the field element
  396.      * @return direction
  397.      */
  398.     private <T extends CalculusFieldElement<T>> FieldVector3D<T> directionAt(final T d1, final T d2, final double sign) {
  399.         final T cos1 = FastMath.cos(d1);
  400.         final T cos2 = FastMath.cos(d2);
  401.         final T a1   = cos1.subtract(cos2.multiply(dotF1F2)).multiply(d);
  402.         final T a2   = cos2.subtract(cos1.multiply(dotF1F2)).multiply(d);
  403.         final T ac   = FastMath.sqrt(a1.multiply(a1.add(a2.multiply(2 * dotF1F2))).add(a2.square()).negate().add(1).multiply(d));
  404.         return new FieldVector3D<>(a1, focus1, a2, focus2, FastMath.copySign(ac, sign), crossF1F2);
  405.     }

  407. }
Created with JaCoCo 0.8.12.202403310830