sslnjz / geodesy

Libraries of geodesy functions implemented in c++
MIT License
2 stars 1 forks source link

ClosestPointDistanceToClosestPointOnSegment has some problems with some pairs on globe... hmmm... #6

Open SkybuckFlying opened 1 year ago

SkybuckFlying commented 1 year ago

See picture in second comment for problem.

See here for example pairs fail, near end of video:

https://www.youtube.com/live/-EsixpO9mBk?feature=share

// engineer who provided it to you for assistance. // Define some earth constants const MAJA = (6378137.0); // semi major axis of ref ellipsoid const FLAT = (1.0/298.2572235630); // flattening coef of ref ellipsoid. // These are derived values from the above WGS-84 values const ESQR = (FLAT (2.0-FLAT)); // 1st eccentricity squared const OMES = (1.0 - ESQR); // 1 minus eccentricity squared const EFOR = (ESQR ESQR); // Sqr of the 1st eccentricity squared const ASQR = (MAJA * MAJA); // semi major axis squared := nlmaja**

procedure ConvertECEFToLTP ( const ParaEcef : TEcef; var ParaLtp : TLtp ); var a0,a1,a2,a3,a4,b0,b1,b2,b3 : double; b,c0,opqk,qk,qkc,qks,f,fprm : double; k : integer; ytemp, xtemp : double; begin // Convert from ParaEcef (XYZ) to LTP (ParaLtp.Lat/ParaLtp.Lon/ParaLtp.Alt)

//-------------------------------------------
// b := (0*0 + 1*1) / semi_major_axis_squared
// c := (z*z) / semi_major_axis_squared
//-------------------------------------------
b :=
    (
        (ParaEcef.x * ParaEcef.x) +
        (ParaEcef.y * ParaEcef.y)
    ) / ASQR;
c0 :=
    (ParaEcef.z * ParaEcef.z) / ASQR;

//-------------------------------------------
// a0 := c * one_minus_eccentricity_sqr
// a1 := 2 * a0
// a2 := a0 + b - first_eccentricity_to_fourth
// a3 := -2.0 * first_eccentricity_to_fourth
// a4 := - first_eccentricity_to_fourth
//-------------------------------------------
a0 := OMES * c0;
a1 := 2.0 * a0;
a2 := a0 + b - EFOR;
a3 := -2.0 * EFOR;
a4 := - EFOR;

//-------------------------------------------
// b0 := 4 * a0, b1 := 3 * a1
// b2 := 2 * a2, b3 := a3
//-------------------------------------------
b0 := 4.0 * a0;
b1 := 3.0 * a1;
b2 := 2.0 * a2;
b3 := a3;

//-------------------------------------------
// Compute First Eccentricity Squared
//-------------------------------------------
qk := ESQR;

// for (k := 1; k <:= 3; k++) for k := 1 to 3 do begin qks := qk qk; qkc := qk qks; f := (a0 qks qks) + (a1 qkc) + (a2 qks) + (a3 qk) + a4; fprm := (b0 qkc) + (b1 qks) + (b2 qk) + b3; qk := qk - (f / fprm); end;

//-------------------------------------------
// Compute latitude, longitude, altitude
//-------------------------------------------
opqk := 1.0 + qk;
if
(
    (ParaEcef.x = 0.0) and
    (ParaEcef.y = 0.0)
) then
// on the earth's axis
begin
    // We are sitting EXACTLY on the earth's axis
    // Probably at the center or on or near one of the poles
    ParaLtp.Lon := 0.0; // as good as any other value
    if (ParaEcef.z >= 0.0) then
    begin
        ParaLtp.Lat := PI / 2; // ParaLtp.Alt above north pole
    end else
    begin
        ParaLtp.Lat := -PI / 2; // ParaLtp.Alt above south pole
    end;
