Advertisement
Guest User

A*

a guest
Apr 10th, 2013
157
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 6.35 KB | None | 0 0
  1. using UnityEngine;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. using System;
  5. using System.Net;
  6. using UnityEditor;
  7.  
  8. public class AStarSolver
  9. {
  10.     public Node[,] grid;
  11.     int gridLength;
  12.     int gridWidth;
  13.    
  14.     Vector3 startNode;
  15.     Vector3 endNode;
  16.     Vector3 curNode;
  17.    
  18.     float gridSize;
  19.    
  20.     List<Vector3> openList = new List<Vector3>();
  21.     List<Vector3> closedList = new List<Vector3>();
  22.    
  23.     List<Vector3> pathNodes;
  24.    
  25.     GameObject boundaries;
  26.    
  27.     public AStarSolver(GameObject bounds)
  28.     {
  29.         this.boundaries = bounds;
  30.     }
  31.    
  32.     public void Solve(Vector3 startNode, Vector3 endNode)
  33.     {
  34.         this.startNode = startNode;
  35.         this.endNode = endNode;
  36.        
  37.         bool canSearch = true;
  38.        
  39.         try
  40.         {
  41.             if(!grid[(int)startNode.x, (int)startNode.z].walkable)
  42.                 canSearch = false;
  43.         }
  44.         catch{}
  45.        
  46.         try
  47.         {
  48.             if(!grid[(int)endNode.x, (int)endNode.z].walkable)
  49.                 canSearch = false;
  50.         }
  51.         catch{}
  52.        
  53.         if(canSearch)
  54.         {
  55.             openList.Add(startNode);
  56.             curNode = new Vector3(0,0,0);
  57.            
  58.             while(openList.Count != 0)
  59.             {
  60.                 curNode = GetNodeWithLowestTotal(openList);
  61.                
  62.                 if(curNode == endNode)
  63.                 {
  64.                     break; //Path complete.
  65.                 }
  66.                 else
  67.                 {
  68.                     openList.Remove(curNode);
  69.                     closedList.Add(curNode);
  70.                    
  71.                     List<Vector3> adjacentNodes = GetAdjacentNodes(curNode);
  72.                    
  73.                     foreach(Vector3 adjacentNode in adjacentNodes)
  74.                     {
  75.                         if(!openList.Contains(adjacentNode) && !closedList.Contains(adjacentNode))
  76.                         {
  77.                             openList.Add (adjacentNode);
  78.                            
  79.                             Node node = grid[(int)adjacentNode.x, (int)adjacentNode.z];
  80.                             node.cost = grid[(int)curNode.x, (int)curNode.z].cost + 10;
  81.                             node.heuristic = ManhattanDistance(adjacentNode);
  82.                             node.total = node.cost + node.heuristic;
  83.                         }
  84.                     }
  85.                    
  86.                     //foreach diagonalnode
  87.                 }
  88.             }
  89.             TracePath();
  90.         }
  91.     }
  92.    
  93.     private void TracePath()
  94.     {
  95.         bool startFound = false;
  96.        
  97.         Vector3 currentNode = endNode;
  98.         pathNodes = new List<Vector3>();
  99.         while(startFound == false)
  100.         {
  101.             List<Vector3> adjacentNodes = GetAdjacentNodes(currentNode);
  102.            
  103.             foreach(Vector3 adjacentNode in adjacentNodes)
  104.             {
  105.                 if(adjacentNode == startNode)
  106.                     startFound = true; 
  107.                
  108.                 if(closedList.Contains(adjacentNode) || openList.Contains(adjacentNode))
  109.                 {
  110.                     if(grid[(int)adjacentNode.x, (int)adjacentNode.z].cost <=   grid[(int)currentNode.x, (int)currentNode.z].cost
  111.                     && grid[(int)adjacentNode.x, (int)adjacentNode.z].cost > 0)
  112.                     {
  113.                         currentNode = adjacentNode;
  114.                         pathNodes.Add(adjacentNode);
  115.                        
  116.                         break;
  117.                     }
  118.                 }
  119.             }
  120.         }
  121.     }
  122.    
  123.     public List<Vector3> GetPath()
  124.     {
  125.         List<Vector3> list = pathNodes;
  126.         list.Reverse();
  127.         return list;
  128.     }
  129.    
  130.     public List<Vector3> GetPathReversed()
  131.     {
  132.         return pathNodes;
  133.     }
  134.    
  135.     private int ManhattanDistance(Vector3 adjacentNode)
  136.     {
  137.         int manhattan = Math.Abs((int)(endNode.x - adjacentNode.x)) + Math.Abs((int)(endNode.z - adjacentNode.z));
  138.         return manhattan;
  139.     }
  140.    
  141.     private Vector3 GetNodeWithLowestTotal(List<Vector3> openList)
  142.     {
  143.         Vector3 nodeWithLowestTotal = new Vector3(77777777, 77777777, 77777777);
  144.         int lowestTotal = 77777777;
  145.        
  146.         foreach(Vector3 openNode in openList)
  147.         {
  148.             if(grid[(int)openNode.x, (int)openNode.z].total <= lowestTotal) //Errors no matter what I try.
  149.             {
  150.                 lowestTotal = grid[(int)openNode.x, (int)openNode.z].total;
  151.                 nodeWithLowestTotal = new Vector3(openNode.x, openNode.z);
  152.             }
  153.         }
  154.        
  155.         return nodeWithLowestTotal;
  156.     }
  157.    
  158.     private List<Vector3> GetAdjacentNodes(Vector3 curNode)
  159.     {
  160.         List<Vector3> adjacentNodes = new List<Vector3>();
  161.         Vector3 adjacentNode;
  162.        
  163.         Vector3 bounds = boundaries.renderer.bounds.size;
  164.        
  165.         try
  166.         {
  167.             //in front
  168.             adjacentNode = new Vector3(curNode.x, curNode.y, curNode.z + 1);
  169.             if(adjacentNode.z < bounds.z && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
  170.                 adjacentNodes.Add(adjacentNode);
  171.         }
  172.         catch{}
  173.        
  174.         try
  175.         {
  176.             //behind
  177.             adjacentNode = new Vector3(curNode.x, curNode.y, curNode.z - 1);
  178.             if(adjacentNode.z > -bounds.z && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
  179.                 adjacentNodes.Add(adjacentNode);
  180.         }
  181.         catch{}
  182.        
  183.         try
  184.         {
  185.             //left
  186.             adjacentNode = new Vector3(curNode.x - 1, curNode.y, curNode.z);
  187.             if(adjacentNode.x > -bounds.x && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
  188.                 adjacentNodes.Add(adjacentNode);
  189.         }
  190.         catch{}
  191.        
  192.         try
  193.         {
  194.             //right
  195.             adjacentNode = new Vector3(curNode.x + 1, curNode.y, curNode.z);
  196.             if(adjacentNode.x < bounds.x && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
  197.                 adjacentNodes.Add(adjacentNode);
  198.         }
  199.         catch{}
  200.        
  201.         return adjacentNodes;
  202.     }
  203.    
  204.     public void GenerateGraph(float gridScale, float bufferRadius)
  205.     {
  206.         //Generates the node graph used in the grid.
  207.         //Gridsize is how much distance between cells, smaller = more precise
  208.         //Buffer radius is how much of a buffer is left around obsticles.
  209.        
  210.         Bounds bounds = boundaries.renderer.bounds;
  211.         this.gridSize = gridScale;
  212.        
  213.         //top left corner of cube
  214.         Vector3 topLeftCorner = bounds.center - bounds.extents + new Vector3(0, bounds.size.y,0);
  215.         gridWidth = Mathf.RoundToInt(bounds.size.x / gridSize);
  216.         gridLength = Mathf.RoundToInt(bounds.size.z / gridSize);
  217.        
  218.         grid = new Node[gridWidth, gridLength];
  219.        
  220.         for(int x = 0; x < gridWidth; x++)
  221.         {
  222.             for(int z = 0; z < gridLength; z++)
  223.             {
  224.                 Vector3 currentPosition = topLeftCorner + new Vector3(x * gridSize, 0, z * gridSize);
  225.                 RaycastHit hit;
  226.                
  227.                 //create new node for grid
  228.                 grid[x,z] = new Node(new Vector3((int)(topLeftCorner.x + x * gridSize),0,(int)(topLeftCorner.z + z * gridSize)), false);
  229.                
  230.                 if(Physics.Raycast(currentPosition, -Vector3.up, out hit, bounds.size.y))
  231.                 {
  232.                     grid[x,z].pos.y = hit.point.y;
  233.                     if(Vector3.Angle(hit.normal, currentPosition) < 50)
  234.                     {
  235.                         if(hit.collider.gameObject.tag == "Walkable")
  236.                         {
  237.                             grid[x,z].walkable = true;
  238.                         }
  239.                     }
  240.                 }
  241.             }
  242.         }
  243.     }
  244.    
  245.     public void DrawNodeGraph()
  246.     {
  247.         for(int x = 0; x < gridWidth; x++)
  248.         {
  249.             for(int z = 0; z < gridLength; z++)
  250.             {
  251.                 Gizmos.DrawSphere(grid[x,z].pos, 0.5f);
  252.             }
  253.         }
  254.     }
  255.    
  256.     public void DrawPath()
  257.     {
  258.         List<Vector3> path = GetPath();
  259.        
  260.         for(int i = 0; i < path.Count; i++)
  261.         {
  262.             Gizmos.DrawSphere(path[i], 1f);
  263.             if(i+1 < path.Count)
  264.                 Gizmos.DrawLine(path[i], path[i+1]);
  265.         }
  266.     }
  267. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement