Bearing calculation issues

Archive
1

I am writing a GeoInfosystems library for the FEZ, but I think I have run into an issue.

The first method I wanted to include was to calculate the bearing between two sets of coordinates, but the calculations are coming up wrong.

For examples:

			Location pointA = new Location(40.7486, -73.9864, 0);
			Location pointB = new Location(40.9486, -72.9866, 0);

			Debug.Print(GPSTools.BearingToLocation(pointA, pointB).ToString());

returns 47.5. The correct heading for those coordinates should be 74 degrees.

Here is my code:

/*
 * GPS Tools
 *	Coded by Chris Seto 2010
 *	
 * This sofware is released under the Apache 2.0 license, copyright Chris Seto 2010
 * */
using System;
using ElzeKool;
using ChrisSeto.GeoInfoSystems.System;

namespace ChrisSeto.GeoInfoSystems
{
	/// <summary>
	/// Methods for working with waypoints
	/// </summary>
	public static class GPSTools
	{
		/// <summary>
		/// Units of distance
		/// </summary>
		public enum Distance
		{
			Miles,
			Feet,
			Kilometers,
			Meters,
		}

		/// <summary>
		/// Calculate the distance between two Locations
		/// </summary>
		/// <param name="pointA"></param>
		/// <param name="pointB"></param>
		/// <param name="distanceUnits"></param>
		/// <returns></returns>
		public static double DistanceBetweenLocations(Location pointA, Location pointB, Distance distanceUnits)
		{

			return 0;
		}

		/// <summary>
		/// Calculate the inital bearing between two Locations
		/// </summary>
		/// <param name="pointA"></param>
		/// <param name="pointB"></param>
		/// <param name="headingType"></param>
		/// <returns></returns>
		public static double BearingToLocation(Location pointA, Location pointB)
		{
			// Convert both locations from degrees to radians
			pointA = LocationToRad(pointA);
			pointB = LocationToRad(pointB);

			double partOne = exMath.Sin(pointB.Longitude - pointA.Longitude) * exMath.Cos(pointB.Latitude);
			double partTwo = exMath.Cos(pointA.Latitude) * exMath.Sin(pointB.Latitude) - exMath.Sin(pointA.Latitude) * exMath.Cos(pointB.Latitude) * exMath.Cos(pointB.Longitude - pointA.Longitude);
			double heading = AdditionalMath.ToDeg(exMath.Atan2(partOne, partTwo) % 2 * exMath.PI);

			// Solve for compass wrap around
			if (heading < 0)
				heading += 360;

			return heading;
		}

		/// <summary>
		/// Return a new location in radians
		/// </summary>
		/// <param name="pointA"></param>
		/// <returns></returns>
		public static Location LocationToRad(Location pointA)
		{
			return new Location(AdditionalMath.ToRad(pointA.Latitude), AdditionalMath.ToRad(pointA.Longitude), pointA.Altitude);
		}

	}

	/// <summary>
	/// Misc. math functions not available in Elze Kool's lib
	/// </summary>
	public static class AdditionalMath
	{
		/// <summary>
		/// Degrees to radians
		/// </summary>
		/// <param name="x"></param>
		/// <returns></returns>
		public static double ToRad(double x)
		{
			return exMath.PI * x / 180.00F;
		}

		/// <summary>
		/// Radians to degrees
		/// </summary>
		/// <param name="x"></param>
		/// <returns></returns>
		public static double ToDeg(double x)
		{
			return x * 180.00F / exMath.PI;
		}
	}
}

Anybody have any idea what I screwed up?

This is a helpful webform to check values: Calculate distance and bearing between two Latitude/Longitude points using haversine formula in JavaScript

2

Why you do “% 2 * exMath.PI” in the following line?

double heading = AdditionalMath.ToDeg(exMath.Atan2(partOne, partTwo) % 2 * exMath.PI);

try this:

double heading = AdditionalMath.ToDeg(exMath.Atan2(partOne, partTwo));

Now you get 74.8606662209009

Linqpad sample:


void Main()
{
	Location pointA = new Location(40.7486, -73.9864);
	Location pointB = new Location(40.9486, -72.9866);
	BearingToLocation(pointA, pointB).Dump();
}

double BearingToLocation(Location pointA, Location pointB)
{
	// Convert both locations from degrees to radians
	pointA = LocationToRad(pointA);
	pointB = LocationToRad(pointB);

	double partOne = Math.Sin(pointB.lon - pointA.lon) * Math.Cos(pointB.lat);
	double partTwo = Math.Cos(pointA.lat) * Math.Sin(pointB.lat) -
					 Math.Sin(pointA.lat) * Math.Cos(pointB.lat) * Math.Cos(pointB.lon - pointA.lon);
						
	double heading = AdditionalMath.ToDeg(Math.Atan2(partOne, partTwo));

	// Solve for compass wrap around
	if (heading < 0)
		heading += 360;

	return heading;
}

/// <summary>
/// Return a new location in radians
/// </summary>
/// <param name="pointA"></param>
/// <returns></returns>
Location LocationToRad(Location pointA)
{
	return new Location(AdditionalMath.ToRad(pointA.lat), AdditionalMath.ToRad(pointA.lon));
}

