Unit UGeodesy;
// Developed by Peter Walker, used for SeaNav Program dated 20110818
interface
{Geo to Grid - Redfearn's Formula}
Procedure CalcEN(Lat,Long,Cm,a,f,ko,EFO,NFO :extended;
Var East,North,Conv1Deg,PSF: Extended);
{Grid to Geo - Redfearn's Formula}
procedure CalcLatLon(East,North,Cm,a,f,ko,EFO,NFO: extended ;
Var LatDeg,LongDeg,convDeg,PSF: extended);
{transformations coord frame}
Procedure CalcXYZ1(Lat1,Lon1,Ht1,a1,f1: extended ; var X1,Y1,Z1 : extended);
Procedure CalcXYZ2(X1,Y1,Z1,Dx,Dy,Dz,Rx,Ry,Rz,Sc: extended ;
var X2,Y2,Z2 : extended);
{transformations position vector}
Procedure CalcXYZ2pv(X1,Y1,Z1,Dx,Dy,Dz,Rx,Ry,Rz,Sc: extended ;
var X2,Y2,Z2 : extended);
Procedure CalcLatLon2(X2,Y2,Z2,a2,f2: extended; Var LatDeg2,LonDeg2,Ht2: extended);
{Molodensky}
procedure CalcDelta(Gf1,Ga1,Lat1Deg,Lon1Deg,Ht1,dx,dy,dz,da,df : extended;
var dLatDeg,dLonDeg,dHt2 : extended);
implementation
Uses Math;
{Geo to Grid}
Procedure CalcEN(Lat,Long,Cm,a,f,ko,EFO,NFO :extended;
Var East,North,Conv1Deg,PSF: extended);
var
e2,S,C,v,w,latr,vp,t,p,m,A0,A2,A4,A6,
ETerm1, ETerm2, ETerm3, ETerm4,NTerm1, NTerm2, NTerm3, NTerm4,
CTerm1, CTerm2, CTerm3, CTerm4 : extended;
begin
{Formula use Lat, Long, CM}
latr := DegToRad(lat);
e2 := 2*f-SQR(f);
v := a / Power((1-e2*sqr(sin(latr))),(1/2));
p := a * (1-e2)/power(1-e2*Sqr(sin(latR)),3/2);
w := DegToRad(Long-cm);
vp := v/p;
t := tan(Latr);
A0 := 1-e2/4-3*e2*e2/64-5*e2*e2*e2/256;
A2 := (3/8)*(e2+e2*e2/4+15*e2*e2*e2/128);
A4 := (15/256)*(e2*e2+3*e2*e2*e2/4);
A6 := 35*e2*e2*e2/3072;
m := a*(A0*latr-A2*sin(2*latR)+A4*sin(4*latR)-A6*sin(6*latR));
c := cos(latr);
s := sin(latr);
{Formula 4.5}
ETerm1 := v*w*c;
ETerm2 := v*w*w*w/6*c*c*c*(vp-t*t);
ETerm3 := v*(w*w*w*w*w/120*c*c*c*c*c*
(4*vp*vp*vp*(1-6*t*t)+vp*vp*(1+8*t*t)-vp*(2*t*t)+t*t*t*t));
ETerm4 := v*(w*w*w*w*w*w*w/5040)*c*c*c*c*c*c*c*
(61-479*t*t+179*t*t*t*t-t*t*t*t*t*t);
NTerm1 := v*s*(w*w/2)*c;
NTerm2 := v*s*(w*w*w*w/24)*c*c*c*
(4*vp*vp+vp-t*t) ;
NTerm3 := v*s*(w*w*w*w*w*w/720)*c*c*c*c*c*
(8*vp*vp*vp*vp*(11-24*t*t)-28*vp*vp*vp*(1-6*t*t)+
vp*vp*(1-32*t*t)-vp*(2*t*t)+t*t*t*t);
NTerm4 := v*s*(w*w*w*w*w*w*w*w/40320)*c*c*c*c*c*c*c*
(1385-3111*t*t+543*t*t*t*t-t*t*t*t*t*t);
East := ko * (ETERM1+ETERM2+ETERM3+ETERM4);
North := ko * (m + NTERM1+NTERM2+NTERM3+NTERM4);
East := East + EFO;
North := North + NFO;
CTerm1 := -s*w;
CTerm2 := -s*(w*w*w/3)*c*c*(2*vp*vp-vp);
CTerm3 := -s*(w*w*w*w*w/15)*c*c*c*c*
(vp*vp*vp*vp*(11-24*t*t)-vp*vp*vp*(11-36*t*t)+
2*vp*vp*(1-7*t*t)+vp*t*t);
Conv1Deg := CTerm1 + Cterm2 + CTerm3;
Conv1Deg := RadToDeg(Conv1Deg);{*180/pi;}
PSF := ko*(1+(w*w/2)*c*c*vp +
(w*w*w*w/24)*c*c*c*c*(4*vp*vp*vp*(1-6*t*t)+vp*vp*(1+24*t*t)-4*vp*t*t)+
(w*w*w*w*w*w/720)*c*c*c*c*c*c*(61-148*t*t+16*t*t*t*t));
end;
{conversions}
procedure CalcLatLon(East,North,CM,a,f,ko,EFO,NFO: extended ;
Var LatDeg,LongDeg,convDeg,PSF: extended);
var
ED, ND, m ,n, G, O, LATD, sec,e2,
PD,VD,TD,VP,x,z,LATR,w,CTERM1,CTERM2,CTERM3, ED2, KOPV, y, y2 : extended;
begin
ED := East - EFO;
ND := North - NFO;
m := ND / ko ;
n := f / (2-f);
G := a*(1-n)*(1-SQR(n))*(1+(9/4)*sqr(n)+(225/64)*sqr(n)*sqr(n))*(pi/180);
O := (m*PI)/(G*180);
LATD := O+((3*n/2)-(27*n*n*n/32))*sin(2*O)
+((21*n*n/16)-(55*n*n*n*n/32))*sin(4*O)
+(151*n*n*n/96)*sin(6*O)
+(1097*n*n*n*n/512)*sin(8*O);
LATDEG := LATD*180/pi;
sec := 1/cos(latd);
TD := sin((latD))/cos((latD));
e2 := 2*f-SQR(f);
PD := a * (1-e2)/power(1-e2*Sqr(sin(latd)),3/2);
VD := a / Power((1-e2*sqr(sin(latd))),(1/2));
vp := vd/pd;
x := ED/(ko*vd);
z := TD/(ko*PD);
LATR := LATD - z * x * ED/2
+ z *(x*x*x*ED/24)*(-4*vp*vp+9*vp*(1-TD*TD)+12*TD*TD)
- z *(x*x*x*x*x*ED/720)*(8*vp*vp*vp*vp*(11-24*td*td)-12
*vp*vp*vp*(21-71*td*td)
+15*vp*vp*(15-98*td*td+15*td*td*td*td)+180*vp*(5*td*td-3
*td*td*td*td)+360*td*td*td*td)
+ z *(x*x*x*x*x*x*x*ED/40320)*
(1385+3633*td*td+4095*td*td*td*td+1575*td*td*td*td*td*td);
LATDEG := LATR * 180 /PI;
{long}
w := sec * x
-sec*x*x*x/6*(vp+2*td*td)
+sec*x*x*x*x*x/120*(-4*vp*vp*vp*(1-6*td*td)+vp*vp*(9-68*td*td)
+72*vp*td*td+24*td*td*td*td)
-sec*x*x*x*x*x*x*x/5040*(61+662*td*td+1320*td*td*td*td+720
*td*td*td*td*td*td);
LONGDEG := w * 180 / PI + cm;
CTerm1 := -td*x;
CTerm2 := td*x*x*x/3*(-2*vp*vp+3*vp+td*td);
CTerm3 := -td*x*x*x*x*x/15*(vp*vp*vp*vp*(11-24*td*td)-3*vp*vp*vp
*(8-23*td*td)+5*vp*vp*(3-14*td*td)+30*vp*td*td+3*
td*td*td*td);
{CTerm4 := }
convDeg := CTerm1 + Cterm2 + CTerm3;
convDeg := convDeg*180/pi;
{Point Scale Factor}
y := Ed/(ko*sqrt(pd*vd));
y2 := y*y;
PSF := ko*(1+ y2/2 +
y2*y2/24*(4*vp*(1-6*td*td)-3*(1-16*td*td)-24*td*td/vp)+
y2*y2*y2/720);
end;
{Transformations} {lat1 and lon1 in degrees}
Procedure CalcXYZ1(Lat1,Lon1,Ht1,a1,f1: extended ; var X1,Y1,Z1 : extended);
var
rLat1,rLon1: extended;
e2, sLat, cLat, sLon, cLon, SinSqrLat, v : extended;
begin
rLat1 := DegToRad(Lat1);
rLon1 := DegToRad(Lon1) ;
e2 := 2*f1-Sqr(f1); {e2 = Sqr(e)} {b := a*(1-f);}
sLat := sin(rLat1);
cLat := cos(rLat1);
sLon := sin(rLon1);
cLon := cos(rLon1);
SinSqrLat := sLat * sLat;
v := a1 / sqrt(1-e2*SinSqrLat);
{ Lat Lon Ht to Cartesians Ht = N + h }
X1 := (v + Ht1) * cLat * cLon;
Y1 := (v + Ht1) * cLat * sLon;
Z1 := (v * (1-e2) + Ht1) * sLat;
end;
Procedure CalcXYZ2(X1,Y1,Z1,Dx,Dy,Dz,Rx,Ry,Rz,Sc: extended ;
var X2,Y2,Z2 : extended);
begin
Rx := DegToRad((Rx * 10e-5)/3600);
Ry := DegToRad((Ry * 10e-5)/3600);
Rz := DegToRad((Rz * 10e-5)/3600);
Sc := 1 + Sc*1e-6;
X2 := Dx + (Sc) * ((1*X1) + (Rz*Y1) + (-Ry*Z1));
Y2 := Dy + (Sc) * ((-Rz*X1) + (1*Y1) + (Rx*Z1));
Z2 := Dz + (Sc) * ((Ry*X1) + (-Rx*Y1) + (1*Z1));
end;
Procedure CalcXYZ2pv(X1,Y1,Z1,Dx,Dy,Dz,Rx,Ry,Rz,Sc: extended ;
var X2,Y2,Z2 : extended);
begin
Rx := DegToRad((Rx * 10e-5)/3600);
Ry := DegToRad((Ry * 10e-5)/3600);
Rz := DegToRad((Rz * 10e-5)/3600);
Sc := 1 + Sc*1e-6;
X2 := Dx + (Sc) * ((1*X1) + (-Rz*Y1) + (Ry*Z1));
Y2 := Dy + (Sc) * ((Rz*X1) + (1*Y1) + (-Rx*Z1));
Z2 := Dz + (Sc) * ((-Ry*X1) + (Rx*Y1) + (1*Z1));
end;
Procedure CalcLatLon2(X2,Y2,Z2,a2,f2: extended; Var LatDeg2,LonDeg2,Ht2: extended);
var
b2,e2,ed2, Lat2, Lon2,P2, Theta,sinTheta,cosTheta,
Sin3Theta, Cos3Theta,v,SinSqrLat : extended;
Begin
{Params sys 2}
b2 := a2*(1-f2);
e2 := 2*f2-Sqr(f2);
ed2 := e2 + (e2*e2) / (1-e2);
P2 := Sqrt(Sqr(X2)+Sqr(Y2)); {(X2^2 + Y2^2)^1/2}
if p2 = 0 then exit;
Theta := DegToRad(ArcTan2(( P2 * b2),(Z2*a2)));
sinTheta := Sin(Theta);
CosTheta := Cos(Theta);
Sin3Theta := sinTheta * sinTheta * sinTheta;
Cos3Theta := cosTheta * cosTheta * cosTheta;
Lat2 := ArcTan2((P2-e2*a2*Cos3Theta),(Z2+ed2*b2*Sin3Theta));
LatDeg2 := Lat2;
Lon2 := ArcTan2(X2,Y2);
LonDeg2 := Lon2;
SinSqrLat := Sin(RadToDeg(Lat2)) * Sin(RadToDeg(Lat2));
v := a2 / sqrt(1-e2*SinSqrLat);
Ht2 := p2/cos(RadToDeg(Lat2))-v;
End;
function Power1(y,x:extended):extended;
Var
negY : Boolean;
YX : extended;
begin
{ Raise y to the power of x }
negY := false;
if y < 0 then
begin
negY := true;
y := abs(y);
end;
if y = 0 then y := 1e-20;
YX := Exp(Ln(y) * x );
if NegY then YX := -(YX);
Result := YX;
end;
{Molodensky}
procedure CalcDelta(Gf1,Ga1,Lat1Deg,Lon1Deg,Ht1,dx,dy,dz,da,df : extended;
var dLatDeg,dLonDeg,dHt2 : extended);
var
Lat,Long,e2,slat,clat,slon,clon,ssqlat, adb, rn, rm, tlat, tlon,th : extended;
begin
{da := -da; df := -df;}
Lat := DegToRad(Lat1Deg);
Long := DegToRad(Lon1Deg);
e2 := 2*Gf1-sqr(Gf1);
slat := sin(Lat);
clat := cos(Lat);
slon := sin(Long);
clon := cos(Long);
ssqlat := slat * slat;
adb := 1/(1-Gf1);
rn := Ga1 / sqrt(1- e2 * ssqlat);
rm := Ga1 * (1- e2) /
power1( (1- e2 * ssqlat), 1.5 );
tlat := -dx * slat * clon - dy * slat * slon +
dz * clat;
tlat := tlat + da * (rn * e2 * slat * clat)
/ Ga1;
tlat := tlat + df * (rm * adb + rn / adb) * slat * clat;
tlat := tlat / (rm + Ht1);
tlon := (-dx * slon + dy * clon) /
((rn + Ht1) * clat);
th := dx * clat * clon + dy * clat * slon +
dz * slat;
th := th - da * Ga1 / rn + df * rn *
ssqlat / adb;
dLatDeg := RadToDeg(tlat);
dLonDeg := RadToDeg(tlon);
dHt2 := th;
end;
end.
No comments:
Post a Comment