void Update() { if (startX != -1 && !finishFill) { if (q.Count != 0) { int[] dx = { 1, 0, -1, 0 }; int[] dy = { 0, 1, 0, -1 }; Coord n = q.Dequeue(); //color current block red fm.ColorBlock(n.x, n.y, red); //if previous node was not start node, color it gradient if (lastX != -1 && (lastX != startX || lastY != startY)) { fm.ColorBlock(lastX, lastY, Color.Lerp(blue, green, (float)nodes[lastX, lastY].value / solution)); } //if we reach destination if (n.x == endX && n.y == endY) { //travel through parent nodes to store path while (n.x != startX || n.y != startY) { n = nodes[n.x, n.y].parent.GetCoord(); path.Add(n); } // -2 instead of -1 because we dont want to overwrite starting node travel = path.Count - 2; q.Clear(); finishFill = true; return; } //add neighbors to queue if unvisited for (int i = 0; i < 4; i++) { int newX = n.x + dx[i]; int newY = n.y + dy[i]; if (newX >= 0 && newX < fm.floor.GetLength(0) && newY >= 0 && newY < fm.floor.GetLength(1)) { float calcDist = nodes[n.x, n.y].value + 1 + Tools.HeightDiff(fm.floor[n.x, n.y], fm.floor[newX, newY]); if (nodes[newX, newY].value <= 0 || calcDist < nodes[newX, newY].value) { nodes[newX, newY].parent = nodes[n.x, n.y]; nodes[newX, newY].value = calcDist; q.Enqueue(new Coord(newX, newY)); } } } lastX = n.x; lastY = n.y; } else { //if we run out of nodes to explore, finish coloring if (lastX != -1 && (lastX != startX || lastY != startY)) { fm.ColorBlock(lastX, lastY, Color.Lerp(blue, green, (float)nodes[lastX, lastY].value / solution)); } } } if (finishFill && travel >= 0) { fm.ColorBlock(path[travel].x, path[travel].y, purple); travel--; } }
private void Update() { if (startX != -1 && !finishAStar) { if (mh.HasNext()) { Node n = mh.Pop(); int[] dx = { 1, 0, -1, 0 }; int[] dy = { 0, 1, 0, -1 }; //color current block red fm.ColorBlock(n.x, n.y, red); //if previous node was not start node, color it gradient if (lastX != -1 && (lastX != startX || lastY != startY)) { fm.ColorBlock(lastX, lastY, Color.Lerp(blue, green, (float)nodes[lastX, lastY].value / solution)); } //check if we arrived at destination if (n.x == endX && n.y == endY) { mh.Clear(); //travel through parent nodes to store path while (n.x != startX || n.y != startY) { n = nodes[n.x, n.y].parent; path.Add(n.GetCoord()); } // -2 instead of -1 because we dont want to overwrite starting node travel = path.Count - 2; finishAStar = true; return; } lastX = n.x; lastY = n.y; for (int i = 0; i < 4; i++) { int newX = n.x + dx[i]; int newY = n.y + dy[i]; if (newX >= 0 && newX < fm.floor.GetLength(0) && newY >= 0 && newY < fm.floor.GetLength(1)) { //if not a wall if (fm.floor[newX, newY].value == 0) { float tempH = nodes[n.x, n.y].value + 1 + Vector2.Distance(new Vector2(newX, newY), new Vector2(endX, endY)); if (tempH < nodes[newX, newY].heuristic) { nodes[newX, newY].heuristic = tempH; nodes[newX, newY].value = nodes[n.x, n.y].value + 1; nodes[newX, newY].parent = nodes[n.x, n.y]; mh.Insert(nodes[newX, newY]); } } } } } else { //if we run out of nodes to explore, finish coloring if (lastX != -1 && (lastX != startX || lastY != startY)) { fm.ColorBlock(lastX, lastY, Color.Lerp(blue, green, (float)nodes[lastX, lastY].value / solution)); } } } if (finishAStar && travel >= 0) { fm.ColorBlock(path[travel].x, path[travel].y, purple); travel--; } }