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;
}
private static int compareRap(RoadAndPr a, RoadAndPr b)
{
if (a.pr < b.pr)
return -1;
else if (a.pr == b.pr)
return 0;
else
return 1;
}