1 |
/* |
2 |
* RESTRICTED RIGHTS LEGEND |
3 |
* |
4 |
* BBNT Solutions LLC |
5 |
* A Verizon Company |
6 |
* 10 Moulton Street |
7 |
* Cambridge, MA 02138 |
8 |
* (617) 873-3000 |
9 |
* |
10 |
* Copyright BBNT Solutions LLC 2001, 2002 All Rights Reserved |
11 |
* |
12 |
*/ |
13 |
|
14 |
package dk.thoerup.traininfoservice.geo; |
15 |
|
16 |
import java.util.ArrayList; |
17 |
import java.util.Enumeration; |
18 |
|
19 |
|
20 |
|
21 |
/** |
22 |
* A class that represents a point on the Earth as a three dimensional unit |
23 |
* length vector, rather than latitude and longitude. For the theory and an |
24 |
* efficient implementation using partial evaluation see: |
25 |
* http://openmap.bbn.com/~kanderso/lisp/performing-lisp/essence.ps |
26 |
* |
27 |
* This implementation matches the theory carefully, but does not use partial |
28 |
* evaluation. |
29 |
* |
30 |
* <p> |
31 |
* For the area calculation see: http://math.rice.edu/~pcmi/sphere/ |
32 |
* |
33 |
* @author Ken Anderson |
34 |
* @author Sachin Date |
35 |
* @author Ben Lubin |
36 |
* @author Michael Thome |
37 |
* @version $Revision: 1.4.2.15 $ on $Date: 2007/02/13 20:00:54 $ |
38 |
*/ |
39 |
public class Geo { |
40 |
|
41 |
/*************************************************************************** |
42 |
* Constants for the shape of the earth. see |
43 |
* http://www.gfy.ku.dk/%7Eiag/HB2000/part4/groten.htm |
44 |
**************************************************************************/ |
45 |
// Replaced by Length constants. |
46 |
// public static final double radiusKM = 6378.13662; // in KM. |
47 |
// public static final double radiusNM = 3443.9182; // in NM. |
48 |
// Replaced with WGS 84 constants |
49 |
// public static final double flattening = 1.0/298.25642; |
50 |
public static final double flattening = 1.0 / 298.257223563; |
51 |
public static final double FLATTENING_C = (1.0 - flattening) |
52 |
* (1.0 - flattening); |
53 |
|
54 |
public static final double METERS_PER_NM = 1852; |
55 |
private static final double NPD_LTERM1 = 111412.84 / METERS_PER_NM; |
56 |
private static final double NPD_LTERM2 = -93.5 / METERS_PER_NM; |
57 |
private static final double NPD_LTERM3 = 0.118 / METERS_PER_NM; |
58 |
|
59 |
private double x; |
60 |
private double y; |
61 |
private double z; |
62 |
|
63 |
/** |
64 |
* Compute nautical miles per degree at a specified latitude (in degrees). |
65 |
* Calculation from NIMA: http://pollux.nss.nima.mil/calc/degree.html |
66 |
*/ |
67 |
public final static double npdAtLat(double latdeg) { |
68 |
double lat = (latdeg * Math.PI) / 180.0; |
69 |
return (NPD_LTERM1 * Math.cos(lat) + NPD_LTERM2 * Math.cos(3 * lat) + NPD_LTERM3 |
70 |
* Math.cos(5 * lat)); |
71 |
} |
72 |
|
73 |
/** Convert from geographic to geocentric latitude (radians) */ |
74 |
public static double geocentricLatitude(double geographicLatitude) { |
75 |
return Math.atan((Math.tan(geographicLatitude) * FLATTENING_C)); |
76 |
} |
77 |
|
78 |
/** Convert from geocentric to geographic latitude (radians) */ |
79 |
public static double geographicLatitude(double geocentricLatitude) { |
80 |
return Math.atan(Math.tan(geocentricLatitude) / FLATTENING_C); |
81 |
} |
82 |
|
83 |
/** Convert from degrees to radians. */ |
84 |
public static double radians(double degrees) { |
85 |
return Length.DECIMAL_DEGREE.toRadians(degrees); |
86 |
} |
87 |
|
88 |
/** Convert from radians to degrees. */ |
89 |
public static double degrees(double radians) { |
90 |
return Length.DECIMAL_DEGREE.fromRadians(radians); |
91 |
} |
92 |
|
93 |
/** Convert radians to kilometers. * */ |
94 |
public static double km(double radians) { |
95 |
return Length.KM.fromRadians(radians); |
96 |
} |
97 |
|
98 |
/** Convert kilometers to radians. * */ |
99 |
public static double kmToAngle(double km) { |
100 |
return Length.KM.toRadians(km); |
101 |
} |
102 |
|
103 |
/** Convert radians to nauticalMiles. * */ |
104 |
public static double nm(double radians) { |
105 |
return Length.NM.fromRadians(radians); |
106 |
} |
107 |
|
108 |
/** Convert nautical miles to radians. * */ |
109 |
public static double nmToAngle(double nm) { |
110 |
return Length.NM.toRadians(nm); |
111 |
} |
112 |
|
113 |
public Geo() {} |
114 |
|
115 |
/** |
116 |
* Construct a Geo from its latitude and longitude. |
117 |
* |
118 |
* @param lat latitude in decimal degrees. |
119 |
* @param lon longitude in decimal degrees. |
120 |
*/ |
121 |
public Geo(double lat, double lon) { |
122 |
initialize(lat, lon); |
123 |
} |
124 |
|
125 |
/** |
126 |
* Construct a Geo from its latitude and longitude. |
127 |
* |
128 |
* @param lat latitude. |
129 |
* @param lon longitude. |
130 |
* @param isDegrees should be true if the lat/lon are specified in decimal |
131 |
* degrees, false if they are radians. |
132 |
*/ |
133 |
public Geo(double lat, double lon, boolean isDegrees) { |
134 |
if (isDegrees) { |
135 |
initialize(lat, lon); |
136 |
} else { |
137 |
initializeRadians(lat, lon); |
138 |
} |
139 |
} |
140 |
|
141 |
/** Construct a Geo from its parts. */ |
142 |
public Geo(double x, double y, double z) { |
143 |
this.x = x; |
144 |
this.y = y; |
145 |
this.z = z; |
146 |
} |
147 |
|
148 |
/** Construct a Geo from another Geo. */ |
149 |
public Geo(Geo geo) { |
150 |
this(geo.x, geo.y, geo.z); |
151 |
} |
152 |
|
153 |
public static final Geo makeGeoRadians(double latr, double lonr) { |
154 |
double rlat = geocentricLatitude(latr); |
155 |
double c = Math.cos(rlat); |
156 |
return new Geo(c * Math.cos(lonr), c * Math.sin(lonr), Math.sin(rlat)); |
157 |
} |
158 |
|
159 |
public static final Geo makeGeoDegrees(double latd, double lond) { |
160 |
return makeGeoRadians(radians(latd), radians(lond)); |
161 |
} |
162 |
|
163 |
public static final Geo makeGeo(double x, double y, double z) { |
164 |
return new Geo(x, y, z); |
165 |
} |
166 |
|
167 |
public static final Geo makeGeo(Geo p) { |
168 |
return new Geo(p.x, p.y, p.z); |
169 |
} |
170 |
|
171 |
/** |
172 |
* Initialize this Geo to match another. |
173 |
* |
174 |
* @param g |
175 |
*/ |
176 |
public void initialize(Geo g) { |
177 |
x = g.x; |
178 |
y = g.y; |
179 |
z = g.z; |
180 |
} |
181 |
|
182 |
/** |
183 |
* Initialize this Geo with new parameters. |
184 |
* |
185 |
* @param x |
186 |
* @param y |
187 |
* @param z |
188 |
*/ |
189 |
public void initialize(double x, double y, double z) { |
190 |
this.x = x; |
191 |
this.y = y; |
192 |
this.z = z; |
193 |
} |
194 |
|
195 |
/** |
196 |
* Initialize this Geo with to represent coordinates. |
197 |
* |
198 |
* @param lat latitude in decimal degrees. |
199 |
* @param lon longitude in decimal degrees. |
200 |
*/ |
201 |
public void initialize(double lat, double lon) { |
202 |
initializeRadians(radians(lat), radians(lon)); |
203 |
} |
204 |
|
205 |
/** |
206 |
* Initialize this Geo with to represent coordinates. |
207 |
* |
208 |
* @param lat latitude in radians. |
209 |
* @param lon longitude in radians. |
210 |
*/ |
211 |
public void initializeRadians(double lat, double lon) { |
212 |
double rlat = geocentricLatitude(lat); |
213 |
double c = Math.cos(rlat); |
214 |
x = c * Math.cos(lon); |
215 |
y = c * Math.sin(lon); |
216 |
z = Math.sin(rlat); |
217 |
} |
218 |
|
219 |
/** |
220 |
* Find the midpoint Geo between this one and another on a Great Circle line |
221 |
* between the two. The result is undefined of the two points are antipodes. |
222 |
* |
223 |
* @param g2 |
224 |
* @return midpoint Geo. |
225 |
*/ |
226 |
public Geo midPoint(Geo g2) { |
227 |
return add(g2).normalize(); |
228 |
} |
229 |
|
230 |
/** |
231 |
* Find the midpoint Geo between this one and another on a Great Circle line |
232 |
* between the two. The result is undefined of the two points are antipodes. |
233 |
* |
234 |
* @param g2 |
235 |
* @param ret a Geo value to set returned values in. Do not pass in a null |
236 |
* value. |
237 |
* @return midpoint Geo. |
238 |
*/ |
239 |
public Geo midPoint(Geo g2, Geo ret) { |
240 |
return add(g2).normalize(ret); |
241 |
} |
242 |
|
243 |
public Geo interpolate(Geo g2, double x) { |
244 |
return scale(x).add(g2.scale(1 - x)).normalize(); |
245 |
} |
246 |
|
247 |
/** |
248 |
* |
249 |
* @param g2 |
250 |
* @param x |
251 |
* @param ret Do not pass in a null value. |
252 |
* @return |
253 |
*/ |
254 |
public Geo interpolate(Geo g2, double x, Geo ret) { |
255 |
return scale(x).add(g2.scale(1 - x, ret), ret).normalize(ret); |
256 |
} |
257 |
|
258 |
public String toString() { |
259 |
return "Geo[" + getLatitude() + "," + getLongitude() + "]"; |
260 |
} |
261 |
|
262 |
public double getLatitude() { |
263 |
return degrees(geographicLatitude(Math.atan2(z, |
264 |
Math.sqrt(x * x + y * y)))); |
265 |
} |
266 |
|
267 |
public double getLongitude() { |
268 |
return degrees(Math.atan2(y, x)); |
269 |
} |
270 |
|
271 |
public double getLatitudeRadians() { |
272 |
return geographicLatitude(Math.atan2(z, Math.sqrt(x * x + y * y))); |
273 |
} |
274 |
|
275 |
public double getLongitudeRadians() { |
276 |
return Math.atan2(y, x); |
277 |
} |
278 |
|
279 |
/** |
280 |
* Reader for x, in internal axis representation (positive to the right side |
281 |
* of screen). |
282 |
* |
283 |
* @return |
284 |
*/ |
285 |
public final double x() { |
286 |
return this.x; |
287 |
} |
288 |
|
289 |
/** |
290 |
* Reader for y in internal axis reprensentation (positive into screen). |
291 |
* |
292 |
* @return |
293 |
*/ |
294 |
public final double y() { |
295 |
return this.y; |
296 |
} |
297 |
|
298 |
/** |
299 |
* Reader for z in internal axis representation (positive going to top of |
300 |
* screen). |
301 |
* |
302 |
* @return |
303 |
*/ |
304 |
public final double z() { |
305 |
return this.z; |
306 |
} |
307 |
|
308 |
public void setLength(double r) { |
309 |
// It's tempting to call getLatitudeRadians() here, but it changes the |
310 |
// angle. I think we want to keep the angles the same, and just extend |
311 |
// x, y, z, and then let the latitudes get refigured out for the |
312 |
// ellipsoid when they are asked for. |
313 |
double rlat = Math.atan2(z, Math.sqrt(x * x + y * y)); |
314 |
double rlon = getLongitudeRadians(); |
315 |
|
316 |
double c = r * Math.cos(rlat); |
317 |
x = c * Math.cos(rlon); |
318 |
y = c * Math.sin(rlon); |
319 |
z = r * Math.sin(rlat); |
320 |
} |
321 |
|
322 |
/** North pole. */ |
323 |
public static final Geo north = new Geo(0.0, 0.0, 1.0); |
324 |
|
325 |
/** Dot product. */ |
326 |
public double dot(Geo b) { |
327 |
return (this.x() * b.x() + this.y() * b.y() + this.z() * b.z()); |
328 |
} |
329 |
|
330 |
/** Dot product. */ |
331 |
public static double dot(Geo a, Geo b) { |
332 |
return (a.x() * b.x() + a.y() * b.y() + a.z() * b.z()); |
333 |
} |
334 |
|
335 |
/** Euclidian length. */ |
336 |
public double length() { |
337 |
return Math.sqrt(this.dot(this)); |
338 |
} |
339 |
|
340 |
/** Multiply this by s. * */ |
341 |
public Geo scale(double s) { |
342 |
return scale(s, new Geo()); |
343 |
} |
344 |
|
345 |
/** |
346 |
* Multiply this by s. |
347 |
* |
348 |
* @return ret that was passed in, filled in with scaled values. Do not pass |
349 |
* in a null value. |
350 |
*/ |
351 |
public Geo scale(double s, Geo ret) { |
352 |
ret.initialize(this.x() * s, this.y() * s, this.z() * s); |
353 |
return ret; |
354 |
} |
355 |
|
356 |
/** Returns a unit length vector parallel to this. */ |
357 |
public Geo normalize() { |
358 |
return this.scale(1.0 / this.length()); |
359 |
} |
360 |
|
361 |
/** |
362 |
* Returns a unit length vector parallel to this. |
363 |
* |
364 |
* @return ret with normalized values. Do not pass in a null value. |
365 |
*/ |
366 |
public Geo normalize(Geo ret) { |
367 |
return this.scale(1.0 / this.length(), ret); |
368 |
} |
369 |
|
370 |
/** Vector cross product. */ |
371 |
public Geo cross(Geo b) { |
372 |
return cross(b, new Geo()); |
373 |
} |
374 |
|
375 |
/** |
376 |
* Vector cross product. |
377 |
* |
378 |
* @return ret Do not pass in a null value. |
379 |
*/ |
380 |
public Geo cross(Geo b, Geo ret) { |
381 |
ret.initialize(this.y() * b.z() - this.z() * b.y(), this.z() * b.x() |
382 |
- this.x() * b.z(), this.x() * b.y() - this.y() * b.x()); |
383 |
return ret; |
384 |
} |
385 |
|
386 |
/** Eqvivalent to this.cross(b).length(). */ |
387 |
public double crossLength(Geo b) { |
388 |
double x = this.y() * b.z() - this.z() * b.y(); |
389 |
double y = this.z() * b.x() - this.x() * b.z(); |
390 |
double z = this.x() * b.y() - this.y() * b.x(); |
391 |
return Math.sqrt(x * x + y * y + z * z); |
392 |
} |
393 |
|
394 |
/** Eqvivalent to <code>this.cross(b).normalize()</code>. */ |
395 |
public Geo crossNormalize(Geo b) { |
396 |
return crossNormalize(b, new Geo()); |
397 |
} |
398 |
|
399 |
/** |
400 |
* Eqvivalent to <code>this.cross(b).normalize()</code>. |
401 |
* |
402 |
* @return ret Do not pass in a null value. |
403 |
*/ |
404 |
public Geo crossNormalize(Geo b, Geo ret) { |
405 |
double x = this.y() * b.z() - this.z() * b.y(); |
406 |
double y = this.z() * b.x() - this.x() * b.z(); |
407 |
double z = this.x() * b.y() - this.y() * b.x(); |
408 |
double L = Math.sqrt(x * x + y * y + z * z); |
409 |
|
410 |
ret.initialize(x / L, y / L, z / L); |
411 |
return ret; |
412 |
} |
413 |
|
414 |
/** |
415 |
* Eqvivalent to <code>this.cross(b).normalize()</code>. |
416 |
* |
417 |
* @return ret Do not pass in a null value. |
418 |
*/ |
419 |
public static Geo crossNormalize(Geo a, Geo b, Geo ret) { |
420 |
return a.crossNormalize(b, ret); |
421 |
} |
422 |
|
423 |
/** Returns this + b. */ |
424 |
public Geo add(Geo b) { |
425 |
return add(b, new Geo()); |
426 |
} |
427 |
|
428 |
/* |
429 |
* @return ret Do not pass in a null value. |
430 |
*/ |
431 |
public Geo add(Geo b, Geo ret) { |
432 |
ret.initialize(this.x() + b.x(), this.y() + b.y(), this.z() + b.z()); |
433 |
return ret; |
434 |
} |
435 |
|
436 |
/** Returns this - b. */ |
437 |
public Geo subtract(Geo b) { |
438 |
return subtract(b, new Geo()); |
439 |
} |
440 |
|
441 |
/** |
442 |
* Returns this - b. * |
443 |
* |
444 |
* @return ret Do not pass in a null value. |
445 |
*/ |
446 |
public Geo subtract(Geo b, Geo ret) { |
447 |
ret.initialize(this.x() - b.x(), this.y() - b.y(), this.z() - b.z()); |
448 |
return ret; |
449 |
} |
450 |
|
451 |
public boolean equals(Geo v2) { |
452 |
return this.x == v2.x && this.y == v2.y && this.z == v2.z; |
453 |
} |
454 |
|
455 |
/** Angular distance, in radians between this and v2. */ |
456 |
public double distance(Geo v2) { |
457 |
return Math.atan2(v2.crossLength(this), v2.dot(this)); |
458 |
} |
459 |
|
460 |
/** Angular distance, in radians between v1 and v2. */ |
461 |
public static double distance(Geo v1, Geo v2) { |
462 |
return v1.distance(v2); |
463 |
} |
464 |
|
465 |
/** Angular distance, in radians between the two lat lon points. */ |
466 |
public static double distance(double lat1, double lon1, double lat2, |
467 |
double lon2) { |
468 |
return Geo.distance(new Geo(lat1, lon1), new Geo(lat2, lon2)); |
469 |
} |
470 |
|
471 |
/** Distance in kilometers. * */ |
472 |
public double distanceKM(Geo v2) { |
473 |
return km(distance(v2)); |
474 |
} |
475 |
|
476 |
/** Distance in kilometers. * */ |
477 |
public static double distanceKM(Geo v1, Geo v2) { |
478 |
return v1.distanceKM(v2); |
479 |
} |
480 |
|
481 |
/** Distance in kilometers. * */ |
482 |
public static double distanceKM(double lat1, double lon1, double lat2, |
483 |
double lon2) { |
484 |
return Geo.distanceKM(new Geo(lat1, lon1), new Geo(lat2, lon2)); |
485 |
} |
486 |
|
487 |
/** Distance in nautical miles. * */ |
488 |
public double distanceNM(Geo v2) { |
489 |
return nm(distance(v2)); |
490 |
} |
491 |
|
492 |
/** Distance in nautical miles. * */ |
493 |
public static double distanceNM(Geo v1, Geo v2) { |
494 |
return v1.distanceNM(v2); |
495 |
} |
496 |
|
497 |
/** Distance in nautical miles. * */ |
498 |
public static double distanceNM(double lat1, double lon1, double lat2, |
499 |
double lon2) { |
500 |
return Geo.distanceNM(new Geo(lat1, lon1), new Geo(lat2, lon2)); |
501 |
} |
502 |
|
503 |
/** Azimuth in radians from this to v2. */ |
504 |
public double azimuth(Geo v2) { |
505 |
/* |
506 |
* n1 is the great circle representing the meridian of this. n2 is the |
507 |
* great circle between this and v2. The azimuth is the angle between |
508 |
* them but we specialized the cross product. |
509 |
*/ |
510 |
// Geo n1 = north.cross(this); |
511 |
// Geo n2 = v2.cross(this); |
512 |
// crossNormalization is needed to geos of different length. |
513 |
Geo n1 = north.crossNormalize(this); |
514 |
Geo n2 = v2.crossNormalize(this); |
515 |
double az = Math.atan2(-north.dot(n2), n1.dot(n2)); |
516 |
return (az > 0.0) ? az : 2.0 * Math.PI + az; |
517 |
} |
518 |
|
519 |
/** |
520 |
* Given 3 points on a sphere, p0, p1, p2, return the angle between them in |
521 |
* radians. |
522 |
*/ |
523 |
public static double angle(Geo p0, Geo p1, Geo p2) { |
524 |
return Math.PI - p0.cross(p1).distance(p1.cross(p2)); |
525 |
} |
526 |
|
527 |
/** |
528 |
* Computes the area of a polygon on the surface of a unit sphere given an |
529 |
* enumeration of its point.. For a non unit sphere, multiply this by the |
530 |
* radius of sphere squared. |
531 |
*/ |
532 |
public static double area(Enumeration vs) { |
533 |
int count = 0; |
534 |
double area = 0; |
535 |
Geo v0 = (Geo) vs.nextElement(); |
536 |
Geo v1 = (Geo) vs.nextElement(); |
537 |
Geo p0 = v0; |
538 |
Geo p1 = v1; |
539 |
Geo p2 = null; |
540 |
while (vs.hasMoreElements()) { |
541 |
count = count + 1; |
542 |
p2 = (Geo) vs.nextElement(); |
543 |
area = area + angle(p0, p1, p2); |
544 |
p0 = p1; |
545 |
p1 = p2; |
546 |
} |
547 |
|
548 |
count = count + 1; |
549 |
p2 = v0; |
550 |
area = area + angle(p0, p1, p2); |
551 |
p0 = p1; |
552 |
p1 = p2; |
553 |
|
554 |
count = count + 1; |
555 |
p2 = v1; |
556 |
area = area + angle(p0, p1, p2); |
557 |
|
558 |
return area - (count - 2) * Math.PI; |
559 |
} |
560 |
|
561 |
/** |
562 |
* Is the point, p, within radius radians of the great circle segment |
563 |
* between this and v2? |
564 |
*/ |
565 |
public boolean isInside(Geo v2, double radius, Geo p) { |
566 |
// Allocate a Geo to be reused for all of these calculations, instead of |
567 |
// creating 3 of them that are just thrown away. There's one more we |
568 |
// still need to allocate, for dp below. |
569 |
Geo tmp = new Geo(); |
570 |
|
571 |
/* |
572 |
* gc is a unit vector perpendicular to the plane defined by v1 and v2 |
573 |
*/ |
574 |
Geo gc = this.crossNormalize(v2, tmp); |
575 |
|
576 |
/* |
577 |
* |gc . p| is the size of the projection of p onto gc (the normal of |
578 |
* v1,v2) cos(pi/2-r) is effectively the size of the projection of a |
579 |
* vector along gc of the radius length. If the former is larger than |
580 |
* the latter, than p is further than radius from arc, so must not be |
581 |
* isInside |
582 |
*/ |
583 |
if (Math.abs(gc.dot(p)) > Math.cos((Math.PI / 2.0) - radius)) |
584 |
return false; |
585 |
|
586 |
/* |
587 |
* If p is within radius of either endpoint, then we know it isInside |
588 |
*/ |
589 |
if (this.distance(p) <= radius || v2.distance(p) <= radius) |
590 |
return true; |
591 |
|
592 |
/* d is the vector from the v2 to v1 */ |
593 |
Geo d = v2.subtract(this, tmp); |
594 |
|
595 |
/* L is the length of the vector d */ |
596 |
double L = d.length(); |
597 |
|
598 |
/* n is the d normalized to length=1 */ |
599 |
Geo n = d.normalize(tmp); |
600 |
|
601 |
/* dp is the vector from p to v1 */ |
602 |
Geo dp = p.subtract(this, new Geo()); |
603 |
|
604 |
/* size is the size of the projection of dp onto n */ |
605 |
double size = n.dot(dp); |
606 |
|
607 |
/* p is inside iff size>=0 and size <= L */ |
608 |
return (0 <= size && size <= L); |
609 |
} |
610 |
|
611 |
/** |
612 |
* do the segments v1-v2 and p1-p2 come within radius (radians) of each |
613 |
* other? |
614 |
*/ |
615 |
public static boolean isInside(Geo v1, Geo v2, double radius, Geo p1, Geo p2) { |
616 |
return v1.isInside(v2, radius, p1) || v1.isInside(v2, radius, p2) |
617 |
|| p1.isInside(p2, radius, v1) || p1.isInside(p2, radius, v2); |
618 |
} |
619 |
|
620 |
/** |
621 |
* Static version of isInside uses conventional (decimal degree) |
622 |
* coordinates. |
623 |
*/ |
624 |
public static boolean isInside(double lat1, double lon1, double lat2, |
625 |
double lon2, double radius, double lat3, |
626 |
double lon3) { |
627 |
return (new Geo(lat1, lon1)).isInside(new Geo(lat2, lon2), |
628 |
radius, |
629 |
new Geo(lat3, lon3)); |
630 |
} |
631 |
|
632 |
/** |
633 |
* Is Geo p inside the time bubble along the great circle segment from this |
634 |
* to v2 looking forward forwardRadius and backward backwardRadius. |
635 |
*/ |
636 |
public boolean inBubble(Geo v2, double forwardRadius, double backRadius, |
637 |
Geo p) { |
638 |
return distance(p) <= ((v2.subtract(this) |
639 |
.normalize() |
640 |
.dot(p.subtract(this)) > 0.0) ? forwardRadius : backRadius); |
641 |
} |
642 |
|
643 |
/** Returns the point opposite this point on the earth. */ |
644 |
public Geo antipode() { |
645 |
return this.scale(-1.0, new Geo()); |
646 |
} |
647 |
|
648 |
/** |
649 |
* Returns the point opposite this point on the earth. * |
650 |
* |
651 |
* @return ret Do not pass in a null value. |
652 |
*/ |
653 |
public Geo antipode(Geo ret) { |
654 |
return this.scale(-1.0, ret); |
655 |
} |
656 |
|
657 |
/** |
658 |
* Find the intersection of the great circle between this and q and the |
659 |
* great circle normal to r. |
660 |
* <p> |
661 |
* |
662 |
* That is, find the point, y, lying between this and q such that |
663 |
* |
664 |
* <pre> |
665 |
* |
666 |
* y = [x*this + (1-x)*q]*c |
667 |
* where c = 1/y.dot(y) is a factor for normalizing y. |
668 |
* y.dot(r) = 0 |
669 |
* substituting: |
670 |
* [x*this + (1-x)*q]*c.dot(r) = 0 or |
671 |
* [x*this + (1-x)*q].dot(r) = 0 |
672 |
* x*this.dot(r) + (1-x)*q.dot(r) = 0 |
673 |
* x*a + (1-x)*b = 0 |
674 |
* x = -b/(a - b) |
675 |
* |
676 |
* </pre> |
677 |
* |
678 |
* We assume that this and q are less than 180 degrees appart. When this and |
679 |
* q are 180 degrees appart, the point -y is also a valid intersection. |
680 |
* <p> |
681 |
* Alternatively the intersection point, y, satisfies y.dot(r) = 0 |
682 |
* y.dot(this.crossNormalize(q)) = 0 which is satisfied by y = |
683 |
* r.crossNormalize(this.crossNormalize(q)); |
684 |
* |
685 |
*/ |
686 |
public Geo intersect(Geo q, Geo r) { |
687 |
return intersect(q, r, new Geo()); |
688 |
} |
689 |
|
690 |
/** |
691 |
* Find the intersection of the great circle between this and q and the |
692 |
* great circle normal to r. |
693 |
* <p> |
694 |
* |
695 |
* That is, find the point, y, lying between this and q such that |
696 |
* |
697 |
* <pre> |
698 |
* |
699 |
* y = [x*this + (1-x)*q]*c |
700 |
* where c = 1/y.dot(y) is a factor for normalizing y. |
701 |
* y.dot(r) = 0 |
702 |
* substituting: |
703 |
* [x*this + (1-x)*q]*c.dot(r) = 0 or |
704 |
* [x*this + (1-x)*q].dot(r) = 0 |
705 |
* x*this.dot(r) + (1-x)*q.dot(r) = 0 |
706 |
* x*a + (1-x)*b = 0 |
707 |
* x = -b/(a - b) |
708 |
* |
709 |
* </pre> |
710 |
* |
711 |
* We assume that this and q are less than 180 degrees appart. When this and |
712 |
* q are 180 degrees appart, the point -y is also a valid intersection. |
713 |
* <p> |
714 |
* Alternatively the intersection point, y, satisfies y.dot(r) = 0 |
715 |
* y.dot(this.crossNormalize(q)) = 0 which is satisfied by y = |
716 |
* r.crossNormalize(this.crossNormalize(q)); |
717 |
* |
718 |
* @return ret Do not pass in a null value. |
719 |
*/ |
720 |
public Geo intersect(Geo q, Geo r, Geo ret) { |
721 |
double a = this.dot(r); |
722 |
double b = q.dot(r); |
723 |
double x = -b / (a - b); |
724 |
// This still results in one Geo being allocated and lost, in the |
725 |
// q.scale call. |
726 |
return this.scale(x, ret).add(q.scale(1.0 - x), ret).normalize(ret); |
727 |
} |
728 |
|
729 |
/** alias for computeCorridor(path, radius, radians(10), true) * */ |
730 |
/* |
731 |
public static Geo[] computeCorridor(Geo[] path, double radius) { |
732 |
return computeCorridor(path, radius, radians(10.0), true); |
733 |
}*/ |
734 |
|
735 |
/** |
736 |
* Wrap a fixed-distance corridor around an (open) path, as specified by an |
737 |
* array of Geo. |
738 |
* |
739 |
* @param path Open path, must not have repeated points or consecutive |
740 |
* antipodes. |
741 |
* @param radius Distance from path to widen corridor, in angular radians. |
742 |
* @param err maximum angle of rounded edges, in radians. If 0, will |
743 |
* directly cut outside bends. |
744 |
* @param capp iff true, will round end caps |
745 |
* @return a closed polygon representing the specified corridor around the |
746 |
* path. |
747 |
* |
748 |
*/ |
749 |
/* |
750 |
public static Geo[] computeCorridor(Geo[] path, double radius, double err, |
751 |
boolean capp) { |
752 |
if (path == null || radius <= 0.0) { |
753 |
return new Geo[] {}; |
754 |
} |
755 |
// assert path!=null; |
756 |
// assert radius > 0.0; |
757 |
|
758 |
int pl = path.length; |
759 |
if (pl < 2) |
760 |
return null; |
761 |
|
762 |
// final polygon will be right[0],...,right[n],left[m],...,left[0] |
763 |
ArrayList right = new ArrayList((int) (pl * 1.5)); |
764 |
ArrayList left = new ArrayList((int) (pl * 1.5)); |
765 |
|
766 |
Geo g0 = null; // previous point |
767 |
Geo n0 = null; // previous normal vector |
768 |
Geo l0 = null; |
769 |
Geo r0 = null; |
770 |
|
771 |
Geo g1 = path[0]; // current point |
772 |
|
773 |
for (int i = 1; i < pl; i++) { |
774 |
Geo g2 = path[i]; // next point |
775 |
Geo n1 = g1.crossNormalize(g2); // n is perpendicular to the vector |
776 |
// from g1 to g2 |
777 |
n1 = n1.scale(radius); // normalize to radius |
778 |
// these are the offsets on the g2 side at g1 |
779 |
Geo r1b = g1.add(n1); |
780 |
Geo l1b = g1.subtract(n1); |
781 |
|
782 |
if (n0 == null) { |
783 |
if (capp && err > 0) { |
784 |
// start cap |
785 |
Geo[] arc = approximateArc(g1, l1b, r1b, err); |
786 |
for (int j = arc.length - 1; j >= 0; j--) { |
787 |
right.add(arc[j]); |
788 |
} |
789 |
} else { |
790 |
// no previous point - we'll just be square |
791 |
right.add(l1b); |
792 |
left.add(r1b); |
793 |
} |
794 |
// advance normals |
795 |
l0 = l1b; |
796 |
r0 = r1b; |
797 |
} else { |
798 |
// otherwise, compute a more complex shape |
799 |
|
800 |
// these are the right and left on the g0 side of g1 |
801 |
Geo r1a = g1.add(n0); |
802 |
Geo l1a = g1.subtract(n0); |
803 |
|
804 |
double handed = g0.cross(g1).dot(g2); // right or left handed |
805 |
// divergence |
806 |
if (handed > 0) { // left needs two points, right needs 1 |
807 |
if (err > 0) { |
808 |
Geo[] arc = approximateArc(g1, l1b, l1a, err); |
809 |
for (int j = arc.length - 1; j >= 0; j--) { |
810 |
right.add(arc[j]); |
811 |
} |
812 |
} else { |
813 |
right.add(l1a); |
814 |
right.add(l1b); |
815 |
} |
816 |
l0 = l1b; |
817 |
|
818 |
Geo ip = Intersection.segmentsIntersect(r0, |
819 |
r1a, |
820 |
r1b, |
821 |
g2.add(n1)); |
822 |
// if they intersect, take the intersection, else use the |
823 |
// points and punt |
824 |
if (ip != null) { |
825 |
left.add(ip); |
826 |
} else { |
827 |
left.add(r1a); |
828 |
left.add(r1b); |
829 |
} |
830 |
r0 = ip; |
831 |
} else { |
832 |
Geo ip = Intersection.segmentsIntersect(l0, |
833 |
l1a, |
834 |
l1b, |
835 |
g2.subtract(n1)); |
836 |
// if they intersect, take the intersection, else use the |
837 |
// points and punt |
838 |
if (ip != null) { |
839 |
right.add(ip); |
840 |
} else { |
841 |
right.add(l1a); |
842 |
right.add(l1b); |
843 |
} |
844 |
l0 = ip; |
845 |
if (err > 0) { |
846 |
Geo[] arc = approximateArc(g1, r1a, r1b, err); |
847 |
for (int j = 0; j < arc.length; j++) { |
848 |
left.add(arc[j]); |
849 |
} |
850 |
} else { |
851 |
left.add(r1a); |
852 |
left.add(r1b); |
853 |
} |
854 |
r0 = r1b; |
855 |
} |
856 |
} |
857 |
|
858 |
// advance points |
859 |
g0 = g1; |
860 |
n0 = n1; |
861 |
g1 = g2; |
862 |
} |
863 |
|
864 |
// finish it off |
865 |
Geo rn = g1.subtract(n0); |
866 |
Geo ln = g1.add(n0); |
867 |
if (capp && err > 0) { |
868 |
// end cap |
869 |
Geo[] arc = approximateArc(g1, ln, rn, err); |
870 |
for (int j = arc.length - 1; j >= 0; j--) { |
871 |
right.add(arc[j]); |
872 |
} |
873 |
} else { |
874 |
right.add(rn); |
875 |
left.add(ln); |
876 |
} |
877 |
|
878 |
int ll = right.size(); |
879 |
int rl = left.size(); |
880 |
Geo[] result = new Geo[ll + rl]; |
881 |
for (int i = 0; i < ll; i++) { |
882 |
result[i] = (Geo) right.get(i); |
883 |
} |
884 |
int j = ll; |
885 |
for (int i = rl - 1; i >= 0; i--) { |
886 |
result[j++] = (Geo) left.get(i); |
887 |
} |
888 |
return result; |
889 |
}*/ |
890 |
|
891 |
/** simple vector angle (not geocentric!) */ |
892 |
static double simpleAngle(Geo p1, Geo p2) { |
893 |
return Math.acos(p1.dot(p2) / (p1.length() * p2.length())); |
894 |
} |
895 |
|
896 |
/** |
897 |
* compute a polygonal approximation of an arc centered at pc, beginning at |
898 |
* p0 and ending at p1, going clockwise and including the two end points. |
899 |
* |
900 |
* @param pc center point |
901 |
* @param p0 starting point |
902 |
* @param p1 ending point |
903 |
* @param err The maximum angle between approximates allowed, in radians. |
904 |
* Smaller values will look better but will result in more returned |
905 |
* points. |
906 |
* @return |
907 |
*/ |
908 |
/* |
909 |
public static final Geo[] approximateArc(Geo pc, Geo p0, Geo p1, double err) { |
910 |
|
911 |
double theta = angle(p0, pc, p1); |
912 |
// if the rest of the code is undefined in this situation, just skip it. |
913 |
if (Double.isNaN(theta)) { |
914 |
return new Geo[] { p0, p1 }; |
915 |
} |
916 |
|
917 |
int n = (int) (2.0 + Math.abs(theta / err)); // number of points |
918 |
// (counting the end |
919 |
// points) |
920 |
Geo[] result = new Geo[n]; |
921 |
result[0] = p0; |
922 |
double dtheta = theta / (n - 1); |
923 |
|
924 |
double rho = 0.0; // angle starts at 0 (directly at p0) |
925 |
|
926 |
for (int i = 1; i < n - 1; i++) { |
927 |
rho += dtheta; |
928 |
// Rotate p0 around this so it has the right azimuth. |
929 |
result[i] = Rotation.rotate(pc, 2.0 * Math.PI - rho, p0, new Geo()); |
930 |
} |
931 |
result[n - 1] = p1; |
932 |
|
933 |
return result; |
934 |
} |
935 |
|
936 |
public final Geo[] approximateArc(Geo p0, Geo p1, double err) { |
937 |
return approximateArc(this, p0, p1, err); |
938 |
}*/ |
939 |
|
940 |
/** @deprecated use </b>#offset(double, double) */ |
941 |
/* |
942 |
public Geo geoAt(double distance, double azimuth) { |
943 |
return offset(distance, azimuth); |
944 |
}*/ |
945 |
|
946 |
/** |
947 |
* Returns a Geo that is distance (radians), and azimuth (radians) away from |
948 |
* this. |
949 |
* |
950 |
* @param distance distance of this to the target point in radians. |
951 |
* @param azimuth Direction of target point from this, in radians, clockwise |
952 |
* from north. |
953 |
* @note this is undefined at the north pole, at which point "azimuth" is |
954 |
* undefined. |
955 |
*/ |
956 |
/* |
957 |
public Geo offset(double distance, double azimuth) { |
958 |
return offset(distance, azimuth, new Geo()); |
959 |
}*/ |
960 |
|
961 |
/** |
962 |
* Returns a Geo that is distance (radians), and azimuth (radians) away from |
963 |
* this. |
964 |
* |
965 |
* @param distance distance of this to the target point in radians. |
966 |
* @param azimuth Direction of target point from this, in radians, clockwise |
967 |
* from north. |
968 |
* @note this is undefined at the north pole, at which point "azimuth" is |
969 |
* undefined. |
970 |
* @return ret Do not pass in a null value. |
971 |
*/ |
972 |
/* |
973 |
public Geo offset(double distance, double azimuth, Geo ret) { |
974 |
// m is normal the the meridian through this. |
975 |
Geo m = this.crossNormalize(north, ret); |
976 |
// p is a point on the meridian distance <tt>distance</tt> from this. |
977 |
// Geo p = (new Rotation(m, distance)).rotate(this); |
978 |
Geo p = Rotation.rotate(m, distance, this, ret); |
979 |
// Rotate p around this so it has the right azimuth. |
980 |
return Rotation.rotate(this, 2.0 * Math.PI - azimuth, p, ret); |
981 |
} |
982 |
|
983 |
public static Geo offset(Geo origin, double distance, double azimuth) { |
984 |
return origin.offset(distance, azimuth); |
985 |
}*/ |
986 |
|
987 |
/** |
988 |
* |
989 |
* @param origin |
990 |
* @param distance |
991 |
* @param azimuth |
992 |
* @param ret |
993 |
* @return ret Do not pass in a null value. |
994 |
*/ |
995 |
/* |
996 |
public static Geo offset(Geo origin, double distance, double azimuth, |
997 |
Geo ret) { |
998 |
return origin.offset(distance, azimuth, ret); |
999 |
}*/ |
1000 |
|
1001 |
/* |
1002 |
* //same as offset, except using trig instead of vector mathematics public |
1003 |
* Geo trig_offset(double distance, double azimuth) { double latr = |
1004 |
* getLatitudeRadians(); double lonr = getLongitudeRadians(); |
1005 |
* |
1006 |
* double coslat = Math.cos(latr); double sinlat = Math.sin(latr); double |
1007 |
* cosaz = Math.cos(azimuth); double sinaz = Math.sin(azimuth); double sind = |
1008 |
* Math.sin(distance); double cosd = Math.cos(distance); |
1009 |
* |
1010 |
* return makeGeoRadians(Math.asin(sinlat * cosd + coslat * sind * cosaz), |
1011 |
* Math.atan2(sind * sinaz, coslat * cosd - sinlat * sind * cosaz) + lonr); } |
1012 |
*/ |
1013 |
|
1014 |
// |
1015 |
// Follows are a series of Geo array operations as useful utilities |
1016 |
// |
1017 |
/** |
1018 |
* convert a String containing space-separated pairs of comma-separated |
1019 |
* decimal lat-lon pairs into a Geo array. |
1020 |
*/ |
1021 |
public static Geo[] posToGa(String coords) { |
1022 |
return posToGa(coords.split(" ")); |
1023 |
} |
1024 |
|
1025 |
/** |
1026 |
* Convert an array of strings with comma-separated decimal lat,lon pairs |
1027 |
* into a Geo array |
1028 |
*/ |
1029 |
public static Geo[] posToGa(String[] coords) { |
1030 |
// convert to floating lat/lon degrees |
1031 |
Geo[] ga = new Geo[coords.length]; |
1032 |
for (int i = 0; i < coords.length; i++) { |
1033 |
String[] ll = coords[i].split(","); |
1034 |
ga[i] = Geo.makeGeoDegrees(Double.parseDouble(ll[0]), |
1035 |
Double.parseDouble(ll[1])); |
1036 |
} |
1037 |
return ga; |
1038 |
} |
1039 |
|
1040 |
/** |
1041 |
* Convert a Geo array into a floating point lat lon array (alternating lat |
1042 |
* and lon values). |
1043 |
* |
1044 |
* @return the ll array provided, or a new array of lla is null. |
1045 |
*/ |
1046 |
public static double[] GaToLLa(Geo[] ga, double[] lla) { |
1047 |
if (lla == null) { |
1048 |
lla = new double[2 * ga.length]; |
1049 |
} |
1050 |
|
1051 |
for (int i = 0; i < ga.length; i++) { |
1052 |
Geo g = ga[i]; |
1053 |
lla[i * 2] = g.getLatitude(); |
1054 |
lla[i * 2 + 1] = g.getLongitude(); |
1055 |
} |
1056 |
return lla; |
1057 |
} |
1058 |
|
1059 |
/** |
1060 |
* Convert a Geo array into a floating point lat lon array (alternating lat |
1061 |
* and lon values). |
1062 |
* |
1063 |
* @return the ll array provided, or a new array of lla is null. |
1064 |
*/ |
1065 |
public static float[] GaToLLa(Geo[] ga, float[] lla) { |
1066 |
if (lla == null) { |
1067 |
lla = new float[2 * ga.length]; |
1068 |
} |
1069 |
|
1070 |
for (int i = 0; i < ga.length; i++) { |
1071 |
Geo g = ga[i]; |
1072 |
lla[i * 2] = (float) g.getLatitude(); |
1073 |
lla[i * 2 + 1] = (float) g.getLongitude(); |
1074 |
} |
1075 |
return lla; |
1076 |
} |
1077 |
|
1078 |
/** |
1079 |
* Convert a Geo array into a floating point lat lon array (alternating lat |
1080 |
* and lon values) |
1081 |
*/ |
1082 |
public static float[] GaToLLa(Geo[] ga) { |
1083 |
return GaToLLa(ga, new float[2 * ga.length]); |
1084 |
} |
1085 |
|
1086 |
/** |
1087 |
* Return a Geo array with the duplicates removed. May arbitrarily mutate |
1088 |
* the input array. |
1089 |
*/ |
1090 |
public static Geo[] removeDups(Geo[] ga) { |
1091 |
Geo[] r = new Geo[ga.length]; |
1092 |
int p = 0; |
1093 |
for (int i = 0; i < ga.length; i++) { |
1094 |
if (p == 0 || !(r[p - 1].equals(ga[i]))) { |
1095 |
r[p] = ga[i]; |
1096 |
p++; |
1097 |
} |
1098 |
} |
1099 |
if (p != ga.length) { |
1100 |
Geo[] x = new Geo[p]; |
1101 |
System.arraycopy(r, 0, x, 0, p); |
1102 |
return x; |
1103 |
} else { |
1104 |
return ga; |
1105 |
} |
1106 |
} |
1107 |
|
1108 |
/** |
1109 |
* Convert a float array of alternating lat and lon pairs into a Geo array. |
1110 |
*/ |
1111 |
public static Geo[] LLaToGa(float[] lla) { |
1112 |
return LLaToGa(lla, true); |
1113 |
} |
1114 |
|
1115 |
/** |
1116 |
* Convert a float array of alternating lat and lon pairs into a Geo array. |
1117 |
*/ |
1118 |
public static Geo[] LLaToGa(float[] lla, boolean isDegrees) { |
1119 |
Geo[] r = new Geo[lla.length / 2]; |
1120 |
for (int i = 0; i < lla.length / 2; i++) { |
1121 |
if (isDegrees) { |
1122 |
r[i] = Geo.makeGeoDegrees(lla[i * 2], lla[i * 2 + 1]); |
1123 |
} else { |
1124 |
r[i] = Geo.makeGeoRadians(lla[i * 2], lla[i * 2 + 1]); |
1125 |
} |
1126 |
} |
1127 |
return r; |
1128 |
} |
1129 |
|
1130 |
/** |
1131 |
* Convert a double array of alternating lat and lon pairs into a Geo array. |
1132 |
*/ |
1133 |
public static Geo[] LLaToGa(double[] lla) { |
1134 |
return LLaToGa(lla, true); |
1135 |
} |
1136 |
|
1137 |
/** |
1138 |
* Convert a double array of alternating lat and lon pairs into a Geo array. |
1139 |
*/ |
1140 |
public static Geo[] LLaToGa(double[] lla, boolean isDegrees) { |
1141 |
Geo[] r = new Geo[lla.length / 2]; |
1142 |
for (int i = 0; i < lla.length / 2; i++) { |
1143 |
if (isDegrees) { |
1144 |
r[i] = Geo.makeGeoDegrees(lla[i * 2], lla[i * 2 + 1]); |
1145 |
} else { |
1146 |
r[i] = Geo.makeGeoRadians(lla[i * 2], lla[i * 2 + 1]); |
1147 |
} |
1148 |
} |
1149 |
return r; |
1150 |
} |
1151 |
|
1152 |
/** |
1153 |
* return a float array of alternating lat lon pairs where the first and |
1154 |
* last pair are the same, thus closing the path, by adding a point if |
1155 |
* needed. Does not mutate the input. |
1156 |
*/ |
1157 |
public static float[] closeLLa(float[] lla) { |
1158 |
int l = lla.length; |
1159 |
int s = (l / 2) - 1; |
1160 |
if (lla[0] == lla[s * 2] && lla[1] == lla[s * 2 + 1]) { |
1161 |
return lla; |
1162 |
} else { |
1163 |
float[] llx = new float[l + 2]; |
1164 |
for (int i = 0; i < l; i++) { |
1165 |
llx[i] = lla[i]; |
1166 |
} |
1167 |
llx[l] = lla[0]; |
1168 |
llx[l + 1] = lla[1]; |
1169 |
return llx; |
1170 |
} |
1171 |
} |
1172 |
|
1173 |
/** |
1174 |
* return a Geo array where the first and last elements are the same, thus |
1175 |
* closing the path, by adding a point if needed. Does not mutate the input. |
1176 |
*/ |
1177 |
public static Geo[] closeGa(Geo[] ga) { |
1178 |
int l = ga.length; |
1179 |
if (ga[0].equals(ga[l - 1])) { |
1180 |
return ga; |
1181 |
} else { |
1182 |
Geo[] x = new Geo[l + 1]; |
1183 |
System.arraycopy(ga, 0, x, 0, l); |
1184 |
x[l] = ga[0]; |
1185 |
return x; |
1186 |
} |
1187 |
} |
1188 |
} |