git.gag.com Git - debian/openrocket/blob - src/net/sf/openrocket/util/GeodeticComputationStrategy.java

   1 package net.sf.openrocket.util;
   2
   3 import net.sf.openrocket.l10n.Translator;
   4 import net.sf.openrocket.startup.Application;
   5
   6 /**
   7  * A strategy that performs computations on WorldCoordinates.
   8  */
   9 public enum GeodeticComputationStrategy {
  10
  11
  12         /**
  13          * Perform no geodetic computations.  The addCoordinate method does nothing and
  14          * getCoriolisAcceleration returns Coordinate.NUL.
  15          */
  16         NONE {
  17                 @Override
  18                 public WorldCoordinate addCoordinate(WorldCoordinate latlon, Coordinate delta) {
  19                         return latlon;
  20                 }
  21
  22                 @Override
  23                 public Coordinate getCoriolisAcceleration(WorldCoordinate latlon, Coordinate velocity) {
  24                         return Coordinate.NUL;
  25                 }
  26         },
  27
  28         /**
  29          * Perform geodetic computations with a spherical Earch approximation.
  30          */
  31         SPHERICAL {
  32
  33                 @Override
  34                 public WorldCoordinate addCoordinate(WorldCoordinate location, Coordinate delta) {
  35                         double newAlt = location.getAltitude() + delta.z;
  36
  37                         // bearing (in radians, clockwise from north);
  38                         // d/R is the angular distance (in radians), where d is the distance traveled and R is the earth’s radius
  39                         double d = MathUtil.hypot(delta.x, delta.y);
  40                         double bearing = Math.atan(delta.x / delta.y);
  41                         if (delta.y < 0)
  42                                 bearing = bearing + Math.PI;
  43
  44                         // Calculate the new lat and lon
  45                         double newLat, newLon;
  46                         double sinLat = Math.sin(location.getLatitudeRad());
  47                         double cosLat = Math.cos(location.getLatitudeRad());
  48                         double sinDR = Math.sin(d / WorldCoordinate.REARTH);
  49                         double cosDR = Math.cos(d / WorldCoordinate.REARTH);
  50
  51                         newLat = Math.asin(sinLat * cosDR + cosLat * sinDR * Math.cos(bearing));
  52                         newLon = location.getLongitudeRad() + Math.atan2(Math.sin(bearing) * sinDR * cosLat, cosDR - sinLat * Math.sin(newLat));
  53
  54                         if (Double.isNaN(newLat)) {
  55                                 newLat = location.getLatitudeRad();
  56                         }
  57
  58                         if (Double.isNaN(newLon)) {
  59                                 newLon = location.getLongitudeRad();
  60                         }
  61
  62                         return new WorldCoordinate(Math.toDegrees(newLat), Math.toDegrees(newLon), newAlt);
  63                 }
  64
  65                 @Override
  66                 public Coordinate getCoriolisAcceleration(WorldCoordinate latlon, Coordinate velocity) {
  67                         return computeCoriolisAcceleration(latlon, velocity);
  68                 }
  69
  70         },
  71
  72         /**
  73          * Perform geodetic computations on a WGS84 reference ellipsoid using Vincenty Direct Solution.
  74          */
  75         WGS84 {
  76
  77                 @Override
  78                 public WorldCoordinate addCoordinate(WorldCoordinate location, Coordinate delta) {
  79                         double newAlt = location.getAltitude() + delta.z;
  80
  81                         // bearing (in radians, clockwise from north);
  82                         // d/R is the angular distance (in radians), where d is the distance traveled and R is the earth’s radius
  83                         double d = MathUtil.hypot(delta.x, delta.y);
  84                         double bearing = Math.atan(delta.x / delta.y);
  85                         if (delta.y < 0)
  86                                 bearing = bearing + Math.PI;
  87
  88                         // Calculate the new lat and lon
  89                         double newLat, newLon;
  90                         double ret[] = dirct1(location.getLatitudeRad(), location.getLongitudeRad(), bearing, d, 6378137, 1.0 / 298.25722210088);
  91                         newLat = ret[0];
  92                         newLon = ret[1];
  93
  94                         if (Double.isNaN(newLat)) {
  95                                 newLat = location.getLatitudeRad();
  96                         }
  97
  98                         if (Double.isNaN(newLon)) {
  99                                 newLon = location.getLongitudeRad();
 100                         }
 101                         return new WorldCoordinate(newLat, newLon, newAlt);
 102                 }
 103
 104                 @Override
 105                 public Coordinate getCoriolisAcceleration(WorldCoordinate latlon, Coordinate velocity) {
 106                         return computeCoriolisAcceleration(latlon, velocity);
 107                 }
 108         };
 109
 110
 111         private static final Translator trans = Application.getTranslator();
 112
 113         private static final double PRECISION_LIMIT = 0.5e-13;
 114
 115
 116         /**
 117          * Return the name of this geodetic computation method.
 118          */
 119         public String getName() {
 120                 return trans.get(name().toLowerCase() + ".name");
 121         }
 122
 123         /**
 124          * Return a description of the geodetic computation methods.
 125          */
 126         public String getDescription() {
 127                 return trans.get(name().toLowerCase() + ".desc");
 128         }
 129
 130         @Override
 131         public String toString() {
 132                 return getName();
 133         }
 134
 135
 136         /**
 137          * Add a cartesian movement coordinate to a WorldCoordinate.
 138          */
 139         public abstract WorldCoordinate addCoordinate(WorldCoordinate latlon, Coordinate delta);
 140
 141
 142         /**
 143          * Compute the coriolis acceleration at a specified WorldCoordinate and velocity.
 144          */
 145         public abstract Coordinate getCoriolisAcceleration(WorldCoordinate latlon, Coordinate velocity);
 146
 147
 148
 149
 150
 151         private static Coordinate computeCoriolisAcceleration(WorldCoordinate latlon, Coordinate velocity) {
 152
 153                 double sinlat = Math.sin(latlon.getLatitudeRad());
 154                 double coslat = Math.cos(latlon.getLatitudeRad());
 155
 156                 double v_n = velocity.y;
 157                 double v_e = -1 * velocity.x;
 158                 double v_u = velocity.z;
 159
 160                 // Not exactly sure why I have to reverse the x direction, but this gives the precession in the
 161                 // correct direction (e.g, flying north in northern hemisphere should cause defection to the east (+ve x))
 162                 // All the directions are very confusing because they are tied to the wind direction (to/from?), in which
 163                 // +ve x or east according to WorldCoordinate is what everything is relative to.
 164                 // The directions of everything need so thought, ideally the wind direction and launch rod should be
 165                 // able to be set independently and in terms of bearing with north == +ve y.
 166
 167                 Coordinate coriolis = new Coordinate(2.0 * WorldCoordinate.EROT * (v_n * sinlat - v_u * coslat),
 168                                                                                                 2.0 * WorldCoordinate.EROT * (-1.0 * v_e * sinlat),
 169                                                                                                 2.0 * WorldCoordinate.EROT * (v_e * coslat)
 170                                                                                         );
 171                 return coriolis;
 172         }
 173
 174
 175
 176         // ******************************************************************** //
 177         // The Vincenty Direct Solution.
 178         // Code from GeoConstants.java, Ian Cameron Smith, GPL
 179         // ******************************************************************** //
 180
 181         /**
 182          * Solution of the geodetic direct problem after T. Vincenty.
 183          * Modified Rainsford's method with Helmert's elliptical terms.
 184          * Effective in any azimuth and at any distance short of antipodal.
 185          *
 186          * Programmed for the CDC-6600 by lcdr L. Pfeifer, NGS Rockville MD,
 187          * 20 Feb 1975.
 188          *
 189          * @param       glat1           The latitude of the starting point, in radians,
 190          *                                              positive north.
 191          * @param       glon1           The latitude of the starting point, in radians,
 192          *                                              positive east.
 193          * @param       azimuth         The azimuth to the desired location, in radians
 194          *                                              clockwise from north.
 195          * @param       dist            The distance to the desired location, in meters.
 196          * @param       axis            The semi-major axis of the reference ellipsoid,
 197          *                                              in meters.
 198          * @param       flat            The flattening of the reference ellipsoid.
 199          * @return                              An array containing the latitude and longitude
 200          *                                              of the desired point, in radians, and the
 201          *                                              azimuth back from that point to the starting
 202          *                                              point, in radians clockwise from north.
 203          */
 204         private static double[] dirct1(double glat1, double glon1,
 205                                                                         double azimuth, double dist,
 206                                                                         double axis, double flat) {
 207                 double r = 1.0 - flat;
 208
 209                 double tu = r * Math.sin(glat1) / Math.cos(glat1);
 210
 211                 double sf = Math.sin(azimuth);
 212                 double cf = Math.cos(azimuth);
 213
 214                 double baz = 0.0;
 215
 216                 if (cf != 0.0)
 217                         baz = Math.atan2(tu, cf) * 2.0;
 218
 219                 double cu = 1.0 / Math.sqrt(tu * tu + 1.0);
 220                 double su = tu * cu;
 221                 double sa = cu * sf;
 222                 double c2a = -sa * sa + 1.0;
 223
 224                 double x = Math.sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + 1.0;
 225                 x = (x - 2.0) / x;
 226                 double c = 1.0 - x;
 227                 c = (x * x / 4.0 + 1) / c;
 228                 double d = (0.375 * x * x - 1.0) * x;
 229                 tu = dist / r / axis / c;
 230                 double y = tu;
 231
 232                 double sy, cy, cz, e;
 233                 do {
 234                         sy = Math.sin(y);
 235                         cy = Math.cos(y);
 236                         cz = Math.cos(baz + y);
 237                         e = cz * cz * 2.0 - 1.0;
 238
 239                         c = y;
 240                         x = e * cy;
 241                         y = e + e - 1.0;
 242                         y = (((sy * sy * 4.0 - 3.0) * y * cz * d / 6.0 + x) *
 243                                                                         d / 4.0 - cz) * sy * d + tu;
 244                 } while (Math.abs(y - c) > PRECISION_LIMIT);
 245
 246                 baz = cu * cy * cf - su * sy;
 247                 c = r * Math.sqrt(sa * sa + baz * baz);
 248                 d = su * cy + cu * sy * cf;
 249                 double glat2 = Math.atan2(d, c);
 250                 c = cu * cy - su * sy * cf;
 251                 x = Math.atan2(sy * sf, c);
 252                 c = ((-3.0 * c2a + 4.0) * flat + 4.0) * c2a * flat / 16.0;
 253                 d = ((e * cy * c + cz) * sy * c + y) * sa;
 254                 double glon2 = glon1 + x - (1.0 - c) * d * flat;
 255                 baz = Math.atan2(sa, baz) + Math.PI;
 256
 257                 double[] ret = new double[3];
 258                 ret[0] = glat2;
 259                 ret[1] = glon2;
 260                 ret[2] = baz;
 261                 return ret;
 262         }
 263
 264
 265 }