void FindPath(Vector3 a_StartPos, Vector3 a_TargetPos)
{
Node StartNode = nodeMesh.NodeFromWorldPoint(a_StartPos); //Gets the node closest to the starting position
Node TargetNode = nodeMesh.NodeFromWorldPoint(a_TargetPos); //Gets the node closest to the target position
if (StartNode != null && TargetNode != null)
{
FastPriorityQueue <Node> OpenList = new FastPriorityQueue <Node>(200);
//SimplePriorityQueue<Node> OpenList = new FastPriorityQueue<Node>(maxN_Calculations);//List of nodes for the open list
HashSet <Node> ClosedList = new HashSet <Node>(); //Hashset of nodes for the closed list
OpenList.Enqueue(StartNode, StartNode.FCost); //Add the starting node to the open list to begin the program
while (OpenList.Count > 0) //Whilst there is something in the open list
{
Node CurrentNode = OpenList.Dequeue(); //Create a node and set it to the first item in the open list
OpenList.ResetNode(CurrentNode);
//OpenList.Remove(CurrentNode);//Remove that from the open list
ClosedList.Add(CurrentNode); //And add it to the closed list
if (CurrentNode == TargetNode) //If the current node is the same as the target node
{
GetFinalPath(StartNode, TargetNode); //Calculate the final path
}
//Debug.Log("el nodo en cuestion tiene x vecinos" + CurrentNode.NeighbNodes.Count);
Node[] nNodes = nodeMesh.GetNeighbNodes(CurrentNode);
for (int i = 0; i < nNodes.Length; i++)
{
Node NeighborNode = nNodes[i];
if (NeighborNode == null)
{
continue;
}
if (NeighborNode.BIsWall || ClosedList.Contains(NeighborNode))
{
continue; //If the neighbor is a wall or has already been checked // Skip it
}
int MoveCost = CurrentNode.IgCost + GetManhattenDistance(CurrentNode, NeighborNode); //Get the F cost of that neighbor
if (MoveCost < NeighborNode.IgCost || !OpenList.Contains(NeighborNode)) //If the f cost is greater than the g cost or it is not in the open list
{
NeighborNode.IgCost = MoveCost; //Set the g cost to the f cost
NeighborNode.IhCost = GetManhattenDistance(NeighborNode, TargetNode); //Set the h cost
NeighborNode.ParentNode1 = CurrentNode; //Set the parent of the node for retracing steps
if (!OpenList.Contains(NeighborNode)) //If the neighbor is not in the openlist
{
//Add it to the list
OpenList.Enqueue(NeighborNode, NeighborNode.FCost);
}
}
}
}
//while (OpenList.Count > 0)
//{
// OpenList.ResetNode(OpenList.Dequeue());
//}
}
}
private void Start()
{
NodeNavMesh navMesh = FindObjectOfType <NodeNavMesh>();
Node mNode = navMesh.NodeFromWorldPoint(transform.position);
nodes = navMesh.GetNodesArraund(mNode, ((int)RadiusSpawnArea / 2));
StartCoroutine(Spawning());
}
void GetNodes()
{
Node mNode = world.NodeFromWorldPoint(transform.position);
nodes = world.GetNodesArraund(mNode, 6);
}