private double MeasurementProbability(Point p, Edge e, double miu_o, double sigma_o, double miu_b, double sigma_b)
{
//observation probability
double p_lng, p_lat;
p_lng = p.X;
p_lat = p.Y;
Point p_prj = e.projectFrom(p);
double p_prj_lng, p_prj_lat;
p_prj_lng = p_prj.X;
p_prj_lat = p_prj.Y;
double great_circle_dis = p.DistanceFrom(p_prj);
double measurementProb1, measurementProb2 = 0;
double var1 = 1.0 / (Math.Sqrt(2 * Math.PI) * sigma_o);
double var2 = (great_circle_dis - miu_o) / sigma_o;
//direction analysis
//double theta = Geometry.getAngle(e);
double derta = Geometry.getAnglediff(p.Direction, e);
double var11 = 1.0 / (Math.Sqrt(2 * Math.PI) * sigma_b);
double var22 = (derta - miu_b) / sigma_b;
measurementProb1 = var1 * Math.Exp(-0.5 * Math.Pow(var2, 2));
measurementProb2 = var11 * Math.Exp(-0.5 * Math.Pow(var22, 2));
//Console.WriteLine("{0},{1}",great_circle_dis,measurementProb);
return(measurementProb1 * measurementProb2 * 100);
}
public double getLength(Graph g, Point p1, Point p2)
{
path_edges = this.getPath_EdgeList(g);
Edge e1 = path_edges[0];
Edge e11 = path_edges[1];
Edge e2 = path_edges[path_edges.Count - 2];
Edge e22 = path_edges[path_edges.Count - 1];
Point p1_prj = e1.projectFrom(p1);
Point p2_prj = e22.projectFrom(p2);
double length = RouteDistance(p1_prj, e1.getConnectedV(e11), p2_prj, e2.getConnectedV(e22), g, e1.ID == e2.ID);
return(length);
}
/// <summary>
/// calculate route distance by search the shortest route between the from vertex of the two edges
/// this method may be not accurate.
/// <returns></returns>
private Edgespp RouteDistance(Point p1, Edge e1, Point p2, Edge e2, Graph g)
{
double routeDis = 0;
Point p1_prj = e1.projectFrom(p1);
Vertex e1_to = e1.To;
double dis1 = p1_prj.DistanceFrom(e1_to.toPoint());
Point p2_prj = e2.projectFrom(p2);
Vertex e2_from = e2.From;
double dis2 = p2_prj.DistanceFrom(e2_from.toPoint());
if (e1 == e2)
{
return(new Edgespp(p1_prj.DistanceFrom(p2_prj), new List <Edge>()
{
e1
}));
}
else if (e1_to.ID == e2_from.ID)
{
routeDis = dis1 + dis2;
return(new Edgespp(routeDis, new List <Edge>()
{
e1, e2
}));
}
else
{
Dijkstra dij = new Dijkstra(g);
LinkedList <Vertex> shortestPath_vertices = dij.ShortestPath_ScoreValue(e1_to, e2_from, _distance_limit);
//Route rs = getminRoute(F, e1.ID, e2.ID, p1, p2);
if (shortestPath_vertices == null)
{
return(null);
}
List <Edge> shortestPath_edges = dij.MakeShortestPath_EdgeList();
//List<Edge> shortestPath_edges = rs.getPath_EdgeList(m_graph);
for (int i = 0; i < shortestPath_edges.Count; i++)
{
routeDis += shortestPath_edges[i].Length;
}
if (shortestPath_edges.Contains(e1))
{
routeDis -= dis1;
shortestPath_edges.Remove(e1);
}
else
{
routeDis += dis1;
}
if (shortestPath_edges.Contains(e2))
{
routeDis -= dis2;
shortestPath_edges.Remove(e2);
}
else
{
routeDis += dis2;
}
return(new Edgespp(routeDis, shortestPath_edges));
}
}