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.models.earth.tessellation;
18
19 import java.util.ArrayList;
20 import java.util.HashMap;
21 import java.util.List;
22 import java.util.Map;
23
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.geometry.euclidean.twod.Vector2D;
26 import org.hipparchus.geometry.partitioning.Region.Location;
27 import org.hipparchus.geometry.spherical.twod.S2Point;
28 import org.hipparchus.geometry.spherical.twod.SphericalPolygonsSet;
29 import org.hipparchus.util.FastMath;
30 import org.hipparchus.util.Precision;
31 import org.hipparchus.util.SinCos;
32 import org.orekit.bodies.Ellipse;
33 import org.orekit.bodies.GeodeticPoint;
34 import org.orekit.bodies.OneAxisEllipsoid;
35 import org.orekit.errors.OrekitException;
36 import org.orekit.errors.OrekitMessages;
37
38 /** Class creating a mesh for spherical zones tessellation.
39 * @author Luc Maisonobe
40 */
41 class Mesh {
42
43 /** Underlying ellipsoid. */
44 private final OneAxisEllipsoid ellipsoid;
45
46 /** Zone of interest to tessellate. */
47 private final SphericalPolygonsSet zone;
48
49 /** Zone covered by the mesh. */
50 private SphericalPolygonsSet coverage;
51
52 /** Aiming used for orienting tiles. */
53 private final TileAiming aiming;
54
55 /** Distance between nodes in the along direction. */
56 private final double alongGap;
57
58 /** Distance between nodes in the across direction. */
59 private final double acrossGap;
60
61 /** Map containing nodes. */
62 private final Map<Long, Node> nodes;
63
64 /** Minimum along tile index. */
65 private int minAlongIndex;
66
67 /** Maximum along tile index. */
68 private int maxAlongIndex;
69
70 /** Minimum across tile index. */
71 private int minAcrossIndex;
72
73 /** Maximum across tile index. */
74 private int maxAcrossIndex;
75
76 /** Simple constructor.
77 * @param ellipsoid underlying ellipsoid
78 * @param zone zone of interest to tessellate
79 * @param aiming aiming used for orienting tiles
80 * @param alongGap distance between nodes in the along direction
81 * @param acrossGap distance between nodes in the across direction
82 * @param start location of the first node.
83 */
84 Mesh(final OneAxisEllipsoid ellipsoid, final SphericalPolygonsSet zone,
85 final TileAiming aiming, final double alongGap, final double acrossGap,
86 final S2Point start) {
87 this.ellipsoid = ellipsoid;
88 this.zone = zone;
89 this.coverage = null;
90 this.aiming = aiming;
91 this.alongGap = alongGap;
92 this.acrossGap = acrossGap;
93 this.nodes = new HashMap<>();
94 this.minAlongIndex = 0;
95 this.maxAlongIndex = 0;
96 this.minAcrossIndex = 0;
97 this.maxAcrossIndex = 0;
98
99 // safety check for singular points (typically poles)
100 for (final GeodeticPoint singular : aiming.getSingularPoints()) {
101 final S2Point s2p = new S2Point(singular.getLongitude(), 0.5 * FastMath.PI - singular.getLatitude());
102 if (zone.checkPoint(s2p) != Location.OUTSIDE) {
103 throw new OrekitException(OrekitMessages.CANNOT_COMPUTE_AIMING_AT_SINGULAR_POINT,
104 FastMath.toDegrees(singular.getLatitude()),
105 FastMath.toDegrees(singular.getLongitude()));
106 }
107 }
108
109 // create an enabled first node at origin
110 final Node origin = new Node(start, 0, 0);
111 origin.setEnabled();
112
113 // force the first node to be considered inside
114 // It may appear outside if the zone is very thin and
115 // BSPTree.checkPoint selects a very close but wrong
116 // tree leaf tree for the point. Even in this case,
117 // we want the mesh to be properly defined and surround
118 // the area
119 origin.forceInside();
120
121 store(origin);
122
123 }
124
125 /** Get the minimum along tile index for enabled nodes.
126 * @return minimum along tile index for enabled nodes
127 */
128 public int getMinAlongIndex() {
129 return minAlongIndex;
130 }
131
132 /** Get the maximum along tile index for enabled nodes.
133 * @return maximum along tile index for enabled nodes
134 */
135 public int getMaxAlongIndex() {
136 return maxAlongIndex;
137 }
138
139 /** Get the minimum along tile index for enabled nodes for a specific across index.
140 * @param acrossIndex across index to use
141 * @return minimum along tile index for enabled nodes for a specific across index
142 * or {@link #getMaxAlongIndex() getMaxAlongIndex() + 1} if there
143 * are no nodes with the specified acrossIndex.
144 */
145 public int getMinAlongIndex(final int acrossIndex) {
146 for (int alongIndex = minAlongIndex; alongIndex <= maxAlongIndex; ++alongIndex) {
147 final Node node = getNode(alongIndex, acrossIndex);
148 if (node != null && node.isEnabled()) {
149 return alongIndex;
150 }
151 }
152 return maxAlongIndex + 1;
153 }
154
155 /** Get the maximum along tile index for enabled nodes for a specific across index.
156 * @param acrossIndex across index to use
157 * @return maximum along tile index for enabled nodes for a specific across index
158 * or {@link #getMinAlongIndex() getMinAlongIndex() - 1} if there
159 * are no nodes with the specified acrossIndex.
160 */
161 public int getMaxAlongIndex(final int acrossIndex) {
162 for (int alongIndex = maxAlongIndex; alongIndex >= minAlongIndex; --alongIndex) {
163 final Node node = getNode(alongIndex, acrossIndex);
164 if (node != null && node.isEnabled()) {
165 return alongIndex;
166 }
167 }
168 return minAlongIndex - 1;
169 }
170
171 /** Get the minimum across tile index.
172 * @return minimum across tile index
173 */
174 public int getMinAcrossIndex() {
175 return minAcrossIndex;
176 }
177
178 /** Get the maximum across tile index.
179 * @return maximum across tile index
180 */
181 public int getMaxAcrossIndex() {
182 return maxAcrossIndex;
183 }
184
185 /** Get the number of nodes.
186 * @return number of nodes
187 */
188 public int getNumberOfNodes() {
189 return nodes.size();
190 }
191
192 /** Get the distance between nodes in the along direction.
193 * @return distance between nodes in the along direction
194 */
195 public double getAlongGap() {
196 return alongGap;
197 }
198
199 /** Get the distance between nodes in the across direction.
200 * @return distance between nodes in the across direction
201 */
202 public double getAcrossGap() {
203 return acrossGap;
204 }
205
206 /** Retrieve a node from its indices.
207 * @param alongIndex index in the along direction
208 * @param acrossIndex index in the across direction
209 * @return node or null if no nodes are available at these indices
210 * @see #addNode(int, int)
211 */
212 public Node getNode(final int alongIndex, final int acrossIndex) {
213 return nodes.get(key(alongIndex, acrossIndex));
214 }
215
216 /** Add a node.
217 * <p>
218 * This method is similar to {@link #getNode(int, int) getNode} except
219 * it creates the node if it doesn't alreay exists. All created nodes
220 * have a status set to {@code disabled}.
221 * </p>
222 * @param alongIndex index in the along direction
223 * @param acrossIndex index in the across direction
224 * @return node at specified indices, guaranteed to be non-null
225 * @see #getNode(int, int)
226 */
227 public Node addNode(final int alongIndex, final int acrossIndex) {
228
229 // create intermediate (disabled) nodes, up to specified indices
230 Node node = getExistingAncestor(alongIndex, acrossIndex);
231 while (node.getAlongIndex() != alongIndex || node.getAcrossIndex() != acrossIndex) {
232 final Direction direction;
233 if (node.getAlongIndex() < alongIndex) {
234 direction = Direction.PLUS_ALONG;
235 } else if (node.getAlongIndex() > alongIndex) {
236 direction = Direction.MINUS_ALONG;
237 } else if (node.getAcrossIndex() < acrossIndex) {
238 direction = Direction.PLUS_ACROSS;
239 } else {
240 direction = Direction.MINUS_ACROSS;
241 }
242 final S2Point s2p = node.move(direction.motion(node, alongGap, acrossGap));
243 node = new Node(s2p, direction.neighborAlongIndex(node), direction.neighborAcrossIndex(node));
244 store(node);
245 }
246
247 return node;
248
249 }
250
251 /** Find the closest existing ancestor of a node.
252 * <p>
253 * The path from origin to any node is first in the along direction,
254 * and then in the across direction. Using always the same path pattern
255 * ensures consistent distortion of the mesh.
256 * </p>
257 * @param alongIndex index in the along direction
258 * @param acrossIndex index in the across direction
259 * @return an existing node in the path from origin to specified indices
260 */
261 private Node getExistingAncestor(final int alongIndex, final int acrossIndex) {
262
263 // start from the desired node indices
264 int l = alongIndex;
265 int c = acrossIndex;
266 Node node = getNode(l, c);
267
268 // rewind the path backward, up to origin,
269 // that is first in the across direction, then in the along direction
270 // the loop WILL end as there is at least one node at (0, 0)
271 while (node == null) {
272 if (c != 0) {
273 c += c > 0 ? -1 : +1;
274 } else {
275 l += l > 0 ? -1 : +1;
276 }
277 node = getNode(l, c);
278 }
279
280 // we have found an existing ancestor
281 return node;
282
283 }
284
285 /** Get the nodes that lie inside the interest zone.
286 * @return nodes that lie inside the interest zone
287 */
288 public List<Node> getInsideNodes() {
289 final List<Node> insideNodes = new ArrayList<>();
290 for (final Map.Entry<Long, Node> entry : nodes.entrySet()) {
291 if (entry.getValue().isInside()) {
292 insideNodes.add(entry.getValue());
293 }
294 }
295 return insideNodes;
296 }
297
298 /** Find the existing node closest to a location.
299 * @param alongIndex index in the along direction
300 * @param acrossIndex index in the across direction
301 * @return node or null if no node is available at these indices
302 */
303 public Node getClosestExistingNode(final int alongIndex, final int acrossIndex) {
304
305 // we know at least the first node (at 0, 0) exists, we use its
306 // taxicab distance to the desired location as a search limit
307 final int maxD = FastMath.max(FastMath.abs(alongIndex), FastMath.abs(acrossIndex));
308
309 // search for an existing point in increasing taxicab distances
310 for (int d = 0; d < maxD; ++d) {
311 for (int deltaAcross = 0; deltaAcross <= d; ++deltaAcross) {
312 final int deltaAlong = d - deltaAcross;
313 Node node = getNode(alongIndex - deltaAlong, acrossIndex - deltaAcross);
314 if (node != null) {
315 return node;
316 }
317 if (deltaAcross != 0) {
318 node = getNode(alongIndex - deltaAlong, acrossIndex + deltaAcross);
319 if (node != null) {
320 return node;
321 }
322 }
323 if (deltaAlong != 0) {
324 node = getNode(alongIndex + deltaAlong, acrossIndex - deltaAcross);
325 if (node != null) {
326 return node;
327 }
328 if (deltaAcross != 0) {
329 node = getNode(alongIndex + deltaAlong, acrossIndex + deltaAcross);
330 if (node != null) {
331 return node;
332 }
333 }
334 }
335 }
336 }
337
338 // at least the first node always exists
339 return getNode(0, 0);
340
341 }
342
343 /** Find the existing node closest to a location.
344 * @param location reference location in Cartesian coordinates
345 * @return node or null if no node is available at these indices
346 */
347 public Node getClosestExistingNode(final Vector3D location) {
348 Node selected = null;
349 double min = Double.POSITIVE_INFINITY;
350 for (final Map.Entry<Long, Node> entry : nodes.entrySet()) {
351 final double distance = Vector3D.distance(location, entry.getValue().getV());
352 if (distance < min) {
353 selected = entry.getValue();
354 min = distance;
355 }
356 }
357 return selected;
358 }
359
360 /** Get the oriented list of <em>enabled</em> nodes at mesh boundary, in taxicab geometry.
361 * @param simplified if true, don't include intermediate points along almost straight lines
362 * @return list of nodes
363 */
364 public List<Node> getTaxicabBoundary(final boolean simplified) {
365
366 final List<Node> boundary = new ArrayList<>();
367 if (nodes.size() < 2) {
368 boundary.add(getNode(0, 0));
369 } else {
370
371 // search for the lower left corner
372 Node lowerLeft = null;
373 for (int i = minAlongIndex; lowerLeft == null && i <= maxAlongIndex; ++i) {
374 for (int j = minAcrossIndex; lowerLeft == null && j <= maxAcrossIndex; ++j) {
375 lowerLeft = getNode(i, j);
376 if (lowerLeft != null && !lowerLeft.isEnabled()) {
377 lowerLeft = null;
378 }
379 }
380 }
381
382 // loop counterclockwise around the mesh
383 Direction direction = Direction.MINUS_ACROSS;
384 Node node = lowerLeft;
385 do {
386 boundary.add(node);
387 Node neighbor = null;
388 do {
389 direction = direction.next();
390 neighbor = getNode(direction.neighborAlongIndex(node),
391 direction.neighborAcrossIndex(node));
392 } while (neighbor == null || !neighbor.isEnabled());
393 direction = direction.next().next();
394 node = neighbor;
395 } while (node != lowerLeft);
396 }
397
398 // filter out infinitely thin parts corresponding to spikes
399 // joining outliers points to the main mesh
400 boolean changed = true;
401 while (changed && boundary.size() > 1) {
402 changed = false;
403 final int n = boundary.size();
404 for (int i = 0; i < n; ++i) {
405 final int previousIndex = (i + n - 1) % n;
406 final int nextIndex = (i + 1) % n;
407 if (boundary.get(previousIndex) == boundary.get(nextIndex)) {
408 // the current point is an infinitely thin spike, remove it
409 boundary.remove(FastMath.max(i, nextIndex));
410 boundary.remove(FastMath.min(i, nextIndex));
411 changed = true;
412 break;
413 }
414 }
415 }
416
417 if (simplified) {
418 for (int i = 0; i < boundary.size(); ++i) {
419 final int n = boundary.size();
420 final Node previous = boundary.get((i + n - 1) % n);
421 final int pl = previous.getAlongIndex();
422 final int pc = previous.getAcrossIndex();
423 final Node current = boundary.get(i);
424 final int cl = current.getAlongIndex();
425 final int cc = current.getAcrossIndex();
426 final Node next = boundary.get((i + 1) % n);
427 final int nl = next.getAlongIndex();
428 final int nc = next.getAcrossIndex();
429 if (pl == cl && cl == nl || pc == cc && cc == nc) {
430 // the current point is a spurious intermediate in a straight line, remove it
431 boundary.remove(i--);
432 }
433 }
434 }
435
436 return boundary;
437
438 }
439
440 /** Get the zone covered by the mesh.
441 * @return mesh coverage
442 */
443 public SphericalPolygonsSet getCoverage() {
444
445 if (coverage == null) {
446
447 // lazy build of mesh coverage
448 final List<Mesh.Node> boundary = getTaxicabBoundary(true);
449 final S2Point[] vertices = new S2Point[boundary.size()];
450 for (int i = 0; i < vertices.length; ++i) {
451 vertices[i] = boundary.get(i).getS2P();
452 }
453 coverage = new SphericalPolygonsSet(zone.getTolerance(), vertices);
454 }
455
456 // as caller may modify the BSP tree, we must provide a copy of our safe instance
457 return (SphericalPolygonsSet) coverage.copySelf();
458
459 }
460
461 /** Store a node.
462 * @param node to add
463 */
464 private void store(final Node node) {
465
466 // the new node invalidates current estimation of the coverage
467 coverage = null;
468
469 nodes.put(key(node.alongIndex, node.acrossIndex), node);
470
471 }
472
473 /** Convert along and across indices to map key.
474 * @param alongIndex index in the along direction
475 * @param acrossIndex index in the across direction
476 * @return key map key
477 */
478 private long key(final int alongIndex, final int acrossIndex) {
479 return ((long) alongIndex) << 32 | (((long) acrossIndex) & 0xFFFFl);
480 }
481
482 /** Container for mesh nodes. */
483 public class Node {
484
485 /** Node position in spherical coordinates. */
486 private final S2Point s2p;
487
488 /** Node position in Cartesian coordinates. */
489 private final Vector3D v;
490
491 /** Normalized along tile direction. */
492 private final Vector3D along;
493
494 /** Normalized across tile direction. */
495 private final Vector3D across;
496
497 /** Indicator for node location with respect to interest zone. */
498 private boolean insideZone;
499
500 /** Index in the along direction. */
501 private final int alongIndex;
502
503 /** Index in the across direction. */
504 private final int acrossIndex;
505
506 /** Indicator for construction nodes used only as intermediate points (disabled) vs. real nodes (enabled). */
507 private boolean enabled;
508
509 /** Create a node.
510 * @param s2p position in spherical coordinates (my be null)
511 * @param alongIndex index in the along direction
512 * @param acrossIndex index in the across direction
513 */
514 private Node(final S2Point s2p, final int alongIndex, final int acrossIndex) {
515 final GeodeticPoint gp = new GeodeticPoint(0.5 * FastMath.PI - s2p.getPhi(), s2p.getTheta(), 0.0);
516 this.v = ellipsoid.transform(gp);
517 this.s2p = s2p;
518 this.along = aiming.alongTileDirection(v, gp);
519 this.across = Vector3D.crossProduct(v, along).normalize();
520 this.insideZone = zone.checkPoint(s2p) != Location.OUTSIDE;
521 this.alongIndex = alongIndex;
522 this.acrossIndex = acrossIndex;
523 this.enabled = false;
524 }
525
526 /** Set the enabled property.
527 */
528 public void setEnabled() {
529
530 // store status
531 this.enabled = true;
532
533 // update min/max indices
534 minAlongIndex = FastMath.min(minAlongIndex, alongIndex);
535 maxAlongIndex = FastMath.max(maxAlongIndex, alongIndex);
536 minAcrossIndex = FastMath.min(minAcrossIndex, acrossIndex);
537 maxAcrossIndex = FastMath.max(maxAcrossIndex, acrossIndex);
538
539 }
540
541 /** Check if a node is enabled.
542 * @return true if the node is enabled
543 */
544 public boolean isEnabled() {
545 return enabled;
546 }
547
548 /** Get the node position in spherical coordinates.
549 * @return vode position in spherical coordinates
550 */
551 public S2Point getS2P() {
552 return s2p;
553 }
554
555 /** Get the node position in Cartesian coordinates.
556 * @return vode position in Cartesian coordinates
557 */
558 public Vector3D getV() {
559 return v;
560 }
561
562 /** Get the normalized along tile direction.
563 * @return normalized along tile direction
564 */
565 public Vector3D getAlong() {
566 return along;
567 }
568
569 /** Get the normalized across tile direction.
570 * @return normalized across tile direction
571 */
572 public Vector3D getAcross() {
573 return across;
574 }
575
576 /** Force the node location to be considered inside interest zone.
577 */
578 private void forceInside() {
579 insideZone = true;
580 }
581
582 /** Check if the node location is inside interest zone.
583 * @return true if the node location is inside interest zone
584 */
585 public boolean isInside() {
586 return insideZone;
587 }
588
589 /** Get the index in the along direction.
590 * @return index in the along direction
591 */
592 public int getAlongIndex() {
593 return alongIndex;
594 }
595
596 /** Get the index in the across direction.
597 * @return index in the across direction
598 */
599 public int getAcrossIndex() {
600 return acrossIndex;
601 }
602
603 /** Move to a nearby point along surface.
604 * <p>
605 * The motion will be approximated, assuming the body surface has constant
606 * curvature along the motion direction. The approximation will be accurate
607 * for short distances, and error will increase as distance increases.
608 * </p>
609 * @param motion straight motion, which must be curved back to surface
610 * @return arrival point, approximately at specified distance from node
611 */
612 public S2Point move(final Vector3D motion) {
613
614 // safety check for too small motion
615 if (motion.getNorm() < Precision.EPSILON * v.getNorm()) {
616 return s2p;
617 }
618
619 // find elliptic plane section
620 final Vector3D normal = Vector3D.crossProduct(v, motion);
621 final Ellipse planeSection = ellipsoid.getPlaneSection(v, normal);
622
623 // find the center of curvature (point on the evolute) below start point
624 final Vector2D omega2D = planeSection.getCenterOfCurvature(planeSection.toPlane(v));
625 final Vector3D omega3D = planeSection.toSpace(omega2D);
626
627 // compute approximated arrival point, assuming constant radius of curvature
628 final Vector3D delta = v.subtract(omega3D);
629 final double theta = motion.getNorm() / delta.getNorm();
630 final SinCos sc = FastMath.sinCos(theta);
631 final Vector3D approximated = new Vector3D(1, omega3D,
632 sc.cos(), delta,
633 sc.sin() / theta, motion);
634
635 // convert to spherical coordinates
636 final GeodeticPoint approximatedGP = ellipsoid.transform(approximated, ellipsoid.getBodyFrame(), null);
637 return new S2Point(approximatedGP.getLongitude(), 0.5 * FastMath.PI - approximatedGP.getLatitude());
638
639 }
640
641 }
642
643 }