source: branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Graph/EarthDistanceCostCalculator.cs @ 8488

using System;

namespace HeuristicLab.Problems.RoutePlanning.Graph {
  public class EarthDistanceCostCalculator : ICostCalculator {
    private const double earthMeanRadius = 6371.0 * 1000.0;

    #region ICostCalculator Members

    public float Costs(Edge<Vertex> edge) {
      return edge.Weight;
    }

    public float EstimatedCosts(Vertex source, Vertex destination) {
      return (float)EarthDistance(source.Logitude, source.Latitude, destination.Logitude, destination.Latitude);
    }

    #endregion

    private double DegToRad(double alpha) {
      return alpha * Math.PI / 180;
    }

    private double AngularDistance(double startLon, double startLat, double endLon, double endLat) {
      double lat1 = DegToRad(startLat);
      double lat2 = DegToRad(endLat);
      double long1 = DegToRad(startLon);
      double long2 = DegToRad(endLat);

      double x = Math.Sin(lat1) * Math.Sin(lat2) + Math.Cos(lat1) * Math.Cos(lat2) * Math.Cos(long1 - long2);
      x = Math.Min(1, Math.Max(0, x));

      return Math.Acos(x);
    }

    private double EarthDistance(double startLon, double startLat, double endLon, double endLat) {
      return earthMeanRadius * AngularDistance(startLon, startLat, endLon, endLat);
    }
  }
}