การเขียนโปรแกรมเพื่อคำนวณระยะทางและอะซิมัท (Distance/Azimuth) บน Ellipsoid ด้วย Lazarus (ตอนที่ 1)

17/10/2009 Leave a comment
  • ตอนก่อนหน้านี้ ผมเขียนโปรแกรมแปลงพิกัดระหว่าง UTM และ Geographic (Lat/Long) และและถ้าไม่เขียนการหาระยะทางและอะซิมัท (เมื่อกำหนดจุด Latitude, Longitude ให้สองจุด) ก็ดูจะขาดอะไรไปอย่าง

Model ที่ใช้ในการคำนวณ

  • สัณฐานหรือรูปทรงที่ใช้แทนโลก ใช้กันอยู่ 2 แบบ คือ ทรงกลม(Spherical)และทรงรี(Ellipsoid) แต่ทรงรีจะใกล้เคียงกับกับโลกเรามากกว่า ดังนั้นการหาระยะทางและทิศทาง(อะซิมัท) บนทรงรีจะให้ความละเอียดถูกต้องมากกว่า
รูปทรงที่ใช้แทนสัณฐานของโลก
รูปทรงที่ใช้แทนสัณฐานของโลก (ภาพอ้างอิงจาก http://www.codeguru.com/cpp/cpp/algorithms/article.php/c5115/)
  • การคำนวณระยะทางและอะซิมัทบนทรงกลมนั้นง่ายกว่าการคำนวณบนทรงรี การคำนวณระยะทางบนทรงกลมเช่นต้องการทราบระยะทางจากจุด A ไปจุด B เราจะใช้ plane คือแผ่นเรียบตัดผ่านจุด A และจุด B และที่สำคัญคือต้องผ่านจุดศูนย์กลางทรงกลมด้วย เส้นที่เกิดจากแผ่น plane มาตัดกับทรงกลมจะเรียกว่า  Great Circle ระยะที่สั้นที่สุดบนผิวทรงกลมก็คือระยะที่ไปตาม Great circle นี่เอง
Great circle
Great circle อ้างอิงจาก http://www.codeguru.com/cpp/cpp/algorithms/article.php/c5115/
  • ส่วนทรงรีที่สัณฐานใกล้เคียงกับโลกมากกว่า จะยุบลงที่ขั้วโลก โป่งออกที่เส้นศูนย์สูตร เราจะหาระยะทางและอะซิมัทโดยอิงสัณฐานของทรงรีโดยใช้สูตรอันลือชื่อของ Thaddeus Vincenty

สูตรที่ใช้อ้างอิงในการคำนวณ (Vincenty’s formulae)

  • Thaddeus Vincenty เกิดที่โปแลนด์ อพยพเข้าสหรัฐอเมริกาเมื่อสงครามโลกครั้งที่สอง ทำงานให้กับ U.S. Air Force และ  National Geodetic Survey ตามลำดับ ที่เป็นที่รู้จักกันมากที่สุดก็คือ Vincenty’s Formulae นี้เอง เขาเผยแพร่สูตรนี้เมื่อปี 1975 ซึ่งให้ความละเอียดสูงมากจนถึงครึ่งมิลลิเมตร
  • สำหรับสูตรของคุณ Thaddeus Vincenty ถ้าเห็นสูตรยาวๆแล้วสารอะดรีนาลีนหลั่งออกมาก็ ไปดาวน์โหลดได้ที่ http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
  • สำหรับสูตรที่จะนำเสนอในที่นี้ผมแปลมาจาก Javascript ดูได้ที่ http://www.movable-type.co.uk/scripts/latlong-vincenty.html สำหรับสูตรของ Vincenty จะมีการอินทิเกรต แต่ในทางโปรแกรมมิ่งจะใช้เทคนิค iteration แทนทำให้สูตรที่จะโปรแกรมดูกระชับมาก
  • สำหรับสูตรที่นำสรุปจาก Vincenty’s formulae คำนวณหาระยะทางและอะซิมัทจากจุด 2 จุด แสดงให้เห็นดังข้างล่าง
a, b = major & minor semiaxes of the ellipsoid  
f = flattening (a−b)/a  
φ1, φ2 = geodetic latitude  
L = difference in longitude  
U1 = atan((1−f).tanφ1) (U is ‘reduced latitude’)  
U2 = atan((1−f).tanφ2)  
λ = L (first approximation)  
iterate until change in λ is negligible (e.g. 10-12 ≈ 0.06mm) {  
  sinσ = √[ (cosU2.sinλ)² + (cosU1.sinU2 − sinU1.cosU2.cosλ)² ] (14)
  cosσ = sinU1.sinU2 + cosU1.cosU2.cosλ (15)
  σ = atan2(sinσ, cosσ) (16)
  sinα = cosU1.cosU2.sinλ / sinσ (17)
  cos²α = 1 − sin²α (trig identity; §6)  
  cos2σm = cosσ − 2.sinU1.sinU2/cos²α (18)
  C = f/16.cos²α.[4+f.(4−3.cos²α)] (10)
  λ′ = L + (1−C).f.sinα.{σ+C.sinσ.[cos2σm+C.cosσ.(−1+2.cos²2σm)]} (11)
     
u² = cos²α.(a²−b²)/b²  
A = 1+u²/16384.{4096+u².[−768+u².(320−175.u²)]} (3)
B = u²/1024.{256+u².[−128+u².(74−47.u²)]} (4)
Δσ = B.sinσ.{cos2σm+B/4.[cosσ.(−1+2.cos²2σm) − B/6.cos2σm.(−3+4.sin²σ).(−3+4.cos²2σm)]} (6)
s = b.A.(σ−Δσ) (19)
α1 = atan2(cosU2.sinλ, cosU1.sinU2 − sinU1.cosU2.cosλ) (20)
α2 = atan2(cosU1.sinλ, −sinU1.cosU2 + cosU1.sinU2.cosλ) (21)

 

where :

  • s is the distance (in the same units as a & b)
  • α1 is the initial bearing, or forward azimuth
  • α2 is the final bearing (in direction p1→p2)

สิ่งที่ต้องเตรียมก่อนจะโปรแกรม

เริ่มต้นโปรแกรม

  • ผมจะสร้างยูนิตที่ทำหน้าที่คำนวณชื่อ GeodesicCompute (อยู่ในไฟล์ geodesiccompute.pas) โค๊ดดังที่กล่าวไปแล้วจาก Javascript แต่ดัดแปลงให้เป็นโค๊ด OOP ในยูนิตนี้มี 2 class คือ TGeodesicInverse ทำหน้าที่คำนวณระยะทางและอะซิมัทเมื่อป้อนค่าพิกัดเป็น Latitude,Longitude 2 จุด เรียกการคำนวณนี้ว่า Inverse
  • และอีกยูนิตหนึ่งคือ TGeodesicDirect(อยู่ในไฟล์ geodesiccompute.pas) ทำหน้าที่คำนวณหาค่าพิกัดจุดที่สอง (Lat/Long) เมื่อกำหนดค่าพิกัดจุดที่หนึ่งให้และ กำหนดอะซิมัทและระยะทาง เรียกการคำนวณนี้สั้นๆว่า Direct

GeodesicCompute Unit

  • โค๊ดที่ผมเขียนดัดแปลงมาจาก Javascript เป็นไปดัง source code ด้านล่าง
unit GeodesicCompute;

{$mode objfpc}{$H+}

interface

uses
  Classes, SysUtils, GeoEllipsoids, Math;

  type
    TZoneHemisphere = (hemEast, hemWest, hemNorth, hemSouth);

    TCoordinate = record
    X : double;
    Y : double;
  end;

    //Geodesic computation by Vincenty's formulae.
    //Compute geodesic azimuth & distance.
    TGeodesicInverse = class
    private
      FEllipsoid : TEllipsoid;
      FGeoCoor1  : TCoordinate;
      FGeoCoor2  : TCoordinate;
      FEllDist, FInitAzi, FFinalAzi : double;
      procedure SetGeoCoordinate1 (const geocoor1 : TCoordinate);
      procedure SetGeoCoordinate2 (const geocoor2 : TCoordinate);
      procedure SetEllipsoid(const Ellipsoid : TEllipsoid);
      function  GeodesicInverse(lat1, lon1, lat2, lon2 : double) : double;
    public
      property GeoCoordinate1 : TCoordinate write SetGeoCoordinate1;
      property GeoCoordinate2 : TCoordinate write SetGeoCoordinate2;
      property Distance : double read FEllDist;
      property InitialAzimuth  : double read FInitAzi;
      property FinalAzimuth : double read FFinalAzi;
      property Ellipsoid : TEllipsoid write SetEllipsoid;
      procedure Compute;
      constructor Create;
      destructor Destroy; override;
    end;

    TGeodesicDirect = class
    private
      FEllipsoid : TEllipsoid;
      FGeoCoor1  : TCoordinate;
      FGeoCoor2  : TCoordinate;
      FAzimuth   : double;
      FDistance  : double;
      procedure SetGeoCoordinate1 (const geocoor1 : TCoordinate);
      procedure SetEllipsoid(const Ellipsoid : TEllipsoid);
      procedure SetAzimuth(const azi : double);
      procedure SetDistance(const dist : double);
      function  GeodesicDirect(lat, lon, azimuth, distance : double) : TCoordinate;
    public
      property GeoCoordinate1 : TCoordinate write SetGeoCoordinate1;
      property GeoCoordinate2 : TCoordinate read FGeoCoor2;
      property Azimuth : double write SetAzimuth;
      property Distance : double write SetDistance;
      property Ellipsoid : TEllipsoid read FEllipsoid write SetEllipsoid;
      procedure Compute;
      constructor Create;
      destructor Destroy; override;
    end;
implementation

// TGeodesicInverse
procedure TGeodesicInverse.SetGeoCoordinate1 (const geocoor1 : TCoordinate);
begin
  FGeoCoor1 := geocoor1;
end;

procedure TGeodesicInverse.SetGeoCoordinate2 (const geocoor2 : TCoordinate);
begin
  FGeoCoor2 := geocoor2;
end;

procedure TGeodesicInverse.SetEllipsoid(const Ellipsoid : TEllipsoid);
begin
  FEllipsoid.EllipsoidName := Ellipsoid.EllipsoidName;
  FEllipsoid.MajorAxis := Ellipsoid.MajorAxis;
  FEllipsoid.InvFlattening := Ellipsoid.InvFlattening;
end;

procedure TGeodesicInverse.Compute;
begin
  FEllDist := GeodesicInverse(FGeoCoor1.Y, FGeoCoor1.X, FGeoCoor2.Y, FGeoCoor2.X);
end;

function TGeodesicInverse.GeodesicInverse(lat1, lon1, lat2, lon2 : double) : double;
var
  a, b, f , e : double;
  dlat1, dlon1, dlat2, dlon2 : double;
  L, U1, U2 : double;
  sinU1, cosU1, sinU2, cosU2 : double;
  lambda, lambdaP, sinlambda, coslambda, sinsigma : double;
  cossigma, sinalpha, sigma, cosSqalpha, cos2sigmaM : double;
  cossigmaM, C, uSq, AA, BB, deltaSigma : double;
  az1, az2 : double;
  iterLimit : integer;
  diff : double;
begin
  dlat1 := PI/180 * lat1;
  dlon1 := PI/180 * lon1;
  dlat2 := PI/180 * lat2;
  dlon2 := PI/180 * lon2;

  a := FEllipsoid.MajorAxis;
  f := 1/FEllipsoid.InvFlattening;
  b := a*(1 - f);
  L := dlon2-dlon1;
  U1 := arctan((1-f) * tan(dlat1));
  U2 := arctan((1-f) * tan(dlat2));
  sinU1 := sin(U1);
  cosU1 := cos(U1);
  sinU2 := sin(U2);
  cosU2 := cos(U2);
  lambda := L;
  iterLimit := 100;
  repeat
  begin
    sinlambda := sin(lambda);
    coslambda := cos(lambda);
    sinsigma  := sqrt((cosU2*sinLambda) * (cosU2*sinlambda) +
                (cosU1*sinU2-sinU1*cosU2*coslambda) * (cosU1*sinU2-sinU1*cosU2*coslambda));
    if (sinsigma = 0) then
    begin
      result := 0; // co-incident points
      exit;
    end;
    cossigma := sinU1*sinU2 + cosU1*cosU2*coslambda;
    sigma := arctan2(sinsigma, cossigma);
    sinalpha := cosU1 * cosU2 * sinlambda / sinsigma;
    cosSqAlpha := 1 - sinAlpha*sinAlpha;
    cos2SigmaM := cosSigma - 2*sinU1*sinU2/cosSqAlpha;
    if (IsNaN(cos2sigmaM)) then cosSigmaM := 0; // equatorial line: cosSqAlpha=0 (6)
    C := f/16*cosSqalpha*(4+f*(4-3*cosSqalpha));
    lambdaP := lambda;
    lambda := L + (1-C) * f * sinAlpha *
              (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));
    dec(iterLimit);
  end;
  diff := abs(lambda-lambdaP);
  Until ((diff < 1e-12) and (iterLimit > 0));

  if (iterLimit = 0) then
  begin
    result := NaN;   // formula failed to converge
    exit;
  end;

  uSq := cosSqAlpha * (a*a - b*b) / (b*b);
  AA := 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
  BB := uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));
  deltaSigma := BB*sinsigma*(cos2sigmaM+BB/4*(cossigma*(-1+2*cos2sigmaM*cos2sigmaM)-
    BB/6*cos2sigmaM*(-3+4*sinsigma*sinsigma)*(-3+4*cos2sigmaM*cos2sigmaM)));
  result := b*AA*(sigma-deltaSigma);  //return Ellipse Distance.
  //initial azimuth.
  az1 := arctan2((cosU2*sinlambda),(cosU1*sinU2 - sinU1*cosU2*coslambda));
  //final azimuth.
  az2 := arctan2((cosU1*sinlambda),(-sinU1*cosU2 + cosU1*sinU2*coslambda));

  FInitAzi := az1 * 180/PI;
  if (FInitAzi < 0) then FInitAzi := 360 + FInitAzi;

  FFinalAzi := az2 * 180/PI;
  if (FFinalAzi < 0) then FFinalAzi := 360 + FFinalAzi;
