private List <MapNode> findPath(MapNode start, MapNode end, bool eightDirection = false, bool useBinaryHeap = true) { if (useBinaryHeap) { _open = new BinaryHeapOpenList <MapNode>(); } else { _open = new ListOpenList <MapNode>(); } _closed = new List <MapNode>(); List <MapNode> result = new List <MapNode>(); start.parentNode = null; //设置或者不设置为0,问题不大。唯一的可能问题是寻路多次之后有可能溢出 start.g = 0; start.h = 0; start.f = 0; //----------------------- if (end == null) { return(result); } if (!end.isPassAble()) { return(result); } end.parentNode = null; _open.push(start); while ((_open.getCount() > 0) && (_closed.IndexOf(end) == -1)) { //if (_open is ListOpenList<MapNode>) //{ // (_open as ListOpenList<MapNode>).printSelf(); //} _current = _open.getSmallest(); checkAround(_current, end, eightDirection); _closed.Add(_current); } MapNode c = end; while (c != null) { result.Insert(0, c); c = c.parentNode; } if (result.Count == 1) { result.Clear(); result.Add(start); } return(result); }
public AStar(IEnumerable<IntVector3> initialLocations, IAStarTarget target) { this.MaxNodeCount = 200000; this.CancellationToken = CancellationToken.None; m_target = target; m_nodeMap = new Dictionary<IntVector3, AStarNode>(); m_openList = new BinaryHeap<AStarNode>(); foreach (var p in initialLocations) { ushort g = 0; ushort h = m_target.GetHeuristic(p); var node = new AStarNode(p, null); node.G = g; node.H = h; m_openList.Add(node); m_nodeMap.Add(p, node); } }
public AStar(IEnumerable <IntVector3> initialLocations, IAStarTarget target) { this.MaxNodeCount = 200000; this.CancellationToken = CancellationToken.None; m_target = target; m_nodeMap = new Dictionary <IntVector3, AStarNode>(); m_openList = new BinaryHeap <AStarNode>(); foreach (var p in initialLocations) { ushort g = 0; ushort h = m_target.GetHeuristic(p); var node = new AStarNode(p, null); node.G = g; node.H = h; m_openList.Add(node); m_nodeMap.Add(p, node); } }
public AStarImpl(IAStarEnvironment environment, IntPoint3 src, DirectionSet srcPositioning, IAStarTarget target, int maxNodeCount = 200000, CancellationToken? cancellationToken = null) { m_environment = environment; m_src = src; m_srcPositioning = srcPositioning; m_maxNodeCount = maxNodeCount; m_cancellationToken = cancellationToken.HasValue ? cancellationToken.Value : CancellationToken.None; m_target = target; m_nodeMap = new Dictionary<IntPoint3, AStarNode>(); m_openList = new BinaryHeap<AStarNode>(); AddInitialNodes(); }
public Path FindPath(Cell startCell, Cell endCell, bool allowDiagonals, int movementPoints = (short)-1) { var success = false; var matrix = new PathNode[Map.MapSize + 1]; IOpenList openList = m_useLogNodeSearch ? (IOpenList) new LogOpenList(new ComparePfNodeMatrix(matrix)) : new LinearOpenList(new ComparePfNodeMatrix(matrix)); var closedList = new List <PathNode>(); var location = startCell; var counter = 0; if (movementPoints == 0) { return(Path.GetEmptyPath(m_map, startCell)); } matrix[location.Id].Cell = location; matrix[location.Id].Parent = null; matrix[location.Id].Cost = 0; matrix[location.Id].Heuristic = 0; matrix[location.Id].Status = NodeState.Open; var distToEnd = startCell.ManhattanDistanceTo(endCell); var endCellAux = startCell; openList.Push(location); while (openList.Count > 0) { location = openList.Pop(); matrix[location.Id].Status = NodeState.Closed; if (location == endCell) { success = true; break; } if (counter > SearchLimit) { return(Path.GetEmptyPath(m_map, startCell)); } for (int i = 0; i < 8; i++) { var isDiagonal = DiagonalsDirections.Contains(i); if (isDiagonal && !allowDiagonals) { continue; } var newLocation = location.GetNearestCellInDirection(Directions[i]); if (newLocation == null) { continue; } if (newLocation.Id < 0 || newLocation.Id >= Map.MapSize) { continue; } if (matrix[newLocation.Id].Status == NodeState.Closed) { continue; } if (!m_context.IsCellWalkable(newLocation)) { continue; } double baseCost; if (newLocation == endCell) { baseCost = 1; } else { baseCost = GetCellCost(newLocation, m_throughEntities); } double cost = matrix[location.Id].Cost + baseCost * (isDiagonal ? DiagonalCost : HorizontalCost); // adjust the cost if the current cell is aligned with the start cell or the end cell if (m_throughEntities) { bool alignedWithEnd = newLocation.X + newLocation.Y == endCell.X + endCell.Y || newLocation.X - newLocation.Y == endCell.X - endCell.Y; bool alignedWithStart = newLocation.X + newLocation.Y == startCell.X + startCell.Y || newLocation.X - newLocation.Y == startCell.X - startCell.Y; if (newLocation.X + newLocation.Y != endCell.X + endCell.Y && newLocation.X - newLocation.Y != endCell.X - endCell.Y || newLocation.X + newLocation.Y != startCell.X + startCell.Y && newLocation.X - newLocation.Y != startCell.X - startCell.Y) { cost += newLocation.ManhattanDistanceTo(endCell); cost += newLocation.ManhattanDistanceTo(startCell); } // tests diagonales now if (newLocation.X == endCell.X || newLocation.Y == endCell.Y) { cost -= 3; } if (alignedWithEnd || !isDiagonal) { cost -= 2; } if (newLocation.X == startCell.X || newLocation.Y == startCell.Y) { cost -= 3; } if (alignedWithStart) { cost -= 2; } var currentDistToEnd = newLocation.ManhattanDistanceTo(endCell); if (currentDistToEnd < distToEnd) { // if aligned with end if (newLocation.X == endCell.X || newLocation.Y == endCell.Y || alignedWithEnd) { distToEnd = currentDistToEnd; endCellAux = newLocation; } } } if (matrix[newLocation.Id].Status == NodeState.Open) { if (matrix[newLocation.Id].Cost <= cost) { continue; } matrix[newLocation.Id].Parent = location; matrix[newLocation.Id].Cost = cost; } else { matrix[newLocation.Id].Cell = newLocation; matrix[newLocation.Id].Parent = location; matrix[newLocation.Id].Cost = cost; matrix[newLocation.Id].Heuristic = GetHeuristic(newLocation, endCell); openList.Push(newLocation); } matrix[newLocation.Id].Status = NodeState.Open; } counter++; } if (success) { var node = matrix[endCell.Id]; // use auxiliary end if not found if (node.Status != NodeState.Closed) { node = matrix[endCellAux.Id]; } while (node.Parent != null) { closedList.Add(node); node = matrix[node.Parent.Id]; } closedList.Add(node); } closedList.Reverse(); if (allowDiagonals) { return(CreateAndOptimisePath(closedList)); } else { if (movementPoints > 0 && closedList.Count > movementPoints + 1) { return(new Path(m_map, closedList.Take(movementPoints + 1).Select(entry => entry.Cell))); } return(new Path(m_map, closedList.Select(entry => entry.Cell))); } }
/// <summary> /// 对比测试用的非启发算法,不要使用 /// </summary> /// <param name="startX"></param> /// <param name="startY"></param> /// <param name="endX"></param> /// <param name="endY"></param> /// <param name="eightDirection"></param> /// <param name="useBinaryHeap"></param> /// <returns></returns> public List <MapNode> findPathByXYNoInspire(int startX, int startY, int endX, int endY, bool eightDirection = false) { //Debug.Log(startX + " " + startY + " " + endX + " " + endY); List <MapNode> result = new List <MapNode>(); if (startX == endX && startY == endY) { result.Add(_mapdata[startY, startX]); return(result); } MapNode start = _mapdata[startY, startX]; MapNode end = _mapdata[endY, endX]; _open = new ListOpenList <MapNode>(); _open.push(start); start.parentNode = null; start.g = start.h = start.f = 0; end.parentNode = null; _closed = new List <MapNode>(); while (_open.getCount() > 0 && (_closed.IndexOf(end) == -1)) { //不去是用启发算法去找一个可能最优的,而是随便找一个来计算 MapNode mn = (_open as ListOpenList <MapNode>).pop(); //检查周围 if (!eightDirection) { for (int i = 0; i < 4; i++) { int _x = mn.x + _xd[i]; int _y = mn.y + _yd[i]; if (_x < 0 || _y < 0 || (_x >= _width) || (_y >= _height)) { continue; } MapNode testN = _mapdata[_y, _x]; if (_closed.IndexOf(testN) != -1) { continue; } if (!testN.isPassAble()) { _closed.Add(testN); testN.parentNode = mn; continue; } if (_open.indexOf(testN) != -1) { if (testN.g > mn.g + testN.weight) { testN.g = mn.g + testN.weight; testN.parentNode = mn; } continue; } else { _open.push(testN); testN.parentNode = mn; testN.g = mn.g + testN.weight; } } } else { for (int i = 0; i < 8; i++) { int _x = mn.x + _xde[i]; int _y = mn.y + _yde[i]; if (_x < 0 || _y < 0 || (_x >= _width) || (_y >= _height)) { continue; } MapNode testN = _mapdata[_y, _x]; if (_closed.IndexOf(testN) != -1) { continue; } if (!testN.isPassAble()) { _closed.Add(testN); testN.parentNode = mn; continue; } if (_open.indexOf(testN) != -1) { if (testN.g > mn.g + testN.weight) { testN.g = mn.g + testN.weight; testN.parentNode = mn; } continue; } else { _open.push(testN); testN.parentNode = mn; testN.g = mn.g + testN.weight; } } } //加入closed _closed.Add(mn); } MapNode c = end; while (c != null) { result.Insert(0, c); c = c.parentNode; } if (result.Count == 1) { result.Clear(); result.Add(start); } if (result.Count < 2) { //找不到路 } return(result); }