public void CorrectDistanceCalculation_Test()
{
var expected = 891.9;
var actual = GeoPoint.GetDistance(new GeoPoint(50, 3), new GeoPoint(58, 4));
Assert.AreEqual(expected, actual, 0.1);
}
public void CorrectDistanceCalculation_Test2()
{
var expected = 0.2;
var actual = GeoPoint.GetDistance(new GeoPoint(89.9, 0), new GeoPoint(89.9, 1));
Assert.AreEqual(expected, actual, 1);
}
private static IList <Edge> getCompletePath(IList <Edge> path, Graph graph)
{
if (path == null || path.Count <= 1)
{
return(path);
}
List <Edge> fullPath = new List <Edge>();
Edge lastEdge = path[0];
fullPath.Add(lastEdge);
for (int i = 1; i < path.Count; ++i)
{
Edge edge = path[i];
if (edge == null)
{
continue;
}
if (lastEdge != null && edge.Start != lastEdge.End)
{
double maxDist = 2000;
if (GeoPoint.GetDistance(lastEdge.End.Point, edge.Start.Point) < maxDist) //fill with shortest path if less than five
{
var edges = graph.FindPath(lastEdge.End, edge.Start, maxDist * 1.5);
if (edges != null)
{
fullPath.AddRange(edges);
}
}
}
fullPath.Add(edge);
lastEdge = edge;
}
return(fullPath);
}
/// <summary>
/// Get the simplified ln version of transportation prob
/// </summary>
/// <param name="MAX_DIST">The maximum allowed distance between 2 adjacent node</param>
/// <param name="e1"></param>
/// <param name="p1"></param>
/// <param name="e2"></param>
/// <param name="p2"></param>
/// <returns></returns>
private double getTransitionProbility(Edge e1, GeoPoint p1, Edge e2, GeoPoint p2, int MAX_DIST = 10000)
{
double prob = double.NegativeInfinity;
double diff = 0;
//1.get difference
double dist = GeoPoint.GetDistance(p1, p2);
//double maxDist = Math.Min(dist + 200, dist * 2 + 25);
double maxDist = Math.Max(dist + 300, dist * 1.5);
Polyline route = null;
if (maxDist < MAX_DIST)
{
route = graph.FindPath(e1, p1, e2, p2, maxDist);
}
if (route != null)
{
double routeLength = route.Length;
if (routeLength < maxDist)
{
diff = Math.Abs(dist - routeLength);
//get prob with diff
//prob = 1 / beta * Math.Exp(-diff / beta);
prob = diff * sBeta;
}
}
return(prob);
}
/// <summary>
/// Build paths within the error ellipse
/// </summary>
/// <param name="currentNode"></param>
/// <returns></returns>
private List <EdgePath> buildPath(TrjTreeNode currentNode)
{
Vertex startV = currentNode.v;
int predIdx = currentNode.idx;
int currentIdx = currentNode.idx + 1;
GeoPoint currentPoint = trj[currentIdx].point;
GeoPoint nextPoint = trj[currentIdx + 1].point;
int interval = (int)(trj[currentIdx + 1].t - trj[currentIdx].t);
double maxSpeed = Constants.MAX_SPEED;
Queue <EdgePath> rawCandidatePaths = new Queue <EdgePath>();
List <EdgePath> candidatePaths = new List <EdgePath>();
//define the maximum allowed distance, i.e., the 2c of the error ellipse
double maxDistance = Math.Min(interval * maxSpeed, Math.Max(GeoPoint.GetDistance(currentPoint, nextPoint) * 2, GeoPoint.GetDistance(currentPoint, nextPoint) + GPS_ERROR_DISTANCE));
//get initial candidate
if (currentNode.path != null && currentNode.path.Count > 0)
{
rawCandidatePaths.Enqueue(new EdgePath()
{
currentNode.path.LastEdge
});
}
else
{
Trace.Assert(currentNode.v.InEdges.Count > 0);
//Choose the first element of the InEdges
EdgePath path = new EdgePath(currentNode.v);
rawCandidatePaths.Enqueue(path);
}
while (rawCandidatePaths.Count > 0)
{
EdgePath path = rawCandidatePaths.Dequeue();
//extend it
foreach (Edge e in path.End.OutEdges)
{
if (path.Contains(e.End)) //avoid duplicate vertices
{
continue;
}
EdgePath newPath = new EdgePath(path);
newPath.Add(e);
GeoPoint p = e.End.Point;
GeoPoint result;
int projectType = e.projectFrom(nextPoint, out result);
double distance = GeoPoint.GetDistance(result, nextPoint);
if (projectType == 0 && distance < GPS_ERROR_DISTANCE)
{
candidatePaths.Add(newPath);
}
else if (GeoPoint.GetDistance(p, currentPoint) + GeoPoint.GetDistance(p, nextPoint) < maxDistance)
{
//need further extension
rawCandidatePaths.Enqueue(newPath);
}
}
}
return(candidatePaths);
}
public void GetDistanceTest()
{
GeoPoint point1 = new GeoPoint(2, 4);
GeoPoint point2 = new GeoPoint(26, 9);
double distance = GeoPoint.GetDistance(point1, point2);
Assert.That(distance, Is.EqualTo(24.5));
}
public void GetDistanceToTest()
{
GeoPoint point1 = new GeoPoint(2, 2);
GeoPoint point2 = new GeoPoint(4, 5);
double distance = GeoPoint.GetDistance(point1, point2);
double distanceto = point1.GetDistanceTo(point2);
Assert.That(distance, Is.EqualTo(distanceto));
}
private double getDev(MotionVector start, MotionVector end, MotionVector middle)
{
Debug.Assert(end.t > start.t && middle.t > start.t && middle.t < end.t);
GeoPoint pStart = start.point, pEnd = end.point;
double lat, lng;
lat = (pEnd.Lat - pStart.Lat) / (end.t - start.t) * (middle.t - start.t);
lng = (pEnd.Lng - pStart.Lng) / (end.t - start.t) * (middle.t - start.t);
double dev = GeoPoint.GetDistance(new GeoPoint(lat, lng), middle.point);
return(dev);
}
private void drawPath(List <Edge> path)
{
if (path.Count < 1)
{
return;
}
// 1. Get full path
List <Edge> fullPath = new List <Edge>();
Edge lastEdge = path[0];
fullPath.Add(lastEdge);
for (int i = 1; i < path.Count; ++i)
{
Edge edge = path[i];
if (edge == null)
{
continue;
}
if (lastEdge != null && edge.Start != lastEdge.End)
{
double maxDist = 2000;
if (GeoPoint.GetDistance(lastEdge.End.Point, edge.Start.Point) < maxDist) //fill with shortest path if less than five
{
var edges = graph.FindPath(lastEdge.End, edge.Start, maxDist * 1.5);
if (edges != null)
{
fullPath.AddRange(edges);
}
}
}
fullPath.Add(edge);
lastEdge = edge;
}
List <Coordinate> route = new List <Coordinate>();
foreach (Edge edge in fullPath)
{
if (edge == null)
{
continue;
}
route.AddRange(edge.Geo.ToCoordinateArray());
}
drawStdLine(new LineString(route.ToArray()));
RefreshMap();
}
private void CheckForCollision()
{
foreach (var ac in AircraftList.ToList())
{
foreach (var ac2 in AircraftList.ToList())
{
if (ac == ac2)
{
break;
}
var distance = GeoPoint.GetDistance(ac.Position, ac2.Position);
if (distance < 500)
{
OnCollision(ac, new CollisionEventArgs(ac2));
}
}
}
}
private Flight GenerateRandomEntity(IList <Location> locations, DateTime startDate, Random random)
{
int attempt = 0;
double distance;
Location location1, location2;
while (true)
{
location1 = locations.ElementAt(random.Next(0, locations.Count()));
location2 = locations.ElementAt(random.Next(0, locations.Count()));
distance = GeoPoint.GetDistance((double)location1.Latitude, (double)location1.Longitude, (double)location2.Latitude, (double)location2.Longitude);
if (ValidateLocations(location1, location2, distance) || attempt++ >= 100)
{
break;
}
}
startDate = startDate
.AddHours(random.Next(-startDate.Hour, 22 - startDate.Hour))
.AddMinutes(random.Next(0, 60));
var distanceKm = distance / 1000;
var flight = new Flight()
{
Name = location1.IATA + "-" + location2.IATA,
StartDateTime = startDate,
EndDateTime = startDate.AddHours((distanceKm != 0) ? Options.Boeing767.Speed / distanceKm * 1.4 : 0),
StartLocationId = location1.Id,
StartLocation = location1,
EndLocationId = location2.Id,
EndLocation = location2
};
return(flight);
}
public void NullArgument_FailTest()
{
GeoPoint.GetDistance(new GeoPoint(0, 0), null);
GeoPoint.GetDistance(null, new GeoPoint(0, 0));
GeoPoint.GetDistance(null, null);
}
public void ArgumentOutOfRange_FailTest()
{
GeoPoint.GetDistance(new GeoPoint(505, -343), new GeoPoint(4334, 32313));
}
//public EdgeList FindPath(Vertex src, Vertex dest, double maxDist = double.MaxValue)
//{
// BigList<Node> openTable = new BigList<Node>();
// HashSet<long> openVertexId = new HashSet<long>();
// HashSet<Node> closedSet = new HashSet<Node>();
// GeoPoint srcPoint = src.ToPoint();
// GeoPoint destPoint = dest.ToPoint();
// //push initial node
// double estimatedCost = GeoPoint.GetDistance(destPoint, srcPoint);
// Node n = new Node(src, 0, estimatedCost, null, null);
// openTable.Add(n);
// openVertexId.Add(src.ID);
// //Begin search
// Node solution = null;
// while (openTable.Count > 0)
// {
// //Pop the
// //Node parent = openTable[0];
// Node parent = openTable[0];
// openTable.RemoveAt(0);
// closedSet.Add(parent);
// openVertexId.Remove(parent.v.ID);
// if (parent.Weight > maxDist)
// {
// //Stop searching
// break;
// }
// if (parent.v == dest)
// {
// solution = parent;
// break;
// }
// //Get children
// List<Edge> edges = parent.v.OutEdges;
// for (int i = 0; i < edges.Count; i++)
// {
// Edge currentEdge = edges[i];
// double g = parent.g + currentEdge.Length;
// Vertex v = currentEdge.End;
// double h = GeoPoint.GetDistance(v.ToPoint(), destPoint);
// Node newNode = new Node(v, g, h, currentEdge, parent);
// if (closedSet.Contains(newNode))
// {
// Node tmpNode = closedSet.Where(node => node.v.ID == v.ID).FirstOrDefault();
// if (newNode.Weight >= tmpNode.Weight)
// {
// //this is a visited node
// continue;
// }
// else
// {
// closedSet.Remove(tmpNode);
// }
// }
// if (openVertexId.Contains(v.ID))
// {
// //Check if it has a lower cost
// Node oldNode = null;
// int idx = 0;
// for (; idx < openTable.Count; idx++)
// {
// if (openTable[idx].v == v)
// {
// oldNode = openTable[idx];
// break;
// }
// }
// Debug.Assert(idx < openTable.Count);
// if (oldNode.Weight > h + g)
// {
// openTable.RemoveAt(idx);
// oldNode.g = g;
// oldNode.h = h;
// oldNode.e = currentEdge;
// oldNode.parent = parent;
// openTable.Add(oldNode);
// }
// }
// else
// {
// openTable.Add(newNode);
// openVertexId.Add(newNode.v.ID);
// }
// }
// }
// //Find path
// List<Edge> path = null;
// EdgeList list = null;
// if (solution != null)
// {
// path = new List<Edge>();
// while (solution.parent != null)
// {
// path.Add(solution.e);
// solution = solution.parent;
// }
// path.Reverse();
// list = new EdgeList(path);
// }
// return list;
//}
//private void testEqual(BigList<Node> a, HashSet<long> b)
//{
// Debug.Assert(a.Count == b.Count);
// foreach (var n in a)
// {
// Debug.Assert(b.Contains(n.v.ID));
// }
//}
public HashSet <Edge> GetCandiateEdges(Vertex src, GeoPoint destPoint, double maxCost, double maxDist)
{
SortedDictionary <Node, Node> openTable = new SortedDictionary <Node, Node>();
Dictionary <long, Node> openVertexId = new Dictionary <long, Node>(); //Used to get a node by node id
Dictionary <long, Node> closedSet = new Dictionary <long, Node>();
GeoPoint srcPoint = src.ToPoint();
HashSet <Edge> cands = new HashSet <Edge>();
//GeoPoint destPoint = dest;
//push initial node
double estimatedCost = GeoPoint.GetDistance(destPoint, srcPoint);
Node n = new Node(src, 0, estimatedCost, null, null);
openTable.Add(n, n);
openVertexId.Add(src.ID, n);
//Begin search
while (openTable.Count > 0)
{
//Pop the node with minimum weight
Node parent = openTable.Keys.First();
openTable.Remove(parent);
openVertexId.Remove(parent.v.ID);
closedSet.Add(parent.v.ID, parent);
if (parent.Weight > maxCost)
{
//Stop searching
break;
}
if (parent.e != null && parent.e.DistFrom(destPoint) < maxDist)
{
cands.Add(parent.e);
}
//Get children
List <Edge> edges = parent.v.OutEdges;
for (int i = 0; i < edges.Count; i++)
{
Edge currentEdge = edges[i];
//double g = parent.g + currentEdge.Length;
double g = parent.g;
GeoPoint result;
if (parent.e != null)
{
g += parent.e.Length;
}
Vertex v = currentEdge.End;
int type = currentEdge.projectFrom(destPoint, out result);
double h = 0;
if (type == 0)
{
h = GeoPoint.GetDistance(currentEdge.Start.ToPoint(), result) + GeoPoint.GetDistance(result, destPoint);
}
else
{
h = currentEdge.Length + GeoPoint.GetDistance(currentEdge.End.ToPoint(), destPoint);
}
Node tmpNode = null;
if (closedSet.TryGetValue(v.ID, out tmpNode))
{
if (g + h >= tmpNode.Weight)
{
//this is a visited node
continue;
}
else
{
closedSet.Remove(v.ID);
}
}
if (openVertexId.TryGetValue(v.ID, out tmpNode))
{
//Check if it has a lower cost
if (tmpNode.Weight > h + g)
{
Debug.Assert(openTable.ContainsKey(tmpNode));
openTable.Remove(tmpNode);
tmpNode.g = g;
tmpNode.h = h;
tmpNode.e = currentEdge;
tmpNode.parent = parent;
openTable.Add(tmpNode, tmpNode);
openVertexId[v.ID] = tmpNode;
}
}
else
{
Node newNode = new Node(v, g, h, currentEdge, parent);
openTable.Add(newNode, newNode);
openVertexId.Add(v.ID, newNode);
}
}
}
return(cands);
}
public EdgePath FindPath(Vertex src, Vertex dest, double maxDist = double.MaxValue)
{
SortedDictionary <Node, Node> openTable = new SortedDictionary <Node, Node>();
Dictionary <long, Node> openVertexId = new Dictionary <long, Node>(); //Used to get a node by node id
Dictionary <long, Node> closedSet = new Dictionary <long, Node>();
GeoPoint srcPoint = src.ToPoint();
GeoPoint destPoint = dest.ToPoint();
//push initial node
double estimatedCost = GeoPoint.GetDistance(destPoint, srcPoint);
Node n = new Node(src, 0, estimatedCost, null, null);
openTable.Add(n, n);
openVertexId.Add(src.ID, n);
//Begin search
Node solution = null;
bool removed = false;
while (openTable.Count > 0)
{
//Pop the node with minimum weight
Node parent = openTable.Keys.First();
removed = openTable.Remove(parent);
Debug.Assert(removed);
removed = openVertexId.Remove(parent.v.ID);
Debug.Assert(removed);
closedSet.Add(parent.v.ID, parent);
if (parent.Weight > maxDist)
{
//Stop searching
break;
}
if (parent.v == dest)
{
solution = parent;
break;
}
//Get children
List <Edge> edges = parent.v.OutEdges;
for (int i = 0; i < edges.Count; i++)
{
Edge currentEdge = edges[i];
double g = parent.g + currentEdge.Length;
Vertex v = currentEdge.End;
double h = GeoPoint.GetDistance(v.ToPoint(), destPoint);
Node tmpNode = null;
if (closedSet.TryGetValue(v.ID, out tmpNode))
{
if (g + h >= tmpNode.Weight)
{
//this is a visited node
continue;
}
else
{
removed = closedSet.Remove(v.ID);
Debug.Assert(removed);
}
}
if (openVertexId.TryGetValue(v.ID, out tmpNode))
{
//Check if it has a lower cost
if (tmpNode.Weight > h + g)
{
Debug.Assert(openTable.ContainsKey(tmpNode));
removed = openTable.Remove(tmpNode);
Debug.Assert(removed);
tmpNode.g = g;
tmpNode.h = h;
tmpNode.e = currentEdge;
tmpNode.parent = parent;
openTable.Add(tmpNode, tmpNode);
openVertexId[v.ID] = tmpNode;
}
}
else
{
Node newNode = new Node(v, g, h, currentEdge, parent);
openTable.Add(newNode, newNode);
openVertexId.Add(v.ID, newNode);
}
}
}
//Find path
List <Edge> path = null;
EdgePath list = null;
if (solution != null)
{
path = new List <Edge>();
while (solution.parent != null)
{
path.Add(solution.e);
solution = solution.parent;
}
path.Reverse();
list = new EdgePath(path);
}
return(list);
}
