PolygonalFieldOfView.java

  1. /* Copyright 2002-2025 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 coneType type of defining cone
  75.      * @param meridian point defining the reference meridian for one contact
  76.      * point between defining cone and polygon (i.e. either a polygon edge
  77.      * middle point or a polygon vertex)
  78.      * @param radius defining cone angular radius
  79.      * @param n number of sides of the polygon
  80.      * @param margin angular margin to apply to the zone (if positive,
  81.      * points outside of the raw FoV but close enough to the boundary are
  82.      * considered visible; if negative, points inside of the raw FoV
  83.      * but close enough to the boundary are considered not visible)
  84.      * @since 10.1
  85.      */
  86.     public PolygonalFieldOfView(final Vector3D center, final DefiningConeType coneType,
  87.                                 final Vector3D meridian, final double radius,
  88.                                 final int n, final double margin) {

  90.         super(margin);

  92.         final double   verticesRadius = coneType.verticesRadius(radius, n);
  93.         final Vector3D vertex         = coneType.createVertex(center, meridian, verticesRadius, n);
  94.         this.zone                     = new SphericalPolygonsSet(center, vertex, verticesRadius,
  95.                                                                  n, 1.0e-12 * verticesRadius);

  97.         final Rotation r = new Rotation(center, MathUtils.TWO_PI / n, RotationConvention.VECTOR_OPERATOR);
  98.         final S2Point[] support = new S2Point[n];
  99.         support[0] = new S2Point(vertex);
  100.         for (int i = 1; i < n; ++i) {
  101.             support[i] = new S2Point(r.applyTo(support[i - 1].getVector()));
  102.         }
  103.         this.cap = new EnclosingBall<>(new S2Point(center), Vector3D.angle(center, vertex), support);

  105.     }

  107.     /** Get the interior zone.
  108.      * @return the interior zone
  109.      */
  110.     public SphericalPolygonsSet getZone() {
  111.         return zone;
  112.     }

  114.     /** {@inheritDoc} */
  115.     @Override
  116.     public double offsetFromBoundary(final Vector3D lineOfSight, final double angularRadius,
  117.                                      final VisibilityTrigger trigger) {

  119.         final S2Point los             = new S2Point(lineOfSight);
  120.         final double  margin          = getMargin();
  121.         final double  correctedRadius = trigger.radiusCorrection(angularRadius);
  122.         final double  deadBand        = margin + angularRadius;

  124.         // for faster computation, we start using only the surrounding cap, to filter out
  125.         // far away points (which correspond to most of the points if the Field Of View is small)
  126.         final double crudeDistance = cap.getCenter().distance(los) - cap.getRadius();
  127.         if (crudeDistance > deadBand + 0.01) {
  128.             // we know we are strictly outside of the zone,
  129.             // use the crude distance to compute the (positive) return value
  130.             return crudeDistance + correctedRadius - margin;
  131.         }

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

  137.     }

  139.     /** {@inheritDoc} */
  140.     @Override
  141.     public Vector3D projectToBoundary(final Vector3D lineOfSight) {
  142.         return ((S2Point) zone.projectToBoundary(new S2Point(lineOfSight)).getProjected()).getVector();
  143.     }

  145.     /** {@inheritDoc} */
  146.     @Override
  147.     public List<List<GeodeticPoint>> getFootprint(final Transform fovToBody,
  148.                                                   final OneAxisEllipsoid body,
  149.                                                   final double angularStep) {

  151.         final Frame     bodyFrame = body.getBodyFrame();
  152.         final Vector3D  position  = fovToBody.transformPosition(Vector3D.ZERO);
  153.         final double    r         = position.getNorm();
  154.         if (body.isInside(position)) {
  155.             throw new OrekitException(OrekitMessages.POINT_INSIDE_ELLIPSOID);
  156.         }

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

  160.         final List<Vertex> boundary = zone.getBoundaryLoops();
  161.         for (final Vertex loopStart : boundary) {
  162.             int count = 0;
  163.             final List<GeodeticPoint> loop  = new ArrayList<>();
  164.             boolean intersectionsFound      = false;
  165.             for (Edge edge = loopStart.getOutgoing();
  166.                  count == 0 || edge.getStart() != loopStart;
  167.                  edge = edge.getEnd().getOutgoing()) {
  168.                 ++count;
  169.                 final int    n     = (int) FastMath.ceil(edge.getLength() / angularStep);
  170.                 final double delta =  edge.getLength() / n;
  171.                 for (int i = 0; i < n; ++i) {
  172.                     final Vector3D awaySC      = new Vector3D(r, edge.getPointAt(i * delta));
  173.                     final Vector3D awayBody    = fovToBody.transformPosition(awaySC);
  174.                     final Line     lineOfSight = new Line(position, awayBody, 1.0e-3);
  175.                     GeodeticPoint  gp          = body.getIntersectionPoint(lineOfSight, position,
  176.                                                                            bodyFrame, null);
  177.                     if (gp != null &&
  178.                         Vector3D.dotProduct(awayBody.subtract(position),
  179.                                             body.transform(gp).subtract(position)) < 0) {
  180.                         // the intersection is in fact on the half-line pointing
  181.                         // towards the back side, it is a spurious intersection
  182.                         gp = null;
  183.                     }

  185.                     if (gp != null) {
  186.                         // the line of sight does intersect the body
  187.                         intersectionsFound = true;
  188.                     } else {
  189.                         // the line of sight does not intersect body
  190.                         // we use a point on the limb
  191.                         gp = body.transform(body.pointOnLimb(position, awayBody), bodyFrame, null);
  192.                     }

  194.                     // add the point in front of the list
  195.                     // (to ensure the loop will be in trigonometric orientation)
  196.                     loop.add(0, gp);

  198.                 }
  199.             }

  201.             if (intersectionsFound) {
  202.                 // at least some of the points did intersect the body,
  203.                 // this loop contributes to the footprint
  204.                 footprint.add(loop);
  205.             }

  207.         }

  209.         if (footprint.isEmpty()) {
  210.             // none of the Field Of View loops cross the body
  211.             // either the body is outside of Field Of View, or it is fully contained
  212.             // we check the center
  213.             final Vector3D bodyCenter = fovToBody.getStaticInverse().transformPosition(Vector3D.ZERO);
  214.             if (zone.checkPoint(new S2Point(bodyCenter)) != Region.Location.OUTSIDE) {
  215.                 // the body is fully contained in the Field Of View
  216.                 // we use the full limb as the footprint
  217.                 final Vector3D x        = bodyCenter.orthogonal();
  218.                 final Vector3D y        = Vector3D.crossProduct(bodyCenter, x).normalize();
  219.                 final double   sinEta   = body.getEquatorialRadius() / r;
  220.                 final double   sinEta2  = sinEta * sinEta;
  221.                 final double   cosAlpha = (FastMath.cos(angularStep) + sinEta2 - 1) / sinEta2;
  222.                 final int      n        = (int) FastMath.ceil(MathUtils.TWO_PI / FastMath.acos(cosAlpha));
  223.                 final double   delta    = MathUtils.TWO_PI / n;
  224.                 final List<GeodeticPoint> loop = new ArrayList<>(n);
  225.                 for (int i = 0; i < n; ++i) {
  226.                     final SinCos sc = FastMath.sinCos(i * delta);
  227.                     final Vector3D outside = new Vector3D(r * sc.cos(), x,
  228.                                                           r * sc.sin(), y);
  229.                     loop.add(body.transform(body.pointOnLimb(position, outside), bodyFrame, null));
  230.                 }
  231.                 footprint.add(loop);
  232.             }
  233.         }

  235.         return footprint;

  237.     }

  239.     /** Enumerate for cone/polygon relative position.
  240.      * @since 10.1
  241.      */
  242.     public enum DefiningConeType {

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

  247.             /** {@inheritDoc}*/
  248.             @Override
  249.             protected double verticesRadius(final double radius, final int n) {
  250.                 // convert the inside (edges middle points) radius to outside (vertices) radius
  251.                 return FastMath.atan(FastMath.tan(radius) / FastMath.cos(FastMath.PI / n));
  252.             }

  254.             /** {@inheritDoc}*/
  255.             @Override
  256.             protected Vector3D createVertex(final Vector3D center, final Vector3D meridian,
  257.                                             final double verticesRadius, final int n) {
  258.                 // convert the edge middle meridian to a vertex
  259.                 final SinCos scA = FastMath.sinCos(FastMath.PI / n);
  260.                 final SinCos scR = FastMath.sinCos(verticesRadius);
  261.                 final Vector3D z = center.normalize();
  262.                 final Vector3D y = Vector3D.crossProduct(center, meridian).normalize();
  263.                 final Vector3D x = Vector3D.crossProduct(y, z);
  264.                 return new Vector3D(scR.sin() * scA.cos(), x, scR.sin() * scA.sin(), y, scR.cos(), z);
  265.             }

  267.         },

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

  272.             /** {@inheritDoc}*/
  273.             @Override
  274.             protected double verticesRadius(final double radius, final int n) {
  275.                 return radius;
  276.             }

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

  290.         };

  292.         /** Compute radius of cone going through vertices.
  293.          * @param radius defining cone angular radius
  294.          * @param n number of sides of the polygon
  295.          * @return radius of cone going through vertices
  296.          */
  297.         protected abstract double verticesRadius(double radius, int n);

  299.         /** Create a vertex.
  300.          * @param center center of the polygon (the center is in the inside part)
  301.          * @param meridian point defining the reference meridian for one contact
  302.          * point between defining cone and polygon (i.e. either a polygon edge
  303.          * middle point or a polygon vertex)
  304.          * @param verticesRadius defining radius of cone passing through vertices
  305.          * @param n number of sides of the polygon
  306.          * @return created vertex
  307.          */
  308.         protected abstract Vector3D createVertex(Vector3D center, Vector3D meridian,
  309.                                                  double verticesRadius, int n);

  311.     }

  313. }
Created with JaCoCo 0.8.12.202403310830