Mesh.java
/* Copyright 2002-2020 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.models.earth.tessellation;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.geometry.euclidean.twod.Vector2D;
import org.hipparchus.geometry.partitioning.Region.Location;
import org.hipparchus.geometry.spherical.twod.S2Point;
import org.hipparchus.geometry.spherical.twod.SphericalPolygonsSet;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.Precision;
import org.orekit.bodies.Ellipse;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
/** Class creating a mesh for spherical zones tessellation.
* @author Luc Maisonobe
*/
class Mesh {
/** Underlying ellipsoid. */
private final OneAxisEllipsoid ellipsoid;
/** Zone of interest to tessellate. */
private final SphericalPolygonsSet zone;
/** Zone covered by the mesh. */
private SphericalPolygonsSet coverage;
/** Aiming used for orienting tiles. */
private final TileAiming aiming;
/** Distance between nodes in the along direction. */
private final double alongGap;
/** Distance between nodes in the across direction. */
private final double acrossGap;
/** Map containing nodes. */
private final Map<Long, Node> nodes;
/** Minimum along tile index. */
private int minAlongIndex;
/** Maximum along tile index. */
private int maxAlongIndex;
/** Minimum across tile index. */
private int minAcrossIndex;
/** Maximum across tile index. */
private int maxAcrossIndex;
/** Simple constructor.
* @param ellipsoid underlying ellipsoid
* @param zone zone of interest to tessellate
* @param aiming aiming used for orienting tiles
* @param alongGap distance between nodes in the along direction
* @param acrossGap distance between nodes in the across direction
* @param start location of the first node.
*/
Mesh(final OneAxisEllipsoid ellipsoid, final SphericalPolygonsSet zone,
final TileAiming aiming, final double alongGap, final double acrossGap,
final S2Point start) {
this.ellipsoid = ellipsoid;
this.zone = zone;
this.coverage = null;
this.aiming = aiming;
this.alongGap = alongGap;
this.acrossGap = acrossGap;
this.nodes = new HashMap<Long, Node>();
this.minAlongIndex = 0;
this.maxAlongIndex = 0;
this.minAcrossIndex = 0;
this.maxAcrossIndex = 0;
// safety check for singular points (typically poles)
for (final GeodeticPoint singular : aiming.getSingularPoints()) {
final S2Point s2p = new S2Point(singular.getLongitude(), 0.5 * FastMath.PI - singular.getLatitude());
if (zone.checkPoint(s2p) != Location.OUTSIDE) {
throw new OrekitException(OrekitMessages.CANNOT_COMPUTE_AIMING_AT_SINGULAR_POINT,
FastMath.toDegrees(singular.getLatitude()),
FastMath.toDegrees(singular.getLongitude()));
}
}
// create an enabled first node at origin
final Node origin = new Node(start, 0, 0);
origin.setEnabled();
// force the first node to be considered inside
// It may appear outside if the zone is very thin and
// BSPTree.checkPoint selects a very close but wrong
// tree leaf tree for the point. Even in this case,
// we want the mesh to be properly defined and surround
// the area
origin.forceInside();
store(origin);
}
/** Get the minimum along tile index for enabled nodes.
* @return minimum along tile index for enabled nodes
*/
public int getMinAlongIndex() {
return minAlongIndex;
}
/** Get the maximum along tile index for enabled nodes.
* @return maximum along tile index for enabled nodes
*/
public int getMaxAlongIndex() {
return maxAlongIndex;
}
/** Get the minimum along tile index for enabled nodes for a specific across index.
* @param acrossIndex across index to use
* @return minimum along tile index for enabled nodes for a specific across index
* or {@link #getMaxAlongIndex() getMaxAlongIndex() + 1} if there
* are no nodes with the specified acrossIndex.
*/
public int getMinAlongIndex(final int acrossIndex) {
for (int alongIndex = minAlongIndex; alongIndex <= maxAlongIndex; ++alongIndex) {
final Node node = getNode(alongIndex, acrossIndex);
if (node != null && node.isEnabled()) {
return alongIndex;
}
}
return maxAlongIndex + 1;
}
/** Get the maximum along tile index for enabled nodes for a specific across index.
* @param acrossIndex across index to use
* @return maximum along tile index for enabled nodes for a specific across index
* or {@link #getMinAlongIndex() getMinAlongIndex() - 1} if there
* are no nodes with the specified acrossIndex.
*/
public int getMaxAlongIndex(final int acrossIndex) {
for (int alongIndex = maxAlongIndex; alongIndex >= minAlongIndex; --alongIndex) {
final Node node = getNode(alongIndex, acrossIndex);
if (node != null && node.isEnabled()) {
return alongIndex;
}
}
return minAlongIndex - 1;
}
/** Get the minimum across tile index.
* @return minimum across tile index
*/
public int getMinAcrossIndex() {
return minAcrossIndex;
}
/** Get the maximum across tile index.
* @return maximum across tile index
*/
public int getMaxAcrossIndex() {
return maxAcrossIndex;
}
/** Get the number of nodes.
* @return number of nodes
*/
public int getNumberOfNodes() {
return nodes.size();
}
/** Get the distance between nodes in the along direction.
* @return distance between nodes in the along direction
*/
public double getAlongGap() {
return alongGap;
}
/** Get the distance between nodes in the across direction.
* @return distance between nodes in the across direction
*/
public double getAcrossGap() {
return acrossGap;
}
/** Retrieve a node from its indices.
* @param alongIndex index in the along direction
* @param acrossIndex index in the across direction
* @return node or null if no nodes are available at these indices
* @see #addNode(int, int)
*/
public Node getNode(final int alongIndex, final int acrossIndex) {
return nodes.get(key(alongIndex, acrossIndex));
}
/** Add a node.
* <p>
* This method is similar to {@link #getNode(int, int) getNode} except
* it creates the node if it doesn't alreay exists. All created nodes
* have a status set to {@code disabled}.
* </p>
* @param alongIndex index in the along direction
* @param acrossIndex index in the across direction
* @return node at specified indices, guaranteed to be non-null
* @see #getNode(int, int)
*/
public Node addNode(final int alongIndex, final int acrossIndex) {
// create intermediate (disabled) nodes, up to specified indices
Node node = getExistingAncestor(alongIndex, acrossIndex);
while (node.getAlongIndex() != alongIndex || node.getAcrossIndex() != acrossIndex) {
final Direction direction;
if (node.getAlongIndex() < alongIndex) {
direction = Direction.PLUS_ALONG;
} else if (node.getAlongIndex() > alongIndex) {
direction = Direction.MINUS_ALONG;
} else if (node.getAcrossIndex() < acrossIndex) {
direction = Direction.PLUS_ACROSS;
} else {
direction = Direction.MINUS_ACROSS;
}
final S2Point s2p = node.move(direction.motion(node, alongGap, acrossGap));
node = new Node(s2p, direction.neighborAlongIndex(node), direction.neighborAcrossIndex(node));
store(node);
}
return node;
}
/** Find the closest existing ancestor of a node.
* <p>
* The path from origin to any node is first in the along direction,
* and then in the across direction. Using always the same path pattern
* ensures consistent distortion of the mesh.
* </p>
* @param alongIndex index in the along direction
* @param acrossIndex index in the across direction
* @return an existing node in the path from origin to specified indices
*/
private Node getExistingAncestor(final int alongIndex, final int acrossIndex) {
// start from the desired node indices
int l = alongIndex;
int c = acrossIndex;
Node node = getNode(l, c);
// rewind the path backward, up to origin,
// that is first in the across direction, then in the along direction
// the loop WILL end as there is at least one node at (0, 0)
while (node == null) {
if (c != 0) {
c += c > 0 ? -1 : +1;
} else {
l += l > 0 ? -1 : +1;
}
node = getNode(l, c);
}
// we have found an existing ancestor
return node;
}
/** Get the nodes that lie inside the interest zone.
* @return nodes that lie inside the interest zone
*/
public List<Node> getInsideNodes() {
final List<Node> insideNodes = new ArrayList<Node>();
for (final Map.Entry<Long, Node> entry : nodes.entrySet()) {
if (entry.getValue().isInside()) {
insideNodes.add(entry.getValue());
}
}
return insideNodes;
}
/** Find the existing node closest to a location.
* @param alongIndex index in the along direction
* @param acrossIndex index in the across direction
* @return node or null if no node is available at these indices
*/
public Node getClosestExistingNode(final int alongIndex, final int acrossIndex) {
// we know at least the first node (at 0, 0) exists, we use its
// taxicab distance to the desired location as a search limit
final int maxD = FastMath.max(FastMath.abs(alongIndex), FastMath.abs(acrossIndex));
// search for an existing point in increasing taxicab distances
for (int d = 0; d < maxD; ++d) {
for (int deltaAcross = 0; deltaAcross <= d; ++deltaAcross) {
final int deltaAlong = d - deltaAcross;
Node node = getNode(alongIndex - deltaAlong, acrossIndex - deltaAcross);
if (node != null) {
return node;
}
if (deltaAcross != 0) {
node = getNode(alongIndex - deltaAlong, acrossIndex + deltaAcross);
if (node != null) {
return node;
}
}
if (deltaAlong != 0) {
node = getNode(alongIndex + deltaAlong, acrossIndex - deltaAcross);
if (node != null) {
return node;
}
if (deltaAcross != 0) {
node = getNode(alongIndex + deltaAlong, acrossIndex + deltaAcross);
if (node != null) {
return node;
}
}
}
}
}
// at least the first node always exists
return getNode(0, 0);
}
/** Find the existing node closest to a location.
* @param location reference location in Cartesian coordinates
* @return node or null if no node is available at these indices
*/
public Node getClosestExistingNode(final Vector3D location) {
Node selected = null;
double min = Double.POSITIVE_INFINITY;
for (final Map.Entry<Long, Node> entry : nodes.entrySet()) {
final double distance = Vector3D.distance(location, entry.getValue().getV());
if (distance < min) {
selected = entry.getValue();
min = distance;
}
}
return selected;
}
/** Get the oriented list of <em>enabled</em> nodes at mesh boundary, in taxicab geometry.
* @param simplified if true, don't include intermediate points along almost straight lines
* @return list of nodes
*/
public List<Node> getTaxicabBoundary(final boolean simplified) {
final List<Node> boundary = new ArrayList<Node>();
if (nodes.size() < 2) {
boundary.add(getNode(0, 0));
} else {
// search for the lower left corner
Node lowerLeft = null;
for (int i = minAlongIndex; lowerLeft == null && i <= maxAlongIndex; ++i) {
for (int j = minAcrossIndex; lowerLeft == null && j <= maxAcrossIndex; ++j) {
lowerLeft = getNode(i, j);
if (lowerLeft != null && !lowerLeft.isEnabled()) {
lowerLeft = null;
}
}
}
// loop counterclockwise around the mesh
Direction direction = Direction.MINUS_ACROSS;
Node node = lowerLeft;
do {
boundary.add(node);
Node neighbor = null;
do {
direction = direction.next();
neighbor = getNode(direction.neighborAlongIndex(node),
direction.neighborAcrossIndex(node));
} while (neighbor == null || !neighbor.isEnabled());
direction = direction.next().next();
node = neighbor;
} while (node != lowerLeft);
}
// filter out infinitely thin parts corresponding to spikes
// joining outliers points to the main mesh
boolean changed = true;
while (changed && boundary.size() > 1) {
changed = false;
final int n = boundary.size();
for (int i = 0; i < n; ++i) {
final int previousIndex = (i + n - 1) % n;
final int nextIndex = (i + 1) % n;
if (boundary.get(previousIndex) == boundary.get(nextIndex)) {
// the current point is an infinitely thin spike, remove it
boundary.remove(FastMath.max(i, nextIndex));
boundary.remove(FastMath.min(i, nextIndex));
changed = true;
break;
}
}
}
if (simplified) {
for (int i = 0; i < boundary.size(); ++i) {
final int n = boundary.size();
final Node previous = boundary.get((i + n - 1) % n);
final int pl = previous.getAlongIndex();
final int pc = previous.getAcrossIndex();
final Node current = boundary.get(i);
final int cl = current.getAlongIndex();
final int cc = current.getAcrossIndex();
final Node next = boundary.get((i + 1) % n);
final int nl = next.getAlongIndex();
final int nc = next.getAcrossIndex();
if ((pl == cl && cl == nl) || (pc == cc && cc == nc)) {
// the current point is a spurious intermediate in a straight line, remove it
boundary.remove(i--);
}
}
}
return boundary;
}
/** Get the zone covered by the mesh.
* @return mesh coverage
*/
public SphericalPolygonsSet getCoverage() {
if (coverage == null) {
// lazy build of mesh coverage
final List<Mesh.Node> boundary = getTaxicabBoundary(true);
final S2Point[] vertices = new S2Point[boundary.size()];
for (int i = 0; i < vertices.length; ++i) {
vertices[i] = boundary.get(i).getS2P();
}
coverage = new SphericalPolygonsSet(zone.getTolerance(), vertices);
}
// as caller may modify the BSP tree, we must provide a copy of our safe instance
return (SphericalPolygonsSet) coverage.copySelf();
}
/** Store a node.
* @param node to add
*/
private void store(final Node node) {
// the new node invalidates current estimation of the coverage
coverage = null;
nodes.put(key(node.alongIndex, node.acrossIndex), node);
}
/** Convert along and across indices to map key.
* @param alongIndex index in the along direction
* @param acrossIndex index in the across direction
* @return key map key
*/
private long key(final int alongIndex, final int acrossIndex) {
return ((long) alongIndex) << 32 | (((long) acrossIndex) & 0xFFFFl);
}
/** Container for mesh nodes. */
public class Node {
/** Node position in spherical coordinates. */
private final S2Point s2p;
/** Node position in Cartesian coordinates. */
private final Vector3D v;
/** Normalized along tile direction. */
private final Vector3D along;
/** Normalized across tile direction. */
private final Vector3D across;
/** Indicator for node location with respect to interest zone. */
private boolean insideZone;
/** Index in the along direction. */
private final int alongIndex;
/** Index in the across direction. */
private final int acrossIndex;
/** Indicator for construction nodes used only as intermediate points (disabled) vs. real nodes (enabled). */
private boolean enabled;
/** Create a node.
* @param s2p position in spherical coordinates (my be null)
* @param alongIndex index in the along direction
* @param acrossIndex index in the across direction
*/
private Node(final S2Point s2p, final int alongIndex, final int acrossIndex) {
final GeodeticPoint gp = new GeodeticPoint(0.5 * FastMath.PI - s2p.getPhi(), s2p.getTheta(), 0.0);
this.v = ellipsoid.transform(gp);
this.s2p = s2p;
this.along = aiming.alongTileDirection(v, gp);
this.across = Vector3D.crossProduct(v, along).normalize();
this.insideZone = zone.checkPoint(s2p) != Location.OUTSIDE;
this.alongIndex = alongIndex;
this.acrossIndex = acrossIndex;
this.enabled = false;
}
/** Set the enabled property.
*/
public void setEnabled() {
// store status
this.enabled = true;
// update min/max indices
minAlongIndex = FastMath.min(minAlongIndex, alongIndex);
maxAlongIndex = FastMath.max(maxAlongIndex, alongIndex);
minAcrossIndex = FastMath.min(minAcrossIndex, acrossIndex);
maxAcrossIndex = FastMath.max(maxAcrossIndex, acrossIndex);
}
/** Check if a node is enabled.
* @return true if the node is enabled
*/
public boolean isEnabled() {
return enabled;
}
/** Get the node position in spherical coordinates.
* @return vode position in spherical coordinates
*/
public S2Point getS2P() {
return s2p;
}
/** Get the node position in Cartesian coordinates.
* @return vode position in Cartesian coordinates
*/
public Vector3D getV() {
return v;
}
/** Get the normalized along tile direction.
* @return normalized along tile direction
*/
public Vector3D getAlong() {
return along;
}
/** Get the normalized across tile direction.
* @return normalized across tile direction
*/
public Vector3D getAcross() {
return across;
}
/** Force the node location to be considered inside interest zone.
*/
private void forceInside() {
insideZone = true;
}
/** Check if the node location is inside interest zone.
* @return true if the node location is inside interest zone
*/
public boolean isInside() {
return insideZone;
}
/** Get the index in the along direction.
* @return index in the along direction
*/
public int getAlongIndex() {
return alongIndex;
}
/** Get the index in the across direction.
* @return index in the across direction
*/
public int getAcrossIndex() {
return acrossIndex;
}
/** Move to a nearby point along surface.
* <p>
* The motion will be approximated, assuming the body surface has constant
* curvature along the motion direction. The approximation will be accurate
* for short distances, and error will increase as distance increases.
* </p>
* @param motion straight motion, which must be curved back to surface
* @return arrival point, approximately at specified distance from node
*/
public S2Point move(final Vector3D motion) {
// safety check for too small motion
if (motion.getNorm() < Precision.EPSILON * v.getNorm()) {
return s2p;
}
// find elliptic plane section
final Vector3D normal = Vector3D.crossProduct(v, motion);
final Ellipse planeSection = ellipsoid.getPlaneSection(v, normal);
// find the center of curvature (point on the evolute) below start point
final Vector2D omega2D = planeSection.getCenterOfCurvature(planeSection.toPlane(v));
final Vector3D omega3D = planeSection.toSpace(omega2D);
// compute approximated arrival point, assuming constant radius of curvature
final Vector3D delta = v.subtract(omega3D);
final double theta = motion.getNorm() / delta.getNorm();
final Vector3D approximated = new Vector3D(1, omega3D,
FastMath.cos(theta), delta,
FastMath.sin(theta) / theta, motion);
// convert to spherical coordinates
final GeodeticPoint approximatedGP = ellipsoid.transform(approximated, ellipsoid.getBodyFrame(), null);
return new S2Point(approximatedGP.getLongitude(), 0.5 * FastMath.PI - approximatedGP.getLatitude());
}
}
}