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

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