Version 0.3 implements LatLong-to-distance calculations.
Spherical calculations use Haversine.
Ellipsoidal calculations use Vincenty.
Code:
// GEODESY © 2016 StephenG1CMZ
LOCAL VER:="0.3";
LOCAL NL:=CHAR(10);
LOCAL SP:=" ";
G_Use(); //FORWARD
G_RadiusB(); //FORWARD
G_MeanRadius(); //FORWARD
//We assume RA≥RB also Trig in Radians
//Geoid Data
EXPORT WGS84a_m:=6378137.0;
EXPORT WGS84a_km:=WGS84a_m/1000;
LOCAL WGS84fINV:=298.257223563;
EXPORT WGS84f:=1/WGS84fINV;
LOCAL WGS84b_m:=G_RadiusB(WGS84a_m,WGS84f);
LOCAL WGS84b_km:=WGS84b_m/1000;
LOCAL RA:=WGS84a_m; //RADIUS EQ
LOCAL FLATTEN:=WGS84f;
LOCAL RB:=G_RadiusB(RA,FLATTEN);
LOCAL RR:=G_MeanRadius(RA,RB);
LOCAL EPSILON_V:=3ᴇ−11;
LOCAL LoopSafe:=140;
//FORMAT:
//RA RB F
EXPORT G_XGNAME:=1;
EXPORT G_XRA:=2;
EXPORT G_XF:=4;
EXPORT G_EarthABF:={
{"WGS84_m", WGS84a_m,0,WGS84f},
{"WGS84_km", WGS84a_m/1000,0,WGS84f},
{"Bessel_m", 6377397.155,0,1/299.1528128},
{"International_m",6378388.000,0,1/297},
{"Sphere", 1,0,0}};
EXPORT G_ThisEll:=G_EarthABF(1);
EXPORT G_XLAT:=1;
EXPORT G_XLON:=2;
LOCAL ESS_UNITS:="m";
EXPORT G_About()
BEGIN
PRINT();
PRINT("Geodesy API V 0.3 © 2016 StephenG1CMZ");
PRINT("NOT FOR NAVIGATIONAL USE");
PRINT("No liability accepted");
PRINT("Expect the ordering of Vincenty results to change in later versions");
END;
// STD MATHS
D2R(DEGS)
BEGIN
RETURN DEGS*(π/180);
END;
R2D(RADS)
BEGIN
RETURN RADS*(180/π);
END;
ATAN2YX(YY,XX)
BEGIN //MY ATAN2
IF XX==0 AND YY==0 THEN
RETURN 0; //CONVENIENT;
END;
//-π..π THE SPECIAL CHAR BELOW IS i=sqrt(−1), NOT CHINESE
RETURN ARG(XX+*YY);//BUILT-IN HANDLES MOST. END;
END;
HAV(THETA)
//TRIG: HAVERSINE=SIN^2(THETA/2)
BEGIN
LOCAL SN:=SIN(THETA/2);
RETURN SN^2;
END;
// END STD MATHS
EXPORT G_RadiusB(RadiusA,Flatten)
//RETURN POLAR RADIUS
BEGIN
RETURN (1-Flatten)*RadiusA;//RADIUS POLAR
END;
EXPORT G_Use(ST)
//A RADIUS EQU
//B RADIUS POL
//AT PRESENT RB IS ALWAYS DERIVED FROM F
//LATER ALLOW F TO BE DERIVED
BEGIN
LOCAL II,POS;
FOR II FROM 1 TO SIZE(G_EarthABF) DO
POS:=INSTRING(G_EarthABF(G_XGNAME),ST);
IF POS THEN
G_ThisEll:=G_EarthABF;
RA:=G_ThisEll(G_XRA);
FLATTEN:=G_ThisEll(G_XF);
RB:=G_RadiusB(RA,FLATTEN);
RETURN POS;
END;
END;
RETURN POS;//0=>UNCHANGED
END;
EXPORT G_DxVincentyDo(RA,RB,FLATTEN,Lat1,Lon1,Lat2,Lon2)
// GEODESY INVERSE PROBLEM ELLIPSOIDAL
// USE: RA AND FLATTEN FROM ThisEll
// RETURNS DIST IN SAME UNITS AS R
// LAT AND LON IN DEGREES
BEGIN
LOCAL NUM,DEN;
LOCAL LAT1:=HMS→(Lat1);
LOCAL LAT2:=HMS→(Lat2);
LOCAL LON1:=HMS→(Lon1);
LOCAL LON2:=HMS→(Lon2);
LOCAL UU1,SINUU1,COSUU1; //U1 AUX LATITUDE
LOCAL UU2,SINUU2,COSUU2; //U2 AUX LATITUDE
LOCAL USQ;
LOCAL AAA,BBB;
LOCAL LONDIFFradians; //L=DIFF IN LON
LOCAL PHI1,PHI2;
LOCAL LAMBDA,SINLAMBDA,COSLAMBDA,LAMBDAOLD;
LOCAL SIGMA,SIN_SIGMA,COS_SIGMA,DELTA_SIGMA,SINSQ_SIGMA; //AUX ARC LENGTH
LOCAL SIN_ALPHA,COSSQ_ALPHA;
LOCAL COS_2SIGMAM,COSSQ_2SIGMAM;
LOCAL CC;
LOCAL BRACKET_TERM;
LOCAL LOOPCOUNT;
LOCAL DISTS;
LOCAL AZ_FWD,AZ_FNL,AZ_REV;
// CALC L
LONDIFFradians:=D2R(LON2-LON1); //DIFF IN LON
// CALC U1 U2
PHI1:=D2R(LAT1);
PHI2:=D2R(LAT2);
UU1:=ATAN((1-FLATTEN)*TAN(PHI1));//CORRECT
UU2:=ATAN((1-FLATTEN)*TAN(PHI2));//CORRECT
//TO CALC LAMBDA:
// PRE-CALC U1 TRIG ONCEONLY
SINUU1:=SIN(UU1); COSUU1:=COS(UU1);
// PRE-CALC U2 TRIG EACH POSITION
SINUU2:=SIN(UU2); COSUU2:=COS(UU2);
//ITERATE CALCULATING LAMBDA
LOOPCOUNT:=0;
LAMBDA:=LONDIFFradians; //V13: DIFF IN LON ON AUX. FIRST APPROX
REPEAT
LOOPCOUNT:=LOOPCOUNT+1;
SINLAMBDA:=SIN(LAMBDA); COSLAMBDA:=COS(LAMBDA); //PRECALC
//V14
BRACKET_TERM:=(COSUU1*SINUU2 - SINUU1*COSUU2*COSLAMBDA);
SINSQ_SIGMA:=(COSUU2*SINLAMBDA)^2 + (BRACKET_TERM)^2;
IF SINSQ_SIGMA==0 THEN
//PRINT("SINσ 0: NO DISTANCE");
//0 DISTANCE MIGHT SUGGEST UNDERFLOW
RETURN {1,0,0,0,0,0};//VALID ZERO
END;
SIN_SIGMA:=√(SINSQ_SIGMA);
//V15
COS_SIGMA:=SINUU1*SINUU2 + COSUU1*COSUU2*COSLAMBDA;
//V16
SIGMA:=ATAN2YX(SIN_SIGMA,COS_SIGMA);
//V17
SIN_ALPHA:=(COSUU1*COSUU2*SINLAMBDA)/SIN_SIGMA;
COSSQ_ALPHA:=1-(SIN_ALPHA*SIN_ALPHA);
//V18
IF COSSQ_ALPHA==0 THEN
//PRINT("AVOID COSSQα 0");
COS_2SIGMAM:=0;
ELSE
COS_2SIGMAM:=COS_SIGMA - (2*SINUU1*SINUU2/COSSQ_ALPHA);
END;
CC:=(FLATTEN/16)*COSSQ_ALPHA*(4 + FLATTEN*(4 - 3*COSSQ_ALPHA));
LAMBDAOLD:=LAMBDA;
LAMBDA:=LONDIFFradians + (1-CC)*FLATTEN*SIN_ALPHA*(SIGMA + CC*SIN_SIGMA*(COS_2SIGMAM + CC*COS_SIGMA*(−1+2*(COS_2SIGMAM*COS_2SIGMAM))));
//PRINT(LAMBDA);
UNTIL (ABS(LAMBDA-LAMBDAOLD)≤EPSILON_V) OR (LOOPCOUNT≥LoopSafe);
//V19
USQ:=COSSQ_ALPHA*((RA^2 - RB^2)/(RB^2));
//(V3)
AAA:=1 + (USQ/16384)*(4096 + USQ*(−768 + USQ*(320 - 175*USQ)));
//(V4)
BBB:= (USQ/ 1024)*( 256 + USQ*(−128 + USQ*( 74 - 47*USQ)));
//(V6)
COSSQ_2SIGMAM:=(COS_2SIGMAM)^2;
DELTA_SIGMA:=(1/6)*BBB*COS_2SIGMAM*(−3+4*(SIN_ALPHA)^2)*(−3+4*COSSQ_2SIGMAM) ;
DELTA_SIGMA:=(1/4)*BBB*(COS_SIGMA*(−1+2*COSSQ_2SIGMAM)) - DELTA_SIGMA ;
DELTA_SIGMA:=BBB*SIN_SIGMA*(COS_2SIGMAM + DELTA_SIGMA);
//PRINT("Δσ "+DELTA_SIGMA);
DISTS:=AAA*(SIGMA - DELTA_SIGMA);//ANGULAR ≤π Radians
DISTS:=RB*DISTS; //same units as radius
//V20:FORWARD AZIMUTH
//CHECKED SINLAMBDA:ORIGINAL (LON2-LON1) OR LOOPED VALUE? USE LOOPED
AZ_FWD:=ATAN2YX((COSUU2*SINLAMBDA),BRACKET_TERM);
//V21:FINAL AND REVERSE AZIMUTH
NUM:=(COSUU1*SINLAMBDA);
DEN:=(−SINUU1*COSUU2 + COSUU1*SINUU2*COSLAMBDA);
AZ_FNL:=ATAN2YX(NUM,DEN);
AZ_REV:=ATAN2YX(−NUM,−DEN);
//RETURN RESULTS
DISTS:=ROUND(DISTS,3);//VINCENTY CLAIMS mm,
AZ_FWD:=R2D(AZ_FWD);
AZ_FWD:=IFTE(AZ_FWD<0,((AZ_FWD+360) MOD 360),AZ_FWD);//NO NEGATIVE BEARINGS
AZ_FNL:=R2D(AZ_FNL);
AZ_FNL:=IFTE(AZ_FNL<0,((AZ_FNL+360) MOD 360),AZ_FNL);//NO NEGATIVE BEARINGS
AZ_REV:=R2D(AZ_REV);
AZ_REV:=IFTE(AZ_REV<0,((AZ_REV+360) MOD 360),AZ_REV);
// VALIDITY: CONVERGENCE FAILURE
// DISTS: SAME UNITS AS RADIUS
// AZIMUTHS: POSITIVE 0..360
// LOOPCOUNT: CONVERGENCE INFO (LOWER BETTER):DIAGNOSTIC ONLY
// EXPECT SEQUENCE ORDER TO CHANGE
// ESPECIALLY AZIMUTHS
RETURN {(LOOPCOUNT≠LoopSafe),DISTS,(AZ_FWD),(AZ_FNL),(AZ_REV),LOOPCOUNT};
END;
EXPORT G_DxVincenty(Elevation,Point1,Point2)
// GEODESY INVERSE PROBLEM ELLIPSOIDAL
// USE: RA AND FLATTEN FROM ThisEll TO DERIVE RB
// RETURNS DIST IN SAME UNITS AS R
// LAT AND LON IN DEGREES
BEGIN
LOCAL LAT1:=Point1(G_XLAT);
LOCAL LON1:=Point1(G_XLON);
LOCAL LAT2:=Point2(G_XLAT);
LOCAL LON2:=Point2(G_XLON);
LOCAL RA:=G_ThisEll(G_XRA)+Elevation;
LOCAL FLATTEN:=G_ThisEll(G_XF);
LOCAL RB:=G_RadiusB(RA,FLATTEN);
RETURN G_DxVincentyDo(RA,RB,FLATTEN,LAT1,LON1,LAT2,LON2);
END;
// Main Spherical Geodesy routines-
EXPORT G_MeanRadius (RA,RB)
//In Geodesy, this is usually used as a radius for spherical
BEGIN
RETURN (2*RA+RB)/3;
END;
EXPORT G_DistHav(Point1,Point2)
BEGIN
LOCAL LAT1:=HMS→(Point1(G_XLAT));
LOCAL LAT2:=HMS→(Point2(G_XLAT));
LOCAL LON1:=HMS→(Point1(G_XLON));
LOCAL LON2:=HMS→(Point2(G_XLON));
LOCAL DLON:=D2R(LON2-LON1);
LOCAL DLAT:=D2R(LAT2-LAT1);
LOCAL AA:=HAV(DLAT) + (COS(D2R(LAT1))*COS(D2R(LAT2))*HAV(DLON));
LOCAL CC:=2*ASIN(MIN(1,√(AA)));//MIN 1 AVOIDS ROUNDING OUTRANGE
LOCAL DD:=RR*CC;
RETURN ROUND(DD,−6);
END;
// End Main Spherical Routines
G_EllStats(PR)
//PR:PRINT ELSE TEXTOUT
BEGIN
LOCAL II;
LOCAL ST:={};
ST(1):=G_ThisEll(G_XGNAME);
ST(2):="RA: "+RA;
ST(3):="RB: "+RB;
ST(4):="f: "+(FLATTEN);
IF FLATTEN≠0 THEN
ST(5):="1/f: "+(1/FLATTEN);
END;
IF PR THEN
FOR II FROM 1 TO SIZE(ST) DO
PRINT(ST(II));
END;
ELSE
FOR II FROM 1 TO SIZE(ST) DO
TEXTOUT_P(ST(II),0,20*II+100);
END;
END;
END;
G_EX(LAT1,LON1,LAT2,LON2)
//An example displaying results
BEGIN
LOCAL TM,RESULT1,RESULT2,DISTS;
PRINT(" ");
PRINT("From LAT "+HMS→(LAT1)+" "+→HMS(LAT1));
PRINT("From LON "+HMS→(LON1)+" "+→HMS(LON1));
PRINT("To LAT "+HMS→(LAT2)+" "+→HMS(LAT2));
PRINT("To LON "+HMS→(LON2)+" "+→HMS(LON2));
TM:=TICKS;
RESULT1:=G_DistHav({LAT1,LON1},{LAT2,LON2});
RESULT2:=G_DxVincenty(0,{LAT1,LON1},{LAT2,LON2});
TM:=TICKS-TM;
PRINT("Elapsed "+(TM/1000)+" s");
PRINT("Spherical Haversine: "+(RESULT1)+" m");
PRINT("Ellipsoidal Vincenty: "+(RESULT2(2))+" m");
PRINT(" AZ "+(→HMS(RESULT2(3))));
PRINT(" AZ "+(→HMS(RESULT2(4))));
PRINT(" AZ "+(→HMS(RESULT2(5))));
PRINT("Validity: " +(RESULT2(1))+" "+(RESULT2(6)));
//PRINT(RESULT2);
//RESULT:=Z_GEODESY(LAT1,LON1,LAT2,LON2);
END;
EXPORT G_NewEll(LST)
BEGIN
G_EarthABF(0):=LST;
RETURN 1;
END;
EXPORT ExampleUse()
//A short example of key procedures
BEGIN
LOCAL OKC,DIST,RESULT,EXAMPLEPOINT,XLAT,XLON;
OKC:=G_NewEll({"Add your own",{RA,0,FLATTEN}});//Insert into List. Use use to use it.
OKC:=G_Use("WGS84_m");//DEFAULT
IF OKC THEN //Found in list
DIST:=G_DistHav({1,2},{3,4});
//RESULT:=G_DxVincentyDo(RA,RB,FLATTEN,LAT1,LON1,LAT2,LON2);
RESULT:=G_DxVincenty(0,{1,2},{3,4});
//Variables prefixed X index into data structures
EXAMPLEPOINT:={"From Here",1,2};
XLAT:=2; XLON:=3;
XLAT:=1; XLON:=2; //REINSTATE CURRENT DEFAULT
EXAMPLEPOINT:={1,2};
//and similarly for geoid structure
END;
END;
EXPORT G_DrawEll(Flatten)
//The stats are omitted because flattening drawn may differ
//makes illustrate changing f easier
//for now use myRadiusA of 100 pixels
BEGIN
LOCAL XX,YY;
LOCAL DX,DY;
LOCAL MyRadiusA:=100;
LOCAL HEIGHT:=100;
LOCAL WIDTH:=100;
//LOCAL SCALE:=RadiusA/WIDTH;
LOCAL EWIDTH:=MyRadiusA;
LOCAL EHEIGHT:=((1-Flatten)*MyRadiusA);//RADIUS POLAR
RECT();
//G_EllStats(0);
FOR YY FROM −HEIGHT TO HEIGHT DO
//PRINT(YY);
FOR XX FROM −WIDTH TO WIDTH DO
DX:=XX/(EWIDTH);
DY:=YY/(EHEIGHT);
IF (DX^2+DY^2)≤1 THEN
//PRINT({WIDTH+10+XX,HEIGHT+20+YY});
PIXON_P(WIDTH+100+XX,HEIGHT+20+YY,#009900h);
END;//IF
END;//FOR
END;//FOR
//G_EllStats(0);
WAIT;
END;
G_TEST()
//A few tests. More needed.
BEGIN
G_EX(50°03′58.76″,−005°42′53.10″,58°38′38.48″,−003°04′12.34″);//50PERCENT
PRINT("EXPECT 969 954_m");
G_EX(−37°57′3.72030″,144°25′29.52440″,−37°39′10.15610″,143°55′35.38390″);
PRINT("EXPECTED: 54972.271m");//VINCENTY AUSTRALIA
PRINT("306d52 05 * 127d10 25 * 307d10 25.07 ");
G_EX(31.8300167,35.0662833,31.83000000,35.0708167);
PRINT("PYTHON: CIRCLE 428.4 VIN 429.1677 m");
//G_EX(0,0,0.5,179.5);
G_EX(0,0,0.5,179.7);//WILL NOT CONVERGE
//G_EX(0,0,0,90);
//G_EX(0,0,0.5,0.5);
G_EX(2,2,2,2);
G_EX(50°03′58.76″,−005°42′53.10″,58°38′38.48″,−003°04′12.34″);//50PERCENT
PRINT("EXPECT 969 954_m");
//IF 0 THEN
G_EX(50°3′59″,−5°42′53″,58°38′38″,−3°4′12″);
PRINT("RECHECK: EXPECTED 968.9km 009DEG0711");
IF 0 THEN
G_EX(−37°57′3.72030″,144°25′29.52440″,−37°39′10.15610″,143°55′35.38390″);//EXPECTS 54972m
PRINT("EXPECTED: 54972.271m ");
PRINT("306d52:5.37 / 127d10:25.07");//VINCENTY AUSTRALIA
//IF 1 THEN
G_EX(31.8300167,35.0662833,31.83000000,35.0708167);
PRINT("PYTHON: CIRCLE 428.4 VIN 429.1677 m");
//IF 0 THEN
G_EX(0,0,0.5,179.5);
G_EX(0,0,0.5,179.7);//WILL NOT CONVERGE
//IF 1 THEN
G_EX(0,0,0,90);
G_EX(0,0,0.5,0.5);
G_EX(2,2,2,2);
END;
//PRINT("SEMI: "+π*ESS(1));//MAX
END;
EXPORT GEODESY()
BEGIN
G_About();
G_EllStats(1);
PRINT("Esc");WAIT;
G_TEST();
END;
Search
Member List
Calendar
Help