end else
begin
    ytemp := opqk * ParaEcef.z;
    xtemp := sqrt
    (
        ( ParaEcef.x * ParaEcef.x ) +
        ( ParaEcef.y * ParaEcef.y )
    );
    ParaLtp.Lat := arctan2( ytemp, xtemp );
    ParaLtp.Lon := arctan2( ParaEcef.y, ParaEcef.x );
end;
ParaLtp.Alt :=
    (
        1.0 - OMES / ESQR * qk
    ) * MAJA *
    sqrt
    (
        (
            b / (opqk * opqk)
        ) + c0
    );

end; // ConvertECEFToLTP()

// written by Skybuck Flying ! ;) :) =D function ClosestPointDistanceToClosestPointOnSegment ( ParaPointX, ParaPointY, ParaPointZ : double; ParaStartX, ParaStartY, ParaStartZ : double; ParaEndX, ParaEndY, ParaEndZ : double; ParaRadius : double {= 6371e3} // (Mean) radius of earth (defaults to radius in metres). ) : double; var vPoint, vSegmentPoint1, vSegmentPoint2, vClosestPointOnSegment : TLatLonNvectorSpherical; begin vSegmentPoint1.FromXYZ2( ParaStartX, ParaStartY, ParaStartZ, ParaRadius ); vSegmentPoint2.FromXYZ2( ParaEndX, ParaEndY, ParaEndZ, ParaRadius ); vPoint.FromXYZ2( ParaPointX, ParaPointY, ParaPointZ, ParaRadius );

vClosestPointOnSegment := nearestPointOnSegment
(
    vPoint,
    vSegmentPoint1,
    vSegmentPoint2,
    ParaRadius
);

result := distanceTo
(
    vPoint, vClosestPointOnSegment, ParaRadius
);

end;

function ClosestPointDistanceToClosestPointOnSegmentV2 ( ParaPointX, ParaPointY, ParaPointZ : double; ParaStartX, ParaStartY, ParaStartZ : double; ParaEndX, ParaEndY, ParaEndZ : double; ParaRadius : double {= 6371e3} // (Mean) radius of earth (defaults to radius in metres). ) : double; var vPoint, vSegmentPoint1, vSegmentPoint2, vClosestPointOnSegment : TLatLonNvectorSpherical; vEcef : Tecef; vLtp : Tltp; begin // convert start x,y,z to lat lng vEcef.X := ParaStartX; vEcef.Y := ParaStartY; vEcef.Z := ParaStartZ;

ConvertECEFToLTP
(
    vEcef,
    vLtp
);

vSegmentPoint1.mLat := vLtp.Lat;
vSegmentPoint1.mLon := vLtp.Lon;

// convert end x,y,z to lat lang
vEcef.X := ParaEndX;
vEcef.Y := ParaEndY;
vEcef.Z := ParaEndZ;

ConvertECEFToLTP
(
    vEcef,
    vLtp
);

vSegmentPoint2.mLat := vLtp.Lat;
vSegmentPoint2.mLon := vLtp.Lon;

// ParaRadius ??

// convert point x,y,z to lat lng
vEcef.X := ParaPointX;
vEcef.Y := ParaPointY;
vEcef.Z := ParaPointZ;

ConvertECEFToLTP
(
    vEcef,
    vLtp
);

vPoint.mLat := vLtp.Lat;
vPoint.mLon := vLtp.Lon;

vClosestPointOnSegment := nearestPointOnSegment
(
    vPoint,
    vSegmentPoint1,
    vSegmentPoint2,
    ParaRadius
);

result := distanceTo
(
    vPoint, vClosestPointOnSegment, ParaRadius
);

end;

Any idea what could be wrong ? If you need more code let me know...

SkybuckFlying commented 1 year ago

ProblemV1

sslnjz commented 1 year ago

Hi there,

Long time offline cause of actual work this year, and seems that you have some misunderstood for this project, I was scheduled to re-structured this project for more easy to build or easy to read, the base compiler will be VS2022(MSVC v143) with CMAKE 3.20, Clion 2023.2 for non-windows platform with GCC 11, or Clang 16.

Thanks for all your comments and recommend code.