nedframe.java

openmap java写的开源数字地图程序. 用applet实现,可以像google map 那样放大缩小地图.

//**********************************************************************
//
//<copyright>
//
//BBN Technologies
//10 Moulton Street
//Cambridge, MA 02138
//[phone]
//
//Copyright (C) BBNT Solutions LLC. All rights reserved.
//
//</copyright>
//**********************************************************************
//
//$Source: /cvs/distapps/openmap/src/openmap/com/bbn/openmap/proj/coords/NedFrame.java,v $
//$RCSfile: NedFrame.java,v $
//$Revision: 1.3.2.1 $
//$Date: 2004/10/14 18:27:39 $
//$Author: dietrick $
//
//***********************************************************

package com.bbn.openmap.proj.coords;

import com.bbn.openmap.LatLonPoint;
import com.bbn.openmap.proj.ProjMath;

/**
 * Encapsulates the NED (North-East-Down) coordinate system. This is
 * also know as local tangent plane (LTP).
 * <p>
 * Author: Robert Hayes
 * <p>
 */
public class NedFrame {

    /**
     * The x component of the coordinate (NORTH).
     */
    protected float x;
    /**
     * The y component of the coordinate (EAST).
     */
    protected float y;
    /**
     * The z component of the coordinate (DOWN).
     */
    protected float z;

    /**
     * Construct a default NedFrame. x,y,z are all set to zero.
     */
    public NedFrame() {
        x = 0.0f;
        y = 0.0f;
        z = 0.0f;
    }

    /**
     * Construct a NedFrame from ECEF vector and latitude and
     * longitude.
     * 
     * @param x ecef x.
     * @param y ecef y.
     * @param z ecef z.
     * @param lat = latitude in degrees.
     * @param lon = longitude in degrees.
     */
    public NedFrame(float x, float y, float z, double lat, double lon) {
        // All calculations are done using radians!

        double ecef[] = new double[3];
        double ned[] = new double[3];

        double latitude = ProjMath.degToRad(lat);
        double longitude = ProjMath.degToRad(lon);

        ecef[0] = x;
        ecef[1] = y;
        ecef[2] = z;

        ecef2ned(ned, latitude, longitude, ecef);

        this.x = (float) ned[0];
        this.y = (float) ned[1];
        this.z = (float) ned[2];
    }

    /**
     * Construct a NedFrame from a ECEF vector and a LatLonPoint.
     * 
     * @param ecefVector
     */
    public NedFrame(double[] ecefVector, LatLonPoint llpt) {
        // All calculations are done using radians!

        double ecef[] = new double[3];
        double ned[] = new double[3];
        double lat_ = (double) llpt.getLatitude();
        double lon_ = (double) llpt.getLongitude();
        double latitude = ProjMath.degToRad(lat_);
        double longitude = ProjMath.degToRad(lon_);

        ecef[0] = ecefVector[0];
        ecef[1] = ecefVector[1];
        ecef[2] = ecefVector[2];

        ecef2ned(ned, latitude, longitude, ecef);

        this.x = (float) ned[0];
        this.y = (float) ned[1];
        this.z = (float) ned[2];

    }

    /**
     * Convert to a geocentric frame using a LatLonPoint.
     * 
     * @param llpt
     * @return a vector of ecef values
     */
    public double[] toGeocentricFrame(LatLonPoint llpt) {
        // All calculations are done using radians!
        double ecef[] = new double[3];
        double ned[] = new double[3];
        double lat_ = (double) llpt.getLatitude();
        double lon_ = (double) llpt.getLongitude();
        double latitude = ProjMath.degToRad(lat_);
        double longitude = ProjMath.degToRad(lon_);

        ned2ecef(ned, latitude, longitude, ecef);

        return ecef;
    }

