override public void doStep() { if (openSet.Count == 0 && closedSet.Count == 0) { initAlgorithm(); } else if (Finished || openSet.Count == 0) { Finished = true; return; } Node activeNode = openSet [0]; foreach (Node node in openSet) { if (nodeData [node].FCost <= nodeData [activeNode].FCost) { activeNode = node; } } openSet.Remove(activeNode); closedSet.Add(activeNode); activeNode.Type = NodeType.CLOSED; if (activeNode == grid.TargetNode) { Finished = true; paintPath(); return; } foreach (Node neighbour in grid.getNeighbours(activeNode)) { if (closedSet.Contains(neighbour)) { continue; } AStarData neighbourData; if (!nodeData.TryGetValue(neighbour, out neighbourData)) { neighbourData = new AStarData(); nodeData.Add(neighbour, neighbourData); } int newCostToNeighbour = nodeData [activeNode].GCost + getNodeDistance(activeNode, neighbour); if (newCostToNeighbour < neighbourData.GCost || !openSet.Contains(neighbour)) { neighbourData.GCost = newCostToNeighbour; neighbourData.HCost = getNodeDistance(neighbour, grid.TargetNode); neighbourData.Parent = activeNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); neighbour.Type = NodeType.OPEN; } } } }
public Map2dAStar(Func <bool> canJump, MazeLevel maze, VisionMapping vision, Transform transform, GameObject prefab_debug_astar) { this.maze = maze; this.prefab_debug_astar = prefab_debug_astar; SetAstarSourceData(vision, () => maze.Map, canJump, ResetCalcSpace, (Coord c, out Coord n) => { AStarData a = calcSpace.At(c); Coord f = a.from; //e = a._edge; n = f != Coord.NegativeOne ? f : c; return(a._edge); }, (c, f, e) => { AStarData a = calcSpace.At(c); a._edge = e; a.from = f; }, c => calcSpace.At(c).f, (c, f) => { calcSpace.At(c).f = f; }, c => calcSpace.At(c).g, (c, f) => { calcSpace.At(c).g = f; }); ResetCalcSpace(Map.GetSize()); Vector3 p = transform.position; Coord here = maze.GetCoord(p); Start(here, here); if (prefab_debug_astar != null) { maze.seen.onChange += RefreshVision; vision.onChange += RefreshVision; } }
public AStarData(AStarData data, bool ventured) { this.previous_tile = data.previous_tile; this.distance = data.distance; this.waste = data.waste; this.ventured = ventured; }
public AStarGrid(int x, int y, int g, int h) { X = x; Y = y; data = new AStarData(g, h); }
private List <NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end) { AStarData startData = new AStarData { cameFrom = null, fScore = 0, gScore = 0, hScore = 0 }; Dictionary <NavMesh2DNode, NodeDataPair> openList = new Dictionary <NavMesh2DNode, NodeDataPair>(); Dictionary <NavMesh2DNode, NodeDataPair> closedList = new Dictionary <NavMesh2DNode, NodeDataPair>(); openList.Add(start, new NodeDataPair { n = start, d = startData }); while (openList.Count != 0) { NodeDataPair current = null; { float lowestFscore = float.MaxValue; foreach (NodeDataPair n in openList.Values) { if (n.d.fScore < lowestFscore) { lowestFscore = n.d.fScore; current = n; } } } openList.Remove(current.n); closedList.Add(current.n, current); if (closedList.ContainsKey(end)) { //found a path List <NavMesh2DNode> pathFound = new List <NavMesh2DNode>(); ReconstructPath(current, ref pathFound); pathFound.Reverse(); //Path path = new Path(startNode, endNode); //FindGoingTo(endNode, null); //ReconstructPath(startNode, ref path); //path.Smooth(); return(pathFound); } foreach (NavMesh2DConnection connection in current.n.connections) { NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex); if (closedList.ContainsKey(connectedMesh2DNode)) { continue; } NodeDataPair pairAlreadyInOpenlist = null; if (openList.ContainsKey(connectedMesh2DNode)) { pairAlreadyInOpenlist = openList[connectedMesh2DNode]; } if (pairAlreadyInOpenlist == null) { //if the neighbor doesn't exist AStarData nextNodeData = new AStarData(); nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position); nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find //paths that are less dangerous, etc. danger being the heucistic. nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore; nextNodeData.cameFrom = current; openList.Add(connectedMesh2DNode, new NodeDataPair { n = connectedMesh2DNode, d = nextNodeData }); } else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore) { //if the neighbor exists and has bigger g score pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position); pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position); //connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position); pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore; pairAlreadyInOpenlist.d.cameFrom = current; } } } return(null); }
private List<NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end) { AStarData startData = new AStarData { cameFrom = null, fScore = 0, gScore = 0, hScore = 0 }; Dictionary<NavMesh2DNode, NodeDataPair> openList = new Dictionary<NavMesh2DNode, NodeDataPair>(); Dictionary<NavMesh2DNode, NodeDataPair> closedList = new Dictionary<NavMesh2DNode, NodeDataPair>(); openList.Add(start, new NodeDataPair { n = start, d = startData }); while (openList.Count != 0) { NodeDataPair current = null; { float lowestFscore = float.MaxValue; foreach (NodeDataPair n in openList.Values) { if (n.d.fScore < lowestFscore) { lowestFscore = n.d.fScore; current = n; } } } openList.Remove(current.n); closedList.Add(current.n, current); if (closedList.ContainsKey(end)) { //found a path List<NavMesh2DNode> pathFound = new List<NavMesh2DNode>(); ReconstructPath(current, ref pathFound); pathFound.Reverse(); //Path path = new Path(startNode, endNode); //FindGoingTo(endNode, null); //ReconstructPath(startNode, ref path); //path.Smooth(); return pathFound; } foreach (NavMesh2DConnection connection in current.n.connections) { NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex); if (closedList.ContainsKey(connectedMesh2DNode)) continue; NodeDataPair pairAlreadyInOpenlist = null; if (openList.ContainsKey(connectedMesh2DNode)) pairAlreadyInOpenlist = openList[connectedMesh2DNode]; if (pairAlreadyInOpenlist == null) { //if the neighbor doesn't exist AStarData nextNodeData = new AStarData(); nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position); nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find //paths that are less dangerous, etc. danger being the heucistic. nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore; nextNodeData.cameFrom = current; openList.Add(connectedMesh2DNode, new NodeDataPair { n = connectedMesh2DNode, d = nextNodeData }); } else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore) { //if the neighbor exists and has bigger g score pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position); pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position); pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore; pairAlreadyInOpenlist.d.cameFrom = current; } } } return null; }
/// <summary> /// Sets up the searchSpace for using A*. /// </summary> /// <returns> the modified searchspace, the startnode, and the end node.</returns> /// <param name="_searchSpace">The searchspace of the function.</param> /// <param name="_start"> The start node if known, otherwise this is generated.</param> /// <param name="_goal">The goal node if known, otherwise this is generated.</param> private AStarData AStarSetup (List<GameObject> _searchSpace, GameObject _start, GameObject _goal) { //List<List<GameObject>> aStarData = new List<List<GameObject>> (); GameObject start, goal; //if start is not provided, generate one if (_start) { start = _start; } else { start = GenerateRandomNode (_searchSpace); } //if goal is not provided, generate one if (_goal) { goal = _goal; } else { goal = GenerateRandomNode (_searchSpace); } //set start's cost so far to 0 start.GetComponent<TruncOct> ().nodeData.tentativeDist = 0; //setHeuristics _searchSpace = SetHeuristics (_searchSpace, start, goal); //wrap start and goal nodes into mono-member lists for packing into aStarData List<GameObject> startHolder = new List<GameObject> (1) {start}, goalHolder = new List<GameObject> (1) {goal}; AStarData dataHolder = new AStarData (); dataHolder.searchSpace = _searchSpace; dataHolder.startObject = start; dataHolder.goalObject = goal; return dataHolder; }
public static List <Position> DrawLine(Position start, Position destination, DrawContoller draw_option) { Stopwatch timer = new Stopwatch(); timer.Reset(); timer.Start(); if (start == destination) { return(new List <Position>() { start }); } Dictionary <Position, AStarData> data_map = new Dictionary <Position, AStarData>(); List <Position> quick_search = new List <Position>(); List <Position> medium_search = new List <Position>(); List <Position> slow_search = new List <Position>(); // first position is tile, second position is x = distance travelled and y = waste quick_search.Add(start); data_map.Add(start, new AStarData(new Position(-1, -1), 0, 0)); List <Position> result = new List <Position>(); Position current_tile = start; bool path_found = false; while (quick_search.Count + medium_search.Count + slow_search.Count > 0 && !path_found && !draw_option.QuickStop()) { data_map[current_tile] = new AStarData(data_map[current_tile], true); Position goal_direction = GetDirection(current_tile, destination); for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { Position adjacent_tile = GetAdjacent(current_tile, i, j); if (map.ContainsKey(adjacent_tile)) { if (adjacent_tile == destination) { if (draw_option.GetDistance(current_tile, adjacent_tile) > 0) { float distance = data_map[current_tile].distance + draw_option.GetDistance(current_tile, adjacent_tile); float waste = data_map[current_tile].waste; data_map.Add(adjacent_tile, new AStarData(current_tile, distance, waste)); path_found = true; } } else { float distance = draw_option.GetDistance(current_tile, adjacent_tile); if (distance > 0) { distance += data_map[current_tile].distance; float waste = data_map[current_tile].waste + Mathf.Abs(goal_direction.x - i) + Mathf.Abs(goal_direction.y - j); if (!data_map.ContainsKey(adjacent_tile)) { data_map.Add(adjacent_tile, new AStarData(current_tile, distance, waste)); } else { if (data_map[adjacent_tile].distance > distance) { data_map[adjacent_tile] = new AStarData(current_tile, distance, waste, data_map[adjacent_tile].ventured); } } if (!data_map[adjacent_tile].ventured) { if (waste == data_map[current_tile].waste) { if (!quick_search.Contains(adjacent_tile)) { quick_search.Add(adjacent_tile); } if (medium_search.Contains(adjacent_tile)) { medium_search.Remove(adjacent_tile); } else if (slow_search.Contains(adjacent_tile)) { slow_search.Remove(adjacent_tile); } } else if (waste - data_map[current_tile].waste < 2) { if (!medium_search.Contains(adjacent_tile)) { medium_search.Add(adjacent_tile); } if (slow_search.Contains(adjacent_tile)) { slow_search.Remove(adjacent_tile); } } else { if (!slow_search.Contains(adjacent_tile)) { slow_search.Add(adjacent_tile); } } } } } } } } if (quick_search.Count > 0) { //Sort Quick Search quick_search.Sort((tile1, tile2) => data_map[tile1].waste.CompareTo(data_map[tile2].waste)); current_tile = quick_search[0]; quick_search.Remove(current_tile); } else if (medium_search.Count > 0) { //Sort medium_search medium_search.Sort((tile1, tile2) => data_map[tile1].waste.CompareTo(data_map[tile2].waste)); current_tile = medium_search[0]; medium_search.Remove(current_tile); } else if (slow_search.Count > 0) { slow_search.Sort((tile1, tile2) => data_map[tile1].waste.CompareTo(data_map[tile2].waste)); current_tile = slow_search[0]; slow_search.Remove(current_tile); } } if (!data_map.ContainsKey(destination)) { throw new NoPathException(start + " to " + destination + " has no path"); } AStarData item = data_map[destination]; result.Add(destination); while (data_map.ContainsKey(item.previous_tile)) { result.Add(item.previous_tile); item = data_map[item.previous_tile]; } result.Reverse(); return(result); }