1 /* Copyright 2013-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.rugged.intersection.duvenhage;
18
19 import org.hipparchus.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.util.FastMath;
21 import org.orekit.bodies.GeodeticPoint;
22 import org.orekit.rugged.api.AlgorithmId;
23 import org.orekit.rugged.errors.DumpManager;
24 import org.orekit.rugged.errors.RuggedException;
25 import org.orekit.rugged.errors.RuggedInternalError;
26 import org.orekit.rugged.errors.RuggedMessages;
27 import org.orekit.rugged.intersection.IntersectionAlgorithm;
28 import org.orekit.rugged.raster.Tile;
29 import org.orekit.rugged.raster.TileUpdater;
30 import org.orekit.rugged.raster.TilesCache;
31 import org.orekit.rugged.utils.ExtendedEllipsoid;
32 import org.orekit.rugged.utils.NormalizedGeodeticPoint;
33
34 /** Digital Elevation Model intersection using Bernardt Duvenhage's algorithm.
35 * <p>
36 * The algorithm is described in the 2009 paper:
37 * <a href="https://researchspace.csir.co.za/dspace/bitstream/10204/3041/1/Duvenhage_2009.pdf">Using
38 * An Implicit Min/Max KD-Tree for Doing Efficient Terrain Line of Sight Calculations</a>.
39 * </p>
40 * @author Luc Maisonobe
41 * @author Guylaine Prat
42 */
43 public class DuvenhageAlgorithm implements IntersectionAlgorithm {
44
45 /** Step size when skipping from one tile to a neighbor one, in meters. */
46 private static final double STEP = 0.01;
47
48 /** Maximum number of attempts to refine intersection.
49 * <p>
50 * This parameter is intended to prevent infinite loops.
51 * </p>
52 * @since 2.1 */
53 private static final int MAX_REFINING_ATTEMPTS = 100;
54
55 /** Cache for DEM tiles. */
56 private final TilesCache<MinMaxTreeTile> cache;
57
58 /** Flag for flat-body hypothesis. */
59 private final boolean flatBody;
60
61 /** Algorithm Id.
62 * @since 2.2 */
63 private final AlgorithmId algorithmId;
64
65 /** Simple constructor.
66 * @param updater updater used to load Digital Elevation Model tiles
67 * @param maxCachedTiles maximum number of tiles stored in the cache
68 * @param flatBody if true, the body is considered flat, i.e. lines computed
69 * from entry/exit points in the DEM are considered to be straight lines also
70 * in geodetic coordinates. The sagitta resulting from real ellipsoid curvature
71 * is therefore <em>not</em> corrected in this case. As this computation is not
72 * costly (a few percents overhead), it is highly recommended to set this parameter
73 * to {@code false}. This flag is mainly intended for comparison purposes with other systems
74 * @param isOverlappingTiles flag to tell if the DEM tiles are overlapping:
75 * true if overlapping; false otherwise.
76 */
77 public DuvenhageAlgorithm(final TileUpdater updater, final int maxCachedTiles,
78 final boolean flatBody, final boolean isOverlappingTiles) {
79 this.cache = new TilesCache<MinMaxTreeTile>(new MinMaxTreeTileFactory(), updater,
80 maxCachedTiles, isOverlappingTiles);
81 this.flatBody = flatBody;
82 this.algorithmId = flatBody ? AlgorithmId.DUVENHAGE_FLAT_BODY : AlgorithmId.DUVENHAGE;
83 }
84
85 /** {@inheritDoc} */
86 @Override
87 public NormalizedGeodeticPoint intersection(final ExtendedEllipsoid ellipsoid,
88 final Vector3D position, final Vector3D los) {
89
90 DumpManager.dumpAlgorithm(this.algorithmId);
91
92 // compute intersection with ellipsoid
93 final NormalizedGeodeticPoint gp0 = ellipsoid.pointOnGround(position, los, 0.0);
94
95 // locate the entry tile along the line-of-sight
96 MinMaxTreeTile tile = cache.getTile(gp0.getLatitude(), gp0.getLongitude());
97
98 NormalizedGeodeticPoint current = null;
99 NormalizedGeodeticPoint positionGP = null;
100 double hMax = tile.getMaxElevation();
101 while (current == null) {
102
103 // find where line-of-sight crosses tile max altitude
104 final Vector3D entryP = ellipsoid.pointAtAltitude(position, los, hMax + STEP);
105 if (Vector3D.dotProduct(entryP.subtract(position), los) < 0) {
106 // the entry point is behind spacecraft!
107
108 // let's see if at least we are above DEM
109 try {
110 positionGP =
111 ellipsoid.transform(position, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
112 final double elevationAtPosition = tile.interpolateElevation(positionGP.getLatitude(), positionGP.getLongitude());
113 if (positionGP.getAltitude() >= elevationAtPosition) {
114 // we can use the current position as the entry point
115 current = positionGP;
116 } else {
117 current = null;
118 }
119 } catch (RuggedException re) {
120 if (re.getSpecifier() == RuggedMessages.OUT_OF_TILE_ANGLES) {
121 // the entry point is in another tile, we can use the current position as the entry point;
122 current = positionGP;
123 }
124 }
125
126 if (current == null) {
127 throw new RuggedException(RuggedMessages.DEM_ENTRY_POINT_IS_BEHIND_SPACECRAFT);
128 }
129
130 } else {
131 current = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
132 }
133
134 if (tile.getLocation(current.getLatitude(), current.getLongitude()) != Tile.Location.HAS_INTERPOLATION_NEIGHBORS) {
135 // the entry point is in another tile
136 tile = cache.getTile(current.getLatitude(), current.getLongitude());
137 hMax = FastMath.max(hMax, tile.getMaxElevation());
138 current = null;
139 }
140
141 }
142
143 // loop along the path
144 while (true) {
145
146 // find where line-of-sight exit tile
147 final LimitPoint exit = findExit(tile, ellipsoid, position, los);
148
149 // compute intersection with Digital Elevation Model
150 final int entryLat = FastMath.max(0,
151 FastMath.min(tile.getLatitudeRows() - 1,
152 tile.getFloorLatitudeIndex(current.getLatitude())));
153 final int entryLon = FastMath.max(0,
154 FastMath.min(tile.getLongitudeColumns() - 1,
155 tile.getFloorLongitudeIndex(current.getLongitude())));
156 final int exitLat = FastMath.max(0,
157 FastMath.min(tile.getLatitudeRows() - 1,
158 tile.getFloorLatitudeIndex(exit.getPoint().getLatitude())));
159 final int exitLon = FastMath.max(0,
160 FastMath.min(tile.getLongitudeColumns() - 1,
161 tile.getFloorLongitudeIndex(exit.getPoint().getLongitude())));
162 NormalizedGeodeticPoint intersection = recurseIntersection(0, ellipsoid, position, los, tile,
163 current, entryLat, entryLon,
164 exit.getPoint(), exitLat, exitLon);
165
166 if (intersection != null) {
167 // we have found the intersection
168 return intersection;
169 } else if (exit.atSide()) {
170 // no intersection on this tile, we can proceed to next part of the line-of-sight
171
172 // select next tile after current point
173 final Vector3D forward = new Vector3D(1.0, ellipsoid.transform(exit.getPoint()), STEP, los);
174 current = ellipsoid.transform(forward, ellipsoid.getBodyFrame(), null, tile.getMinimumLongitude());
175 tile = cache.getTile(current.getLatitude(), current.getLongitude());
176
177 if (tile.interpolateElevation(current.getLatitude(), current.getLongitude()) >= current.getAltitude()) {
178 // extremely rare case! The line-of-sight traversed the Digital Elevation Model
179 // during the very short forward step we used to move to next tile
180 // we consider this point to be OK
181 return current;
182 }
183
184 } else {
185
186 // this should never happen
187 // we should have left the loop with an intersection point
188 // try a fallback non-recursive search
189 intersection = noRecurseIntersection(ellipsoid, position, los, tile,
190 current, entryLat, entryLon,
191 exitLat, exitLon);
192 if (intersection != null) {
193 return intersection;
194 } else {
195 throw new RuggedInternalError(null);
196 }
197 }
198 }
199 }
200
201 /** {@inheritDoc} */
202 @Override
203 public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
204 final Vector3D position, final Vector3D los,
205 final NormalizedGeodeticPoint closeGuess) {
206
207 DumpManager.dumpAlgorithm(this.algorithmId);
208
209 if (flatBody) {
210 // under the (bad) flat-body assumption, the reference point must remain
211 // at DEM entry and exit, even if we already have a much better close guess :-(
212 // this is in order to remain consistent with other systems
213 final Tile tile = cache.getTile(closeGuess.getLatitude(), closeGuess.getLongitude());
214 final Vector3D exitP = ellipsoid.pointAtAltitude(position, los, tile.getMinElevation());
215 final Vector3D entryP = ellipsoid.pointAtAltitude(position, los, tile.getMaxElevation());
216 final NormalizedGeodeticPoint entry = ellipsoid.transform(entryP, ellipsoid.getBodyFrame(), null,
217 tile.getMinimumLongitude());
218 return tile.cellIntersection(entry, ellipsoid.convertLos(entryP, exitP),
219 tile.getFloorLatitudeIndex(closeGuess.getLatitude()),
220 tile.getFloorLongitudeIndex(closeGuess.getLongitude()));
221
222 } else {
223 // regular curved ellipsoid model
224
225 NormalizedGeodeticPoint currentGuess = closeGuess;
226 // normally, we should succeed at first attempt but in very rare cases
227 // we may loose the intersection (typically because some corrections introduced
228 // between the first intersection and the refining have slightly changed the
229 // relative geometry between Digital Elevation Model and Line Of Sight).
230 // In these rare cases, we have to recover a new intersection
231 for (int i = 0; i < MAX_REFINING_ATTEMPTS; ++i) {
232
233 final Vector3D delta = ellipsoid.transform(currentGuess).subtract(position);
234 final double s = Vector3D.dotProduct(delta, los) / los.getNormSq();
235 final Vector3D projectedP = new Vector3D(1, position, s, los);
236 final GeodeticPoint projected = ellipsoid.transform(projectedP, ellipsoid.getBodyFrame(), null);
237 final NormalizedGeodeticPoint normalizedProjected =
238 new NormalizedGeodeticPoint(projected.getLatitude(),
239 projected.getLongitude(),
240 projected.getAltitude(),
241 currentGuess.getLongitude());
242 final Tile tile = cache.getTile(normalizedProjected.getLatitude(), normalizedProjected.getLongitude());
243
244 final Vector3D topoLOS = ellipsoid.convertLos(normalizedProjected, los);
245 final int iLat = tile.getFloorLatitudeIndex(normalizedProjected.getLatitude());
246 final int iLon = tile.getFloorLongitudeIndex(normalizedProjected.getLongitude());
247 final NormalizedGeodeticPoint foundIntersection = tile.cellIntersection(normalizedProjected, topoLOS, iLat, iLon);
248
249 if (foundIntersection != null) {
250 // nominal case, we were able to refine the intersection
251 return foundIntersection;
252 } else {
253 // extremely rare case: we have lost the intersection
254 // find a start point for new search, leaving the current cell behind
255 final double cellBoundaryLatitude = tile.getLatitudeAtIndex(topoLOS.getY() <= 0 ? iLat : iLat + 1);
256 final double cellBoundaryLongitude = tile.getLongitudeAtIndex(topoLOS.getX() <= 0 ? iLon : iLon + 1);
257 final Vector3D cellExit = new Vector3D(1, selectClosest(latitudeCrossing(ellipsoid, projectedP, los, cellBoundaryLatitude, projectedP),
258 longitudeCrossing(ellipsoid, projectedP, los, cellBoundaryLongitude, projectedP),
259 projectedP),
260 STEP, los);
261 final GeodeticPoint egp = ellipsoid.transform(cellExit, ellipsoid.getBodyFrame(), null);
262 final NormalizedGeodeticPoint cellExitGP = new NormalizedGeodeticPoint(egp.getLatitude(),
263 egp.getLongitude(),
264 egp.getAltitude(),
265 currentGuess.getLongitude());
266 if (tile.interpolateElevation(cellExitGP.getLatitude(), cellExitGP.getLongitude()) >= cellExitGP.getAltitude()) {
267 // extremely rare case! The line-of-sight traversed the Digital Elevation Model
268 // during the very short forward step we used to move to next cell
269 // we consider this point to be OK
270 return cellExitGP;
271 }
272
273 // We recompute fully a new guess, starting from the point after current cell
274 final GeodeticPoint currentGuessGP = intersection(ellipsoid, cellExit, los);
275 currentGuess = new NormalizedGeodeticPoint(currentGuessGP.getLatitude(),
276 currentGuessGP.getLongitude(),
277 currentGuessGP.getAltitude(),
278 projected.getLongitude());
279 }
280
281 }
282
283 // no intersection found
284 return null;
285
286 } // end test on flatbody
287
288 }
289
290 /** {@inheritDoc} */
291 @Override
292 public double getElevation(final double latitude, final double longitude) {
293
294 DumpManager.dumpAlgorithm(this.algorithmId);
295 final Tile tile = cache.getTile(latitude, longitude);
296 return tile.interpolateElevation(latitude, longitude);
297 }
298
299 /** {@inheritDoc} */
300 @Override
301 public AlgorithmId getAlgorithmId() {
302 return this.algorithmId;
303 }
304
305 /** Compute intersection of line with Digital Elevation Model in a sub-tile.
306 * @param depth recursion depth
307 * @param ellipsoid reference ellipsoid
308 * @param position pixel position in ellipsoid frame
309 * @param los pixel line-of-sight in ellipsoid frame
310 * @param tile Digital Elevation Model tile
311 * @param entry line-of-sight entry point in the sub-tile
312 * @param entryLat index to use for interpolating entry point elevation
313 * @param entryLon index to use for interpolating entry point elevation
314 * @param exit line-of-sight exit point from the sub-tile
315 * @param exitLat index to use for interpolating exit point elevation
316 * @param exitLon index to use for interpolating exit point elevation
317 * @return point at which the line first enters ground, or null if does not enter
318 * ground in the search sub-tile
319 */
320 private NormalizedGeodeticPoint recurseIntersection(final int depth, final ExtendedEllipsoid ellipsoid,
321 final Vector3D position, final Vector3D los,
322 final MinMaxTreeTile tile,
323 final NormalizedGeodeticPoint entry, final int entryLat, final int entryLon,
324 final NormalizedGeodeticPoint exit, final int exitLat, final int exitLon) {
325
326 if (depth > 30) {
327 // this should never happen
328 throw new RuggedInternalError(null);
329 }
330
331 if (searchDomainSize(entryLat, entryLon, exitLat, exitLon) < 4) {
332 // we have narrowed the search down to a few cells
333 return noRecurseIntersection(ellipsoid, position, los, tile,
334 entry, entryLat, entryLon, exitLat, exitLon);
335 }
336
337 // find the deepest level in the min/max kd-tree at which entry and exit share a sub-tile
338 final int level = tile.getMergeLevel(entryLat, entryLon, exitLat, exitLon);
339 if (level >= 0 && exit.getAltitude() >= tile.getMaxElevation(exitLat, exitLon, level)) {
340 // the line-of-sight segment is fully above Digital Elevation Model
341 // we can safely reject it and proceed to next part of the line-of-sight
342 return null;
343 }
344
345 NormalizedGeodeticPoint previousGP = entry;
346 int previousLat = entryLat;
347 int previousLon = entryLon;
348 final double angularMargin = STEP / ellipsoid.getEquatorialRadius();
349
350 // introduce all intermediate points corresponding to the line-of-sight
351 // intersecting the boundary between level 0 sub-tiles
352 if (tile.isColumnMerging(level + 1)) {
353 // recurse through longitude crossings
354
355 final int[] crossings = tile.getCrossedBoundaryColumns(previousLon, exitLon, level + 1);
356 for (final int crossingLon : crossings) {
357
358 // compute segment endpoints
359 final double longitude = tile.getLongitudeAtIndex(crossingLon);
360 if (longitude >= FastMath.min(entry.getLongitude(), exit.getLongitude()) - angularMargin &&
361 longitude <= FastMath.max(entry.getLongitude(), exit.getLongitude()) + angularMargin) {
362
363 NormalizedGeodeticPoint crossingGP = null;
364 if (!flatBody) {
365 try {
366 // full computation of crossing point
367 final Vector3D crossingP = ellipsoid.pointAtLongitude(position, los, longitude);
368 crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null,
369 tile.getMinimumLongitude());
370 } catch (RuggedException re) {
371 // in some very rare cases of numerical noise, we miss the crossing point
372 crossingGP = null;
373 }
374 }
375 if (crossingGP == null) {
376 // linear approximation of crossing point
377 final double d = exit.getLongitude() - entry.getLongitude();
378 final double cN = (exit.getLongitude() - longitude) / d;
379 final double cX = (longitude - entry.getLongitude()) / d;
380 crossingGP = new NormalizedGeodeticPoint(cN * entry.getLatitude() + cX * exit.getLatitude(),
381 longitude,
382 cN * entry.getAltitude() + cX * exit.getAltitude(),
383 tile.getMinimumLongitude());
384 }
385 final int crossingLat =
386 FastMath.max(0,
387 FastMath.min(tile.getLatitudeRows() - 1,
388 tile.getFloorLatitudeIndex(crossingGP.getLatitude())));
389
390 // adjust indices as the crossing point is by definition between the sub-tiles
391 final int crossingLonBefore = crossingLon - (entryLon <= exitLon ? 1 : 0);
392 final int crossingLonAfter = crossingLon - (entryLon <= exitLon ? 0 : 1);
393
394 if (inRange(crossingLonBefore, entryLon, exitLon)) {
395 // look for intersection
396 final NormalizedGeodeticPoint intersection;
397 if (searchDomainSize(previousLat, previousLon, crossingLat, crossingLonBefore) <
398 searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
399 intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
400 previousGP, previousLat, previousLon,
401 crossingGP, crossingLat, crossingLonBefore);
402 } else {
403 // we failed to reduce domain size, probably due to numerical problems
404 intersection = noRecurseIntersection(ellipsoid, position, los, tile,
405 previousGP, previousLat, previousLon,
406 crossingLat, crossingLonBefore);
407 }
408 if (intersection != null) {
409 return intersection;
410 }
411 }
412
413 // prepare next segment
414 previousGP = crossingGP;
415 previousLat = crossingLat;
416 previousLon = crossingLonAfter;
417
418 }
419
420 }
421 } else {
422 // recurse through latitude crossings
423 final int[] crossings = tile.getCrossedBoundaryRows(previousLat, exitLat, level + 1);
424 for (final int crossingLat : crossings) {
425
426 // compute segment endpoints
427 final double latitude = tile.getLatitudeAtIndex(crossingLat);
428 if (latitude >= FastMath.min(entry.getLatitude(), exit.getLatitude()) - angularMargin &&
429 latitude <= FastMath.max(entry.getLatitude(), exit.getLatitude()) + angularMargin) {
430
431 NormalizedGeodeticPoint crossingGP = null;
432 if (!flatBody) {
433 // full computation of crossing point
434 try {
435 final Vector3D crossingP = ellipsoid.pointAtLatitude(position, los,
436 tile.getLatitudeAtIndex(crossingLat),
437 ellipsoid.transform(entry));
438 crossingGP = ellipsoid.transform(crossingP, ellipsoid.getBodyFrame(), null,
439 tile.getMinimumLongitude());
440 } catch (RuggedException re) {
441 // in some very rare cases of numerical noise, we miss the crossing point
442 crossingGP = null;
443 }
444 }
445 if (crossingGP == null) {
446 // linear approximation of crossing point
447 final double d = exit.getLatitude() - entry.getLatitude();
448 final double cN = (exit.getLatitude() - latitude) / d;
449 final double cX = (latitude - entry.getLatitude()) / d;
450 crossingGP = new NormalizedGeodeticPoint(latitude,
451 cN * entry.getLongitude() + cX * exit.getLongitude(),
452 cN * entry.getAltitude() + cX * exit.getAltitude(),
453 tile.getMinimumLongitude());
454 }
455 final int crossingLon =
456 FastMath.max(0,
457 FastMath.min(tile.getLongitudeColumns() - 1,
458 tile.getFloorLongitudeIndex(crossingGP.getLongitude())));
459
460 // adjust indices as the crossing point is by definition between the sub-tiles
461 final int crossingLatBefore = crossingLat - (entryLat <= exitLat ? 1 : 0);
462 final int crossingLatAfter = crossingLat - (entryLat <= exitLat ? 0 : 1);
463
464 if (inRange(crossingLatBefore, entryLat, exitLat)) {
465 // look for intersection
466 final NormalizedGeodeticPoint intersection;
467 if (searchDomainSize(previousLat, previousLon, crossingLatBefore, crossingLon) <
468 searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
469 intersection = recurseIntersection(depth + 1, ellipsoid, position, los, tile,
470 previousGP, previousLat, previousLon,
471 crossingGP, crossingLatBefore, crossingLon);
472 } else {
473 intersection = noRecurseIntersection(ellipsoid, position, los, tile,
474 previousGP, previousLat, previousLon,
475 crossingLatBefore, crossingLon);
476 }
477 if (intersection != null) {
478 return intersection;
479 }
480 }
481
482 // prepare next segment
483 previousGP = crossingGP;
484 previousLat = crossingLatAfter;
485 previousLon = crossingLon;
486
487 }
488
489 }
490 }
491
492 if (inRange(previousLat, entryLat, exitLat) && inRange(previousLon, entryLon, exitLon)) {
493 // last part of the segment, up to exit point
494 if (searchDomainSize(previousLat, previousLon, exitLat, exitLon) <
495 searchDomainSize(entryLat, entryLon, exitLat, exitLon)) {
496 return recurseIntersection(depth + 1, ellipsoid, position, los, tile,
497 previousGP, previousLat, previousLon,
498 exit, exitLat, exitLon);
499 } else {
500 return noRecurseIntersection(ellipsoid, position, los, tile,
501 previousGP, previousLat, previousLon,
502 exitLat, exitLon);
503 }
504 } else {
505 return null;
506 }
507
508 }
509
510 /** Compute intersection of line with Digital Elevation Model in a sub-tile, without recursion.
511 * @param ellipsoid reference ellipsoid
512 * @param position pixel position in ellipsoid frame
513 * @param los pixel line-of-sight in ellipsoid frame
514 * @param tile Digital Elevation Model tile
515 * @param entry line-of-sight entry point in the sub-tile
516 * @param entryLat index to use for interpolating entry point elevation
517 * @param entryLon index to use for interpolating entry point elevation
518 * @param exitLat index to use for interpolating exit point elevation
519 * @param exitLon index to use for interpolating exit point elevation
520 * @return point at which the line first enters ground, or null if does not enter
521 * ground in the search sub-tile
522 */
523 private NormalizedGeodeticPoint noRecurseIntersection(final ExtendedEllipsoid ellipsoid,
524 final Vector3D position, final Vector3D los,
525 final MinMaxTreeTile tile,
526 final NormalizedGeodeticPoint entry,
527 final int entryLat, final int entryLon,
528 final int exitLat, final int exitLon) {
529
530 NormalizedGeodeticPoint intersectionGP = null;
531 double intersectionDot = Double.POSITIVE_INFINITY;
532 for (int i = FastMath.min(entryLat, exitLat); i <= FastMath.max(entryLat, exitLat); ++i) {
533 for (int j = FastMath.min(entryLon, exitLon); j <= FastMath.max(entryLon, exitLon); ++j) {
534 final NormalizedGeodeticPoint gp = tile.cellIntersection(entry, ellipsoid.convertLos(entry, los), i, j);
535 if (gp != null) {
536
537 // improve the point, by projecting it back on the 3D line, fixing the small body curvature at cell level
538 final Vector3D delta = ellipsoid.transform(gp).subtract(position);
539 final double s = Vector3D.dotProduct(delta, los) / los.getNormSq();
540 if (s > 0) {
541 final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
542 ellipsoid.getBodyFrame(), null);
543 final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
544 projected.getLongitude(),
545 projected.getAltitude(),
546 gp.getLongitude());
547 final NormalizedGeodeticPoint gpImproved = tile.cellIntersection(normalizedProjected,
548 ellipsoid.convertLos(normalizedProjected, los),
549 i, j);
550
551 if (gpImproved != null) {
552 final Vector3D point = ellipsoid.transform(gpImproved);
553 final double dot = Vector3D.dotProduct(point.subtract(position), los);
554 if (dot < intersectionDot) {
555 intersectionGP = gpImproved;
556 intersectionDot = dot;
557 }
558 }
559 }
560
561 }
562 }
563 }
564
565 return intersectionGP;
566
567 }
568
569 /** Compute the size of a search domain.
570 * @param entryLat index to use for interpolating entry point elevation
571 * @param entryLon index to use for interpolating entry point elevation
572 * @param exitLat index to use for interpolating exit point elevation
573 * @param exitLon index to use for interpolating exit point elevation
574 * @return size of the search domain
575 */
576 private int searchDomainSize(final int entryLat, final int entryLon,
577 final int exitLat, final int exitLon) {
578 return (FastMath.abs(entryLat - exitLat) + 1) * (FastMath.abs(entryLon - exitLon) + 1);
579 }
580
581 /** Check if an index is inside a range.
582 * @param i index to check
583 * @param a first bound of the range (may be either below or above b)
584 * @param b second bound of the range (may be either below or above a)
585 * @return true if i is between a and b (inclusive)
586 */
587 private boolean inRange(final int i, final int a, final int b) {
588 return i >= FastMath.min(a, b) && i <= FastMath.max(a, b);
589 }
590
591 /** Compute a line-of-sight exit point from a tile.
592 * @param tile tile to consider
593 * @param ellipsoid reference ellipsoid
594 * @param position pixel position in ellipsoid frame
595 * @param los pixel line-of-sight in ellipsoid frame
596 * @return exit point
597 */
598 private LimitPoint findExit(final Tile tile, final ExtendedEllipsoid ellipsoid,
599 final Vector3D position, final Vector3D los) {
600
601 // look for an exit at bottom
602 final double reference = tile.getMinimumLongitude();
603 final Vector3D exitP = ellipsoid.pointAtAltitude(position, los, tile.getMinElevation() - STEP);
604 final NormalizedGeodeticPoint exitGP = ellipsoid.transform(exitP, ellipsoid.getBodyFrame(), null, reference);
605
606 switch (tile.getLocation(exitGP.getLatitude(), exitGP.getLongitude())) {
607 case SOUTH_WEST :
608 return new LimitPoint(ellipsoid, reference,
609 selectClosest(latitudeCrossing(ellipsoid, position, los, tile.getMinimumLatitude(), exitP),
610 longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
611 position),
612 true);
613 case WEST :
614 return new LimitPoint(ellipsoid, reference,
615 longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
616 true);
617 case NORTH_WEST:
618 return new LimitPoint(ellipsoid, reference,
619 selectClosest(latitudeCrossing(ellipsoid, position, los, tile.getMaximumLatitude(), exitP),
620 longitudeCrossing(ellipsoid, position, los, tile.getMinimumLongitude(), exitP),
621 position),
622 true);
623 case NORTH :
624 return new LimitPoint(ellipsoid, reference,
625 latitudeCrossing(ellipsoid, position, los, tile.getMaximumLatitude(), exitP),
626 true);
627 case NORTH_EAST :
628 return new LimitPoint(ellipsoid, reference,
629 selectClosest(latitudeCrossing(ellipsoid, position, los, tile.getMaximumLatitude(), exitP),
630 longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
631 position),
632 true);
633 case EAST :
634 return new LimitPoint(ellipsoid, reference,
635 longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
636 true);
637 case SOUTH_EAST :
638 return new LimitPoint(ellipsoid, reference,
639 selectClosest(latitudeCrossing(ellipsoid, position, los, tile.getMinimumLatitude(), exitP),
640 longitudeCrossing(ellipsoid, position, los, tile.getMaximumLongitude(), exitP),
641 position),
642 true);
643 case SOUTH :
644 return new LimitPoint(ellipsoid, reference,
645 latitudeCrossing(ellipsoid, position, los, tile.getMinimumLatitude(), exitP),
646 true);
647 case HAS_INTERPOLATION_NEIGHBORS :
648 return new LimitPoint(exitGP, false);
649
650 default :
651 // this should never happen
652 throw new RuggedInternalError(null);
653 }
654
655 }
656
657 /** Select point closest to line-of-sight start.
658 * @param p1 first point to consider
659 * @param p2 second point to consider
660 * @param position pixel position in ellipsoid frame
661 * @return either p1 or p2, depending on which is closest to position
662 */
663 private Vector3D selectClosest(final Vector3D p1, final Vector3D p2, final Vector3D position) {
664 return Vector3D.distance(p1, position) <= Vector3D.distance(p2, position) ? p1 : p2;
665 }
666
667 /** Get point at some latitude along a pixel line of sight.
668 * @param ellipsoid reference ellipsoid
669 * @param position pixel position (in body frame)
670 * @param los pixel line-of-sight, not necessarily normalized (in body frame)
671 * @param latitude latitude with respect to ellipsoid
672 * @param closeReference reference point used to select the closest solution
673 * when there are two points at the desired latitude along the line
674 * @return point at latitude, or closeReference if no such point can be found
675 */
676 private Vector3D latitudeCrossing(final ExtendedEllipsoid ellipsoid,
677 final Vector3D position, final Vector3D los,
678 final double latitude, final Vector3D closeReference) {
679 try {
680 return ellipsoid.pointAtLatitude(position, los, latitude, closeReference);
681 } catch (RuggedException re) {
682 return closeReference;
683 }
684 }
685
686 /** Get point at some latitude along a pixel line of sight.
687 * @param ellipsoid reference ellipsoid
688 * @param position pixel position (in body frame)
689 * @param los pixel line-of-sight, not necessarily normalized (in body frame)
690 * @param longitude longitude with respect to ellipsoid
691 * @param closeReference reference point used to select the closest solution
692 * when there are two points at the desired longitude along the line
693 * @return point at longitude, or closeReference if no such point can be found
694 */
695 private Vector3D longitudeCrossing(final ExtendedEllipsoid ellipsoid,
696 final Vector3D position, final Vector3D los,
697 final double longitude, final Vector3D closeReference) {
698 try {
699 return ellipsoid.pointAtLongitude(position, los, longitude);
700 } catch (RuggedException re) {
701 return closeReference;
702 }
703 }
704
705 /** Point at tile boundary. */
706 private static class LimitPoint {
707
708 /** Coordinates. */
709 private final NormalizedGeodeticPoint point;
710
711 /** Limit status. */
712 private final boolean side;
713
714 /** Simple constructor.
715 * @param ellipsoid reference ellipsoid
716 * @param referenceLongitude reference longitude lc such that the point longitude will
717 * be normalized between lc-π and lc+π
718 * @param cartesian Cartesian point
719 * @param side if true, the point is on a side limit, otherwise
720 * it is on a top/bottom limit
721 */
722 LimitPoint(final ExtendedEllipsoid ellipsoid, final double referenceLongitude,
723 final Vector3D cartesian, final boolean side) {
724 this(ellipsoid.transform(cartesian, ellipsoid.getBodyFrame(), null, referenceLongitude), side);
725 }
726
727 /** Simple constructor.
728 * @param point coordinates
729 * @param side if true, the point is on a side limit, otherwise
730 * it is on a top/bottom limit
731 */
732 LimitPoint(final NormalizedGeodeticPoint point, final boolean side) {
733 this.point = point;
734 this.side = side;
735 }
736
737 /** Get the point coordinates.
738 * @return point coordinates
739 */
740 public NormalizedGeodeticPoint getPoint() {
741 return point;
742 }
743
744 /** Check if point is on the side of a tile.
745 * @return true if the point is on a side limit, otherwise
746 * it is on a top/bottom limit
747 */
748 public boolean atSide() {
749 return side;
750 }
751
752 }
753 }