end;


constructor TGeodesicInverse.Create;
begin
  inherited create;
  FEllipsoid := TEllipsoid.Create;
end;

destructor TGeodesicInverse.Destroy;
begin
  FEllipsoid.Free;
  inherited Destroy;
end;


// TGeodesicDirect
procedure TGeodesicDirect.SetGeoCoordinate1 (const geocoor1 : TCoordinate);
begin
  FGeoCoor1 := geocoor1;
end;

procedure TGeodesicDirect.SetAzimuth (const azi : double);
begin
  FAzimuth := azi;
end;

procedure TGeodesicDirect.SetDistance (const dist : double);
begin
  FDistance := dist;
end;

procedure TGeodesicDirect.SetEllipsoid(const Ellipsoid : TEllipsoid);
begin
  FEllipsoid.EllipsoidName := Ellipsoid.EllipsoidName;
  FEllipsoid.MajorAxis := Ellipsoid.MajorAxis;
  FEllipsoid.InvFlattening := Ellipsoid.InvFlattening;
end;

procedure TGeodesicDirect.Compute;
begin
  FGeoCoor2 := GeodesicDirect(FGeoCoor1.Y, FGeoCoor1.X, FAzimuth, FDistance);
end;

function TGeodesicDirect.GeodesicDirect(lat, lon, azimuth, distance : double) : TCoordinate;
var
  a, b, f , e, s : double;
  dlat1, dlon1, dlat2, dlon2 : double;
  sinU1, alpha1, sinAlpha, sinAlpha1, cosAlpha1, tanU1, cosU1, sigma1 : double;
  cos2SigmaM, sinSigma, cosSigma, cosSqAlpha, uSq, deltaSigma : double;
  AA, BB, sigma, sigmaP, tmp, lambda, C, L, lat2, revAz : double;

