private NodeGrid GetClosestGrid() { NodeGrid closestGrid = grids[0]; if (grids.Count == 1) { return(closestGrid); } float smallestDistance = Mathf.Infinity; int gridsCount = grids.Count - 1; for (int i = 0; i < gridsCount; i++) { float gridSqrDistance = GetSqrDistance(grids[i].transform.position, PathfinderTransform.position); if (gridSqrDistance <= MinimumDistance) { return(grids[i]); } else if (gridSqrDistance < smallestDistance) { closestGrid = grids[i]; } } return(closestGrid); }
/// <summary> /// Find a path for a character and start moving along the path. /// </summary> /// <param name="objectToMove"></param> /// <param name="start"></param> /// <param name="destination"></param> /// <param name="moveSpeed"></param> /// <param name="turnSpeed"></param> /// <param name="objectRadius"></param> public async void Move(Transform objectToMove, Vector3 start, Vector3 destination, float moveSpeed, float turnSpeed) { Stop(); NodeGrid grid = GetClosestGrid(); while (grid.IsBeingUsed && objectToMove != null) { await Task.Delay(TimeSpan.FromMilliseconds(Time.deltaTime *UpdateMultiplier)); } grid.IsBeingUsed = true; List <Vector3> path = await Task.Run(() => GetPath(start, destination, grid)); grid.IsBeingUsed = false; if (path.Count > 0) { canMove = true; path = await SmoothPath(path); await MoveAlongPath(objectToMove, path, moveSpeed, turnSpeed); } }
private async Task <List <Vector3> > GetPath(Vector3 start, Vector3 goal, NodeGrid grid) { PriorityQueue <Node> openQueue = new PriorityQueue <Node>(); List <Node> modifiedNodes = new List <Node>(); Node[,] nodeArray = grid.GetNodeArray(); Node[,] nodeArrayOther = grid.GetNodeArrayOther(); openQueue.Push(GetStartingNode(nodeArray, nodeArrayOther, start).Result); while (openQueue.Count > 0) { Node currentNode = openQueue.Pop(); if (GetSqrDistance(goal, currentNode.Position) <= MinimumDistance) { List <Vector3> path = new List <Vector3>(); Node pathNode = currentNode; while (!(pathNode is null)) { path.Add(pathNode.Position); pathNode = pathNode.ParentNode; } await ResetPathfinder(modifiedNodes); path.Reverse(); path.Add(goal); return(path); } currentNode.NodeState = NodeState.Closed; int nodeColumn = (int)currentNode.ArrayPosition.x; int nodeRow = (int)currentNode.ArrayPosition.y; // North Task north = CalculateChild(GetChild(nodeColumn, nodeRow - 1, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // North-East Task northEast = CalculateChild(GetChild(nodeColumn + 1, nodeRow - 1, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // East Task east = CalculateChild(GetChild(nodeColumn + 1, nodeRow, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // South-East Task southEast = CalculateChild(GetChild(nodeColumn + 1, nodeRow + 1, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // South Task south = CalculateChild(GetChild(nodeColumn, nodeRow + 1, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // South-West Task southWest = CalculateChild(GetChild(nodeColumn - 1, nodeRow + 1, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // West Task west = CalculateChild(GetChild(nodeColumn - 1, nodeRow, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); // North-West Task northWest = CalculateChild(GetChild(nodeColumn - 1, nodeRow - 1, nodeArray, nodeArrayOther), currentNode, openQueue, modifiedNodes, goal); await Task.WhenAll(north, northEast, east, southEast, south, southWest, west, northWest); } await ResetPathfinder(modifiedNodes); #if UNITY_EDITOR Debug.Log("No path found. Check that grids are available for pathfinding."); #endif return(new List <Vector3>()); }