private void addVertex(TrackQueue opened, Dictionary <Vertex, Track> closed, Track currentTrack, Vertex target, Heuristic heuristic, int i, int j) { Vertex v = new Vertex(i, j); if (!closed.ContainsKey(v)) { Track t = new Track(v, currentTrack); if (i != currentTrack.CurrentVertex.RowIndex && j != currentTrack.CurrentVertex.ColIndex) { heuristic.CalcuatesCost(t, target, 14.14); //diagonal } else { heuristic.CalcuatesCost(t, target, 10); //andando } if (!opened.HasVertex(v)) { opened.Add(t); } else { Track aux = opened.Get(v); if (aux.TotalCost > t.TotalCost) { opened.Remove(aux); aux.Preview = t.Preview; aux.CostG = t.CostG; aux.CostH = t.CostH; opened.Add(aux); } } } }
public bool Start(GameMap map, Vertex origin, Vertex target, Heuristic heuristic, out List <Vertex> path) { TrackQueue opened = new TrackQueue(); Dictionary <Vertex, Track> closed = new Dictionary <Vertex, Track>(); path = null; if (origin == null || target == null || heuristic == null) { return(false); } Track currentTrack = new Track(origin, null); heuristic.CalcuatesCost(currentTrack, target, 0); opened.Add(currentTrack); while (currentTrack != null) { if (currentTrack.CurrentVertex.Equals(target)) { path = getPath(currentTrack); return(true); } else { closed.Add(currentTrack.CurrentVertex, currentTrack); opened.Remove(currentTrack); for (int i = currentTrack.CurrentVertex.RowIndex - 1; i <= currentTrack.CurrentVertex.RowIndex + 1; i++) { if (i >= 0 && i < map.GetSize().Rows) { for (int j = currentTrack.CurrentVertex.ColIndex - 1; j <= currentTrack.CurrentVertex.ColIndex + 1; j++) { if (j >= 0 && j < map.GetSize().Cols) { if (currentTrack.CurrentVertex.RowIndex != i || currentTrack.CurrentVertex.ColIndex != j) { GameObject gameObject = map.GetGameObjectAt(i, j); if (gameObject == null || gameObject.Type != GameObject.GameObjectType.Wall) { addVertex(opened, closed, currentTrack, target, heuristic, i, j); } } } } } } currentTrack = opened.First; } } return(false); }