private void OnDrawGizmos() { if (m_Grid == null) { return; } foreach (Node node in m_Grid.AllNodes()) { if (node.NextNode == null) { continue; } Vector3 start = node.Position; if (node.IsOccupied) { Gizmos.color = Color.blue; Vector3 size = Vector3.one * 0.5f; Gizmos.DrawCube(start, size); continue; } if (node.OccupationAvailability == EOccupationAvailability.Undefined) { Gizmos.color = Color.yellow; } else if (node.OccupationAvailability == EOccupationAvailability.CanOccupy) { Gizmos.color = Color.green; } else { Gizmos.color = Color.red; } //Gizmos.color = Color.red; Vector3 end = node.NextNode.Position; Vector3 dir = end - start; start -= dir * .25f; end -= dir * .75f; Gizmos.DrawLine(start, end); Gizmos.DrawSphere(end, .1f); } }
// Dijkstra algorithm public void UpdateField() { foreach (Node node in m_Grid.AllNodes()) { node.ResetWeight(); } var heap = new SortedSet <Vector2Int>(new DijkstraCompare(m_Grid)); m_Grid.GetNode(m_Target).PathWeight = 0f; heap.Add(m_Target); while (heap.Count > 0) { Vector2Int current = heap.Min; heap.Remove(current); Node currentNode = m_Grid.GetNode(current); foreach (Connection neighbour in GetNeighbours(current)) { Node neighbourNode = m_Grid.GetNode(neighbour.coord); float weightToTarget = currentNode.PathWeight + neighbour.weight; if (weightToTarget < neighbourNode.PathWeight) { if (heap.Contains(neighbour.coord)) { heap.Remove(neighbour.coord); } neighbourNode.NextNode = currentNode; neighbourNode.PathWeight = weightToTarget; heap.Add(neighbour.coord); } } } }