1/*
2 * To change this template, choose Tools | Templates
3 * and open the template in the editor.
4 */
5package jme3tools.navigation;
6
7
8
9/**
10 * A utlity class for performing position calculations
11 *
12 * @author Benjamin Jakobus, based on JMarine (by Cormac Gebruers and Benjamin
13 *          Jakobus)
14 * @version 1.0
15 * @since 1.0
16 */
17public class NavCalculator {
18
19    private double distance;
20    private double trueCourse;
21
22    /* The earth's radius in meters */
23    public static final int WGS84_EARTH_RADIUS = 6378137;
24    private String strCourse;
25
26    /* The sailing calculation type */
27    public static final int MERCATOR = 0;
28    public static final int GC = 1;
29
30    /* The degree precision to use for courses */
31    public static final int RL_CRS_PRECISION = 1;
32
33    /* The distance precision to use for distances */
34    public static final int RL_DIST_PRECISION = 1;
35    public static final int METERS_PER_MINUTE = 1852;
36
37    /**
38     * Constructor
39     * @param P1
40     * @param P2
41     * @param calcType
42     * @since 1.0
43     */
44    public NavCalculator(Position P1, Position P2, int calcType) {
45        switch (calcType) {
46            case MERCATOR:
47                mercatorSailing(P1, P2);
48                break;
49            case GC:
50                greatCircleSailing(P1, P2);
51                break;
52        }
53    }
54
55    /**
56     * Constructor
57     * @since 1.0
58     */
59    public NavCalculator() {
60    }
61
62    /**
63     * Determines a great circle track between two positions
64     * @param p1 origin position
65     * @param p2 destination position
66     */
67    public GCSailing greatCircleSailing(Position p1, Position p2) {
68        return new GCSailing(new int[0], new float[0]);
69    }
70
71    /**
72     * Determines a Rhumb Line course and distance between two points
73     * @param p1 origin position
74     * @param p2 destination position
75     */
76    public RLSailing rhumbLineSailing(Position p1, Position p2) {
77        RLSailing rl = mercatorSailing(p1, p2);
78        return rl;
79    }
80
81    /**
82     * Determines the rhumb line course  and distance between two positions
83     * @param p1 origin position
84     * @param p2 destination position
85     */
86    public RLSailing mercatorSailing(Position p1, Position p2) {
87
88        double dLat = computeDLat(p1.getLatitude(), p2.getLatitude());
89        //plane sailing...
90        if (dLat == 0) {
91            RLSailing rl = planeSailing(p1, p2);
92            return rl;
93        }
94
95        double dLong = computeDLong(p1.getLongitude(), p2.getLongitude());
96        double dmp = (float) computeDMPClarkeSpheroid(p1.getLatitude(), p2.getLatitude());
97
98        trueCourse = (float) Math.toDegrees(Math.atan(dLong / dmp));
99        double degCrs = convertCourse((float) trueCourse, p1, p2);
100        distance = (float) Math.abs(dLat / Math.cos(Math.toRadians(trueCourse)));
101
102        RLSailing rl = new RLSailing(degCrs, (float) distance);
103        trueCourse = rl.getCourse();
104        strCourse = (dLat < 0 ? "S" : "N");
105        strCourse += " " + trueCourse;
106        strCourse += " " + (dLong < 0 ? "W" : "E");
107        return rl;
108
109    }
110
111    /**
112     * Calculate a plane sailing situation - i.e. where Lats are the same
113     * @param p1
114     * @param p2
115     * @return
116     * @since 1.0
117     */
118    public RLSailing planeSailing(Position p1, Position p2) {
119        double dLong = computeDLong(p1.getLongitude(), p2.getLongitude());
120
121        double sgnDLong = 0 - (dLong / Math.abs(dLong));
122        if (Math.abs(dLong) > 180 * 60) {
123            dLong = (360 * 60 - Math.abs(dLong)) * sgnDLong;
124        }
125
126        double redist = 0;
127        double recourse = 0;
128        if (p1.getLatitude() == 0) {
129            redist = Math.abs(dLong);
130        } else {
131            redist = Math.abs(dLong * (float) Math.cos(p1.getLatitude() * 2 * Math.PI / 360));
132        }
133        recourse = (float) Math.asin(0 - sgnDLong);
134        recourse = recourse * 360 / 2 / (float) Math.PI;
135
136        if (recourse < 0) {
137            recourse = recourse + 360;
138        }
139        return new RLSailing(recourse, redist);
140    }
141
142    /**
143     * Converts a course from cardinal XddY to ddd notation
144     * @param tc
145     * @param p1 position one
146     * @param p2 position two
147     * @return
148     * @since 1.0
149     */
150    public static double convertCourse(float tc, Position p1, Position p2) {
151
152        double dLat = p1.getLatitude() - p2.getLatitude();
153        double dLong = p1.getLongitude() - p2.getLongitude();
154        //NE
155        if (dLong >= 0 & dLat >= 0) {
156            return Math.abs(tc);
157        }
158
159        //SE
160        if (dLong >= 0 & dLat < 0) {
161            return 180 - Math.abs(tc);
162        }
163
164        //SW
165        if (dLong < 0 & dLat < 0) {
166            return 180 + Math.abs(tc);
167        }
168
169        //NW
170        if (dLong < 0 & dLat >= 0) {
171            return 360 - Math.abs(tc);
172        }
173        return -1;
174    }
175
176    /**
177     * Getter method for the distance between two points
178     * @return distance
179     * @since 1.0
180     */
181    public double getDistance() {
182        return distance;
183    }
184
185    /**
186     * Getter method for the true course
187     * @return true course
188     * @since 1.0
189     */
190    public double getTrueCourse() {
191        return trueCourse;
192    }
193
194    /**
195     * Getter method for the true course
196     * @return true course
197     * @since 1.0
198     */
199    public String getStrCourse() {
200        return strCourse;
201    }
202
203    /**
204     * Computes the difference in meridional parts for two latitudes in minutes
205     * (based on Clark 1880 spheroid)
206     * @param lat1
207     * @param lat2
208     * @return difference in minutes
209     * @since 1.0
210     */
211    public static double computeDMPClarkeSpheroid(double lat1, double lat2) {
212        double absLat1 = Math.abs(lat1);
213        double absLat2 = Math.abs(lat2);
214
215        double m1 = (7915.704468 * (Math.log(Math.tan(Math.toRadians(45
216                + (absLat1 / 2)))) / Math.log(10))
217                - 23.268932 * Math.sin(Math.toRadians(absLat1))
218                - 0.052500 * Math.pow(Math.sin(Math.toRadians(absLat1)), 3)
219                - 0.000213 * Math.pow(Math.sin(Math.toRadians(absLat1)), 5));
220
221        double m2 = (7915.704468 * (Math.log(Math.tan(Math.toRadians(45
222                + (absLat2 / 2)))) / Math.log(10))
223                - 23.268932 * Math.sin(Math.toRadians(absLat2))
224                - 0.052500 * Math.pow(Math.sin(Math.toRadians(absLat2)), 3)
225                - 0.000213 * Math.pow(Math.sin(Math.toRadians(absLat2)), 5));
226        if ((lat1 <= 0 && lat2 <= 0) || (lat1 > 0 && lat2 > 0)) {
227            return Math.abs(m1 - m2);
228        } else {
229            return m1 + m2;
230        }
231    }
232
233    /**
234     * Computes the difference in meridional parts for a perfect sphere between
235     * two degrees of latitude
236     * @param lat1
237     * @param lat2
238     * @return difference in meridional parts between lat1 and lat2 in minutes
239     * @since 1.0
240     */
241    public static float computeDMPWGS84Spheroid(float lat1, float lat2) {
242        float absLat1 = Math.abs(lat1);
243        float absLat2 = Math.abs(lat2);
244
245        float m1 = (float) (7915.7045 * Math.log10(Math.tan(Math.toRadians(45 + (absLat1 / 2))))
246                - 23.01358 * Math.sin(absLat1 - 0.05135) * Math.pow(Math.sin(absLat1), 3));
247
248        float m2 = (float) (7915.7045 * Math.log10(Math.tan(Math.toRadians(45 + (absLat2 / 2))))
249                - 23.01358 * Math.sin(absLat2 - 0.05135) * Math.pow(Math.sin(absLat2), 3));
250
251        if (lat1 <= 0 & lat2 <= 0 || lat1 > 0 & lat2 > 0) {
252            return Math.abs(m1 - m2);
253        } else {
254            return m1 + m2;
255        }
256    }
257
258    /**
259     * Predicts the position of a target for a given time in the future
260     * @param time the number of seconds from now for which to predict the future
261     *        position
262     * @param speed the miles per minute that the target is traveling
263     * @param currentLat the target's current latitude
264     * @param currentLong the target's current longitude
265     * @param course the target's current course in degrees
266     * @return the predicted future position
267     * @since 1.0
268     */
269    public static Position predictPosition(int time, double speed,
270            double currentLat, double currentLong, double course) {
271        Position futurePosition = null;
272        course = Math.toRadians(course);
273        double futureLong = currentLong + speed * time * Math.sin(course);
274        double futureLat = currentLat + speed * time * Math.cos(course);
275        try {
276            futurePosition = new Position(futureLat, futureLong);
277        } catch (InvalidPositionException ipe) {
278            ipe.printStackTrace();
279        }
280        return futurePosition;
281
282    }
283
284    /**
285     * Computes the coordinate of position B relative to an offset given
286     * a distance and an angle.
287     *
288     * @param offset        The offset position.
289     * @param bearing       The bearing between the offset and the coordinate
290     *                      that you want to calculate.
291     * @param distance      The distance, in meters, between the offset
292     *                      and point B.
293     * @return              The position of point B that is located from
294     *                      given offset at given distance and angle.
295     * @since 1.0
296     */
297    public static Position computePosition(Position initialPos, double heading,
298            double distance) {
299        if (initialPos == null) {
300            return null;
301        }
302        double angle;
303        if (heading < 90) {
304            angle = heading;
305        } else if (heading > 90 && heading < 180) {
306            angle = 180 - heading;
307        } else if (heading > 180 && heading < 270) {
308            angle = heading - 180;
309        } else {
310            angle = 360 - heading;
311        }
312
313        Position newPosition = null;
314
315        // Convert meters into nautical miles
316        distance = distance * 0.000539956803;
317        angle = Math.toRadians(angle);
318        double initialLat = initialPos.getLatitude();
319        double initialLong = initialPos.getLongitude();
320        double dlat = distance * Math.cos(angle);
321        dlat = dlat / 60;
322        dlat = Math.abs(dlat);
323        double newLat = 0;
324        if ((heading > 270 && heading < 360) || (heading > 0 && heading < 90)) {
325            newLat = initialLat + dlat;
326        } else if (heading < 270 && heading > 90) {
327            newLat = initialLat - dlat;
328        }
329        double meanLat = (Math.abs(dlat) / 2.0) + newLat;
330        double dep = (Math.abs(dlat * 60)) * Math.tan(angle);
331        double dlong = dep * (1.0 / Math.cos(Math.toRadians(meanLat)));
332        dlong = dlong / 60;
333        dlong = Math.abs(dlong);
334        double newLong;
335        if (heading > 180 && heading < 360) {
336            newLong = initialLong - dlong;
337        } else {
338            newLong = initialLong + dlong;
339        }
340
341        if (newLong < -180) {
342            double diff = Math.abs(newLong + 180);
343            newLong = 180 - diff;
344        }
345
346        if (newLong > 180) {
347            double diff = Math.abs(newLong + 180);
348            newLong = (180 - diff) * -1;
349        }
350
351        if (heading == 0 || heading == 360 || heading == 180) {
352            newLong = initialLong;
353            newLat = initialLat + dlat;
354        } else if (heading == 90 || heading == 270) {
355            newLat = initialLat;
356//            newLong = initialLong + dlong; THIS WAS THE ORIGINAL (IT WORKED)
357            newLong = initialLong - dlong;
358        }
359        try {
360            newPosition = new Position(newLat,
361                    newLong);
362        } catch (InvalidPositionException ipe) {
363            ipe.printStackTrace();
364            System.out.println(newLat + "," + newLong);
365        }
366        return newPosition;
367    }
368
369    /**
370     * Computes the difference in Longitude between two positions and assigns the
371     * correct sign -westwards travel, + eastwards travel
372     * @param lng1
373     * @param lng2
374     * @return difference in longitude
375     * @since 1.0
376     */
377    public static double computeDLong(double lng1, double lng2) {
378        if (lng1 - lng2 == 0) {
379            return 0;
380        }
381
382        // both easterly
383        if (lng1 >= 0 & lng2 >= 0) {
384            return -(lng1 - lng2) * 60;
385        }
386        //both westerly
387        if (lng1 < 0 & lng2 < 0) {
388            return -(lng1 - lng2) * 60;
389        }
390
391        //opposite sides of Date line meridian
392
393        //sum less than 180
394        if (Math.abs(lng1) + Math.abs(lng2) < 180) {
395            if (lng1 < 0 & lng2 > 0) {
396                return -(Math.abs(lng1) + Math.abs(lng2)) * 60;
397            } else {
398                return Math.abs(lng1) + Math.abs(lng2) * 60;
399            }
400        } else {
401            //sum greater than 180
402            if (lng1 < 0 & lng2 > 0) {
403                return -(360 - (Math.abs(lng1) + Math.abs(lng2))) * 60;
404            } else {
405                return (360 - (Math.abs(lng1) + Math.abs(lng2))) * 60;
406            }
407        }
408    }
409
410    /**
411     * Computes the difference in Longitude between two positions and assigns the
412     * correct sign -westwards travel, + eastwards travel
413     * @param lng1
414     * @param lng2
415     * @return difference in longitude
416     * @since 1.0
417     */
418    public static double computeLongDiff(double lng1, double lng2) {
419        if (lng1 - lng2 == 0) {
420            return 0;
421        }
422
423        // both easterly
424        if (lng1 >= 0 & lng2 >= 0) {
425            return Math.abs(-(lng1 - lng2) * 60);
426        }
427        //both westerly
428        if (lng1 < 0 & lng2 < 0) {
429            return Math.abs(-(lng1 - lng2) * 60);
430        }
431
432        if (lng1 == 0) {
433            return Math.abs(lng2 * 60);
434        }
435
436        if (lng2 == 0) {
437            return Math.abs(lng1 * 60);
438        }
439
440        return (Math.abs(lng1) + Math.abs(lng2)) * 60;
441    }
442
443    /**
444     * Compute the difference in latitude between two positions
445     * @param lat1
446     * @param lat2
447     * @return difference in latitude
448     * @since 1.0
449     */
450    public static double computeDLat(double lat1, double lat2) {
451        //same side of equator
452
453        //plane sailing
454        if (lat1 - lat2 == 0) {
455            return 0;
456        }
457
458        //both northerly
459        if (lat1 >= 0 & lat2 >= 0) {
460            return -(lat1 - lat2) * 60;
461        }
462        //both southerly
463        if (lat1 < 0 & lat2 < 0) {
464            return -(lat1 - lat2) * 60;
465        }
466
467        //opposite sides of equator
468        if (lat1 >= 0) {
469            //heading south
470            return -(Math.abs(lat1) + Math.abs(lat2));
471        } else {
472            //heading north
473            return (Math.abs(lat1) + Math.abs(lat2));
474        }
475    }
476
477    public static class Quadrant {
478
479        private static final Quadrant FIRST = new Quadrant(1, 1);
480        private static final Quadrant SECOND = new Quadrant(-1, 1);
481        private static final Quadrant THIRD = new Quadrant(-1, -1);
482        private static final Quadrant FOURTH = new Quadrant(1, -1);
483        private final int lonMultiplier;
484        private final int latMultiplier;
485
486        public Quadrant(final int xMultiplier, final int yMultiplier) {
487            this.lonMultiplier = xMultiplier;
488            this.latMultiplier = yMultiplier;
489        }
490
491        static Quadrant getQuadrant(double degrees, boolean invert) {
492            if (invert) {
493                if (degrees >= 0 && degrees <= 90) {
494                    return FOURTH;
495                } else if (degrees > 90 && degrees <= 180) {
496                    return THIRD;
497                } else if (degrees > 180 && degrees <= 270) {
498                    return SECOND;
499                }
500                return FIRST;
501            } else {
502                if (degrees >= 0 && degrees <= 90) {
503                    return FIRST;
504                } else if (degrees > 90 && degrees <= 180) {
505                    return SECOND;
506                } else if (degrees > 180 && degrees <= 270) {
507                    return THIRD;
508                }
509                return FOURTH;
510            }
511        }
512    }
513
514    /**
515     * Converts meters to degrees.
516     *
517     * @param meters            The meters that you want to convert into degrees.
518     * @return                  The degree equivalent of the given meters.
519     * @since 1.0
520     */
521    public static double toDegrees(double meters) {
522        return (meters / METERS_PER_MINUTE) / 60;
523    }
524
525    /**
526     * Computes the bearing between two points.
527     *
528     * @param p1
529     * @param p2
530     * @return
531     * @since 1.0
532     */
533    public static int computeBearing(Position p1, Position p2) {
534        int bearing;
535        double dLon = computeDLong(p1.getLongitude(), p2.getLongitude());
536        double y = Math.sin(dLon) * Math.cos(p2.getLatitude());
537        double x = Math.cos(p1.getLatitude()) * Math.sin(p2.getLatitude())
538                - Math.sin(p1.getLatitude()) * Math.cos(p2.getLatitude()) * Math.cos(dLon);
539        bearing = (int) Math.toDegrees(Math.atan2(y, x));
540        return bearing;
541    }
542
543    /**
544     * Computes the angle between two points.
545     *
546     * @param p1
547     * @param p2
548     * @return
549     */
550    public static int computeAngle(Position p1, Position p2) {
551        // cos (adj / hyp)
552        double adj = Math.abs(p1.getLongitude() - p2.getLongitude());
553        double opp = Math.abs(p1.getLatitude() - p2.getLatitude());
554        return (int) Math.toDegrees(Math.atan(opp / adj));
555
556//        int angle = (int)Math.atan2(p2.getLatitude() - p1.getLatitude(),
557//                p2.getLongitude() - p1.getLongitude());
558        //Actually it's ATan2(dy , dx) where dy = y2 - y1 and dx = x2 - x1, or ATan(dy / dx)
559    }
560
561    public static int computeHeading(Position p1, Position p2) {
562        int angle = computeAngle(p1, p2);
563        // NE
564        if (p2.getLongitude() >= p1.getLongitude() && p2.getLatitude() >= p1.getLatitude()) {
565            return angle;
566        } else if (p2.getLongitude() >= p1.getLongitude() && p2.getLatitude() <= p1.getLatitude()) {
567            // SE
568            return 90 + angle;
569        } else if (p2.getLongitude() <= p1.getLongitude() && p2.getLatitude() <= p1.getLatitude()) {
570            // SW
571            return 270 - angle;
572        } else {
573            // NW
574            return 270 + angle;
575        }
576    }
577
578    public static void main(String[] args) {
579        try {
580            int pos = NavCalculator.computeHeading(new Position(0, 0), new Position(10, -10));
581//            System.out.println(pos.getLatitude() + "," + pos.getLongitude());
582            System.out.println(pos);
583        } catch (Exception e) {
584        }
585
586
587
588
589
590    }
591}
592