static int RRT_GetNearestNeighbor(int randNode, List <int> nodes, GroundGrid groundGrid)
{
//TODO: Make sure the nearest neighbor which is selected has Line-of-Sight to the selected randNode
int nearestNeighbor = nodes.ElementAt(0);
float minDist = Vector3.Distance(groundGrid.GetNodePosition(nearestNeighbor), groundGrid.GetNodePosition(randNode));
for (int n = 0; n < nodes.Count; ++n)
{
float newDist = Vector3.Distance(groundGrid.GetNodePosition(nodes.ElementAt(n)), groundGrid.GetNodePosition(randNode));
if (newDist < minDist)
{
minDist = newDist;
nearestNeighbor = nodes.ElementAt(n);
}
}
return(nearestNeighbor);
}
static bool RRT_StepTowards(int start, int end, ref int newNode, GroundGrid groundGrid, Actor actor)
{
//Only able to move one square (in any direction) in a single step
float epsilon = 1.45f * 100 / groundGrid.Columns;
float dist = Mathf.Min(epsilon, Vector3.Distance(groundGrid.GetNodePosition(start), groundGrid.GetNodePosition(end)));
RaycastHit hit;
if (Physics.Raycast(groundGrid.GetNodePosition(start), groundGrid.GetNodePosition(end) - groundGrid.GetNodePosition(start), out hit, dist, 1 << LayerMask.NameToLayer("Obstacles")))
{
//Obstacle between start and end nodes
newNode = -1;
return(false);
}
RaycastHit[] hits;
hits = Physics.RaycastAll(groundGrid.GetNodePosition(start), groundGrid.GetNodePosition(end) - groundGrid.GetNodePosition(start), dist, 1 << LayerMask.NameToLayer("Node"));
float maxDist = 0.0f;
for (int i = 0; i < hits.Length; ++i)
{
Node curNode = hits[i].collider.GetComponent <Node>();
if (curNode.IsOccupied())
{
//Occupied node between start and end nodes
newNode = -1;
return(false);
}
if (hits[i].distance > maxDist)
{
maxDist = hits[i].distance;
newNode = curNode.ID;
}
}
actor.transform.position = groundGrid.GetNodePosition(start);
int numSteps = 5;
for (int step = 0; step < numSteps; ++step)
{
actor.transform.position = Vector3.Lerp(groundGrid.GetNodePosition(start), groundGrid.GetNodePosition(newNode), (float)step / (numSteps - 1));
Collider[] colliderHits = Physics.OverlapBox(actor.transform.position, actor.GetComponent <Collider>().bounds.extents, actor.transform.rotation, 1 << LayerMask.NameToLayer("Obstacles"));
for (int i = 0; i < colliderHits.Length; ++i)
{
//Actor intersects an obstacle on its path from the start to end node
newNode = -1;
return(false);
}
}
return(true);
}
public static IEnumerator RRT(int startNode, int endNode, GroundGrid groundGrid, Actor actor, List <int> outPath)
{
outPath.Clear();
foreach (Transform child in groundGrid.transform)
{
if (child.name == "Line")
{
GameObject.Destroy(child.gameObject);
}
}
int numNodes = groundGrid.Rows * groundGrid.Columns;
const int numAttempts = 2000;
int[] parent = new int[numNodes];
List <int> possibleNodesToPick = new List <int>();
for (int i = 0; i < numNodes; ++i)
{
if (!groundGrid.NodeIsOccupied(i))
{
possibleNodesToPick.Add(i);
}
parent[i] = -1;
}
List <int> nodes = new List <int>();
nodes.Add(startNode);
possibleNodesToPick.Remove(startNode);
for (int i = 0; i < numAttempts; ++i)
{
if (possibleNodesToPick.Count == 0)
{
yield break;
}
int randNode = RRT_GetRandomState(endNode, possibleNodesToPick);
int nearestNeighbor = RRT_GetNearestNeighbor(randNode, nodes, groundGrid);
//Select input to use
//For now any input is valid
//Determine new state
int newNode = -1;
if (RRT_StepTowards(nearestNeighbor, randNode, ref newNode, groundGrid, actor))
{
possibleNodesToPick.Remove(newNode);
nodes.Add(newNode);
DrawLine(groundGrid.GetNodePosition(nearestNeighbor), groundGrid.GetNodePosition(newNode), Color.red, groundGrid.transform, -1);
parent[newNode] = nearestNeighbor;
if (newNode == endNode)
{
foreach (int item in CreatePathFromParent(endNode, parent))
{
outPath.Add(item);
}
groundGrid.DisplayPath(outPath);
yield break;
}
groundGrid.SetNodeExplored(newNode, true);
}
else
{
continue;
}
yield return(null);
}
yield break;
}
void Update()
{
if (state != State.CalculatePath2 && state != State.ShowPath && state != State.None)
{
if (calculatePathCoroutine != null)
{
StopCoroutine(calculatePathCoroutine);
calculatePathCoroutine = null;
}
}
switch (state)
{
case State.CalculatePath:
groundGrid.HidePath();
outPath.Clear();
finishedCalculatingPath = false;
state = State.CalculatePath2;
break;
case State.CalculatePath2:
if (useRRT)
{
if (calculatePathCoroutine == null)
{
calculatePathCoroutine = PathPlanner.RRT(groundGrid.GetStartNodeIndex(), groundGrid.GetEndNodeIndex(), groundGrid, actor, outPath);
StartCoroutine(calculatePathCoroutine);
}
state = State.ShowPath;
}
else
{
if (calculatePathCoroutine == null)
{
calculatePathCoroutine = PathPlanner.A_Star(groundGrid.GetStartNodeIndex(), groundGrid.GetEndNodeIndex(), groundGrid, outPath);
StartCoroutine(calculatePathCoroutine);
}
state = State.ShowPath;
}
break;
case State.ShowPath:
if (outPath.Count > 0)
{
finishedCalculatingPath = true;
actor.transform.position = groundGrid.GetNodePosition(outPath.ElementAt(0));
actor.waypoints.Clear();
foreach (int item in outPath)
{
actor.waypoints.Add(new Actor.Waypoint(groundGrid.GetNodePosition(item), Quaternion.Euler(0, 0, 0)));
}
state = State.None;
}
else if (finishedCalculatingPath)
{
finishedCalculatingPath = false;
state = State.None;
}
break;
case State.RegenerateObstacles:
groundGrid.ResetPath();
state = State.RegenerateObstacles2;
break;
case State.RegenerateObstacles2:
obstacleManager.ResetObstacles();
state = State.RegenerateObstacles3;
break;
case State.CreateObstacles:
case State.RegenerateObstacles3:
obstacleManager.CreateObstacles();
state = State.CalculateObstacleCollisions;
break;
case State.RegenerateGrid:
groundGrid.CreateGrid((int)ResolutionSlider.value, (int)ResolutionSlider.value);
state = State.CalculateObstacleCollisions;
break;
case State.CalculateObstacleCollisions:
groundGrid.UpdateObstacleCollisions(useZones);
state = State.None;
break;
case State.None:
LoadingText.SetActive(false);
break;
default:
LoadingText.SetActive(false);
break;
}
if (Input.GetKeyDown(KeyCode.Escape))
{
Application.Quit();
}
else if (Input.GetKeyDown(KeyCode.R))
{
actor.transform.position = new Vector3(10000, 0, 0);
actor.waypoints.Clear();
state = State.RegenerateObstacles;
LoadingText.SetActive(true);
}
else if (Input.GetMouseButtonDown(0))
{
if (Input.GetKey(KeyCode.LeftAlt) || Input.GetKey(KeyCode.RightAlt))
{
//Create obstacle
RaycastHit hit;
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, Mathf.Infinity, 1 << LayerMask.NameToLayer("Node")))
{
groundGrid.ResetPath();
obstacleManager.CreateObstacle(hit.point);
actor.transform.position = new Vector3(10000, 0, 0);
actor.waypoints.Clear();
state = State.CalculateObstacleCollisions;
LoadingText.SetActive(true);
}
}
else
{
state = State.None;
//Set start node
RaycastHit hit;
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, Mathf.Infinity, 1 << LayerMask.NameToLayer("Node")))
{
Node collidedNode = hit.collider.GetComponent <Node>();
if (!collidedNode.IsOccupied() && collidedNode != groundGrid.startNode && collidedNode != groundGrid.endNode)
{
groundGrid.StartSprite.transform.position = new Vector3(collidedNode.transform.position.x, groundGrid.StartSprite.transform.position.y, collidedNode.transform.position.z);
groundGrid.startNode = collidedNode;
actor.transform.position = new Vector3(10000, 0, 0);
actor.waypoints.Clear();
if (groundGrid.startNode != null && groundGrid.endNode != null)
{
actor.transform.position = groundGrid.StartSprite.transform.position;
state = State.CalculatePath;
LoadingText.SetActive(true);
}
}
}
}
}
else if (Input.GetMouseButtonDown(1))
{
state = State.None;
//Set end node
RaycastHit hit;
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, Mathf.Infinity, 1 << LayerMask.NameToLayer("Node")))
{
Node collidedNode = hit.collider.GetComponent <Node>();
if (!collidedNode.IsOccupied() && collidedNode != groundGrid.startNode && collidedNode != groundGrid.endNode)
{
groundGrid.EndSprite.transform.position = new Vector3(collidedNode.transform.position.x, groundGrid.EndSprite.transform.position.y, collidedNode.transform.position.z);
groundGrid.endNode = collidedNode;
actor.transform.position = new Vector3(10000, 0, 0);
actor.waypoints.Clear();
if (groundGrid.startNode != null && groundGrid.endNode != null)
{
actor.transform.position = groundGrid.StartSprite.transform.position;
state = State.CalculatePath;
LoadingText.SetActive(true);
}
}
}
}
}