java 经纬度与xyz互转

@Data
public class Place {
    private double X;
    private double Y;
    private double Z;
}



public class CoordinateConvertUtil {
    
    /**
     * 经纬度转地固坐标
     *
     * @param place
     * @return
     */
    private static Place llhToXyz(Place place){
        double x = place.getX();
        double y = place.getY();
        double z = place.getZ();
    
        double d2r = Math.PI / 180;
    
        double a = 6378137.0;		//椭球长半轴
        double f = 298.257223563;			//扁率倒数
        double b = a - a / f;
        //double b = 6356752.314245;			//椭球短半轴
        double e = Math.sqrt(a * a - b * b) / a;
    
        double L = x * d2r;
        double B = y * d2r;
        double H = z;
    
        double N = a / Math.sqrt(1 - e * e * Math.sin(B) * Math.sin(B));
        x = (N + H) * Math.cos(B) * Math.cos(L);
        y = (N + H) * Math.cos(B) * Math.sin(L);
        z = (N * (1 - e * e) + H) * Math.sin(B);
        place.setX(x);
        place.setY(y);
        place.setZ(z);
        return place;
    }
    
    /**
     * 地固坐标转经纬度
     *
     * @param place
     * @return
     */
    private static Place xyzToLlh(Place place) {
        double x = place.getX();
        double y = place.getY();
        double z = place.getZ();
    
        double epsilon = 0.000000000000001;
        double d2r = Math.PI / 180;
        double r2d = 180 / Math.PI;
    
        double a = 6378137.0;		//椭球长半轴
        double f = 298.257223563;			//扁率倒数
        double b = a - a / f;
        //double b = 6356752.314245;			//椭球短半轴
        double e = Math.sqrt(a * a - b * b) / a;
    
        double curB = 0;
        double N = 0;
        double calB = Math.atan2(z, Math.sqrt(x * x + y * y));
    
        int counter = 0;
        while (Math.abs(curB - calB) * r2d > epsilon  && counter < 25)
        {
            curB = calB;
            N = a / Math.sqrt(1 - e * e * Math.sin(curB) * Math.sin(curB));
            calB = Math.atan2(z + N * e * e * Math.sin(curB), Math.sqrt(x * x + y * y));
            counter++;
        }
    
        x = Math.atan2(y, x) * r2d;
        y = curB * r2d;
        z = z / Math.sin(curB) - N * (1 - e * e);
        place.setX(x);
        place.setY(y);
        place.setZ(z);
        return place;
    }
}


public class ConvertTest{
	public static void main(String[] args) {
        Place place = new Place(113.6, 38.8, 100);
        System.out.println("原始数据：" + place.toString());
        Place llhToXyz = CoordinateConvertUtil.llhToXyz(place);
        System.out.println("地固坐标：" + place.toString());
        Place xyzToLlh = CoordinateConvertUtil.xyzToLlh(llhToXyz);
        System.out.println("经纬度坐标： " + xyzToLlh.toString());
    }
}