I needed to do a ecef (earth centered earth fixed) to lat lon conversion in delphi. Following the formulaes found in Wikipedia the following procedure was made:
procedure ecef2latlon(X,Y,Z : extended; var lat,lon,h : real);
// returns lat with 6 digs accuracy, eg approx 0.1 m uncertainty.
// Lon as accurate as the intrisic arctan2() function
var
r, e2, f, g,c,s,p,q,r0,u,v,z0,phi,lambda : extended;
const
// These constants are for wgs84
a=6378137.0;
b=6356752.3142;
ecc2=6.69437999014E-3; //
ecc22=6.73949674228E-3;
begin
r:=sqrt(x*x+y*y);
e2:=a*a-b*b;
f:=54.0*b*b*Z*Z;
g:=r*r+(1-ecc2)*Z*Z+ecc2*e2;
c:=ecc2*ecc2*f*r*r/(g*g*g);
s:=power((1+c+sqrt(c*(c+2))),1/3);
p:=f/(3*power((s+1/s+1),2)*g*g);
q:=sqrt(1+2*ecc2*ecc2*p);
r0:=-(p*ecc2*r)/1+q+sqrt(0.5*a*a*(1+1/q)-p*(1-ecc2)*z*z/(q*(1+q))-0.5*p*r*r);
u:=sqrt(power((r-ecc2*r0),2)+z*z);
v:=sqrt(power((r-ecc2*r0),2)+(1-ecc2)*z*z);
z0:=b*b*z/(a*v);
h:=u*(1-b*b/(a*v));
phi:=arctan((z+ecc22*z0)/r);
lambda:=arctan2(y,x);
lat:=phi/pi*180;
lon:=lambda/pi*180;
end;
To check the result, I ported the code to bc:
define cbrt(x) {
return(e(l(x)/3)) }
define sgn(x) {
if (x == 0) {
return(0);
} else if (x < 0) {
return(-1);
} else if (x > 0) {
return(1);
}
}
define abs(x) {
if (x < 0) {
return(-1 * x);
} else {
return(x);
}
}
define atan2(y, x) {
auto pi, fi;
pi = 4 * a(1);
if (y == 0) {
if (x > 0) {
return(0);
} else if (x == 0) {
print "undefined\n";
halt;
} else if (x < 0) {
return(pi);
}
}
fi = a(abs(y/x));
if (x > 0) {
return(fi * sgn(y));
} else if (x == 0) {
return(pi * sgn(y) / 2);
} else if (x < 0) {
return((pi - fi) * sgn(y));
}
}
pi=a(1)*4
x=3147484.89
y=589403.67
z=5497533.82
a=6378137.00000000000000000;
b=6356752.31420000000000000000;
ecc2=6.694379990140000000000000E-3;
ecc22=6.73949674228000000000000E-3;
r=sqrt(x*x+y*y);
e2=a*a-b*b;
f=54.0*b*b*z*z;
g=r^2+(1-ecc2)*z^2+ecc2*e2;
c=ecc2*ecc2*f*r*r/(g*g*g);
s=cbrt(1+c+sqrt(c*(c+2)));
p=f/(3*(s+1/s+1)^2*g*g);
q=sqrt(1+2*ecc2*ecc2*p);
r0=-(p*ecc2*r)/1+q+sqrt(0.5*a*a*(1+1/q)-p*(1-ecc2)*z*z/(q*(1+q))-0.5*p*r*r);
u=sqrt((r-ecc2*r0)^2+z*z);
v=sqrt((r-ecc2*r0)^2+(1-ecc2)*z*z);
z0=b*b*z/(a*v);
h=u*(1-b*b/(a*v));
phi=a((z+ecc22*z0)/r);
lambda=atan2(y,x);
lat=phi/pi*180;
lon=lambda/pi*180;
lat
lon
Saving this as a file, eg ecef.bc and piping it through bc (i.e. bc -l < ecef.bc) should yield the result
59.94721465915728162420 10.60647008394438896040
Using the above set x,y and z values in delphi should of cource yield the same result, give or take rounding errors.
Delphi (v7) and bc gives the same result to the 7th decimal place for latitude (59.9472147) that is approx mm presicion, which should be sufficient in most cases. A commersial, closed source, although mathlab based system I am using agrees with those two to the 5th decimal, eg 10cm presicion. As it is not possible to see what kind of data types are used in the commersial application, I would put the highest trust on bc. Although the code only has been tested in delphi, it is written in pretty general pascal and should work just as well for most pascal compilers.
In php, I use the following function:
function ecef2lla($x,$y,$z){
// WGS84- parameters:
$a = 6378137;
$e = 8.1819190842622e-2;
$b = sqrt(pow($a,2)*(1-pow($e,2))); // ok 2517.0298622171
$ep = sqrt(($a*$a-$b*$b)/pow($b,2)); // ok 2533.9931794567
$p = sqrt($x*$x+$y*$y); // ok
$th = atan2($a*$z,$b*$p);
$lon = atan2($y,$x); // ok
$lat = atan2(($z+($ep*$ep*$b*pow(sin($th),3))),($p-($e*$e*$a*pow(cos($th),3))));
$N = $a/sqrt(1-$e*$e*pow(sin($lat),2));
$alt = $p/cos($lat)-$N;
# return lon in range [0,2*pi)
//$lon = $lon%(2*pi());
$lat=$lat/pi()*180;
$lon=$lon/pi()*180;
$ret['lat']=$lat;
$ret['lon']=$lon;
$ret['alt']=$alt;
return $ret;
}
This yields:
lat=59.947216007022
lon=10.606470083944
I.e. about meter presicion – maybe good enough, maybe not.