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