RTN转换（自用）

将J2000惯性系下的坐标转换到RTN卫星坐标系下

public class RTN {
    //RTN转换矩阵
    static double[][] rtnRot(final double[] p, final double[] v) {
        double[][] r = new double[3][3];
        r[0][0] = p[0] / sqrt(pow(p[0], 2) + pow(p[1], 2) + pow(p[2], 2));
        r[0][1] = p[1] / sqrt(pow(p[0], 2) + pow(p[1], 2) + pow(p[2], 2));
        r[0][2] = p[2] / sqrt(pow(p[0], 2) + pow(p[1], 2) + pow(p[2], 2));
        r[2][0] = (p[1] * v[2] - v[1] * p[2]) / sqrt(pow((p[1] * v[2] - v[1] * p[2]), 2) + pow((p[2] * v[0] - p[0] * v[2]), 2) + pow((p[0] * v[1] - v[0] * p[1]), 2));
        r[2][1] = (p[2] * v[0] - p[0] * v[2]) / sqrt(pow((p[1] * v[2] - v[1] * p[2]), 2) + pow((p[2] * v[0] - p[0] * v[2]), 2) + pow((p[0] * v[1] - v[0] * p[1]), 2));
        r[2][2] = (p[0] * v[1] - v[0] * p[1]) / sqrt(pow((p[1] * v[2] - v[1] * p[2]), 2) + pow((p[2] * v[0] - p[0] * v[2]), 2) + pow((p[0] * v[1] - v[0] * p[1]), 2));
        r[1][0] = -1.0 * (r[2][2] * r[0][1] - r[0][2] * r[2][1]);
        r[1][1] = -1.0 * (r[2][0] * r[0][2] - r[0][0] * r[2][2]);
        r[1][2] = -1.0 * (r[2][1] * r[0][0] - r[0][1] * r[2][0]);
        return r;
    }
    //XYZ转RTN
    static double[] rtn(final double[][] r, final double[] dp){
        double[] RTN = new double[3];
        RTN[0] = r[0][0]*dp[0]+r[0][1]*dp[1]+r[0][2]*dp[2];
        RTN[1] = r[1][0]*dp[0]+r[1][1]*dp[1]+r[1][2]*dp[2];
        RTN[2] = r[2][0]*dp[0]+r[2][1]*dp[1]+r[2][2]*dp[2];
        return RTN;
    }
    //坐标差
    static double[] dis(final double x1, final double y1,final double z1,final double x2, final double y2,final double z2){
        double[] dp = new double[3];
        dp[0] = x1-x2;
        dp[1] = y1-y2;
        dp[2] = z1-z2;
        return dp;
    }

}