Calculate future latitude longitude with initial coordinates, heading and distance

Question

I have been reading through stackoverflow and this site (http://www.movable-type.co.uk/scripts/latlong.html) about how to do this, but I cant get my code to give a correct answer. It is giving a coordinate that isnt in the correct direction. I have been working on this all day and seem to have hit a wall. This is my function:

public static void destination()
{
     double heading = 335.9;
     double startLatitude = 41.8369;
     double startLongitude = 87.6847;

     //Convert to Radians
     startLatitude = startLatitude * Math.PI / 180;
     startLongitude = startLongitude * Math.PI / 180;
     heading = heading * Math.PI / 180;

     int distanceKilometers = 100;
     double angularDistance = distanceKilometers / 6371e3;

     double endLat = Math.Asin((Math.Sin(startLatitude) * Math.Cos(angularDistance)) +
                    (Math.Cos(startLatitude) * Math.Sin(angularDistance) * Math.Cos(heading)));
     double endLong = startLongitude + (Math.Atan2((Math.Sin(heading) * Math.Sin(angularDistance) * Math.Cos(startLatitude)),
                              Math.Cos((angularDistance) - (Math.Sin(startLatitude) * Math.Sin(endLat)))));

     endLong = (endLong + 3 * Math.PI) % (2 * Math.PI) - Math.PI;
     Console.WriteLine("endLatitude: " + (endLat * 180 / Math.PI) + " endLongitude: " + (endLong * 180 / Math.PI));
  }

Answer 1

I use the below function. float provide you a 3 meters precision. If you need more, use double.

  internal class SxMath
  {
    internal const  float      PI                 = (float)Math.PI;
    internal const  float      x2PI               = PI * 2;
    internal const  float      PIDiv2             = PI/2;
    internal const  float      RadPerSec          = (float)(PI / 648000F);
    internal const  float      SecPerRad          = (float)(648000F / PI);
    internal const  float      RadPerDeg          = PI / 180;
    internal const  float      RadPerMin          = PI / 10800;
    internal const  float      DegPerRad          = 180 / PI;
    internal const  float      MinParRad          = (float)(10800.0/PI);
    internal const  float      RadPerMeter        = RadPerMin * (1F/1852F) /* Meter_To_NMs */ ;

    internal static float RealMod(float val,float modval)
    { // Example : RealMod(3,2*PI)=3 , RealMod(2*PI+3,2*PI)=3 , RealMod(-3,2*PI)=2*PI-3 
      float result = (float)Math.IEEERemainder(val,modval);
      if (result<0) result = result + modval;
      return result;
    }
  } // SxMath

  internal struct SxGeoPt
  {
    internal float lat ; // in radians, N positive
    internal float lon ; // in radians, W positive
  } // SxGeoPt

  internal static SxGeoPt GEO_CoorPointInAzim(SxGeoPt p1,float az,float raddist)
    // This procedure provides coordinates of the point p2 located
    // - at a distance raddist of a point p1
    // - in the direction of azimuth az
    // input   p1        <SxGeoPt> coordinates of reference point
    //         raddist   <float>   distance in radian between p1 and p2
    //         az        <float>   azimut of p2 from p1, 
    //                             (az=0, if p1 and p2 on same longitude and P2 north of P1)
    //                             (az=90, if p1 is on equator and p2 on equtor at East of P1)
    // output  p2        <SxGeoPt> coordinates of resulting point
    {
      SxGeoPt result;
      if (p1.lat>SxMath.PIDiv2-SxMath.RadPerMin)
      { if (az<=SxMath.PI) result.lon=az; else result.lon=az-SxMath.PI; result.lat=SxMath.PIDiv2-raddist; }
      else if (p1.lat<-SxMath.PIDiv2+SxMath.RadPerMin)
      { if (az<=SxMath.PI) result.lon=-az; else result.lon=-az+SxMath.PI; result.lat=-SxMath.PIDiv2+raddist; }
      else
      {
        result.lat = (float)Math.Asin((Math.Sin(p1.lat)*Math.Cos(raddist)) + 
                                  (Math.Cos(p1.lat)*Math.Sin(raddist)*Math.Cos(az)));
        float dlon = (float)Math.Atan2( Math.Sin(az)*Math.Sin(raddist)*Math.Cos(p1.lat), 
                                        Math.Cos(raddist)-Math.Sin(p1.lat)*Math.Sin(result.lat));
        result.lon = SxMath.RealMod(p1.lon-dlon+SxMath.PI,SxMath.x2PI)-SxMath.PI;
      }
      return result;
    }

As I extracted code from different classes, I hope that nothing is missing.

To get the input parameter DistInRad from Kilometers:

  float raddist = distanceKilometers * 1000f * SxMath.RadPerMeter ;