begin
  dlat1 := PI/180 * lat;
  dlon1 := PI/180 * lon;

  a := FEllipsoid.MajorAxis;
  f := 1/FEllipsoid.InvFlattening;
  b := a*(1 - f);

  s := distance;
  alpha1 := azimuth * PI/180.0;
  sinAlpha1 := sin(alpha1);
  cosAlpha1 := cos(alpha1);

  tanU1 := (1-f) * tan(dlat1);
  cosU1 := 1 / sqrt((1 + tanU1*tanU1));
  sinU1 := tanU1*cosU1;
  sigma1 := arctan2(tanU1, cosAlpha1);
  sinAlpha := cosU1 * sinAlpha1;
  cosSqAlpha := 1 - sinAlpha*sinAlpha;
  uSq := cosSqAlpha * (a*a - b*b) / (b*b);
  AA := 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
  BB := uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));

  sigma := s / (b*AA);
  sigmaP := 2*PI;
  while (abs(sigma-sigmaP) > 1e-12) do
  begin
    cos2SigmaM := cos(2*sigma1 + sigma);
    sinSigma := sin(sigma);
    cosSigma := cos(sigma);
    deltaSigma := BB*sinSigma*(cos2SigmaM+BB/4*(cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-
                  BB/6*cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));
    sigmaP := sigma;
    sigma := s / (b*AA) + deltaSigma;
  end;

  tmp := sinU1*sinSigma - cosU1*cosSigma*cosAlpha1;
  dlat2 := arctan2(sinU1*cosSigma + cosU1*sinSigma*cosAlpha1,
      (1-f)*sqrt(sinAlpha*sinAlpha + tmp*tmp));
  lambda := arctan2(sinSigma*sinAlpha1, cosU1*cosSigma - sinU1*sinSigma*cosAlpha1);
  C := f/16*cosSqAlpha*(4+f*(4-3*cosSqAlpha));
  L := lambda - (1-C) * f * sinAlpha *
      (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));

  revAz := arctan2(sinAlpha, -tmp);  // final bearing
  result.X := lon + L * 180/PI;
  result.Y := dlat2 * 180/PI;
