Solved: what is wrong with this LatLong->Distance code? A Trigonometry "feature".

[StephenG1CMZ]

Don't be put off by the code - it includes three different implementations all exhibiting the same bug, so you only need to check one...Each one is just a dozen or so lines of trigonometry.

I have coded up three routines for converting a pair of LatLong points to a distance.
HAVERS implements a Haversine spherical Earth calculation
VINCENTYS implements a simplification of Vincenty's formula for a spherical Earth.
These two return practically identical results (trivial rounding in micrometres).

Z-VINCENTY is the normal ellipsoidal formula, perhaps a few km away from the other results.

All 3 exhibit the same bug: Reporting a distance of 1km for an expected distance of 54 km, for example. (The expected result here is from Geoscience Australia).

Since all three implementations are wrong, I assume their is some common error.
I have tried changing my atan2 algorithm and YY XX parameter sequence.
I have tried calculating the longitude difference as abs(LON2-LON1) whereas other sources just subtract. I am aware that VINCENTY has a few superfluous brackets compared to other online sources, but that would not cause all 3 to fail.

I have stared at the code long enough that I need a break before having another look...
Can anyone else see something I have misunderstood in all 3 implementations?
Or recommend a worked example with intermediate values that I can check my code against?
Or is it simply that this application demands more precision than the Prime has?

Code:


 LOCAL SP:=" ";
 LOCAL EPSILON_V:=3ᴇ−11;
 LOCAL LoopSafe:=140;
 LOCAL LOOPCOUNT,VALIDITY;

 LOCAL WGS84RAm:=6378137.0;
 LOCAL WGS84FLATTENINV:=298.257223563;
 LOCAL WGS84FLATTEN:=1/WGS84FLATTENINV; 

 LOCAL RA:=WGS84RAm; //RADIUS EQ
 //LOCAL RA:=6378388;//TEST 
 LOCAL FLATTEN:=WGS84FLATTEN;
 
 LOCAL RB:=IFTE(RA,(1-FLATTEN)*RA,0);//RADIUS POLAR
 LOCAL ESS:={RA,RB,FLATTEN};//DEFINE ELLIPSOID SIZE AND SHAPE
 LOCAL ESS_UNITS:="m";

 D2R(DEGS)
 BEGIN
  RETURN DEGS*(π/180); 
 END;
 R2D(RADS)
 BEGIN
  RETURN RADS*(180/π);
 END;

 ATAN2_UNUSEDYX(YY,XX) 
 BEGIN //MY ATAN2
  IF XX==0 AND YY==0 THEN
   RETURN 0; //CONVENIENT;
  END;
  //-π..π 
  RETURN ARG(XX+*YY);//BUILT-IN HANDLES MOST
 END;
 ATAN2YX(YY,XX)
 BEGIN
  LOCAL RESULT;
  IF XX>0 THEN RETURN ATAN(YY/XX); END;
  IF YY>0 THEN 
   RETURN (π/2 - ATAN(XX/YY));
  END;
  IF YY<0 THEN
    RETURN (−π/2 - ATAN(XX/YY));
  END;
  IF XX<0 THEN
   RETURN (ATAN(YY/XX) + π); //PLUSMINUS TBD
  END; 
  IF YY==0  AND XX==0 THEN RETURN (0); END;
  //NEVER REACH
  PRINT("ATAN2 FAULTY");
  
 END;

 HAV(THETA)
 //HAVERSINE
 BEGIN
  LOCAL HS:=SIN(THETA/2);
  RETURN HS^2;
 END;

 REPORT_SHAPE()
 BEGIN
  LOCAL SP:=" ";

  PRINT();
  PRINT("RA: "+ESS(1)+SP+ESS_UNITS);
  PRINT("RB: "+ESS(2)+SP+ESS_UNITS);
  PRINT("f:  "+ESS(3)); 
  IF ESS(3)≠0 THEN
   PRINT("1/f: "+1/ESS(3));
  END;
  PRINT("Esc");
  WAIT;
 END;

 EXPORT Z_VINCENTY(LAT1,LON1,LAT2,LON2)
 // GEODESY INVERSE PROBLEM ELLIPSOIDAL
 // RETURNS DIST IN SAME UNITS AS R
 // LAT AND LON IN DEGREES 
 BEGIN
  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 DISTS;
  LOCAL AZ_FWD,AZ_REV;

  LOCAL RA:=ESS(1);
  LOCAL RB:=ESS(2);
  LOCAL FLATTEN:=ESS(3);
  //REPORT_SHAPE;
  // CALC L
  LONDIFFradians:=D2R(LON2-LON1); //DIFF IN LON
  
  // CALC U1 U2
  PHI1:=D2R(LAT1); 
  PHI2:=D2R(LAT2);
  UU1:=ATAN(1-FLATTEN)*TAN(PHI1);//TBC
  UU2:=ATAN(1-FLATTEN)*TAN(PHI2);//TBC
  
  //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 
  //PRINT(); PRINT(LAMBDA);

  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};//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);
  //PRINT("LOOP DUN");
 
  //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);
 
  DISTS:=AAA*(SIGMA - DELTA_SIGMA);
  //PRINT("ANGULAR FRACTIONAL DIST: "+DISTS/π);
  DISTS:=RB*DISTS;
  
  //V20:FORWARD AZIMUTH
  AZ_FWD:=ATAN2YX((COSUU2*SINLAMBDA),BRACKET_TERM);
  AZ_FWD:=IFTE(AZ_FWD<0,AZ_FWD+2*π,AZ_FWD);//BEARINGS ARE POSITIVE
  //V21:REVERSE AZIMUTH
  AZ_REV:=ATAN2YX((COSUU1*SINLAMBDA),(−SINUU1*COSUU2 + COSUU1*SINUU2*COSLAMBDA));
  AZ_REV:=IFTE(AZ_REV<0,AZ_REV+2*π,AZ_REV);//BEARINGS ARE POSITIVE
  DISTS:=ROUND(DISTS,0);//VINCENTY CLAIMS mm, 
  //RETURN DIST,AZIMUTH,REVERSE AZIMUTH,(LOOPCOUNT:DIAGNOSTIC)
  RETURN {DISTS,R2D(AZ_FWD),R2D(AZ_REV),LOOPCOUNT};
 END;

 VINCENTYS(RA,RB,LAT1,LON1,LAT2,LON2)
 //SPHERICAL SIMPLIFICATION: WIKIPEDIA
 //USE RR=(2RA+RB)/3
 BEGIN
  LOCAL NUM,DEN;
  LOCAL PHI1,PHI2;
  LOCAL SINPHI1,SINPHI2,COSPHI1,COSPHI2;
  LOCAL COSDELTALAMBDA,DELTALAMBDA;//DELTALONG rad
  LOCAL ANGDIST,DIST;
  LOCAL RR:=(2*RA+RB)/3;

 
  PHI1:=D2R(LAT1);
  PHI2:=D2R(LAT2);
  DELTALAMBDA:=D2R((LON2-LON1));//SHOULD WE ABS?
  COSDELTALAMBDA:=COS(DELTALAMBDA);
  SINPHI1:=SIN(PHI1); COSPHI1:=COS(PHI1); 
  SINPHI2:=SIN(PHI2); COSPHI2:=COS(PHI2);
  NUM:=√(((COSPHI2*SIN(DELTALAMBDA))^2) + (COSPHI1*SINPHI2 - SINPHI1*COSPHI2*COSDELTALAMBDA)^2);
  DEN:=(SINPHI1*SINPHI2 + COSPHI1*COSPHI2*COSDELTALAMBDA);
  ANGDIST:=ATAN2YX(NUM,DEN);
  //PRINT("ANGDIST "+ANGDIST);
  DIST:=RR*ANGDIST;
 END;

