PolygonalFieldOfView.java

  1. /* Copyright 2002-2020 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 java.util.ArrayList;
  20. import java.util.List;

  22. import org.hipparchus.geometry.enclosing.EnclosingBall;
  23. import org.hipparchus.geometry.euclidean.threed.Line;
  24. import org.hipparchus.geometry.euclidean.threed.Rotation;
  25. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  26. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  27. import org.hipparchus.geometry.partitioning.Region;
  28. import org.hipparchus.geometry.spherical.twod.Edge;
  29. import org.hipparchus.geometry.spherical.twod.S2Point;
  30. import org.hipparchus.geometry.spherical.twod.Sphere2D;
  31. import org.hipparchus.geometry.spherical.twod.SphericalPolygonsSet;
  32. import org.hipparchus.geometry.spherical.twod.Vertex;
  33. import org.hipparchus.util.FastMath;
  34. import org.hipparchus.util.MathUtils;
  35. import org.hipparchus.util.SinCos;
  36. import org.orekit.bodies.GeodeticPoint;
  37. import org.orekit.bodies.OneAxisEllipsoid;
  38. import org.orekit.errors.OrekitException;
  39. import org.orekit.errors.OrekitMessages;
  40. import org.orekit.frames.Frame;
  41. import org.orekit.frames.Transform;
  42. import org.orekit.propagation.events.VisibilityTrigger;

  44. /** Class representing a spacecraft sensor Field Of View with polygonal shape.
  45.  * <p>Fields Of View are zones defined on the unit sphere centered on the
  46.  * spacecraft. They can have any shape, they can be split in several
  47.  * non-connected patches and can have holes.</p>
  48.  * @author Luc Maisonobe
  49.  * @since 10.1
  50.  */
  51. public class PolygonalFieldOfView extends AbstractFieldOfView {

  53.     /** Spherical zone. */
  54.     private final SphericalPolygonsSet zone;

  56.     /** Spherical cap surrounding the zone. */
  57.     private final EnclosingBall<Sphere2D, S2Point> cap;

  59.     /** Build a new instance.
  60.      * @param zone interior of the Field Of View, in spacecraft frame
  61.      * @param margin angular margin to apply to the zone (if positive,
  62.      * points outside of the raw FoV but close enough to the boundary are
  63.      * considered visible; if negative, points inside of the raw FoV
  64.      * but close enough to the boundary are considered not visible)
  65.      */
  66.     public PolygonalFieldOfView(final SphericalPolygonsSet zone, final double margin) {
  67.         super(margin);
  68.         this.zone = zone;
  69.         this.cap  = zone.getEnclosingCap();
  70.     }

  72.     /** Build Field Of View with a regular polygon shape.
  73.      * @param center center of the polygon (the center is in the inside part)
  74.      * @param meridian point defining the reference meridian for middle of first edge
  75.      * @param insideRadius distance of the edges middle points to the center
  76.      * (the polygon vertices will therefore be farther away from the center)
  77.      * @param n number of sides of the polygon
  78.      * @param margin angular margin to apply to the zone (if positive,
  79.      * points outside of the raw FoV but close enough to the boundary are
  80.      * considered visible; if negative, points inside of the raw FoV
  81.      * but close enough to the boundary are considered not visible)
  82.      * @deprecated as of 10.1, replaced by {@link #PolygonalFieldOfView(Vector3D,
  83.      * DefiningConeType, Vector3D, double, int, double)}
  84.      */
  85.     @Deprecated
  86.     public PolygonalFieldOfView(final Vector3D center, final Vector3D meridian,
  87.                                 final double insideRadius, final int n,
  88.                                 final double margin) {
  89.         this(center, DefiningConeType.INSIDE_CONE_TOUCHING_POLYGON_AT_EDGES_MIDDLE,
  90.              meridian, insideRadius, n, margin);
  91.     }

  93.     /** Build Field Of View with a regular polygon shape.
  94.      * @param center center of the polygon (the center is in the inside part)
  95.      * @param coneType type of defining cone
  96.      * @param meridian point defining the reference meridian for one contact
  97.      * point between defining cone and polygon (i.e. either a polygon edge
  98.      * middle point or a polygon vertex)
  99.      * @param radius defining cone angular radius
  100.      * @param n number of sides of the polygon
  101.      * @param margin angular margin to apply to the zone (if positive,
  102.      * points outside of the raw FoV but close enough to the boundary are
  103.      * considered visible; if negative, points inside of the raw FoV
  104.      * but close enough to the boundary are considered not visible)
  105.      * @since 10.1
  106.      */
  107.     public PolygonalFieldOfView(final Vector3D center, final DefiningConeType coneType,
  108.                                 final Vector3D meridian, final double radius,
  109.                                 final int n, final double margin) {

  111.         super(margin);

  113.         final double   verticesRadius = coneType.verticesRadius(radius, n);
  114.         final Vector3D vertex         = coneType.createVertex(center, meridian, verticesRadius, n);
  115.         this.zone                     = new SphericalPolygonsSet(center, vertex, verticesRadius,
  116.                                                                  n, 1.0e-12 * verticesRadius);

  118.         final Rotation r = new Rotation(center, MathUtils.TWO_PI / n, RotationConvention.VECTOR_OPERATOR);
  119.         final S2Point[] support = new S2Point[n];
  120.         support[0] = new S2Point(vertex);
  121.         for (int i = 1; i < n; ++i) {
  122.             support[i] = new S2Point(r.applyTo(support[i - 1].getVector()));
  123.         }
  124.         this.cap = new EnclosingBall<>(new S2Point(center), Vector3D.angle(center, vertex), support);

  126.     }

  128.     /** Get the interior zone.
  129.      * @return the interior zone
  130.      */
  131.     public SphericalPolygonsSet getZone() {
  132.         return zone;
  133.     }

  135.     /** {@inheritDoc} */
  136.     @Override
  137.     public double offsetFromBoundary(final Vector3D lineOfSight, final double angularRadius,
  138.                                      final VisibilityTrigger trigger) {

  140.         final S2Point los             = new S2Point(lineOfSight);
  141.         final double  margin          = getMargin();
  142.         final double  correctedRadius = trigger.radiusCorrection(angularRadius);
  143.         final double  deadBand        = margin + angularRadius;

  145.         // for faster computation, we start using only the surrounding cap, to filter out
  146.         // far away points (which correspond to most of the points if the Field Of View is small)
  147.         final double crudeDistance = cap.getCenter().distance(los) - cap.getRadius();
  148.         if (crudeDistance > deadBand + 0.01) {
  149.             // we know we are strictly outside of the zone,
  150.             // use the crude distance to compute the (positive) return value
  151.             return crudeDistance + correctedRadius - margin;
  152.         }

  154.         // we are close, we need to compute carefully the exact offset;
  155.         // we project the point to the closest zone boundary
  156.         return zone.projectToBoundary(los).getOffset() + correctedRadius - margin;

  158.     }

  160.     /** {@inheritDoc} */
  161.     @Override
  162.     public Vector3D projectToBoundary(final Vector3D lineOfSight) {
  163.         return ((S2Point) zone.projectToBoundary(new S2Point(lineOfSight)).getProjected()).getVector();
  164.     }

  166.     /** {@inheritDoc} */
  167.     @Override
  168.     public List<List<GeodeticPoint>> getFootprint(final Transform fovToBody,
  169.                                                   final OneAxisEllipsoid body,
  170.                                                   final double angularStep) {

  172.         final Frame     bodyFrame = body.getBodyFrame();
  173.         final Vector3D  position  = fovToBody.transformPosition(Vector3D.ZERO);
  174.         final double    r         = position.getNorm();
  175.         if (body.isInside(position)) {
  176.             throw new OrekitException(OrekitMessages.POINT_INSIDE_ELLIPSOID);
  177.         }

  179.         final List<List<GeodeticPoint>> footprint = new ArrayList<>();

  181.         final List<Vertex> boundary = zone.getBoundaryLoops();
  182.         for (final Vertex loopStart : boundary) {
  183.             int count = 0;
  184.             final List<GeodeticPoint> loop  = new ArrayList<>();
  185.             boolean intersectionsFound      = false;
  186.             for (Edge edge = loopStart.getOutgoing();
  187.                  count == 0 || edge.getStart() != loopStart;
  188.                  edge = edge.getEnd().getOutgoing()) {
  189.                 ++count;
  190.                 final int    n     = (int) FastMath.ceil(edge.getLength() / angularStep);
  191.                 final double delta =  edge.getLength() / n;
  192.                 for (int i = 0; i < n; ++i) {
  193.                     final Vector3D awaySC      = new Vector3D(r, edge.getPointAt(i * delta));
  194.                     final Vector3D awayBody    = fovToBody.transformPosition(awaySC);
  195.                     final Line     lineOfSight = new Line(position, awayBody, 1.0e-3);
  196.                     GeodeticPoint  gp          = body.getIntersectionPoint(lineOfSight, position,
  197.                                                                            bodyFrame, null);
  198.                     if (gp != null &&
  199.                         Vector3D.dotProduct(awayBody.subtract(position),
  200.                                             body.transform(gp).subtract(position)) < 0) {
  201.                         // the intersection is in fact on the half-line pointing
  202.                         // towards the back side, it is a spurious intersection
  203.                         gp = null;
  204.                     }

  206.                     if (gp != null) {
  207.                         // the line of sight does intersect the body
  208.                         intersectionsFound = true;
  209.                     } else {
  210.                         // the line of sight does not intersect body
  211.                         // we use a point on the limb
  212.                         gp = body.transform(body.pointOnLimb(position, awayBody), bodyFrame, null);
  213.                     }

  215.                     // add the point in front of the list
  216.                     // (to ensure the loop will be in trigonometric orientation)
  217.                     loop.add(0, gp);

  219.                 }
  220.             }

  222.             if (intersectionsFound) {
  223.                 // at least some of the points did intersect the body,
  224.                 // this loop contributes to the footprint
  225.                 footprint.add(loop);
  226.             }

  228.         }

  230.         if (footprint.isEmpty()) {
  231.             // none of the Field Of View loops cross the body
  232.             // either the body is outside of Field Of View, or it is fully contained
  233.             // we check the center
  234.             final Vector3D bodyCenter = fovToBody.getInverse().transformPosition(Vector3D.ZERO);
  235.             if (zone.checkPoint(new S2Point(bodyCenter)) != Region.Location.OUTSIDE) {
  236.                 // the body is fully contained in the Field Of View
  237.                 // we use the full limb as the footprint
  238.                 final Vector3D x        = bodyCenter.orthogonal();
  239.                 final Vector3D y        = Vector3D.crossProduct(bodyCenter, x).normalize();
  240.                 final double   sinEta   = body.getEquatorialRadius() / r;
  241.                 final double   sinEta2  = sinEta * sinEta;
  242.                 final double   cosAlpha = (FastMath.cos(angularStep) + sinEta2 - 1) / sinEta2;
  243.                 final int      n        = (int) FastMath.ceil(MathUtils.TWO_PI / FastMath.acos(cosAlpha));
  244.                 final double   delta    = MathUtils.TWO_PI / n;
  245.                 final List<GeodeticPoint> loop = new ArrayList<>(n);
  246.                 for (int i = 0; i < n; ++i) {
  247.                     final SinCos sc = FastMath.sinCos(i * delta);
  248.                     final Vector3D outside = new Vector3D(r * sc.cos(), x,
  249.                                                           r * sc.sin(), y);
  250.                     loop.add(body.transform(body.pointOnLimb(position, outside), bodyFrame, null));
  251.                 }
  252.                 footprint.add(loop);
  253.             }
  254.         }

  256.         return footprint;

  258.     }

  260.     /** Enumerate for cone/polygon relative position.
  261.      * @since 10.1
  262.      */
  263.     public enum DefiningConeType {

  265.         /** Constant for cones inside polygons and touching it at polygon edges middle points. */
  266.         INSIDE_CONE_TOUCHING_POLYGON_AT_EDGES_MIDDLE() {

  268.             /** {@inheritDoc}*/
  269.             @Override
  270.             protected double verticesRadius(final double radius, final int n) {
  271.                 // convert the inside (edges middle points) radius to outside (vertices) radius
  272.                 return FastMath.atan(FastMath.tan(radius) / FastMath.cos(FastMath.PI / n));
  273.             }

  275.             /** {@inheritDoc}*/
  276.             @Override
  277.             protected Vector3D createVertex(final Vector3D center, final Vector3D meridian,
  278.                                             final double verticesRadius, final int n) {
  279.                 // convert the edge middle meridian to a vertex
  280.                 final SinCos scA = FastMath.sinCos(FastMath.PI / n);
  281.                 final SinCos scR = FastMath.sinCos(verticesRadius);
  282.                 final Vector3D z = center.normalize();
  283.                 final Vector3D y = Vector3D.crossProduct(center, meridian).normalize();
  284.                 final Vector3D x = Vector3D.crossProduct(y, z);
  285.                 return new Vector3D(scR.sin() * scA.cos(), x, scR.sin() * scA.sin(), y, scR.cos(), z);
  286.             }

  288.         },

  290.         /** Constant for cones outside polygons and touching it at polygon vertices. */
  291.         OUTSIDE_CONE_TOUCHING_POLYGON_AT_VERTICES() {

  293.             /** {@inheritDoc}*/
  294.             @Override
  295.             protected double verticesRadius(final double radius, final int n) {
  296.                 return radius;
  297.             }

  299.             /** {@inheritDoc}*/
  300.             @Override
  301.             protected Vector3D createVertex(final Vector3D center, final Vector3D meridian,
  302.                                             final double verticesRadius, final int n) {
  303.                 // convert the vertex meridian to a vertex
  304.                 final SinCos scR = FastMath.sinCos(verticesRadius);
  305.                 final Vector3D z = center.normalize();
  306.                 final Vector3D y = Vector3D.crossProduct(center, meridian).normalize();
  307.                 final Vector3D x = Vector3D.crossProduct(y, z);
  308.                 return new Vector3D(scR.sin(), x, scR.cos(), z);
  309.             }

  311.         };

  313.         /** Compute radius of cone going through vertices.
  314.          * @param radius defining cone angular radius
  315.          * @param n number of sides of the polygon
  316.          * @return radius of cone going through vertices
  317.          */
  318.         protected abstract double verticesRadius(double radius, int n);

  320.         /** Create a vertex.
  321.          * @param center center of the polygon (the center is in the inside part)
  322.          * @param meridian point defining the reference meridian for one contact
  323.          * point between defining cone and polygon (i.e. either a polygon edge
  324.          * middle point or a polygon vertex)
  325.          * @param verticesRadius defining radius of cone passing through vertices
  326.          * @param n number of sides of the polygon
  327.          * @return created vertex
  328.          */
  329.         protected abstract Vector3D createVertex(Vector3D center, Vector3D meridian,
  330.                                                  double verticesRadius, int n);

  332.     }

  334. }
Created with JaCoCo 0.8.6.202009150832