public class Location {
	public double lon;
	public double lat;
	
	public Location(double lat, double lon) {
		this.lon = lon;
		this.lat = lat;
	}
}

public static class AdditionalMath
	{
		/// <summary>
		/// Degrees to radians
		/// </summary>
		/// <param name="x"></param>
		/// <returns></returns>
		public static double ToRad(double x)
		{
			return Math.PI * x / 180.00F;
		}
 
		/// <summary>
		/// Radians to degrees
		/// </summary>
		/// <param name="x"></param>
		/// <returns></returns>
		public static double ToDeg(double x)
		{
			return x * 180.00F / Math.PI;
		}
	}
3

Chris.S : what Math routines are you using for Cos, Sin, …?

Have you tried the grommet ones?

BTW, I’ve already written a library for geo points and angle/distance calculations - haven’t shared it yet since I haven’t finished the unit tests yet. Plus busy with RC car.

4

Hah, that worked perfectly! Thank you Chris!!

@ RJ, I am using Elze Kool’s math lib (MathEx) for Trig functions. Feel free to share your distance and bearing calculations. I would love to see them. Post them on fezzer then post in the forum, I’m sure the staff will give you some points :smiley:

5

OK, another issue.

I now have my distance method done, but it is also returning weird values. Using the same points in the original question, this method returns 19917.153760362871 kilometers, which is not right. It should be around 86.

Any ideas on what I messed up on?

		/// <summary>
		/// Calculate the distance between two Locations
		/// </summary>
		/// <param name="pointA"></param>
		/// <param name="pointB"></param>
		/// <param name="distanceUnits"></param>
		/// <returns></returns>
		public static double DistanceBetweenLocations(Location pointA, Location pointB, Distance distanceUnits)
		{
			// Calculate deltas
			double deltaLat = AdditionalMath.ToRad(pointB.Latitude - pointA.Latitude); 
			double deltaLon = AdditionalMath.ToRad(pointB.Longitude - pointA.Longitude);
			
			double a = exMath.Sin(deltaLat / 2)
				* exMath.Sin(deltaLat / 2)
				+ exMath.Cos(AdditionalMath.ToRad(pointA.Latitude))
				* exMath.Cos(AdditionalMath.ToRad(pointB.Latitude))
				* exMath.Sin(deltaLon / 2)
				* exMath.Sin(deltaLon / 2);

			double c = 2 * exMath.Atan2(exMath.Sqrt(a), exMath.Sqrt(1 - a));
			
			// Switch our output units
			switch (distanceUnits)
			{
				case Distance.Kilometers:
					return c * 6367.5;

				case Distance.Meters:
					return c * (6367.5 * 1000);

				case Distance.Miles:
					return c * 3956.6;

				case Distance.Feet:
					return c * (3956.6 * 5280);

				default:
					throw new ArgumentOutOfRangeException();
			}
			
		}
6

@ Chris,

This will give you the right answer


        private static double DistanceKm(double Latitude1, double Longitude1, double Latitude2, double Longitude2)
        {
            double lat1 = toRad(Latitude1);
            double lng1 = toRad(Longitude1);
            double lat2 = toRad(Latitude2);
            double lng2 = toRad(Longitude2);
            double a, c;

            double earthRadius = 6371;   // Kilometers
            double dLat = (lat2 - lat1);
            double dLng = (lng2 - lng1);

            a = (MathEx.Sin(dLat / 2) * MathEx.Sin(dLat / 2)) +
                (MathEx.Cos(lat1) * MathEx.Cos(lat2)) *
                MathEx.Sin(dLng / 2) * MathEx.Sin(dLng / 2);

            c = 2 * MathEx.Atan2(MathEx.Sqrt(a), MathEx.Sqrt(1 - a));

            return earthRadius * c;
        }

        private static double toRad(double degrees)
        {
            return (degrees * System.Math.PI / 180);
        }
7

OH, after Skew and I went back and forth on this for a while, we found the issue. Elze Kools definition of PI is wrong! change line 45 to

public static readonly double PI = 3.1415926535897931F;

and everything will work as expected.

8

Why are you define a double and assign a float ???
Instead of “static readonly” you should use “const”. PI has always the same value :wink:

For speedup your ToRad() an ToDeg() you can write it this way:

double const ToRad = PI/180;
double const ToDeg = 180/PI
double ToRad(double x) {
    return x * ToRad;
}
double ToDeg(double x) {
   return x * ToDeg;
}

or remove the ToDeg(x) and ToRad(x) functions complete and use the constants directly.

You do lat/2 and lon/2 a few time. Maybe make this calculations in the constructor of the object and define new fields for them.
Think small :wink:

9

Hi Chris,

I wasn’t the original author of Elze Kool’s lib, so I only issued the patch to his source. I didn’t want to leave too many footprint :slight_smile:

10

I got done with the rest of the lib lat night.

It can now calculate the total distance of an array of points, and all calculations are proven sane. I’ll shoot it up to FEZEr when it’s done being worked on.