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