1 | using System;
|
---|
2 | using System.Collections.Generic;
|
---|
3 |
|
---|
4 | namespace HeuristicLab.Problems.RoutePlanning.Osm {
|
---|
5 | public static class Utils {
|
---|
6 | public static double Distance(PointD p1, PointD p2) {
|
---|
7 | double dx = p2.X - p1.X;
|
---|
8 | double dy = p2.Y - p1.Y;
|
---|
9 | return Math.Sqrt(dx * dx + dy * dy);
|
---|
10 | }
|
---|
11 |
|
---|
12 | public static double Distance(double startX, double startY, double endX, double endY) {
|
---|
13 | double dx = endX - startX;
|
---|
14 | double dy = endY - startY;
|
---|
15 | return Math.Sqrt(dx * dx + dy * dy);
|
---|
16 | }
|
---|
17 |
|
---|
18 | public static double LocationDistance(PointD p1, PointD p2) {
|
---|
19 | // The Haversine formula
|
---|
20 | /* dlon = lon2 - lon1
|
---|
21 | dlat = lat2 - lat1
|
---|
22 | a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
|
---|
23 | c = 2 * atan2(sqrt(a), sqrt(1-a))
|
---|
24 | d = R * c
|
---|
25 | */
|
---|
26 |
|
---|
27 | double lat1Rad = p1.Y * (Math.PI / 180.0);
|
---|
28 | double long1Rad = p1.X * (Math.PI / 180.0);
|
---|
29 | double lat2Rad = p2.Y * (Math.PI / 180.0);
|
---|
30 | double long2Rad = p2.X * (Math.PI / 180.0);
|
---|
31 |
|
---|
32 | double longitude = long2Rad - long1Rad;
|
---|
33 | double latitude = lat2Rad - lat1Rad;
|
---|
34 |
|
---|
35 | double a = Math.Pow(Math.Sin(latitude / 2.0), 2.0) +
|
---|
36 | Math.Cos(lat1Rad) * Math.Cos(lat2Rad) *
|
---|
37 | Math.Pow(Math.Sin(longitude / 2.0), 2.0);
|
---|
38 |
|
---|
39 | double c = 2.0 * Math.Asin(Math.Sqrt(a));
|
---|
40 |
|
---|
41 | const double earthRadiusKms = 6376.5;
|
---|
42 | double distance = earthRadiusKms * c;
|
---|
43 |
|
---|
44 | return distance;
|
---|
45 | }
|
---|
46 |
|
---|
47 | public static double PathLength(IList<PointD> points) {
|
---|
48 | double length = 0;
|
---|
49 | for (int i = 1; i < points.Count; i++) {
|
---|
50 | length += Distance((PointD)points[i - 1], (PointD)points[i]);
|
---|
51 | }
|
---|
52 | return length;
|
---|
53 | }
|
---|
54 |
|
---|
55 | public static RectangleD FindBox(IList<PointD> points) {
|
---|
56 | double minX = double.MaxValue;
|
---|
57 | double maxX = double.MinValue;
|
---|
58 | double minY = double.MaxValue;
|
---|
59 | double maxY = double.MinValue;
|
---|
60 |
|
---|
61 | foreach (PointD p in points) {
|
---|
62 | if (p.X < minX)
|
---|
63 | minX = p.X;
|
---|
64 | if (p.X > maxX)
|
---|
65 | maxX = p.X;
|
---|
66 |
|
---|
67 | if (p.Y < minY)
|
---|
68 | minY = p.Y;
|
---|
69 | if (p.Y > maxY)
|
---|
70 | maxY = p.Y;
|
---|
71 | }
|
---|
72 | return new RectangleD(minX, minY, maxX - minX, maxY - minY);
|
---|
73 | }
|
---|
74 |
|
---|
75 | public static double PathDistance(IList<PointD> path1, IList<PointD> path2) {
|
---|
76 | double distance = 0;
|
---|
77 | for (int i = 0; i < path1.Count; i++) {
|
---|
78 | distance += Distance((PointD)path1[i], (PointD)path2[i]);
|
---|
79 | }
|
---|
80 | return distance / path1.Count;
|
---|
81 | }
|
---|
82 | }
|
---|
83 | }
|
---|
84 |
|
---|