    /**
     * Internal conversion routine.
     * 
     * @param ned vector
     * @param latitude in radians.
     * @param longitude in radians
     * @param ecef vector
     */
    public void ecef2ned(double ned[], double latitude, double longitude,
                         double ecef[]) {

        double temp[][] = new double[3][3];

        double clat = Math.cos(latitude);
        double clon = Math.cos(longitude);
        double slat = Math.sin(latitude);
        double slon = Math.sin(longitude);

        temp[0][0] = -slat * clon;
        temp[0][1] = -slat * slon;
        temp[0][2] = clat;

        temp[1][0] = -slon;
        temp[1][1] = clon;
        temp[1][2] = 0.0;

        temp[2][0] = -clat * clon;
        temp[2][1] = -clat * slon;
        temp[2][2] = -slat;

        for (int j = 0; j < 3; ++j) {
            ned[j] = 0.0;
            for (int i = 0; i < 3; i++)
                ned[j] += temp[j][i] * ecef[i];
        }

    }

    /**
     * Internal conversion routine.
     * 
     * @param ned vector
     * @param latitude in radians.
     * @param longitude in radians
     * @param ecef vector
     */
    public void ned2ecef(double ned[], double latitude, double longitude,
                         double ecef[]) {

        double temp[][] = new double[3][3];

        double clat = Math.cos(latitude);
        double clon = Math.cos(longitude);
        double slat = Math.sin(latitude);
        double slon = Math.sin(longitude);

        ned[0] = this.x;
        ned[1] = this.y;
        ned[2] = this.z;

        temp[0][0] = -slat * clon;
        temp[1][0] = -slat * slon;
        temp[2][0] = clat;

        temp[0][1] = -slon;
        temp[1][1] = clon;
        temp[2][1] = 0.0;

        temp[0][2] = -clat * clon;
        temp[1][2] = -clat * slon;
        temp[2][2] = -slat;

        for (int j = 0; j < 3; ++j) {
            ecef[j] = 0.0;
            for (int i = 0; i < 3; i++)
                ecef[j] += temp[j][i] * ned[i];
        }
    }

    /**
     * Copy construct a NedFrame.
     * 
     * @param nedpt NedFrame
     */
    public NedFrame(NedFrame nedpt) {
        x = nedpt.x;
        y = nedpt.y;
        z = nedpt.z;
    }

    /**
     * 
     * @return String representation of NED vector.
     */
    public String toString() {
        return "NedFrame[N=" + x + ",E=" + y + ",D=" + z + "]";
    }

    /**
     * Set x.
     * 
     * @param pX in meters.
     */
    public void setX(float pX) {
        x = pX;
    }

    /**
     * Set y.
     * 
     * @param pY in meters.
     */
    public void setY(float pY) {
        y = pY;
    }

    /**
     * Set z.
     * 
     * @param pZ in meters.
     */
    public void setZ(float pZ) {
        z = pZ;
    }

    /**
     * Set x,y,z.
     * 
     * @param v1 North.
     * @param v2 East.
     * @param v3 Down.
     */
    public void setXYZ(float v1, float v2, float v3) {
        x = v1;
        y = v2;
        z = v3;
    }

    /**
     * Get x.
     * 
     * @return float x.
     */
    public float getX() {
        return x;
    }

    /**
     * Get Y.
     * 
     * @return float y.
     */
    public float getY() {
        return y;
    }

    /**
     * Get Z.
     * 
     * @return float z.
     */
    public float getZ() {
        return z;
    }

    /**
     * Get speed.
     * 
     * @return double speed.
     */
    public double toSpeed() {
        return Math.sqrt(x * x + y * y);
    }

    /**
     * Get heading.
     * 
     * @return double heading in degrees.
     */
    public double toHeading() {
        double radians = Math.atan2(y, x);
        double degrees = Math.toDegrees(radians);
        if (degrees < 0) {
            degrees += degrees + 360;
        }
        return degrees;
    }

    /*
     * public final static void main (String[] args) { // TEST1 double
     * temp[] = new double[3]; temp[0] = 13; temp[1] = 5; temp[2] = 8;
     * 
     * LatLonPoint llpt = new LatLonPoint(40.00,-74.500);
     * System.out.println("llpt" + llpt); System.out.println("ecef:" +
     * temp[0] + " : " + temp[1] + " : " + temp[2]);
     * 
     * NedFrame nedv = new NedFrame(temp,llpt);
     * System.out.println("ned" + nedv);
     * 
     * temp = nedv.toGeocentricFrame(llpt); System.out.println("ecef:" +
     * temp[0] + " : " + temp[1] + " : " + temp[2]); }
     */

}//class