source: branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Osm/Utils.cs @ 8471

using System;
using System.Collections.Generic;

namespace HeuristicLab.Problems.RoutePlanning.Osm {
  public static class Utils {
    public static double Distance(PointD p1, PointD p2) {
      double dx = p2.X - p1.X;
      double dy = p2.Y - p1.Y;
      return Math.Sqrt(dx * dx + dy * dy);
    }

    public static double LocationDistance(PointD p1, PointD p2) {
      // The Haversine formula
      /* dlon = lon2 - lon1
            dlat = lat2 - lat1
            a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
            c = 2 * atan2(sqrt(a), sqrt(1-a)) 
            d = R * c
       */

      double lat1Rad = p1.Y * (Math.PI / 180.0);
      double long1Rad = p1.X * (Math.PI / 180.0);
      double lat2Rad = p2.Y * (Math.PI / 180.0);
      double long2Rad = p2.X * (Math.PI / 180.0);

      double longitude = long2Rad - long1Rad;
      double latitude = lat2Rad - lat1Rad;

      double a = Math.Pow(Math.Sin(latitude / 2.0), 2.0) +
                   Math.Cos(lat1Rad) * Math.Cos(lat2Rad) *
                   Math.Pow(Math.Sin(longitude / 2.0), 2.0);

      double c = 2.0 * Math.Asin(Math.Sqrt(a));

      const double earthRadiusKms = 6376.5;
      double distance = earthRadiusKms * c;

      return distance;
    }

    public static double PathLength(IList<PointD> points) {
      double length = 0;
      for (int i = 1; i < points.Count; i++) {
        length += Distance((PointD)points[i - 1], (PointD)points[i]);
      }
      return length;
    }

    public static RectangleD FindBox(IList<PointD> points) {
      double minX = double.MaxValue;
      double maxX = double.MinValue;
      double minY = double.MaxValue;
      double maxY = double.MinValue;

      foreach (PointD p in points) {
        if (p.X < minX)
          minX = p.X;
        if (p.X > maxX)
          maxX = p.X;

        if (p.Y < minY)
          minY = p.Y;
        if (p.Y > maxY)
          maxY = p.Y;
      }
      return new RectangleD(minX, minY, maxX - minX, maxY - minY);
    }

    public static double PathDistance(IList<PointD> path1, IList<PointD> path2) {
      double distance = 0;
      for (int i = 0; i < path1.Count; i++) {
        distance += Distance((PointD)path1[i], (PointD)path2[i]);
      }
      return distance / path1.Count;
    }
  }
}