private Location3D xyzToLonLat(Location3D location3D,float x,float y,float z){
double lat=location3D.getLatitude();
double lon=location3D.getLongitude();
double a = 6378137;
double b = 6356752.3142;
double PI = 3.141592653589793238;
double f = 1 / 298.2572236;
float dis= (float) Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2));
float dist= (float) Math.sqrt(Math.pow(dis, 2) + Math.pow(z, 2));
float brng = (float) Math.toDegrees(Math.atan2(x,y));
if(brng>180){
brng=brng-360;
}
else if(brng<-180) {
brng=brng+360;
}
if(brng<0){
brng=brng+360;
}
double alpha1 = brng * PI / 180.0;
double sinAlpha1 = Math.sin(alpha1);
double cosAlpha1 = Math.cos(alpha1);
double tanU1 = (1 - f) * Math.tan( lat * PI / 180.0);
double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1));
double sinU1 = tanU1 * cosU1;
double sigma1 = Math.atan2(tanU1, cosAlpha1);
double sinAlpha = cosU1 * sinAlpha1;
double cosSqAlpha = 1 - sinAlpha * sinAlpha;
double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
double cos2SigmaM=0;
double sinSigma=0;
double cosSigma=0;
double sigma = dist / (b * A), sigmaP = 2 * Math.PI;
while (Math.abs(sigma - sigmaP) > 1e-12) {
cos2SigmaM = Math.cos(2 * sigma1 + sigma);
sinSigma = Math.sin(sigma);
cosSigma = Math.cos(sigma);
double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
- B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
sigmaP = sigma;
sigma = dist / (b * A) + deltaSigma;
}
double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
(1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
double L = lambda - (1 - C) * f * sinAlpha
* (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
LatLng get_LatLng =new LatLng( lat2 * 180.0 / Math.PI,lon+( L * 180.0 / Math.PI));
return new Location3D(get_LatLng.latitude,get_LatLng.longitude,location3D.getAltitude()+z);
}
Location3D可以自己定义 这边是引入大疆sdk的类(经纬度以及高度)
xyz分别是 前后 左右 上下