git.gag.com Git - fw/altos/blob - altoslib/AltosGreatCircle.java

   1 /*
   2  * Copyright © 2010 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 package org.altusmetrum.altoslib_1;
  19
  20 import java.lang.Math;
  21
  22 public class AltosGreatCircle {
  23         public double   distance;
  24         public double   bearing;
  25         public double   range;
  26         public double   elevation;
  27
  28         double sqr(double a) { return a * a; }
  29
  30         static final double rad = Math.PI / 180;
  31         static final double earth_radius = 6371.2 * 1000;       /* in meters */
  32
  33         public static final int BEARING_LONG = 0;
  34         public static final int BEARING_SHORT = 1;
  35         public static final int BEARING_VOICE = 2;
  36
  37         public String bearing_words(int length) {
  38                 String [][] bearing_string = {
  39                         {
  40                                 "North", "North North East", "North East", "East North East",
  41                                 "East", "East South East", "South East", "South South East",
  42                                 "South", "South South West", "South West", "West South West",
  43                                 "West", "West North West", "North West", "North North West"
  44                         }, {
  45                                 "N", "NNE", "NE", "ENE",
  46                                 "E", "ESE", "SE", "SSE",
  47                                 "S", "SSW", "SW", "WSW",
  48                                 "W", "WNW", "NW", "NNW"
  49                         }, {
  50                                 "north", "nor nor east", "north east", "east nor east",
  51                                 "east", "east sow east", "south east", "sow sow east",
  52                                 "south", "sow sow west", "south west", "west sow west",
  53                                 "west", "west nor west", "north west", "nor nor west "
  54                         }
  55                 };
  56                 return bearing_string[length][(int)((bearing / 90 * 8 + 1) / 2)%16];
  57         }
  58
  59         public AltosGreatCircle (double start_lat, double start_lon, double start_alt,
  60                                  double end_lat, double end_lon, double end_alt) {
  61                 double lat1 = rad * start_lat;
  62                 double lon1 = rad * -start_lon;
  63                 double lat2 = rad * end_lat;
  64                 double lon2 = rad * -end_lon;
  65
  66                 double d_lon = lon2 - lon1;
  67
  68                 /* From http://en.wikipedia.org/wiki/Great-circle_distance */
  69                 double vdn = Math.sqrt(sqr(Math.cos(lat2) * Math.sin(d_lon)) +
  70                                        sqr(Math.cos(lat1) * Math.sin(lat2) -
  71                                            Math.sin(lat1) * Math.cos(lat2) * Math.cos(d_lon)));
  72                 double vdd = Math.sin(lat1) * Math.sin(lat2) + Math.cos(lat1) * Math.cos(lat2) * Math.cos(d_lon);
  73                 double d = Math.atan2(vdn,vdd);
  74                 double course;
  75
  76                 if (Math.cos(lat1) < 1e-20) {
  77                         if (lat1 > 0)
  78                                 course = Math.PI;
  79                         else
  80                                 course = -Math.PI;
  81                 } else {
  82                         if (d < 1e-10)
  83                                 course = 0;
  84                         else
  85                                 course = Math.acos((Math.sin(lat2)-Math.sin(lat1)*Math.cos(d)) /
  86                                                    (Math.sin(d)*Math.cos(lat1)));
  87                         if (Math.sin(lon2-lon1) > 0)
  88                                 course = 2 * Math.PI-course;
  89                 }
  90                 distance = d * earth_radius;
  91                 bearing = course * 180/Math.PI;
  92
  93                 double height_diff = end_alt - start_alt;
  94                 range = Math.sqrt(distance * distance + height_diff * height_diff);
  95                 elevation = Math.atan2(height_diff, distance) * 180 / Math.PI;
  96         }
  97
  98         public AltosGreatCircle (double start_lat, double start_lon,
  99                                  double end_lat, double end_lon) {
 100                 this(start_lat, start_lon, 0, end_lat, end_lon, 0);
 101         }
 102
 103         public AltosGreatCircle(AltosGPS start, AltosGPS end) {
 104                 this(start.lat, start.lon, start.alt, end.lat, end.lon, end.alt);
 105         }
 106
 107         public AltosGreatCircle() {
 108                 distance = 0;
 109                 bearing = 0;
 110                 range = 0;
 111                 elevation = 0;
 112         }
 113 }