List of usage examples for org.apache.commons.math.util FastMath atan2
public static double atan2(double y, double x)
From source file:edu.berkeley.path.bots.core.Coordinate.java
public double distanceHaversineInMeters(Coordinate otherCoord) { // SRID's must match and not be null. // This can't be null, duh, and we don't check if otherCoord is, which // means the NullPointerException will be thrown. if (null == this.srid() || null == otherCoord.srid()) { throw new ClassCastException("This distance function uses the spheroid distance, but you " + "are using Coordinates with null SRID~s (from a PostgreSQL " + "point?). This doesn't really make any sense and you " + "probably want to use .distanceCarteasianInSRUnits( other )" + "instead."); } else if (!this.srid().equals(otherCoord.srid())) { throw new ClassCastException("The SRID of otherCoord does't match this one."); }/*from w w w . j a va 2s . com*/ final double piOver180 = Math.PI / 180.0; final double earthRadiusInMeters = 6367000; final double lon1 = piOver180 * this.lon(); final double lat1 = piOver180 * this.lat(); final double lon2 = piOver180 * otherCoord.lon(); final double lat2 = piOver180 * otherCoord.lat(); // Haversine formula: final double dlon = lon2 - lon1; final double dlat = lat2 - lat1; final double a = FastMath.sin(dlat / 2) * FastMath.sin(dlat / 2) + FastMath.cos(lat1) * FastMath.cos(lat2) * FastMath.sin(dlon / 2) * FastMath.sin(dlon / 2); final double c = 2 * FastMath.atan2(FastMath.sqrt(a), FastMath.sqrt(1 - a)); return earthRadiusInMeters * c; }
From source file:edu.berkeley.path.bots.core.Coordinate.java
/** * Implementation of Thaddeus Vincenty's algorithms to solve the direct and * inverse geodetic problems.//from w ww .ja va 2 s. c o m * <p/> * This implementation may be as good as the PostGIS provided one, the * errors seem to be pretty small (10 ^ -3 meters), but a thorough * check/analysis has not been done. * <p/> * For more information, see Vincenty's original publication on the NOAA * web-site: <a href="http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf"> * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf</a> * * @param otherCoord * @return the distance calculated this way. * @throws ClassCastException * if the SRID~s don't match or are null. * @throws NullPointerException * if otherCoord is null. */ public double distanceVincentyInMeters(Coordinate otherCoord) { // SRID's must match and not be null. // This can't be null, duh, and we don't check if otherCoord is, which // means the NullPointerException will be thrown. if (null == this.srid() || null == otherCoord.srid()) { throw new ClassCastException("This distance function uses the spheroid distance, but you " + "are using Coordinates with null SRID~s (from a PostgreSQL " + "point?). This doesn't really make any sense and you " + "probably want to use .distanceCarteasianInSRUnits( other )" + "instead."); } else if (!this.srid().equals(otherCoord.srid())) { throw new ClassCastException("The SRID of otherCoord does't match this one."); } // else we are sure they are the same, and not null. final double a; final double b; final double f; // Set a, b, and f. switch (this.srid()) { case 4326: a = 6378137.0; b = 6356752.3142; f = (a - b) / a; break; default: throw new ClassCastException("The given SRID is not entered in this function, you can" + " get it by running the DB query defined in a function" + " in the NETCONFIG project called " + "util.SpatialDatabaseQueries.getSpheroidStringCached()" + ". This should return a spheroid string like:\n" + "\tSPHEROID(\"WGS 84\",6378137,298.257223563)\n" + "which is SPHEROID( desc, d1, d2 ), so then\n" + "\ta = d1\n" + "\tb = ( d2 * d1 - d1 ) / d2\n" + "\tf = 1 / d2\n" + "Then you can add the numbers to the switch statement" + " in the Java."); } // CHECKSTYLE:OFF // Now a, b, and f should be set. // // All constants below refer to Vincenty's publication (see above). // // get parameters as radians final double PiOver180 = Math.PI / 180.0; final double phi1 = PiOver180 * this.lat(); final double lambda1 = PiOver180 * this.lon(); final double phi2 = PiOver180 * otherCoord.lat(); final double lambda2 = PiOver180 * otherCoord.lon(); // calculations final double a2 = a * a; final double b2 = b * b; final double a2b2b2 = (a2 - b2) / b2; final double omega = lambda2 - lambda1; final double tanphi1 = FastMath.tan(phi1); final double tanU1 = (1.0 - f) * tanphi1; final double U1 = FastMath.atan(tanU1); final double sinU1 = FastMath.sin(U1); final double cosU1 = FastMath.cos(U1); final double tanphi2 = FastMath.tan(phi2); final double tanU2 = (1.0 - f) * tanphi2; final double U2 = FastMath.atan(tanU2); final double sinU2 = FastMath.sin(U2); final double cosU2 = FastMath.cos(U2); final double sinU1sinU2 = sinU1 * sinU2; final double cosU1sinU2 = cosU1 * sinU2; final double sinU1cosU2 = sinU1 * cosU2; final double cosU1cosU2 = cosU1 * cosU2; // Below are the re-assignable fields // eq. 13 double lambda = omega; // intermediates we'll need to compute 's' double A = 0.0; double B = 0.0; double sigma = 0.0; double deltasigma = 0.0; double lambda0; final int num_iters = 5; // 20 final double change_threshold = 1e-5; // 0.0000000000001; for (int i = 0; i < num_iters; i++) { lambda0 = lambda; final double sinlambda = FastMath.sin(lambda); final double coslambda = FastMath.cos(lambda); // eq. 14 final double sin2sigma = (cosU2 * sinlambda * cosU2 * sinlambda) + (cosU1sinU2 - sinU1cosU2 * coslambda) * (cosU1sinU2 - sinU1cosU2 * coslambda); final double sinsigma = FastMath.sqrt(sin2sigma); // eq. 15 final double cossigma = sinU1sinU2 + (cosU1cosU2 * coslambda); // eq. 16 sigma = FastMath.atan2(sinsigma, cossigma); // eq. 17 Careful! sin2sigma might be almost 0! final double sinalpha = (sin2sigma == 0) ? 0.0 : cosU1cosU2 * sinlambda / sinsigma; final double alpha = FastMath.asin(sinalpha); final double cosalpha = FastMath.cos(alpha); final double cos2alpha = cosalpha * cosalpha; // eq. 18 Careful! cos2alpha might be almost 0! final double cos2sigmam = cos2alpha == 0.0 ? 0.0 : cossigma - 2 * sinU1sinU2 / cos2alpha; final double u2 = cos2alpha * a2b2b2; final double cos2sigmam2 = cos2sigmam * cos2sigmam; // eq. 3 A = 1.0 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2))); // eq. 4 B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2))); // eq. 6 deltasigma = B * sinsigma * (cos2sigmam + B / 4 * (cossigma * (-1 + 2 * cos2sigmam2) - B / 6 * cos2sigmam * (-3 + 4 * sin2sigma) * (-3 + 4 * cos2sigmam2))); // eq. 10 final double C = f / 16 * cos2alpha * (4 + f * (4 - 3 * cos2alpha)); // eq. 11 (modified) lambda = omega + (1 - C) * f * sinalpha * (sigma + C * sinsigma * (cos2sigmam + C * cossigma * (-1 + 2 * cos2sigmam2))); // see how much improvement we got final double change = FastMath.abs((lambda - lambda0) / lambda); if ((i > 1) && (change < change_threshold)) { break; } } // for // eq. 19 return b * A * (sigma - deltasigma); // CHECKSTYLE:ON }