Advertisement
Danisuper

A* pathfinding unity

Jul 28th, 2016
115
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 10.53 KB | None | 0 0
  1. using UnityEngine;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using System.Collections;
  5.  
  6. public class AStar : MonoBehaviour
  7. {
  8.  
  9.     public List<GameObject> open_list = new List<GameObject>(); //list of the nodes that have to be taken into account
  10.     public List<GameObject> closed_list = new List<GameObject>(); //list of the nodes that have already been considered
  11.     public List<GameObject> adjacent_nodes = new List<GameObject>();
  12.     public List<GameObject> path_nodes = new List<GameObject>(); //the nodes of the final path
  13.     public GameObject target;
  14.     public GameObject my_node;
  15.     GetClosestNode targ_node;
  16.     GetClosestNode actual_node;
  17.     NodeManager node_manager;
  18.     EnemyVars en_vars;
  19.  
  20.     RaycastHit2D ray_dx;
  21.     RaycastHit2D ray_sx;
  22.     RaycastHit2D ray_up;
  23.     RaycastHit2D ray_down;
  24.  
  25.     public Color a;
  26.     public Color b;
  27.     public Color c;
  28.     public Color d;
  29.  
  30.     public bool found_path;
  31.     public bool recalculate_path;
  32.  
  33.     //for the path follow
  34.     public bool reached_dest; //if it reached the target
  35.     public int current_node = 0;
  36.     float node_dist;
  37.     Vector3 dest;
  38.  
  39.     float start_recalculate_time;
  40.     public float recalculate_time;
  41.     public bool delete_lists;
  42.     // Use this for initialization
  43.     void Start()
  44.     {
  45.         targ_node = target.GetComponent<GetClosestNode>();
  46.         actual_node = gameObject.GetComponent<GetClosestNode>();
  47.         node_manager = GameObject.FindGameObjectWithTag("nodes manager").GetComponent<NodeManager>();
  48.         en_vars = gameObject.GetComponent<EnemyVars>();
  49.  
  50.         start_recalculate_time = recalculate_time;
  51.         recalculate_path = true;
  52.         FindPath(actual_node.my_node, targ_node.my_node);
  53.  
  54.     }
  55.  
  56.     // Update is called once per frame
  57.     void Update()
  58.     {
  59.         //starts the pathfinding
  60.         if (recalculate_path)
  61.         {
  62.             FindPath(actual_node.my_node, targ_node.my_node);
  63.             recalculate_path = false;
  64.         }
  65.  
  66.         recalculate_time -= Time.deltaTime;
  67.         if (recalculate_time <= 0)
  68.         {
  69.             recalculate_time = start_recalculate_time;
  70.             recalculate_path = true;
  71.  
  72.         }
  73.  
  74.         //if lines have been spawned recalculate everything
  75.         if (Input.GetKeyDown(KeyCode.Space))
  76.         {
  77.             delete_lists = true;
  78.             ResetPathLists();
  79.  
  80.             if (!delete_lists)
  81.             FindPath(actual_node.my_node, targ_node.my_node);
  82.         }
  83.         else
  84.         {
  85.             //if the target has moved (I check it by seeing if it changed node)
  86.             if(targ_node.changed_node)
  87.             {
  88.                 found_path = false;
  89.                 reached_dest = false;
  90.                 FindPath(actual_node.my_node, targ_node.my_node);
  91.             }
  92.         }
  93.         if(path_nodes.Count>0)
  94.         {
  95.             FollowPath();
  96.         }
  97.         if(reached_dest)
  98.         {
  99.             delete_lists = true;
  100.             ResetPathLists();
  101.         }
  102.     }
  103.  
  104.     public void FindPath(GameObject start_node, GameObject target_node)
  105.     {
  106.         #region DEBUG colors the nodes
  107.         //DEBUG colors the nodes
  108.         if (open_list.Count > 0)
  109.             foreach (GameObject go in open_list)
  110.             {
  111.                 go.GetComponent<SpriteRenderer>().color = a;
  112.             }
  113.  
  114.         if (closed_list.Count > 0)
  115.             foreach (GameObject go in closed_list)
  116.             {
  117.                 if (!path_nodes.Contains(go))
  118.                     go.GetComponent<SpriteRenderer>().color = b;
  119.             }
  120.  
  121.         #endregion
  122.  
  123.         #region A*
  124.         //adds the first node to the open list
  125.         my_node = start_node;
  126.         if (!open_list.Contains(my_node) && !reached_dest)
  127.         {
  128.             open_list.Add(my_node);
  129.         }
  130.  
  131.         if (open_list.Count > 0 && !found_path && !reached_dest)
  132.         {
  133.             //sorts by movement cost
  134.             open_list = open_list.OrderBy(x => x.GetComponent<GetNodeMovementCost>().GetMovementCost(gameObject, x, target_node)).ToList();
  135.             my_node = open_list.First();
  136.             //puts my node in the closed list
  137.             if (!closed_list.Contains(my_node) && !actual_node.changed_node && !found_path && !reached_dest)
  138.             {
  139.                 open_list.Remove(my_node);
  140.                 closed_list.Add(my_node);
  141.             }
  142.             //we found a way!
  143.             if (closed_list.Contains(target_node) && !targ_node.changed_node)
  144.             {
  145.                 BackTracePath();
  146.                 found_path = true;
  147.             }
  148.  
  149.             //finds the adjacent nodes
  150.             FindAdjacentNodes(my_node);
  151.             if (adjacent_nodes.Count > 0)
  152.                 foreach (GameObject node in adjacent_nodes)
  153.                 {
  154.                     if (closed_list.Contains(node))
  155.                     {
  156.                         continue;
  157.                     }
  158.                     if (!open_list.Contains(node))
  159.                     {
  160.                         open_list.Add(node);
  161.                         node.GetComponent<NodeParent>().parent = my_node;
  162.                         node.GetComponent<GetNodeMovementCost>().GetMovementCost(gameObject, node, target_node);
  163.                     }
  164.                 }
  165.         }
  166.     }
  167.  
  168. //It makes crash everything
  169.     void ResetPathLists()
  170.     {
  171.         if (adjacent_nodes.Count > 0 && open_list.Count > 0 && closed_list.Count > 0 && path_nodes.Count > 0 && delete_lists)
  172.         {
  173.             adjacent_nodes.Clear();
  174.             open_list.Clear();
  175.             closed_list.Clear();
  176.             path_nodes.Clear();
  177.  
  178.             foreach (GameObject node in node_manager.nodes)
  179.             {
  180.                 node.GetComponent<SpriteRenderer>().color = Color.black;
  181.             }
  182.         }
  183.         delete_lists = false;
  184.  
  185.     }
  186.  
  187.     #region adds adjacent nodes
  188.     void FindAdjacentNodes(GameObject node)
  189.     {
  190.         //8 dir raycasting
  191.         RaycastHit2D ray_dx = Physics2D.Raycast(node.transform.position, Vector2.right, node_manager.NodesDistance);
  192.         RaycastHit2D ray_sx = Physics2D.Raycast(node.transform.position, -Vector2.right, node_manager.NodesDistance);
  193.         RaycastHit2D ray_up = Physics2D.Raycast(node.transform.position, Vector2.up, node_manager.NodesDistance);
  194.         RaycastHit2D ray_down = Physics2D.Raycast(node.transform.position, -Vector2.up, node_manager.NodesDistance);
  195.         //multiply for the square of 2 to find the lenght of the diagonal
  196.         RaycastHit2D ray_up_dx = Physics2D.Raycast(node.transform.position, Vector2.right + Vector2.up, node_manager.NodesDistance * Mathf.Sqrt(2));
  197.         RaycastHit2D ray_up_sx = Physics2D.Raycast(node.transform.position, -Vector2.right + Vector2.up, node_manager.NodesDistance * Mathf.Sqrt(2));
  198.         RaycastHit2D ray_down_dx = Physics2D.Raycast(node.transform.position, Vector2.right - Vector2.up, node_manager.NodesDistance * Mathf.Sqrt(2));
  199.         RaycastHit2D ray_down_sx = Physics2D.Raycast(node.transform.position, -Vector2.right - Vector2.up, node_manager.NodesDistance * Mathf.Sqrt(2));
  200.  
  201.         AddAdjacentNodes(ray_dx);
  202.         AddAdjacentNodes(ray_sx);
  203.         AddAdjacentNodes(ray_up);
  204.         AddAdjacentNodes(ray_down);
  205.         AddAdjacentNodes(ray_up_dx);
  206.         AddAdjacentNodes(ray_up_sx);
  207.         AddAdjacentNodes(ray_down_dx);
  208.         AddAdjacentNodes(ray_down_sx);
  209.  
  210.     }
  211.  
  212.     void AddAdjacentNodes(RaycastHit2D ray)
  213.     {
  214.         if (ray.collider != null && ray.collider.gameObject.tag == "node" && !adjacent_nodes.Contains(ray.collider.gameObject) &&
  215.             !closed_list.Contains(ray.collider.gameObject) && ray.collider.gameObject.GetComponent<GetIfWalkable>().is_node_walkable)
  216.         {
  217.             adjacent_nodes.Add(ray.collider.gameObject);
  218.         }
  219.     }
  220.     #endregion
  221.  
  222.     //finds the manhattan distance
  223.     static public int Dist(GameObject node1, GameObject node2)
  224.     {
  225.         return Mathf.Abs(node2.GetComponent<GetNodeCoord>().NodeCoordX - node1.GetComponent<GetNodeCoord>().NodeCoordX) +
  226.             Mathf.Abs(node2.GetComponent<GetNodeCoord>().NodeCoordY - node1.GetComponent<GetNodeCoord>().NodeCoordY);
  227.     }
  228.  
  229.     #region path backtracing
  230.     //makes the final path
  231.     void BackTracePath()
  232.     {
  233.         GameObject node = targ_node.my_node;
  234.         do
  235.         {
  236.             if (!path_nodes.Contains(node))
  237.             {
  238.                 path_nodes.Add(node);
  239.             }
  240.  
  241.             node.GetComponent<SpriteRenderer>().color = d;
  242.             node = node.GetComponent<NodeParent>().parent;
  243.  
  244.         } while (node != null);
  245.  
  246.         //puts the gameobject's node at the beginning of the list
  247.         if (!path_nodes.Contains(actual_node.my_node))
  248.         {
  249.             path_nodes.Insert(0, actual_node.my_node);
  250.         }
  251.  
  252.  
  253.         //reverse the list
  254.         path_nodes.Reverse();
  255.     }
  256.     #endregion
  257.     #endregion
  258.     #region path follow
  259.     //follows the path
  260.     public void FollowPath()
  261.     {
  262.         AssignFollowVars();
  263.         //if we didn't reach the destination
  264.         if (reached_dest == false)
  265.         {
  266.             //if the distance is > than 0.1 move towards the node
  267.             if (node_dist > 0.1f)
  268.             {
  269.                 transform.position = Vector3.MoveTowards(transform.position, dest, en_vars.speed * Time.deltaTime);
  270.                 transform.rotation = UtilityFunctions.FaceTo(GetNodePos(current_node), transform.position);
  271.             }
  272.             else
  273.             {
  274.                 current_node++;
  275.             }
  276.             //if the target isn't moving and the gameobject's node is the last in the list or we reached it
  277.             if ((current_node >= path_nodes.Count && !targ_node.changed_node) || actual_node.my_node == targ_node.my_node)
  278.             {
  279.                 reached_dest = true;
  280.                 current_node = 0;
  281.             }
  282.             else
  283.             {
  284.                 reached_dest = false;
  285.             }
  286.         }
  287.         else
  288.         {
  289.             current_node = 0;
  290.         }
  291.     }
  292.     void AssignFollowVars()
  293.     {
  294.         path_nodes.OrderBy(x => Vector2.Distance(path_nodes.First().transform.position, x.transform.position));
  295.         if (!reached_dest)
  296.         {
  297.             dest = GetNodePos(current_node);
  298.             node_dist = Vector2.Distance(transform.position, dest);
  299.         }
  300.     }
  301.     //gets the node position based on its index
  302.     Vector3 GetNodePos(int n_node)
  303.     {
  304.         if (n_node < path_nodes.Count)
  305.         {
  306.             return path_nodes[n_node].transform.position;
  307.         }
  308.         else
  309.         {
  310.             return Vector3.zero;
  311.         }
  312.     }
  313.     #endregion
  314. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement