Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using UnityEngine;
- using System.Collections;
- using System.Collections.Generic;
- using System;
- using System.Net;
- using UnityEditor;
- public class AStarSolver
- {
- public Node[,] grid;
- int gridLength;
- int gridWidth;
- Vector3 startNode;
- Vector3 endNode;
- Vector3 curNode;
- float gridSize;
- List<Vector3> openList = new List<Vector3>();
- List<Vector3> closedList = new List<Vector3>();
- List<Vector3> pathNodes;
- GameObject boundaries;
- public AStarSolver(GameObject bounds)
- {
- this.boundaries = bounds;
- }
- public void Solve(Vector3 startNode, Vector3 endNode)
- {
- this.startNode = startNode;
- this.endNode = endNode;
- bool canSearch = true;
- try
- {
- if(!grid[(int)startNode.x, (int)startNode.z].walkable)
- canSearch = false;
- }
- catch{}
- try
- {
- if(!grid[(int)endNode.x, (int)endNode.z].walkable)
- canSearch = false;
- }
- catch{}
- if(canSearch)
- {
- openList.Add(startNode);
- curNode = new Vector3(0,0,0);
- while(openList.Count != 0)
- {
- curNode = GetNodeWithLowestTotal(openList);
- if(curNode == endNode)
- {
- break; //Path complete.
- }
- else
- {
- openList.Remove(curNode);
- closedList.Add(curNode);
- List<Vector3> adjacentNodes = GetAdjacentNodes(curNode);
- foreach(Vector3 adjacentNode in adjacentNodes)
- {
- if(!openList.Contains(adjacentNode) && !closedList.Contains(adjacentNode))
- {
- openList.Add (adjacentNode);
- Node node = grid[(int)adjacentNode.x, (int)adjacentNode.z];
- node.cost = grid[(int)curNode.x, (int)curNode.z].cost + 10;
- node.heuristic = ManhattanDistance(adjacentNode);
- node.total = node.cost + node.heuristic;
- }
- }
- //foreach diagonalnode
- }
- }
- TracePath();
- }
- }
- private void TracePath()
- {
- bool startFound = false;
- Vector3 currentNode = endNode;
- pathNodes = new List<Vector3>();
- while(startFound == false)
- {
- List<Vector3> adjacentNodes = GetAdjacentNodes(currentNode);
- foreach(Vector3 adjacentNode in adjacentNodes)
- {
- if(adjacentNode == startNode)
- startFound = true;
- if(closedList.Contains(adjacentNode) || openList.Contains(adjacentNode))
- {
- if(grid[(int)adjacentNode.x, (int)adjacentNode.z].cost <= grid[(int)currentNode.x, (int)currentNode.z].cost
- && grid[(int)adjacentNode.x, (int)adjacentNode.z].cost > 0)
- {
- currentNode = adjacentNode;
- pathNodes.Add(adjacentNode);
- break;
- }
- }
- }
- }
- }
- public List<Vector3> GetPath()
- {
- List<Vector3> list = pathNodes;
- list.Reverse();
- return list;
- }
- public List<Vector3> GetPathReversed()
- {
- return pathNodes;
- }
- private int ManhattanDistance(Vector3 adjacentNode)
- {
- int manhattan = Math.Abs((int)(endNode.x - adjacentNode.x)) + Math.Abs((int)(endNode.z - adjacentNode.z));
- return manhattan;
- }
- private Vector3 GetNodeWithLowestTotal(List<Vector3> openList)
- {
- Vector3 nodeWithLowestTotal = new Vector3(77777777, 77777777, 77777777);
- int lowestTotal = 77777777;
- foreach(Vector3 openNode in openList)
- {
- if(grid[(int)openNode.x, (int)openNode.z].total <= lowestTotal) //Errors no matter what I try.
- {
- lowestTotal = grid[(int)openNode.x, (int)openNode.z].total;
- nodeWithLowestTotal = new Vector3(openNode.x, openNode.z);
- }
- }
- return nodeWithLowestTotal;
- }
- private List<Vector3> GetAdjacentNodes(Vector3 curNode)
- {
- List<Vector3> adjacentNodes = new List<Vector3>();
- Vector3 adjacentNode;
- Vector3 bounds = boundaries.renderer.bounds.size;
- try
- {
- //in front
- adjacentNode = new Vector3(curNode.x, curNode.y, curNode.z + 1);
- if(adjacentNode.z < bounds.z && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
- adjacentNodes.Add(adjacentNode);
- }
- catch{}
- try
- {
- //behind
- adjacentNode = new Vector3(curNode.x, curNode.y, curNode.z - 1);
- if(adjacentNode.z > -bounds.z && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
- adjacentNodes.Add(adjacentNode);
- }
- catch{}
- try
- {
- //left
- adjacentNode = new Vector3(curNode.x - 1, curNode.y, curNode.z);
- if(adjacentNode.x > -bounds.x && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
- adjacentNodes.Add(adjacentNode);
- }
- catch{}
- try
- {
- //right
- adjacentNode = new Vector3(curNode.x + 1, curNode.y, curNode.z);
- if(adjacentNode.x < bounds.x && grid[(int)adjacentNode.x, (int)adjacentNode.z].walkable)
- adjacentNodes.Add(adjacentNode);
- }
- catch{}
- return adjacentNodes;
- }
- public void GenerateGraph(float gridScale, float bufferRadius)
- {
- //Generates the node graph used in the grid.
- //Gridsize is how much distance between cells, smaller = more precise
- //Buffer radius is how much of a buffer is left around obsticles.
- Bounds bounds = boundaries.renderer.bounds;
- this.gridSize = gridScale;
- //top left corner of cube
- Vector3 topLeftCorner = bounds.center - bounds.extents + new Vector3(0, bounds.size.y,0);
- gridWidth = Mathf.RoundToInt(bounds.size.x / gridSize);
- gridLength = Mathf.RoundToInt(bounds.size.z / gridSize);
- grid = new Node[gridWidth, gridLength];
- for(int x = 0; x < gridWidth; x++)
- {
- for(int z = 0; z < gridLength; z++)
- {
- Vector3 currentPosition = topLeftCorner + new Vector3(x * gridSize, 0, z * gridSize);
- RaycastHit hit;
- //create new node for grid
- grid[x,z] = new Node(new Vector3((int)(topLeftCorner.x + x * gridSize),0,(int)(topLeftCorner.z + z * gridSize)), false);
- if(Physics.Raycast(currentPosition, -Vector3.up, out hit, bounds.size.y))
- {
- grid[x,z].pos.y = hit.point.y;
- if(Vector3.Angle(hit.normal, currentPosition) < 50)
- {
- if(hit.collider.gameObject.tag == "Walkable")
- {
- grid[x,z].walkable = true;
- }
- }
- }
- }
- }
- }
- public void DrawNodeGraph()
- {
- for(int x = 0; x < gridWidth; x++)
- {
- for(int z = 0; z < gridLength; z++)
- {
- Gizmos.DrawSphere(grid[x,z].pos, 0.5f);
- }
- }
- }
- public void DrawPath()
- {
- List<Vector3> path = GetPath();
- for(int i = 0; i < path.Count; i++)
- {
- Gizmos.DrawSphere(path[i], 1f);
- if(i+1 < path.Count)
- Gizmos.DrawLine(path[i], path[i+1]);
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement