1 package net.sf.openrocket.util;
3 import java.util.Locale;
5 import net.sf.openrocket.l10n.Translator;
6 import net.sf.openrocket.startup.Application;
9 * A strategy that performs computations on WorldCoordinates.
11 * The directions of the coordinate is:
14 * positive Z = UPWARDS
16 public enum GeodeticComputationStrategy {
20 * Perform computations using a flat Earth approximation. addCoordinate computes the
21 * location using a direct meters-per-degree scaling and getCoriolisAcceleration always
25 private static final double METERS_PER_DEGREE_LATITUDE = 111325; // "standard figure"
26 private static final double METERS_PER_DEGREE_LONGITUDE_EQUATOR = 111050;
30 public WorldCoordinate addCoordinate(WorldCoordinate location, Coordinate delta) {
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);
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;
40 return new WorldCoordinate(newLat, newLon, newAlt);
44 public Coordinate getCoriolisAcceleration(WorldCoordinate location, Coordinate velocity) {
45 return Coordinate.NUL;
50 * Perform geodetic computations with a spherical Earch approximation.
55 public WorldCoordinate addCoordinate(WorldCoordinate location, Coordinate delta) {
56 double newAlt = location.getAltitude() + delta.z;
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);
62 // Check for zero movement before computing bearing
63 if (MathUtil.equals(d, 0)) {
64 return new WorldCoordinate(location.getLatitudeDeg(), location.getLongitudeDeg(), newAlt);
67 double bearing = Math.atan2(delta.x, delta.y);
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);
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));
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);
84 return new WorldCoordinate(Math.toDegrees(newLat), Math.toDegrees(newLon), newAlt);
88 public Coordinate getCoriolisAcceleration(WorldCoordinate location, Coordinate velocity) {
89 return computeCoriolisAcceleration(location, velocity);
95 * Perform geodetic computations on a WGS84 reference ellipsoid using Vincenty Direct Solution.
100 public WorldCoordinate addCoordinate(WorldCoordinate location, Coordinate delta) {
101 double newAlt = location.getAltitude() + delta.z;
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);
107 // Check for zero movement before computing bearing
108 if (MathUtil.equals(d, 0)) {
109 return new WorldCoordinate(location.getLatitudeDeg(), location.getLongitudeDeg(), newAlt);
112 double bearing = Math.atan(delta.x / delta.y);
114 bearing = bearing + Math.PI;
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);
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);
127 return new WorldCoordinate(Math.toDegrees(newLat), Math.toDegrees(newLon), newAlt);
131 public Coordinate getCoriolisAcceleration(WorldCoordinate location, Coordinate velocity) {
132 return computeCoriolisAcceleration(location, velocity);
137 private static final Translator trans = Application.getTranslator();
139 private static final double PRECISION_LIMIT = 0.5e-13;
143 * Return the name of this geodetic computation method.
145 public String getName() {
146 return trans.get(name().toLowerCase(Locale.ENGLISH) + ".name");
150 * Return a description of the geodetic computation methods.
152 public String getDescription() {
153 return trans.get(name().toLowerCase(Locale.ENGLISH) + ".desc");
157 public String toString() {
163 * Add a cartesian movement coordinate to a WorldCoordinate.
165 public abstract WorldCoordinate addCoordinate(WorldCoordinate location, Coordinate delta);
169 * Compute the coriolis acceleration at a specified WorldCoordinate and velocity.
171 public abstract Coordinate getCoriolisAcceleration(WorldCoordinate location, Coordinate velocity);
177 private static Coordinate computeCoriolisAcceleration(WorldCoordinate latlon, Coordinate velocity) {
179 double sinlat = Math.sin(latlon.getLatitudeRad());
180 double coslat = Math.cos(latlon.getLatitudeRad());
182 double v_n = velocity.y;
183 double v_e = -1 * velocity.x;
184 double v_u = velocity.z;
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.
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)
202 // ******************************************************************** //
203 // The Vincenty Direct Solution.
204 // Code from GeoConstants.java, Ian Cameron Smith, GPL
205 // ******************************************************************** //
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.
212 * Programmed for the CDC-6600 by lcdr L. Pfeifer, NGS Rockville MD,
215 * @param glat1 The latitude of the starting point, in radians,
217 * @param glon1 The latitude of the starting point, in radians,
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,
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.
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;
235 double tu = r * Math.sin(glat1) / Math.cos(glat1);
237 double sf = Math.sin(azimuth);
238 double cf = Math.cos(azimuth);
243 baz = Math.atan2(tu, cf) * 2.0;
245 double cu = 1.0 / Math.sqrt(tu * tu + 1.0);
248 double c2a = -sa * sa + 1.0;
250 double x = Math.sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + 1.0;
253 c = (x * x / 4.0 + 1) / c;
254 double d = (0.375 * x * x - 1.0) * x;
255 tu = dist / r / axis / c;
258 double sy, cy, cz, e;
262 cz = Math.cos(baz + y);
263 e = cz * cz * 2.0 - 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);
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;
283 double[] ret = new double[3];