public override void CalculatePath() { cameFrom = new Dictionary <Vector2, Vector2>(); costSoFar = new Dictionary <Vector2, double>(); var startPos = mapManager.startPosition; var endPos = mapManager.endPosition; var first = new SimplePriorityQueue <Vector2, double>(); first.Enqueue(startPos, 0); cameFrom[startPos] = startPos; costSoFar[startPos] = 0; while (first.Count > 0) { var current = first.Dequeue(); if (current.Equals(endPos)) { break; } foreach (var next in MapUtils.GetNeighbourTiles(current, mapManager.map)) { double newCost = costSoFar[current] + 1; //changeable later - currently costs always 1 if (mapManager.map[next].state == TileState.Occupied) //if occupied by obstacle { continue; } if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) { var edge = MapUtils.FromVec2ToEdge(current - next); mapManager.map[next].edge = MapUtils.GetOppositeEdge(edge); costSoFar[next] = newCost; double priority = newCost + Heuristic(next, endPos); first.Enqueue(next, priority); cameFrom[next] = current; } } } mapManager.GeneratePathway(); }
public override void CalculatePath() { var map = mapManager.map; _first = new Queue <Vector2>(); _first.Enqueue(mapManager.startPosition); _visited = new HashSet <Vector2>(); _visited.Add(mapManager.startPosition); while (_first.Count > 0) { var current = _first.Dequeue(); if (current == mapManager.endPosition) //end when reaching end position { break; } foreach (var next in MapUtils.GetNeighbourTiles(current, mapManager.map)) { if (map[next].state == TileState.Occupied) //if occupied by obstacle { continue; } if (_visited.Contains(next)) //if already checked { continue; } var edge = MapUtils.FromVec2ToEdge(current - next); map[next].edge = MapUtils.GetOppositeEdge(edge); _first.Enqueue(next); _visited.Add(next); } } mapManager.GeneratePathway(); }