geopoint.java

Open DMT GPS server source code

    *** @return The 'Y' coordinate
    **/
    public double getY()
    {
        return this.latitude;
    }

    /**
    *** Gets the Latitude in radians
    *** @return The Latitude in radians
    **/
    public double getLatitudeRadians()
    {
        return this.getLatitude() * RADIANS;
    }

    /**
    *** Gets the String representation of the Latitude
    *** @return The String representation of the Latitude
    **/
    public String getLatitudeString()
    {
        return getLatitudeString(false);
    }

    /**
    *** Gets the String representation of the Latitude
    *** @param decFormat  The output format
    *** @return The String representation of the Latitude
    **/
    public String getLatitudeString(String decFormat)
    {
        return StringTools.format(this.getLatitude(), decFormat);
    }

    /**
    *** Gets the String representation of the Latitude
    *** @param dms  True to return the Latitude formated as degrees/minutes/seconds
    *** @return The String representation of the Latitude
    **/
    public String getLatitudeString(boolean dms)
    {
        return formatLatitude(this.getLatitude(), dms);
    }
    
    /**
    *** Formats and returns a String representation of the specified Latitude
    *** @param lat  The Latitude to format
    *** @param dms  True to return the Latitude formated as degrees/minutes/seconds
    *** @return The String representation of the Latitude
    **/
    public static String formatLatitude(double lat, boolean dms)
    {
        int fmt = FORMAT_LATITUDE | (dms? FORMAT_DMS : FORMAT_DEC);
        return formatCoord(lat, fmt);
    }
    
    // ------------------------------------------------------------------------

    /**
    *** Sets the Longitude in degrees/minutes/seconds
    *** @param deg  The degrees
    *** @param min  The minutes
    *** @param sec  The seconds
    **/
    public void setLongitude(double deg, double min, double sec)
    {
        this.setLongitude(GeoPoint.convertDmsToDec(deg, min, sec));
    }

    /**
    *** Sets the Longitude in degrees
    *** @param lon  The Longitude
    **/
    public void setLongitude(double lon)
    {
        this.longitude = lon;
    }

    /**
    *** Gets the Longitude in degrees
    *** @return The Longitude in degrees
    **/
    public double getLongitude()
    {
        return this.longitude;
    }

    /**
    *** Gets the 'X' coordinate (same as Longitude)
    *** @return The 'X' coordinate
    **/
    public double getX()
    {
        return this.longitude;
    }

     /**
    *** Gets the Longitude in radians
    *** @return The Longitude in radians
    **/
   public double getLongitudeRadians()
    {
        return this.getLongitude() * RADIANS;
    }

    /**
    *** Gets the String representation of the Longitude
    *** @return The String representation of the Longitude
    **/
    public String getLongitudeString()
    {
        return getLongitudeString(false);
    }

    /**
    *** Gets the String representation of the Longitude
    *** @param decFormat  The output format
    *** @return The String representation of the Longitude
    **/
    public String getLongitudeString(String decFormat)
    {
        return StringTools.format(this.getLongitude(), decFormat);
    }

    /**
    *** Gets the String representation of the Longitude
    *** @param dms  True to return the Longitude formated as degrees/minutes/seconds
    *** @return The String representation of the Longitude
    **/
    public String getLongitudeString(boolean dms)
    {
        return formatLongitude(this.getLongitude(), dms);
    }
    
    /**
    *** Formats and returns a String representation of the specified Longitude
    *** @param lon  The Longitude to format
    *** @param dms  True to return the Longitude formated as degrees/minutes/seconds
    *** @return The String representation of the Longitude
    **/
    public static String formatLongitude(double lon, boolean dms)
    {
        int fmt = FORMAT_LONGITUDE | (dms? FORMAT_DMS : FORMAT_DEC);
        return formatCoord(lon, fmt);
    }
    
    // ------------------------------------------------------------------------

    public  static final int    ENCODE_HIRES_LEN    = 8;
    public  static final int    ENCODE_LORES_LEN    = 6;

    private static final double POW_24              =    16777216.0; // 2^24
    private static final double POW_28              =   268435456.0; // 2^28
    private static final double POW_32              =  4294967296.0; // 2^32

    /**
    *** Encodes the specified GeoPoint into a byte array
    *** @param gp   The GeoPoint to encode
    *** @param enc  The byte array into which the GeoPoint will be encoded
    *** @param ofs  The offset into the byte array where the encoded GeoPoint will be placed
    *** @param len  Either '6', for 6-byte encoding, or '8', for 8-byte encoding
    *** @return The byte array into which the GeoPoint was encoded
    **/
    public static byte[] encodeGeoPoint(GeoPoint gp, byte enc[], int ofs, int len)
    {
        /* null/empty bytes */
        if (enc == null) {
            return null;
        }
        
        /* offset/length out-of-range */
        if (len < 0) { len = enc.length; }
        if ((ofs + len) > enc.length) {
            return null;
        }
        
        /* not enough bytes to encode */
        if (len < ENCODE_LORES_LEN) {
            return null;
        }

        /* lat/lon */
        double lat = gp.getLatitude();
        double lon = gp.getLongitude();
        
        /* standard resolution */
        if ((len >= ENCODE_LORES_LEN) && (len < ENCODE_HIRES_LEN)) {
            // LL-LL-LL LL-LL-LL
            long rawLat24 = (lat != 0.0)? Math.round((lat -  90.0) * (POW_24 / -180.0)) : 0L;
            long rawLon24 = (lon != 0.0)? Math.round((lon + 180.0) * (POW_24 /  360.0)) : 0L;
            long rawAccum = ((rawLat24 << 24) & 0xFFFFFF000000L) | (rawLon24 & 0xFFFFFFL);
            enc[ofs + 0] = (byte)((rawAccum >> 40) & 0xFF);
            enc[ofs + 1] = (byte)((rawAccum >> 32) & 0xFF);
            enc[ofs + 2] = (byte)((rawAccum >> 24) & 0xFF);
            enc[ofs + 3] = (byte)((rawAccum >> 16) & 0xFF);
            enc[ofs + 4] = (byte)((rawAccum >>  8) & 0xFF);
            enc[ofs + 5] = (byte)((rawAccum      ) & 0xFF);
            return enc;
        } 
        
        /* high resolution */
        if (len >= ENCODE_HIRES_LEN) {
            // LL-LL-LL-LL LL-LL-LL-LL
            long rawLat32 = (lat != 0.0)? Math.round((lat -  90.0) * (POW_32 / -180.0)) : 0L;
            long rawLon32 = (lon != 0.0)? Math.round((lon + 180.0) * (POW_32 /  360.0)) : 0L;
            long rawAccum = ((rawLat32 << 32) & 0xFFFFFFFF00000000L) | (rawLon32 & 0xFFFFFFFFL);
            enc[ofs + 0] = (byte)((rawAccum >> 56) & 0xFF);
            enc[ofs + 1] = (byte)((rawAccum >> 48) & 0xFF);
            enc[ofs + 2] = (byte)((rawAccum >> 40) & 0xFF);
            enc[ofs + 3] = (byte)((rawAccum >> 32) & 0xFF);
            enc[ofs + 4] = (byte)((rawAccum >> 24) & 0xFF);
            enc[ofs + 5] = (byte)((rawAccum >> 16) & 0xFF);
            enc[ofs + 6] = (byte)((rawAccum >>  8) & 0xFF);
            enc[ofs + 7] = (byte)((rawAccum      ) & 0xFF);
            return enc;
        }
       
        /* will never reach here */
        return null;

    }

    /**
    *** Decodes a GeoPoint from the specified byte array
    *** @param enc  The byte array from which the GeoPoint will be decoded
    *** @param ofs  The offset into the byte array where the GeoPoint will be decoded
    *** @param len  Either '6', for 6-byte decoding, or '8', for 8-byte decoding
    *** @return The decoded GeoPoint
    **/
    public static GeoPoint decodeGeoPoint(byte enc[], int ofs, int len)
    {
        
        /* null/empty bytes */
        if (enc == null) {
            return null;
        }
        
        /* offset/length out-of-range */
        if (len < 0) { len = enc.length; }
        if ((ofs + len) > enc.length) {
            return null;
        }
        
        /* not enough bytes to decode */
        if (len < 6) {
            return null;
        }
        
        /* 6-byte standard resolution */
        if ((len >= 6) && (len < 8)) {
            // LL-LL-LL LL-LL-LL
            long rawLat24 = (((long)enc[ofs+0] & 0xFF) << 16) | (((long)enc[ofs+1] & 0xFF) << 8) | ((long)enc[ofs+2] & 0xFF);
            long rawLon24 = (((long)enc[ofs+3] & 0xFF) << 16) | (((long)enc[ofs+4] & 0xFF) << 8) | ((long)enc[ofs+5] & 0xFF);
            double lat = (rawLat24 != 0L)? ((((double)rawLat24 + 0.5) * (-180.0 / POW_24)) +  90.0) : 0.0;
            double lon = (rawLon24 != 0L)? ((((double)rawLon24 + 0.5) * ( 360.0 / POW_24)) - 180.0) : 0.0; // was: - 360.0) : 0.0;
            // TODO: handle +/- 90 latitude, and +/- 180 longitude.
            return new GeoPoint(lat, lon);
        }
        
        /* 8-byte high resolution */
        if (len >= 8) {
            // LL-LL-LL-LL LL-LL-LL-LL
            long rawLat32 = (((long)enc[ofs+0] & 0xFF) << 24) | (((long)enc[ofs+1] & 0xFF) << 16) | (((long)enc[ofs+2] & 0xFF) << 8) | ((long)enc[ofs+3] & 0xFF);
            long rawLon32 = (((long)enc[ofs+4] & 0xFF) << 24) | (((long)enc[ofs+5] & 0xFF) << 16) | (((long)enc[ofs+6] & 0xFF) << 8) | ((long)enc[ofs+7] & 0xFF);
            double lat = (rawLat32 != 0L)? ((((double)rawLat32 + 0.5) * (-180.0 / POW_32)) +  90.0) : 0.0;
            double lon = (rawLon32 != 0L)? ((((double)rawLon32 + 0.5) * ( 360.0 / POW_32)) - 180.0) : 0.0; // was: - 360.0) : 0.0;
            // TODO: handle +/- 90 latitude, and +/- 180 longitude.
            return new GeoPoint(lat, lon);
        }
        
        /* will never reach here */
        return null;
        
    }
    
    // ------------------------------------------------------------------------

    /**
    *** Returns the distance to the specified point, in radians
    *** @param dest  The destination point
    *** @return The distance to the specified point, in radians
    **/
    public double radiansToPoint(GeoPoint dest)
    {
        // Flat plane approximations:
        //   http://mathforum.org/library/drmath/view/51833.html
        //   http://mathforum.org/library/drmath/view/62720.html
        if (dest == null) {
            // you pass in 'null', you deserver what you get
            return Double.NaN;
        } else
        if (this.equals(dest)) {
            // If the points are equals, the radians would be NaN
            return 0.0;
        } else {
            try {
                double lat1 = this.getLatitudeRadians(), lon1 = this.getLongitudeRadians();
                double lat2 = dest.getLatitudeRadians(), lon2 = dest.getLongitudeRadians();
                double rad  = 0.0;
                if (UseHaversineDistanceFormula) {
                    // Haversine formula:
                    // "The Haversine formula may be more accurate for small distances"
                    // See: http://www.census.gov/cgi-bin/geo/gisfaq?Q5.1
                    //      http://mathforum.org/library/drmath/view/51879.html
                    // Also, use of the Haversine formula is about twice as fast as the Law of Cosines
                    double dlat = lat2 - lat1;
                    double dlon = lon2 - lon1;
                    double a    = SQ(Math.sin(dlat/2.0)) + (Math.cos(lat1) * Math.cos(lat2) * SQ(Math.sin(dlon/2.0)));
                    rad = 2.0 * Math.atan2(Math.sqrt(a), Math.sqrt(1.0 - a));
                } else {
                    // Law of Cosines for Spherical Trigonometry:
                    // Per http://www.census.gov/cgi-bin/geo/gisfaq?Q5.1 this method isn't recommended:
                    //  "Although this formula is mathematically exact, it is unreliable for
                    //   small distances because the inverse cosine is ill-conditioned."
                    // Note: this problem appears to be less of an issue in Java.  The amount of error
                    // between Law-of-Cosine and Haversine formulas appears small even when calculating
                    // distance aven as low as 1.5 meters.
                    double dlon = lon2 - lon1;
                    rad = Math.acos((Math.sin(lat1) * Math.sin(lat2)) + (Math.cos(lat1) * Math.cos(lat2) * Math.cos(dlon)));
                }

                return rad;

            } catch (Throwable t) { // trap any Math error

                return Double.NaN;

            }
        }
    }

    /**
    *** Returns the distance to the specified point, in kilometers
    *** @param gp  The destination point
    *** @return The distance to the specified point, in kilometers
    **/
    public double kilometersToPoint(GeoPoint gp)
    {
        double radians = this.radiansToPoint(gp);
        return !Double.isNaN(radians)? (EARTH_MEAN_RADIUS_KM * radians) : Double.NaN;
    }

    /**
    *** Returns the distance to the specified point, in meters
    *** @param gp  The destination point
    *** @return The distance to the specified point, in meters
    **/
    public double metersToPoint(GeoPoint gp)
    {
        double radians = this.radiansToPoint(gp);