1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.rugged.intersection;
18
19 import java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.util.FastMath;
24 import org.orekit.bodies.GeodeticPoint;
25 import org.orekit.rugged.api.AlgorithmId;
26 import org.orekit.rugged.errors.DumpManager;
27 import org.orekit.rugged.raster.SimpleTile;
28 import org.orekit.rugged.raster.SimpleTileFactory;
29 import org.orekit.rugged.raster.Tile;
30 import org.orekit.rugged.raster.TileUpdater;
31 import org.orekit.rugged.raster.TilesCache;
32 import org.orekit.rugged.utils.ExtendedEllipsoid;
33 import org.orekit.rugged.utils.NormalizedGeodeticPoint;
34
35
36
37
38
39
40
41
42
43
44 public class BasicScanAlgorithm implements IntersectionAlgorithm {
45
46
47 private final TilesCache<SimpleTile> cache;
48
49
50 private double hMin;
51
52
53 private double hMax;
54
55
56
57 private final AlgorithmId algorithmId;
58
59
60
61
62
63
64
65 public BasicScanAlgorithm(final TileUpdater updater, final int maxCachedTiles, final boolean isOverlappingTiles) {
66 this.cache = new TilesCache<>(new SimpleTileFactory(), updater, maxCachedTiles, isOverlappingTiles);
67 this.hMin = Double.POSITIVE_INFINITY;
68 this.hMax = Double.NEGATIVE_INFINITY;
69 this.algorithmId = AlgorithmId.BASIC_SLOW_EXHAUSTIVE_SCAN_FOR_TESTS_ONLY;
70 }
71
72
73 @Override
74 public NormalizedGeodeticPoint intersection(final ExtendedEllipsoid ellipsoid,
75 final Vector3D position, final Vector3D los) {
76
77 DumpManager.dumpAlgorithm(this.algorithmId);
78
79
80 NormalizedGeodeticPoint entryPoint = null;
81 NormalizedGeodeticPoint exitPoint = null;
82 double minLatitude = Double.NaN;
83 double maxLatitude = Double.NaN;
84 double minLongitude = Double.NaN;
85 double maxLongitude = Double.NaN;
86 final List<SimpleTile> scannedTiles = new ArrayList<>();
87 double centralLongitude = Double.NaN;
88 for (boolean changedMinMax = true; changedMinMax; changedMinMax = checkMinMax(scannedTiles)) {
89
90 scannedTiles.clear();
91
92 entryPoint = ellipsoid.transform(ellipsoid.pointAtAltitude(position, los, Double.isInfinite(hMax) ? 0.0 : hMax),
93 ellipsoid.getBodyFrame(), null,
94 Double.isNaN(centralLongitude) ? 0.0 : centralLongitude);
95 final SimpleTile entryTile = cache.getTile(entryPoint.getLatitude(), entryPoint.getLongitude());
96 if (Double.isNaN(centralLongitude)) {
97 centralLongitude = entryTile.getMinimumLongitude();
98 entryPoint = new NormalizedGeodeticPoint(entryPoint.getLatitude(), entryPoint.getLongitude(),
99 entryPoint.getAltitude(), centralLongitude);
100 }
101 addIfNotPresent(scannedTiles, entryTile);
102
103 exitPoint = ellipsoid.transform(ellipsoid.pointAtAltitude(position, los, Double.isInfinite(hMin) ? 0.0 : hMin),
104 ellipsoid.getBodyFrame(), null, centralLongitude);
105 final SimpleTile exitTile = cache.getTile(exitPoint.getLatitude(), exitPoint.getLongitude());
106 addIfNotPresent(scannedTiles, exitTile);
107
108 minLatitude = FastMath.min(entryPoint.getLatitude(), exitPoint.getLatitude());
109 maxLatitude = FastMath.max(entryPoint.getLatitude(), exitPoint.getLatitude());
110 minLongitude = FastMath.min(entryPoint.getLongitude(), exitPoint.getLongitude());
111 maxLongitude = FastMath.max(entryPoint.getLongitude(), exitPoint.getLongitude());
112
113 if (scannedTiles.size() > 1) {
114
115
116 final double latStep = 0.5 * FastMath.min(entryTile.getLatitudeStep() * entryTile.getLatitudeRows(),
117 exitTile.getLatitudeStep() * exitTile.getLatitudeRows());
118 final double lonStep = 0.5 * FastMath.min(entryTile.getLongitudeStep() * entryTile.getLongitudeColumns(),
119 exitTile.getLongitudeStep() * exitTile.getLongitudeColumns());
120 for (double latitude = minLatitude; latitude <= maxLatitude; latitude += latStep) {
121 for (double longitude = minLongitude; longitude < maxLongitude; longitude += lonStep) {
122 addIfNotPresent(scannedTiles, cache.getTile(latitude, longitude));
123 }
124 }
125 }
126
127 }
128
129
130 NormalizedGeodeticPoint intersectionGP = null;
131 double intersectionDot = Double.POSITIVE_INFINITY;
132 for (final SimpleTile tile : scannedTiles) {
133 for (int i = latitudeIndex(tile, minLatitude); i <= latitudeIndex(tile, maxLatitude); ++i) {
134 for (int j = longitudeIndex(tile, minLongitude); j <= longitudeIndex(tile, maxLongitude); ++j) {
135 final NormalizedGeodeticPoint gp = tile.cellIntersection(entryPoint, ellipsoid.convertLos(entryPoint, los), i, j);
136 if (gp != null) {
137
138
139 final Vector3D delta = ellipsoid.transform(gp).subtract(position);
140 final double s = Vector3D.dotProduct(delta, los) / los.getNormSq();
141 final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
142 ellipsoid.getBodyFrame(), null);
143 final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
144 projected.getLongitude(),
145 projected.getAltitude(),
146 gp.getLongitude());
147 final NormalizedGeodeticPoint gpImproved = tile.cellIntersection(normalizedProjected,
148 ellipsoid.convertLos(normalizedProjected, los),
149 i, j);
150
151 if (gpImproved != null) {
152 final Vector3D point = ellipsoid.transform(gpImproved);
153 final double dot = Vector3D.dotProduct(point.subtract(position), los);
154 if (dot < intersectionDot) {
155 intersectionGP = gpImproved;
156 intersectionDot = dot;
157 }
158 }
159
160 }
161 }
162 }
163 }
164
165 return intersectionGP;
166 }
167
168
169 @Override
170 public NormalizedGeodeticPoint refineIntersection(final ExtendedEllipsoid ellipsoid,
171 final Vector3D position, final Vector3D los,
172 final NormalizedGeodeticPoint closeGuess) {
173 DumpManager.dumpAlgorithm(this.algorithmId);
174 final Vector3D delta = ellipsoid.transform(closeGuess).subtract(position);
175 final double s = Vector3D.dotProduct(delta, los) / los.getNormSq();
176 final GeodeticPoint projected = ellipsoid.transform(new Vector3D(1, position, s, los),
177 ellipsoid.getBodyFrame(), null);
178 final NormalizedGeodeticPoint normalizedProjected = new NormalizedGeodeticPoint(projected.getLatitude(),
179 projected.getLongitude(),
180 projected.getAltitude(),
181 closeGuess.getLongitude());
182 final Tile tile = cache.getTile(normalizedProjected.getLatitude(),
183 normalizedProjected.getLongitude());
184 return tile.cellIntersection(normalizedProjected,
185 ellipsoid.convertLos(normalizedProjected, los),
186 tile.getFloorLatitudeIndex(normalizedProjected.getLatitude()),
187 tile.getFloorLongitudeIndex(normalizedProjected.getLongitude()));
188 }
189
190
191 @Override
192 public double getElevation(final double latitude, final double longitude) {
193 DumpManager.dumpAlgorithm(this.algorithmId);
194 final Tile tile = cache.getTile(latitude, longitude);
195 return tile.interpolateElevation(latitude, longitude);
196 }
197
198
199 @Override
200 public AlgorithmId getAlgorithmId() {
201 return this.algorithmId;
202 }
203
204
205
206
207
208 private boolean checkMinMax(final List<SimpleTile> tiles) {
209
210 boolean changedMinMax = false;
211
212 for (final SimpleTile tile : tiles) {
213
214
215 if (tile.getMinElevation() < hMin) {
216 hMin = tile.getMinElevation();
217 changedMinMax = true;
218 }
219
220
221 if (tile.getMaxElevation() > hMax) {
222 hMax = tile.getMaxElevation();
223 changedMinMax = true;
224 }
225
226 }
227
228 return changedMinMax;
229
230 }
231
232
233
234
235
236 private void addIfNotPresent(final List<SimpleTile> list, final SimpleTile tile) {
237
238
239 for (final SimpleTile existing : list) {
240 if (existing == tile) {
241 return;
242 }
243 }
244
245
246 list.add(tile);
247
248 }
249
250
251
252
253
254
255 private int latitudeIndex(final SimpleTile tile, final double latitude) {
256 final int rawIndex = tile.getFloorLatitudeIndex(latitude);
257 return FastMath.min(FastMath.max(0, rawIndex), tile.getLatitudeRows());
258 }
259
260
261
262
263
264
265 private int longitudeIndex(final SimpleTile tile, final double longitude) {
266 final int rawIndex = tile.getFloorLongitudeIndex(longitude);
267 return FastMath.min(FastMath.max(0, rawIndex), tile.getLongitudeColumns());
268 }
269 }