EllipticalFieldOfView.java
/* Copyright 2002-2021 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.geometry.fov;
import org.hipparchus.CalculusFieldElement;
import org.hipparchus.analysis.differentiation.DSFactory;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.SinCos;
import org.orekit.propagation.events.VisibilityTrigger;
/** Class representing a spacecraft sensor Field Of View with elliptical shape.
* <p>
* Without loss of generality, one can assume that with a suitable rotation
* the ellipse center is along the Z<sub>ell</sub> axis and the ellipse principal axes
* are along the X<sub>ell</sub> and Y<sub>ell</sub> axes. The first defining
* elements for an ellipse are these canonical axes. This class allows specifying
* them by giving directly the Z<sub>ell</sub> axis as the {@code center} of
* the ellipse, and giving a {@code primaryMeridian} vector in the (+X<sub>ell</sub>,
* Z<sub>ell</sub>) half-plane. It is allowed to have {@code primaryMeridian} not
* orthogonal to {@code center} as orthogonality will be fixed internally (i.e
* {@code primaryMeridian} may be different from X<sub>ell</sub>).
* </p>
* <p>
* We can define angular coordinates \((\alpha, \beta)\) as dihedra angles around the
* +Y<sub>ell</sub> and -X<sub>ell</sub> axes respectively to specify points on the
* unit sphere. The corresponding Cartesian coordinates will be
* \[P_{\alpha,\beta}\left(\begin{gather*}
* \frac{\sin\alpha\cos\beta}{\sqrt{1-\sin^2\alpha\sin^2\beta}}\\
* \frac{\cos\alpha\sin\beta}{\sqrt{1-\sin^2\alpha\sin^2\beta}}\\
* \frac{\cos\alpha\cos\beta}{\sqrt{1-\sin^2\alpha\sin^2\beta}}
* \end{gather*}\right)\]
* which shows that angle \(\beta=0\) corresponds to the (X<sub>ell</sub>, Z<sub>ell</sub>)
* plane and that angle \(\alpha=0\) corresponds to the (Y<sub>ell</sub>, Z<sub>ell</sub>)
* plane. Note that at least one of the angles must be different from \(\pm\frac{\pi}{2}\),
* which means that the expression above is singular for points in the (X<sub>ell</sub>,
* Y<sub>ell</sub>) plane.
* </p>
* <p>
* The size of the ellipse is defined by its half aperture angles \(\lambda\) along the
* X<sub>ell</sub> axis and \(\mu\) along the Y<sub>ell</sub> axis.
* For points belonging to the ellipse, we always have \(-\lambda \le \alpha \le +\lambda\)
* and \(-\mu \le \beta \le +\mu\), equalities being reached at the end of principal axes.
* An ellipse defined on the sphere is not a planar ellipse because the four endpoints
* \((\alpha=\pm\lambda, \beta=0)\) and \((\alpha=0, \beta=\pm\mu)\) are not coplanar
* when \(\lambda\neq\mu\).
* </p>
* <p>
* We define an ellipse on the sphere as the locus of points \(P\) such that the sum of
* their angular distance to two foci \(F_+\) and \(F_-\) is constant, all points being on
* the sphere. The relationship between the foci and the two half aperture angles \(\lambda\)
* and \(\mu\) is:
* \[\lambda \ge \mu \Rightarrow F_\pm\left(\begin{gather*}
* \pm\sin\delta\\
* 0\\
* \cos\delta
* \end{gather*}\right)
* \quad\text{with}\quad
* \cos\delta = \frac{\cos\lambda}{\cos\mu}\]
* </p>
* <p>
* and
* \[\mu \ge \lambda \Rightarrow F_\pm\left(\begin{gather*}
* 0\\
* \pm\sin\delta\\
* \cos\delta
* \end{gather*}\right)
* \quad\text{with}\quad
* \cos\delta = \frac{\cos\mu}{\cos\lambda}\]
* </p>
* <p>
* It can be shown that the previous definition is equivalent to define first a regular
* planar ellipse drawn on a plane \(z = z_0\) (\(z_0\) being an arbitrary strictly positive
* number, \(z_0=1\) being the simplest choice) with semi major axis \(a=z_0\tan\lambda\)
* and semi minor axis \(b=z_0\tan\mu\) and then to project it onto the sphere using a
* central projection:
* \[\left\{\begin{align*}
* \left(\frac{x}{z_0\tan\lambda}\right)^2 + \left(\frac{y}{z_0\tan\mu}\right)^2 &= \left(\frac{z}{z_0}\right)^2\\
* x^2 + y^2 + z^2 &= 1
* \end{align*}\right.\]
* </p>
* <p>
* Simplifying first equation by \(z_0\) and eliminating \(z^2\) in it using the second equation gives:
* \[\left\{\begin{align*}
* \left(\frac{x}{\sin\lambda}\right)^2 + \left(\frac{y}{\sin\mu}\right)^2 &= 1\\
* x^2 + y^2 + z^2 &= 1
* \end{align*}\right.\]
* which shows that the previous definition is also equivalent to define first a
* dimensionless planar ellipse on the \((x, y)\) plane and to project it onto the sphere
* using a projection along \(z\).
* </p>
* <p>
* Note however that despite the ellipse on the sphere can be computed as a projection
* of an ellipse on the \((x, y)\) plane, the foci of one ellipse are not the projection of the
* foci of the other ellipse. The foci on the plane are closer to each other by a factor
* \(\cos\mu\) than the projection of the foci \(F_+\) and \(F_-\)).
* </p>
* @author Luc Maisonobe
* @since 10.1
*/
public class EllipticalFieldOfView extends SmoothFieldOfView {
/** Factory for derivatives. */
private static final DSFactory FACTORY = new DSFactory(1, 3);
/** FOV half aperture angle for spreading along X (i.e. rotation around +Y). */
private final double halfApertureAlongX;
/** FOV half aperture angle for spreading along Y (i.e. rotation around -X). */
private final double halfApertureAlongY;
/** tan(halfApertureAlongX). */
private final double tanX;
/** tan(halfApertureAlongX). */
private final double tanY;
/** Unit vector along major axis. */
private final Vector3D u;
/** First focus. */
private final Vector3D focus1;
/** Second focus. */
private final Vector3D focus2;
/** Cross product of foci. */
private final Vector3D crossF1F2;
/** Dot product of foci. */
private final double dotF1F2;
/** Half angle between foci. */
private final double gamma;
/** Scaling factor for normalizing ellipse points. */
private final double d;
/** Angular semi major axis. */
private double a;
/** Build a new instance.
* <p>
* Using a suitable rotation, an elliptical Field Of View can be oriented such
* that the ellipse center is along the Z<sub>ell</sub> axis, one of its principal
* axes is in the (X<sub>ell</sub>, Z<sub>ell</sub>) plane and the other principal
* axis is in the (Y<sub>ell</sub>, Z<sub>ell</sub>) plane. Beware that the ellipse
* principal axis that spreads along the Y<sub>ell</sub> direction corresponds to a
* rotation around -X<sub>ell</sub> axis and that the ellipse principal axis that
* spreads along the X<sub>ell</sub> direction corresponds to a rotation around
* +Y<sub>ell</sub> axis. The naming convention used here is that the angles are
* named after the spreading axis.
* </p>
* @param center direction of the FOV center (i.e. Z<sub>ell</sub>),
* in spacecraft frame
* @param primaryMeridian vector defining the (+X<sub>ell</sub>, Z<sub>ell</sub>)
* half-plane (it is allowed to have {@code primaryMeridian} not orthogonal to
* {@code center} as orthogonality will be fixed internally)
* @param halfApertureAlongX FOV half aperture angle defining the ellipse spreading
* along X<sub>ell</sub> (i.e. it corresponds to a rotation around +Y<sub>ell</sub>)
* @param halfApertureAlongY FOV half aperture angle defining the ellipse spreading
* along Y<sub>ell</sub> (i.e. it corresponds to a rotation around -X<sub>ell</sub>)
* @param margin angular margin to apply to the zone (if positive,
* the Field Of View will consider points slightly outside of the
* zone are still visible)
*/
public EllipticalFieldOfView(final Vector3D center, final Vector3D primaryMeridian,
final double halfApertureAlongX, final double halfApertureAlongY,
final double margin) {
super(center, primaryMeridian, margin);
final Vector3D v;
final double b;
if (halfApertureAlongX >= halfApertureAlongY) {
u = getX();
v = getY();
a = halfApertureAlongX;
b = halfApertureAlongY;
} else {
u = getY();
v = getX().negate();
a = halfApertureAlongY;
b = halfApertureAlongX;
}
final double cos = FastMath.cos(a) / FastMath.cos(b);
final double sin = FastMath.sqrt(1 - cos * cos);
this.halfApertureAlongX = halfApertureAlongX;
this.halfApertureAlongY = halfApertureAlongY;
this.tanX = FastMath.tan(halfApertureAlongX);
this.tanY = FastMath.tan(halfApertureAlongY);
this.focus1 = new Vector3D(+sin, u, cos, getZ());
this.focus2 = new Vector3D(-sin, u, cos, getZ());
this.crossF1F2 = new Vector3D(-2 * sin * cos, v);
this.dotF1F2 = 2 * cos * cos - 1;
this.gamma = FastMath.acos(cos);
this.d = 1.0 / (1 - dotF1F2 * dotF1F2);
}
/** get the FOV half aperture angle for spreading along X<sub>ell</sub> (i.e. rotation around +Y<sub>ell</sub>).
* @return FOV half aperture angle for spreading along X<sub>ell</sub> (i.e. rotation around +Y<sub>ell</sub>
*/
public double getHalfApertureAlongX() {
return halfApertureAlongX;
}
/** get the FOV half aperture angle for spreading along Y<sub>ell</sub> (i.e. rotation around -X<sub>ell</sub>).
* @return FOV half aperture angle for spreading along Y<sub>ell</sub> (i.e. rotation around -X<sub>ell</sub>)
*/
public double getHalfApertureAlongY() {
return halfApertureAlongY;
}
/** Get first focus in spacecraft frame.
* @return first focus in spacecraft frame
*/
public Vector3D getFocus1() {
return focus1;
}
/** Get second focus in spacecraft frame.
* @return second focus in spacecraft frame
*/
public Vector3D getFocus2() {
return focus2;
}
/** {@inheritDoc} */
@Override
public double offsetFromBoundary(final Vector3D lineOfSight, final double angularRadius,
final VisibilityTrigger trigger) {
final double margin = getMargin();
final double correctedRadius = trigger.radiusCorrection(angularRadius);
final double deadBand = margin + angularRadius;
// for faster computation, we start using only the surrounding cap, to filter out
// far away points (which correspond to most of the points if the Field Of View is small)
final double crudeDistance = Vector3D.angle(getZ(), lineOfSight) - a;
if (crudeDistance > deadBand + 0.01) {
// we know we are strictly outside of the zone,
// use the crude distance to compute the (positive) return value
return crudeDistance + correctedRadius - margin;
}
// we are close, we need to compute carefully the exact offset;
// we project the point to the closest zone boundary
final double d1 = Vector3D.angle(lineOfSight, focus1);
final double d2 = Vector3D.angle(lineOfSight, focus2);
final Vector3D closest = projectToBoundary(lineOfSight, d1, d2);
// compute raw offset as an accurate signed angle
final double rawOffset = FastMath.copySign(Vector3D.angle(lineOfSight, closest),
d1 + d2 - 2 * a);
return rawOffset + correctedRadius - getMargin();
}
/** {@inheritDoc} */
@Override
public Vector3D projectToBoundary(final Vector3D lineOfSight) {
final double d1 = Vector3D.angle(lineOfSight, focus1);
final double d2 = Vector3D.angle(lineOfSight, focus2);
return projectToBoundary(lineOfSight, d1, d2);
}
/** Find the direction on Field Of View Boundary closest to a line of sight.
* @param lineOfSight line of sight from the center of the Field Of View support
* unit sphere to the target in spacecraft frame
* @param d1 distance to first focus
* @param d2 distance to second focus
* @return direction on Field Of View Boundary closest to a line of sight
*/
private Vector3D projectToBoundary(final Vector3D lineOfSight, final double d1, final double d2) {
final Vector3D los = lineOfSight.normalize();
final double side = Vector3D.dotProduct(los, crossF1F2);
if (FastMath.abs(side) < 1.0e-12) {
// the line of sight is almost along the major axis
return directionAt(Vector3D.dotProduct(los, u) > 0 ? 0.0 : FastMath.PI);
}
// find an initial point on ellipse, that approximates closest point
final double offset0 = 0.5 * (d1 - d2);
double minOffset = -gamma;
double maxOffset = +gamma;
// find closest ellipse point
DerivativeStructure offset = FACTORY.variable(0, offset0);
for (int i = 0; i < 100; i++) { // this loop usually converges in 1-4 iterations
// distance function we want to minimize
final FieldVector3D<DerivativeStructure> pn = directionAt(offset.add(a), offset.subtract(a).negate(), side);
final DerivativeStructure yn = FieldVector3D.angle(pn, los);
if (yn.getValue() < 1.0e-12) {
// the query point is almost on the ellipse boundary
break;
}
// Halley's iteration on the derivative (since we want the minimum of the distance function)
final double f0 = yn.getPartialDerivative(1);
final double f1 = yn.getPartialDerivative(2);
final double f2 = yn.getPartialDerivative(3);
double dx = -2 * f0 * f1 / (2 * f1 * f1 - f0 * f2);
if (dx * f0 > 0) {
// the Halley's iteration is going towards maximum, not minimum
// try to go past inflection point
dx = -1.5 * f2 / f1;
}
// manage bounds
if (dx < 0) {
maxOffset = offset.getValue();
if (offset.getValue() + dx <= minOffset) {
// we overshoot limit, fall back to bisection
dx = 0.5 * (minOffset - offset.getValue());
}
} else {
minOffset = offset.getValue();
if (offset.getValue() + dx >= maxOffset) {
// we overshoot limit, fall back to bisection
dx = 0.5 * (maxOffset - offset.getValue());
}
}
// apply offset change
offset = offset.add(dx);
// check convergence
if (FastMath.abs(dx) < 1.0e-12) {
break;
}
}
return directionAt(a + offset.getReal(), a - offset.getReal(), side);
}
/** {@inheritDoc} */
@Override
protected Vector3D directionAt(final double angle) {
final SinCos sce = FastMath.sinCos(angle);
final Vector3D dEll = new Vector3D(tanX * sce.cos(), tanY * sce.sin(), 1.0).normalize();
return new Vector3D(dEll.getX(), getX(), dEll.getY(), getY(), dEll.getZ(), getZ());
}
/** Get a direction from distances to foci.
* <p>
* if {@code d1} + {@code d2} = 2 max({@link #getHalfApertureAlongX()}, {@link #getHalfApertureAlongY()}),
* then the point is on the ellipse boundary
* </p>
* @param d1 distance to focus 1
* @param d2 distance to focus 2
* @param sign sign of the ellipse point with respect to F1 ^ F2
* @return direction
*/
private Vector3D directionAt(final double d1, final double d2, final double sign) {
final double cos1 = FastMath.cos(d1);
final double cos2 = FastMath.cos(d2);
final double a1 = (cos1 - cos2 * dotF1F2) * d;
final double a2 = (cos2 - cos1 * dotF1F2) * d;
final double ac = FastMath.sqrt((1 - (a1 * a1 + 2 * a1 * a2 * dotF1F2 + a2 * a2)) * d);
return new Vector3D(a1, focus1, a2, focus2, FastMath.copySign(ac, sign), crossF1F2);
}
/** Get a direction from distances to foci.
* <p>
* if {@code d1} + {@code d2} = 2 max({@link #getHalfApertureAlongX()}, {@link #getHalfApertureAlongY()}),
* then the point is on the ellipse boundary
* </p>
* @param d1 distance to focus 1
* @param d2 distance to focus 2
* @param sign sign of the ellipse point with respect to F1 ^ F2
* @param <T> type of the field element
* @return direction
*/
private <T extends CalculusFieldElement<T>> FieldVector3D<T> directionAt(final T d1, final T d2, final double sign) {
final T cos1 = FastMath.cos(d1);
final T cos2 = FastMath.cos(d2);
final T a1 = cos1.subtract(cos2.multiply(dotF1F2)).multiply(d);
final T a2 = cos2.subtract(cos1.multiply(dotF1F2)).multiply(d);
final T ac = FastMath.sqrt(a1.multiply(a1.add(a2.multiply(2 * dotF1F2))).add(a2.multiply(a2)).negate().add(1).multiply(d));
return new FieldVector3D<>(a1, focus1, a2, focus2, FastMath.copySign(ac, sign), crossF1F2);
}
}