//HAVERSINE AND VINCENTYSPHERICAL RETURN IDENTICAL RESULTS
//APART FROM VERY TRIVIAL ROUNDING
//SO SERVE TO CHECK EACH OTHER
//(TO BE SURE THAT IS ALWAYS SO:CHECK MIN1 GUARD CONDITIONS ETC MATCH)

 HAVERS(LAT1,LON1,LAT2,LON2)
 BEGIN
  LOCAL RR:=(2*ESS(1)+ESS(2))/3;
  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 DD;
 END;

 ATAN2TEST()
 BEGIN
  LOCAL AA,BB,RR,RRR;
  WHILE 1 DO
   AA:=RANDOM();
   BB:=RANDOM();
   RR:=ATAN2YX(AA,BB);
   RRR:=ATAN2YX(AA,BB);
   IF ABS(RR-RRR)>1ᴇ−11 THEN
    PRINT({RR,RRR});
   END;
   IF ABS(RR)>π THEN
    PRINT(RR/π);
   END;
  END;

 END;

 A_VINCENTY(LAT1,LON1,LAT2,LON2)
 BEGIN
  LOCAL TM,RESULT,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;
  RESULT:=HAVERS(LAT1,LON1,LAT2,LON2);
  PRINT("HAVERS:      "+(RESULT/1000)+" km");
  RESULT:=VINCENTYS(ESS(1),ESS(2),LAT1,LON1,LAT2,LON2);
  PRINT("SPHERICAL: "+(RESULT/1000)+" km");
  RESULT:=Z_VINCENTY(LAT1,LON1,LAT2,LON2);
 
  TM:=TICKS-TM;
  
  //PRINT("Elapsed "+(TM/1000)+" s"); 
  PRINT("VALIDITY "+(LOOPCOUNT<LoopSafe)+" "+RESULT(4));//BOOLEAN, LOOPCOUNT (LOWER=>BETTER CONVERGENCE)
  DISTS:=RESULT(1);
  PRINT("DISTANCE: "+(DISTS/1000)+SP+" km");
  //PRINT(" AZ: "+→HMS(RESULT(2)));
  //PRINT(" AZ: "+→HMS(RESULT(3)));
 
 END;

 EXPORT STEPPED()
 BEGIN
  LOCAL II;
  FOR II FROM  1 TO  89 STEP 30 DO
   A_VINCENTY(−II,0,II,0);
  END;

 END;

 EXPORT VINCENTY()
 BEGIN
    //
    //ATAN2TEST();
    //PRINT("TESTED"); WAIT;
  REPORT_SHAPE();
  
  A_VINCENTY(50°03′58.76″,−005°42′53.10″,58°38′38.48″,−003°04′12.34″);//50PERCENT
  PRINT("EXPECT 969 954_m");
 
  A_VINCENTY(−37°57′3.72030″,144°25′29.52440″,−37°39′10.15610″,143°55′35.3839​0″);//EXPECTS 54972m
  PRINT("EXPECTED: 54972m");//VINCENTY AUSTRALIA
  A_VINCENTY(31.8300167,35.0662833,31.83000000,35.0708167);
  PRINT("PYTHON: CIRCLE 428.4 VIN 429.1677 m");
  //A_VINCENTY(0,0,0.5,179.5);
  //A_VINCENTY(0,0,0.5,179.7);//WILL NOT CONVERGE
  //A_VINCENTY(0,0,0,90);
  //A_VINCENTY(0,0,0.5,0.5);
  //A_VINCENTY(2,2,2,2);
  STEPPED();
  //PRINT("SEMI: "+π*ESS(1));//MAX
 END;