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; }