private SearchNode searchSolution(SearchNode node)
{
int roadNum = node.acertainedRoute.Count;
if (roadNum == m_originRoute.Count + 1)
{
if (node.acPr > m_lowBound)
m_lowBound = node.acPr;
return node;
}
TurnNode tn = m_originRoute[roadNum - 1];
Road lastRoad = m_region.getRoadByID(node.acertainedRoute.Last());
if (lastRoad.tailEnd.outRoads == null)
return null;
Normal n = new Normal(tn.observedInAngle, m_sigma);
List<RoadAndPr> sortedRoads = new List<RoadAndPr>();
foreach (Road road in lastRoad.tailEnd.outRoads)
{
if (!m_region.containCross(road.tailEnd)||
m_region.getRoadByID(road.id)==null)
continue;
int switchAngle;
if (road.headEndAngle - tn.observedInAngle >= 180)
switchAngle = road.headEndAngle - 360;
else if (road.headEndAngle - tn.observedInAngle < -180)
switchAngle = road.headEndAngle + 360;
else
switchAngle = road.headEndAngle;
if (Math.Abs(switchAngle - tn.observedInAngle) > m_threshold)
continue;
double pr = n.calPr(switchAngle);
RoadAndPr rap = new RoadAndPr(road, pr);
sortedRoads.Add(rap);
}
sortedRoads.Sort(compareRap);
if (sortedRoads.Count == 0)
return null;
SearchNode solution = null, solutionTemp = null;
for (int i = sortedRoads.Count - 1; i >= 0; i--)
{
double pr = sortedRoads[i].pr;
Road road = sortedRoads[i].road;
if ((decimal)pr <= m_lowBound)
continue;
SearchNode sn = new SearchNode(node);
sn.acertainedRoute.Add(road.id);
sn.acPr *= (decimal)pr;
solutionTemp = searchSolution(sn);
if (solutionTemp != null)
{
if (solution == null || solutionTemp.acPr > solution.acPr)
solution = solutionTemp;
}
}
return solution;
}
public List<int> estRoute(List<TurnNode> originRoute)
{
m_originRoute = originRoute;
m_lowBound = 0;
Normal n = new Normal(m_originRoute[0].observedOutAngle, m_sigma);
SearchNode solution = null, tempSolution = null;
for (int i = 0; i <= m_threshold; i++)
{
int angle = (int)m_originRoute[0].observedOutAngle + i;
if (angle >= 360)
angle -= 360;
else if (angle < 0)
angle += 360;
double pr;
if(m_roadInAngle.ContainsKey(angle))
{
pr = n.calPr((int)m_originRoute[0].observedOutAngle + i);
foreach (Road road in m_roadInAngle[angle])
{
if ((decimal)pr <= m_lowBound)
continue;
SearchNode sn = new SearchNode();
sn.acertainedRoute = new List<int>();
sn.acertainedRoute.Add(road.id);
sn.acPr = (decimal)pr;
tempSolution = searchSolution(sn);
if (tempSolution != null)
{
if (solution == null || tempSolution.acPr > solution.acPr)
solution = tempSolution;
}
}
}
angle = (int)m_originRoute[0].observedOutAngle - i;
if (angle >= 360)
angle -= 360;
else if (angle < 0)
angle += 360;
if(m_roadInAngle.ContainsKey(angle))
{
pr = n.calPr((int)m_originRoute[0].observedOutAngle - i);
foreach (Road road in m_roadInAngle[angle])
{
if ((decimal)pr <= m_lowBound)
continue;
SearchNode sn = new SearchNode();
sn.acertainedRoute = new List<int>();
sn.acertainedRoute.Add(road.id);
sn.acPr = (decimal)pr;
tempSolution = searchSolution(sn);
if (tempSolution != null)
{
if (solution == null || tempSolution.acPr > solution.acPr)
solution = tempSolution;
}
}
}
}
return solution.acertainedRoute;
}
public SearchNode(SearchNode sN)
{
acertainedRoute = new List<int>(sN.acertainedRoute);
acPr = sN.acPr;
}