/* return a list of points to go through or null on fail */ public bool AstarBestPathFrom(Vector3 startPos) { this.startingPosition = startPos; targetPosition.y = startingPosition.y; // movement in 2 D SortedList <float, SampledPositionAstar> toExploreQueue = new SortedList <float, SampledPositionAstar> (); HashSet <Vector3> visited = new HashSet <Vector3> (); SampledPositionAstar currentSampledPosition = new SampledPositionAstar(startingPosition, null, 0.0f, (targetPosition - startingPosition).magnitude); toExploreQueue.Add(0.0f, currentSampledPosition); visited.Add(startingPosition); while (toExploreQueue.Count > 0) { currentSampledPosition = toExploreQueue[toExploreQueue.Keys[0]]; toExploreQueue.RemoveAt(0); /* Termination condition*/ if (MyUtils.near(currentSampledPosition.position, targetPosition, precisionAstar)) { buildSolution(currentSampledPosition); return(true); } else { exploreNeighborhood(currentSampledPosition, visited, toExploreQueue); } } return(false); }
private void buildSolution(SampledPositionAstar currentSampledPosition) { this.result = new LinkedList <Vector3>(); this.result.AddFirst(currentSampledPosition.position); // add final position, also if same of initial this.solutionCost = currentSampledPosition.pastCost; // save cost of solution while (currentSampledPosition.lastPosition != null) { if (debugDraws == true) { Debug.DrawLine(currentSampledPosition.position, currentSampledPosition.lastPosition.position, Color.green, 15.0f); } result.AddFirst(currentSampledPosition.position); currentSampledPosition = currentSampledPosition.lastPosition; } }
/** Explore neighbourhood **/ private void exploreNeighborhood(SampledPositionAstar currentSampledPosition, HashSet <Vector3> visited, SortedList <float, SampledPositionAstar> toExploreQueue) { Vector3 nextPos; nextPos.y = this.startingPosition.y; // altezza del percorso?? SampledPositionAstar nextPosition; for (int i = 0; i < 8; i++) { Vector3 temp = new Vector3(precisionAstar * directions[i, 0], 0.0f, precisionAstar * directions[i, 1]); //Vector3 temp0 = new Vector3(directions[i,0],0.0f,directions[i,1]); //Vector3 temp = Vector3.ClampMagnitude(temp0,precisionAstar); nextPos = currentSampledPosition.position + temp; if (visited.Contains(nextPos)) { continue; //if is visited then continue } else { visited.Add(nextPos); // else mark visited } if (isCollision(nextPos)) { continue; //has collided? } //create the next Hop float newPastCost = (nextPos - currentSampledPosition.position).magnitude + currentSampledPosition.pastCost; float newEstimatedCost = (targetPosition - nextPos).magnitude; nextPosition = new SampledPositionAstar(nextPos, currentSampledPosition, newPastCost, newEstimatedCost); try{ toExploreQueue.Add(newPastCost + newEstimatedCost, nextPosition); //Debug.Log(debugMatrices[i]+"Added: "+nextPosition.position+ " with cost: "+ nextPosition.getCost()); } catch (ArgumentException e) { // Debug.Log("Already visited"+e.ToString()); } if (debugDraws == true) { Debug.DrawLine(currentSampledPosition.position, nextPosition.position, Color.red, 15.0f); } } }