end;


constructor TGeodesicDirect.Create;
begin
  inherited create;
  FEllipsoid := TEllipsoid.Create;
end;

destructor TGeodesicDirect.Destroy;
begin
  FEllipsoid.Free;
  inherited Destroy;
end;

end.
  • สำหรับ source code ที่แสดงดังข้างต้น เป็น class ง่ายๆ เช่น TGeodesicInverse จะมี property สำหรับ input อยู่ 3 ตัวคือ ค่ารูปทรงสัณฐาน Ellipsoid, GeoCoordinate1 และ GeoCoordinate2 เมื่อ input ค่า lat,long 2 ค่าพิกัดให้เสร็จ แล้วก็เรียกเมธอด Compute เพื่อทำการคำนวณ จากนั้นเรียกค่าจาก 2 property คือ Azimuth และ Distance ซึ่งจะเป็นผลลัพธ์การคำนวณ
  • ส่วนคลาส TGeodesicDirect ก็เหมือนๆกัน มี property 4 ตัว คือ Ellipsoid, GeoCoordinate1, Azimuth และ Distance เมื่อรับค่ามาก็จะเรียกเมธอด Compute และให้ผลลัพธ์เป็นค่าพิกัด (lat,long) ของจุดที่ต้องการหา

ปัญหาความเข้ากันของ Widget ในแต่ละ Platform

  • รูปที่จะแสดงให้ดูเป็นปัญหาของ Widget ผมเพิ่งจะประสบความสำเร็จติดตั้ง Widget ของ Qt บน PCLinuxOS เมื่อรันโปรแกรมด้วยชุด source code เดียวกันเปรียบเทียบกับ Win32 บนวินโดส์ และ Gtk2 บน Ubuntu  พบว่า ขนาดของ Lable, EditBox, ComboBox, Groupbox ทั้งหลายบน Gtk2 มีขนาดใหญ่กว่าเพื่อน แต่พบว่าขนาดของ object พวกนี้บน Win32 และ Qt มีขนาดใกล้เคียงกัน เพื่อให้เข้ากับ Gtk2 จึงต้องมาขยับ เช่นตัว Label กับ EditBox จะต้องอยู่ห่างกันพอสมควร ไม่งั้นตัว Label จะล้ำเข้าไปหา EditBox แต่ความสวยงามดูๆ QT จะสวยเนียนกว่าเพื่อน รองลงมาเป็น Gtk2 กับ Win32 ดูรูปผลลัพธ์ของโปรแกรมที่ผมเขียนในตอนนี้
Geodesic computation on Qt widget.
โปรแกรมคำนวณหาระยะทางและอะซิมัทบน Ellipsoid รันด้วย Qt (PCLinuxOS)
โปรแกรมคำนวณหาระยะทางและอะซิมัทบน Ellipsoid รันด้วย Gtk2 (Ubuntu)
โปรแกรมคำนวณหาระยะทางและอะซิมัทบน Ellipsoid รันด้วย Gtk2 (Ubuntu)
โปรแกรมคำนวณหาระยะทางและอะซิมัทบน Ellipsoid รันด้วย Windows
โปรแกรมคำนวณหาระยะทางและอะซิมัทบน Ellipsoid รันด้วย Windows
  • ทิ้งท้ายกันก่อนในตอนนี้ ติดตามกันตอนหน้า

Leave a Reply

Your email address will not be published. Required fields are marked *