1
2
3
4
5
6
7
8
9
10
11
12
13 package org.gscg.common.geo;
14
15 import java.io.Serializable;
16 import java.rmi.RemoteException;
17 import java.util.ArrayList;
18 import java.util.HashMap;
19 import java.util.Map;
20
21 import javax.vecmath.Point2d;
22
23 import nl.tudelft.simulation.dsol.animation.LocatableInterface;
24 import nl.tudelft.simulation.logger.Logger;
25
26 /***
27 * Calculates the distance between two lat/long points. The distance is returned
28 * in km.
29 * <p>
30 *
31 * Copyright (c) 2003-2005 Delft University of Technology, Jaffalaan 5, 2628 BX
32 * Delft, the Netherlands. All rights reserved.
33 *
34 * See for project information <a href="http://www.simulation.tudelft.nl/">
35 * www.simulation.tudelft.nl </a>.
36 *
37 * The source code and binary code of this software is proprietary information
38 * of Delft University of Technology.
39 *
40 * @author <a
41 * href="http://www.tbm.tudelft.nl/webstaf/stijnh/index.htm">Stijn-Pieter
42 * van Houten </a>
43 * @version $Revision: 1.1 $ $Date: 2005/06/16 12:34:08 $
44 * @since 1.0.0
45 */
46 public class CalculateLatLonDistance implements Serializable
47 {
48 /*** the distance */
49 private double d = Double.NaN;
50
51 /*** arga cos */
52 private double argacos = Double.NaN;
53
54 /*** crs12 and crs21 */
55 private double crs12, crs21 = Double.NaN;
56
57 /*** a */
58 private double a = Double.NaN;
59
60 /***
61 * constructs a new CalculateLatLonDistance
62 */
63 public CalculateLatLonDistance()
64 {
65 super();
66 }
67
68 /***
69 * @param location1 location 1
70 * @param location2 location 2
71 * @return returns the distance
72 */
73 public double getDistance(final LocatableInterface location1,
74 final LocatableInterface location2)
75 {
76 try
77 {
78 return this.getDistance(new Point2d(location1.getLocation().x,
79 location1.getLocation().y), new Point2d(location2
80 .getLocation().x, location2.getLocation().y));
81 } catch (RemoteException remoteException)
82 {
83 Logger.severe(this, "getDistance", remoteException);
84 }
85 return Double.NaN;
86 }
87
88 /***
89 * Returns the distance between two points (lat/lon) in kilometres.
90 *
91 * @param location1 location 1
92 * @param location2 location 2
93 * @return returns the distance in kilometres
94 */
95 public double getDistance(final Point2d location1, final Point2d location2)
96 {
97
98
99
100
101
102
103
104
105
106
107
108 double distanceConversionFactor;
109 double lat1, lat2, lon1, lon2;
110
111 lat1 = (Math.PI / 180) * location1.x;
112 lat2 = (Math.PI / 180) * location2.x;
113 lon1 = (Math.PI / 180) * location1.y;
114 lon2 = (Math.PI / 180) * location2.y;
115
116 distanceConversionFactor = this.getDistanceConversionFactor();
117
118 Ells ellipse = this.getEllipsoid(0);
119
120 if (ellipse.getName().equalsIgnoreCase("Sphere"))
121 {
122
123 Map cd = this.crsdist(lat1, lon1, lat2, lon2);
124
125 this.crs12 = ((Double) cd.get("crs12")).doubleValue()
126 * (180 / Math.PI);
127 this.crs21 = ((Double) cd.get("crs21")).doubleValue()
128 * (180 / Math.PI);
129 this.d = ((Double) cd.get("d")).doubleValue() * (180 / Math.PI)
130 * 60 * distanceConversionFactor;
131 } else
132 {
133
134 Map cde = this.crsdistEll(lat1, -lon1, lat2, -lon2, ellipse);
135
136
137 this.crs12 = ((Double) cde.get("crs12")).doubleValue()
138 * (180 / Math.PI);
139 this.crs21 = ((Double) cde.get("crs21")).doubleValue()
140 * (180 / Math.PI);
141 this.d = ((Double) cde.get("d")).doubleValue()
142 * distanceConversionFactor;
143 }
144
145 return this.d;
146 }
147
148 /***
149 *
150 * @param glat1 glat1 initial geodetic latitude in radians N positive
151 * @param glon1 glon1 initial geodetic longitude in radians E positive
152 * @param glat2 glat2 final geodetic latitude in radians N positive
153 * @param glon2 glon2 final geodetic longitude in radians E positive
154 * @param ellipse the ellipse
155 * @return returns a map
156 */
157 private Map crsdistEll(double glat1, final double glon1,
158 final double glat2, final double glon2, final Ells ellipse)
159 {
160
161 this.a = ellipse.a;
162 double f = 1 / ellipse.invf;
163
164 double r, tu1, tu2, cu1, su1, cu2, s1, b1, f1 = Double.NaN;
165 double x = Double.NaN;
166 double sx = Double.NaN;
167 double cx = Double.NaN;
168 double sy = Double.NaN;
169 double cy = Double.NaN;
170 double y = Double.NaN;
171 double sa = Double.NaN;
172 double c2a = Double.NaN;
173 double cz = Double.NaN;
174 double e = Double.NaN;
175 double c = Double.NaN;
176 double d = Double.NaN;
177 double eps = 0.00000000005;
178 double faz, baz, s;
179 double iter = 1;
180 double maxiter = 100;
181 if ((glat1 + glat2 == 0.) && (Math.abs(glon1 - glon2) == Math.PI))
182 {
183 System.out
184 .println("Course and distance between antipodal points is undefined");
185 glat1 = glat1 + 0.00001;
186 }
187 if (glat1 == glat2
188 && (glon1 == glon2 || Math.abs(Math.abs(glon1 - glon2) - 2
189 * Math.PI) < eps))
190 {
191 System.out
192 .println("Points 1 and 2 are identical- course undefined");
193
194 Map result = new HashMap();
195 result.put("d", new Double(0.0));
196 result.put("crs12", new Double(0.0));
197 result.put("crs21", new Double(Math.PI));
198
199 return result;
200 }
201 r = 1 - f;
202 tu1 = r * Math.tan(glat1);
203 tu2 = r * Math.tan(glat2);
204 cu1 = 1. / Math.sqrt(1. + tu1 * tu1);
205 su1 = cu1 * tu1;
206 cu2 = 1. / Math.sqrt(1. + tu2 * tu2);
207 s1 = cu1 * cu2;
208 b1 = s1 * tu2;
209 f1 = b1 * tu1;
210 x = glon2 - glon1;
211 d = x + 1;
212 while ((Math.abs(d - x) > eps) && (iter < maxiter))
213 {
214 iter = iter + 1;
215 sx = Math.sin(x);
216
217 cx = Math.cos(x);
218 tu1 = cu2 * sx;
219 tu2 = b1 - su1 * cu2 * cx;
220 sy = Math.sqrt(tu1 * tu1 + tu2 * tu2);
221 cy = s1 * cx + f1;
222 y = Math.atan2(sy, cy);
223 sa = s1 * sx / sy;
224 c2a = 1 - sa * sa;
225 cz = f1 + f1;
226 if (c2a > 0.)
227 {
228 cz = cy - cz / c2a;
229 }
230 e = cz * cz * 2. - 1;
231 c = ((-3. * c2a + 4.) * f + 4.) * c2a * f / 16;
232 d = x;
233 x = ((e * cy * c + cz) * sy * c + y) * sa;
234 x = (1. - c) * x * f + glon2 - glon1;
235 }
236 faz = modcrs(Math.atan2(tu1, tu2));
237 baz = modcrs(Math.atan2(cu1 * sx, b1 * cx - su1 * cu2) + Math.PI);
238 x = Math.sqrt((1 / (r * r) - 1) * c2a + 1);
239 x += 1;
240 x = (x - 2.) / x;
241 c = 1. - x;
242 c = (x * x / 4. + 1.) / c;
243 d = (0.375 * x * x - 1.) * x;
244 x = e * cy;
245 s = ((((sy * sy * 4. - 3.) * (1. - e - e) * cz * d / 6. - x) * d / 4. + cz)
246 * sy * d + y)
247 * c * this.a * r;
248
249 System.out.println("s: " + s);
250 Map result = new HashMap();
251 result.put("d", new Double(s));
252 result.put("crs12", new Double(faz));
253 result.put("crs21", new Double(baz));
254
255 if (Math.abs(iter - maxiter) < eps)
256 {
257 System.out.println("Algorithm did not converge");
258 }
259 return result;
260 }
261
262 /***
263 * @param x x
264 * @return returns modrcs
265 */
266 private double modcrs(final double x)
267 {
268 return (x % 2 * Math.PI);
269 }
270
271 /***
272 * @param selection the selection
273 * @return returns Ells
274 */
275 private Ells getEllipsoid(final int selection)
276 {
277 ArrayList ellipsoids = new ArrayList();
278
279 ellipsoids.add(0, new Ells("Sphere", 180 * 60 / Math.PI,
280 Double.MAX_VALUE));
281 ellipsoids.add(1, new Ells("WGS84", 6378.137 / 1.852, 298.257223563));
282 ellipsoids.add(2, new Ells("NAD27", 6378.2064 / 1.852, 294.9786982138));
283 ellipsoids.add(3, new Ells("International", 6378.388 / 1.852, 297.0));
284 ellipsoids.add(4, new Ells("Krasovsky", 6378.245 / 1.852, 298.3));
285 ellipsoids.add(5, new Ells("Bessel", 6377.397155 / 1.852, 299.1528));
286 ellipsoids.add(6, new Ells("WGS72", 6378.135 / 1.852, 298.26));
287 ellipsoids.add(7, new Ells("WGS66", 6378.145 / 1.852, 298.25));
288 ellipsoids.add(8, new Ells("FAI sphere", 6371.0 / 1.852, 1000000000.0));
289
290 if (((Ells) ellipsoids.get(selection)).invf == Double.MAX_VALUE)
291 {
292 ((Ells) ellipsoids.get(selection)).invf = 1000000000;
293 }
294
295 return ((Ells) ellipsoids.get(selection));
296 }
297
298 /***
299 * @param lat1 lat1
300 * @param lon1 lon1
301 * @param lat2 lat2
302 * @param lon2 lon2
303 * @return returns a map
304 */
305 private Map crsdist(final double lat1, final double lon1,
306 final double lat2, final double lon2)
307 {
308
309
310 if ((lat1 + lat2 == 0.) && (Math.abs(lon1 - lon2) == Math.PI)
311 && (Math.abs(lat1) != (Math.PI / 180) * 90.))
312 {
313 System.out.println("Course between antipodal points is undefined");
314 return null;
315 }
316
317 this.d = Math.acos(Math.sin(lat1) * Math.sin(lat2) + Math.cos(lat1)
318 * Math.cos(lat2) * Math.cos(lon1 - lon2));
319
320 if ((this.d == 0.) || (lat1 == -(Math.PI / 180) * 90))
321 {
322 this.crs12 = 2 * Math.PI;
323 } else if (lat1 == (Math.PI / 180) * 90.)
324 {
325 this.crs12 = Math.PI;
326 } else
327 {
328 this.argacos = (Math.sin(lat2) - Math.sin(lat1) * Math.cos(this.d))
329 / (Math.sin(this.d) * Math.cos(lat1));
330 if (Math.sin(lon2 - lon1) < 0)
331 {
332 this.crs12 = acosf(this.argacos);
333 } else
334 {
335 this.crs12 = 2 * Math.PI - acosf(this.argacos);
336 }
337 }
338 if ((this.d == 0.) || (lat2 == -(Math.PI / 180) * 90.))
339 {
340 this.crs21 = 0;
341 } else if (lat2 == (Math.PI / 180) * 90.)
342 {
343 this.crs21 = Math.PI;
344 } else
345 {
346 this.argacos = (Math.sin(lat1) - Math.sin(lat2) * Math.cos(this.d))
347 / (Math.sin(this.d) * Math.cos(lat2));
348 if (Math.sin(lon1 - lon2) < 0)
349 {
350 this.crs21 = acosf(this.argacos);
351 } else
352 {
353 this.crs21 = 2 * Math.PI - acosf(this.argacos);
354 }
355 }
356
357 Map result = new HashMap();
358 result.put("d", new Double(this.d));
359 result.put("crs12", new Double(this.crs12));
360 result.put("crs21", new Double(this.crs21));
361 return result;
362 }
363
364 /***
365 * @param x x
366 * @return returns acos
367 */
368 private double acosf(double x)
369 {
370 if (Math.abs(x) > 1)
371 {
372 x /= Math.abs(x);
373 }
374 return Math.acos(x);
375 }
376
377 /***
378 * @return returns the conversion factor
379 */
380 private double getDistanceConversionFactor()
381 {
382 double[] conversion = new double[]{1.0, 1.852, 185200.0 / 160934.40,
383 185200.0 / 30.48};
384 return conversion[1];
385 }
386
387 /***
388 * @param args command line arguments
389 */
390 public static void main(final String[] args)
391 {
392 new CalculateLatLonDistance();
393 }
394
395 /***
396 * Ells.
397 * <p>
398 * Copyright (c) 2003-2005 Delft University of Technology, Jaffalaan 5, 2628
399 * BX Delft, the Netherlands. All rights reserved.
400 *
401 * See for project information <a href="http://www.simulation.tudelft.nl/">
402 * www.simulation.tudelft.nl </a>.
403 *
404 * The source code and binary code of this software is proprietary
405 * information of Delft University of Technology.
406 *
407 * @author <a
408 * href="http://www.tbm.tudelft.nl/webstaf/stijnh/index.htm">Stijn-Pieter
409 * van Houten </a>
410 * @version $Revision: 1.1 $ $Date: 2005/06/16 12:34:08 $
411 * @since 1.1.1 <br>
412 */
413 private static class Ells implements Serializable
414 {
415 /*** infv */
416 private double invf = Double.NaN;
417
418 /*** a */
419 private double a = Double.NaN;
420
421 /*** the name */
422 private String name = null;
423
424 /*** value 1 */
425 private double value1 = Double.NaN;
426
427 /*** value 2 */
428 private double value2 = Double.NaN;
429
430 /***
431 * constructs a new Ells
432 *
433 * @param name the name
434 * @param value1 value 1
435 * @param value2 value 2
436 */
437 public Ells(final String name, final double value1, final double value2)
438 {
439 super();
440 this.name = name;
441 this.value1 = value1;
442 this.value2 = value2;
443 }
444
445 /***
446 * @return returns the name
447 */
448 public String getName()
449 {
450 return this.name;
451 }
452
453 /***
454 * @return returns value1
455 */
456 public double getValue1()
457 {
458 return this.value1;
459 }
460
461 /***
462 * @return returns value2
463 */
464 public double getValue2()
465 {
466 return this.value2;
467 }
468 }
469 }