/** * After updating, we rotate and move towards the nextNodeToTargetPosition */ protected void UpdateNextNodePosition() { while (IsAtNextNodePosition() && this.path.Count > 0) { this.nextNode = this.path.Pop(); this.nextNodePosition.x = EnvironmentManager.environmentManager.GetPositionX(this.nextNode.indexX); this.nextNodePosition.y = EnvironmentManager.environmentManager.GetPositionY(this.nextNode.indexY); } }
protected override void Start() { base.Start(); circleSmallGrid = new GameObject[EnvironmentManager.environmentManager.XN, EnvironmentManager.environmentManager.YM]; path = new Stack <AStarPriorityQueueNode>(); nextNodePosition = transform.position; nextNode = new AStarPriorityQueueNode(0f, EnvironmentManager.environmentManager.GetIndexX(this.nextNodePosition.x), EnvironmentManager.environmentManager.GetIndexY(this.nextNodePosition.y), -1, -1); personalityUniform = MyStaticLibrary.NextRandomUniformArray(5, -1f, 1f); personalityGaussian = MyStaticLibrary.NextRandomGaussianArray(5); }
/** * Places a circle node at index location mostly for demo purposes */ private IEnumerator InstantiateNodeAStarSearch(AStarPriorityQueueNode node, float waitTime, Color color) { yield return(new WaitForSeconds(waitTime)); // node.Print(); Vector2 position = new Vector2(EnvironmentManager.environmentManager.GetPositionX(node.indexX), EnvironmentManager.environmentManager.GetPositionY(node.indexY)); // TODO GameObject nodeGameObject = Instantiate(PrefabReferences.prefabReferences.circleSmall2, position, Quaternion.identity); nodeGameObject.GetComponent <SpriteRenderer>().color = color; if (circleSmallGrid[node.indexX, node.indexY] != null) { Destroy(circleSmallGrid[node.indexX, node.indexY]); } circleSmallGrid[node.indexX, node.indexY] = nodeGameObject; }
private IEnumerator tracePath(AStarPriorityQueueNode[,] nodeGrid, AStarPriorityQueueNode expandedTargetNode) { Stack <AStarPriorityQueueNode> path = new Stack <AStarPriorityQueueNode>(); if (expandedTargetNode != null) { AStarPriorityQueueNode nextExpandedNode = expandedTargetNode; path.Push(nextExpandedNode); yield return(InstantiateNodeAStarSearch(nextExpandedNode, 0.1f, new Color(0, 0, 0))); // for debugging purposes while (nextExpandedNode.parentIndexX != -1) { nextExpandedNode = nodeGrid[nextExpandedNode.parentIndexX, nextExpandedNode.parentIndexY]; path.Push(nextExpandedNode); yield return(InstantiateNodeAStarSearch(nextExpandedNode, 0.1f, new Color(0, 0, 0))); // for debugging purposes } } this.path = path; yield return(null); }
///** // * Updates targetPosition to be the closestHostileAgent position // */ //protected IEnumerator KeepFindPrimeAdversary() { // while (presentAllyList.Count > 0) { // FindPrimeAdversary(); // yield return new WaitForSeconds(1f); // } //} ///** // * Returns closest circle agent who is on a different team or null if none. // */ //protected void FindPrimeAdversary() { // // CircleAgent[] circleAgentArray = FindObjectsOfType<CircleAgent>(); // primeAdversary = null; // float primeAdversaryTransformDistance = float.MaxValue; // for (int pa = 0; pa < presentAdversaryList.Count; pa++) { // if (presentAdversaryList[pa].affinity != agent.affinity && !presentAdversaryList[pa].defunct) { // float distance = MyStaticLibrary.GetDistance(gameObject, presentAdversaryList[pa].gameObject); // if (distance < primeAdversaryTransformDistance) { // primeAdversary = presentAdversaryList[pa]; // primeAdversaryTransformDistance = distance; // } // } // } //} protected virtual IEnumerator FindPathAStarSearch(Vector2 targetPosition) { int frontierSize = EnvironmentManager.environmentManager.XN + EnvironmentManager.environmentManager.YM; // ASSUMPTION: frontierSize > 0 FastPriorityQueue <AStarPriorityQueueNode> frontier = new FastPriorityQueue <AStarPriorityQueueNode>(frontierSize); AStarPriorityQueueNode[,] nodeGrid = new AStarPriorityQueueNode[EnvironmentManager.environmentManager.XN, EnvironmentManager.environmentManager.YM]; // element is null if never in frontier before /** * source node and target node */ // AStarPriorityQueueNode sourceNode = this.path.Skip(1).FirstOrDefault(); // HACK: assume AStarSearch will finish by the time agent reaches next next node int sourceIndexX = EnvironmentManager.environmentManager.GetIndexX(transform.position.x); int sourceIndexY = EnvironmentManager.environmentManager.GetIndexY(transform.position.y); int targetIndexX = EnvironmentManager.environmentManager.GetIndexX(targetPosition.x); int targetIndexY = EnvironmentManager.environmentManager.GetIndexY(targetPosition.y); if (!EnvironmentManager.environmentManager.environmentGraph[sourceIndexX, sourceIndexY, 0] || !EnvironmentManager.environmentManager.environmentGraph[targetIndexX, targetIndexY, 0]) { Debug.Log("astar source or target in entity wall"); // occurs if target corpse slides through wall yield break; } /** * Explore frontier from source node to target node */ AStarPriorityQueueNode expandedTargetNode = null; float sourceNodePathCost = 0f; nodeGrid[sourceIndexX, sourceIndexY] = new AStarPriorityQueueNode(sourceNodePathCost, sourceIndexX, sourceIndexY, -1, -1); float sourceNodePriority = nodeGrid[sourceIndexX, sourceIndexY].pastPathCost + futurePathCostHeuristic(sourceIndexX, sourceIndexY, targetIndexX, targetIndexY); frontier.Enqueue(nodeGrid[sourceIndexX, sourceIndexY], sourceNodePriority); while (frontier.Count > 0) { AStarPriorityQueueNode node = frontier.Dequeue(); yield return(InstantiateNodeAStarSearch(node, 0.01f, new Color(1, 1, 1))); // for debugging purposes if (node.indexX == targetIndexX && node.indexY == targetIndexY) // if path found, then break { expandedTargetNode = node; break; } /** * Expand the node by adding its neighbors to the frontier */ int[][] validNeighborIndicesXY = EnvironmentManager.environmentManager.GetValidNeighborIndicesXY(node.indexX, node.indexY); for (int vv = 0; vv < validNeighborIndicesXY.Length; vv++) { if (validNeighborIndicesXY[vv] != null) { int neighborNodeIndexX = validNeighborIndicesXY[vv][0]; int neighborNodeIndexY = validNeighborIndicesXY[vv][1]; float neighborNodePathCost = node.pastPathCost + EnvironmentManager.unitVectorMagnitudes[vv + 1]; if (nodeGrid[neighborNodeIndexX, neighborNodeIndexY] == null) // node has no parent { if (frontier.Count == frontierSize) { frontierSize *= 2; frontier.Resize(frontierSize); } nodeGrid[neighborNodeIndexX, neighborNodeIndexY] = new AStarPriorityQueueNode(neighborNodePathCost, neighborNodeIndexX, neighborNodeIndexY, node.indexX, node.indexY); float neighborNodePriority = nodeGrid[neighborNodeIndexX, neighborNodeIndexY].pastPathCost + futurePathCostHeuristic(neighborNodeIndexX, neighborNodeIndexY, targetIndexX, targetIndexY); frontier.Enqueue(nodeGrid[neighborNodeIndexX, neighborNodeIndexY], neighborNodePriority); } else if (nodeGrid[neighborNodeIndexX, neighborNodeIndexY].pastPathCost - neighborNodePathCost > epsilon) // node has a worse parent { nodeGrid[neighborNodeIndexX, neighborNodeIndexY].pastPathCost = neighborNodePathCost; nodeGrid[neighborNodeIndexX, neighborNodeIndexY].parentIndexX = node.indexX; nodeGrid[neighborNodeIndexX, neighborNodeIndexY].parentIndexY = node.indexY; float neighborNodePriority = nodeGrid[neighborNodeIndexX, neighborNodeIndexY].pastPathCost + futurePathCostHeuristic(neighborNodeIndexX, neighborNodeIndexY, targetIndexX, targetIndexY); frontier.UpdatePriority(nodeGrid[neighborNodeIndexX, neighborNodeIndexY], neighborNodePriority); } // else { } // this is if the neighbor already has a better parent; note that if a neighbor has already been dequeued from the frontier, then it will have a better parent because its best parent has already been found } } } yield return(tracePath(nodeGrid, expandedTargetNode)); }