public AStarNode(double latestCosts, double costsToEnd, MeshNode node, AStarNode precedent)
{
mLatestCosts = latestCosts;
mCostsToEnd = costsToEnd;
mNode = node;
mPrecedent = precedent;
}
private IList<AStarNode> FindPath(int x, int y)
{
_endNode = _aStarUtils.GetNode(x, y);
if (_beginNode == null)
{
_beginNode = _endNode;
return null;
}
if(_endNode != null && _endNode.walkable)
{
System.DateTime dateTime = System.DateTime.Now;
IList<AStarNode> pathList = _aStarUtils.FindPath(this._beginNode, _endNode);
System.DateTime currentTime = System.DateTime.Now;
System.TimeSpan timeSpan = currentTime.Subtract(dateTime);
Debug.Log(timeSpan.Seconds + "秒" + timeSpan.Milliseconds + "毫秒");
return pathList;
}
return null;
}
public AStarNode2D(AStarCost aStarCost, float cost, int x, int y, AStarNode goalNode = null, AStarNode parent = null )
: base(parent, goalNode, cost)
{
_x = x;
_y = y;
_aStarCost = aStarCost;
}
/// <summary>
/// 添加新节点
/// </summary>
/// <returns>The node.</returns>
/// <param name="data">Data.</param>
public BinaryHeapNode InsertNode(AStarNode data)
{
if(this.headNode != null)
{
BinaryHeapNode parentNode = this.nodes[this.nodeLength >> 1];
BinaryHeapNode node = this.GetNode(data, parentNode);
node.data.binaryHeapNode = node;
if(parentNode.leftNode == null)
{
parentNode.leftNode = node;
}else
{
parentNode.rightNode = node;
}
this.nodes[this.nodeLength] = node;
this.nodeLength ++;
return this.ModifyToRoot(node);
}else
{
this.nodes[1] = this.headNode = this.GetNode(data, null);
this.nodes.Add(this.headNode);
this.headNode.data.binaryHeapNode = this.headNode;
this.nodeLength = 2;
return this.headNode;
}
}
public AStarNode(GraphNode<StateConfig> state, AStarNode parent, float pathCost, float estCost)
{
this.state = state;
this.parent = parent;
this.pathCost = pathCost;
this.estCost = estCost;
}
public List<AStarNode> GetNeighbours(bool firstTime)
{
List<Action> tempList = new List<Action>();
WorldState preConditions = new WorldState();
if(firstTime == true)//returns a list of actions suitable for the goal(a string)
{
preConditions = worldState;
}
else{
preConditions = ActionManager.Instance.getAction(this.name).preConditions;
}
//go thru postConditions for this action
//TODO: gör så det funkar för fler postConditions
/*foreach(KeyValuePair<string, bool> pair in preConditions.getProperties())
{
tempList = ActionManager.Instance.getSuitableActions(pair.Key, pair.Value, preConditions);
}*/
tempList = ActionManager.Instance.getSuitableActions(preConditions);
//Debug.Log("templist count: " + tempList.Count);
foreach(Action action in tempList)
{
//Debug.Log("templist name: " + action.actionName);
AStarNode node = new AStarNode();
node.name = action.GetActionName();
node.parent = this;
node.time = action.time;
suitableActions.Add(node);
}
return suitableActions;
}
static int CompareNodesByFScore(AStarNode x, AStarNode y)
{
if (x.fScore > y.fScore) return 1;
if (x.fScore < y.fScore) return -1;
return 0;
}
/// <summary>
/// Constructor for a node in a 2-dimensional map
/// </summary>
/// <param name="AParent">Parent of the node</param>
/// <param name="AGoalNode">Goal node</param>
/// <param name="ACost">Accumulative cost</param>
/// <param name="AX">X-coordinate</param>
/// <param name="AY">Y-coordinate</param>
public AStarNode2D(AStarNode AParent, AStarNode AGoalNode, double ACost, int AX, int AY)
: base(AParent, AGoalNode, ACost)
{
FX = AX;
FY = AY;
}
public void Initialise( AStarNode _endNode )
{
this.isClosed = false;
this.costToThisPoint = 0.0f;
this.parent = null;
this.distToDestinationNode = this.GetCostTo( _endNode );
}
public List<AStarNode> RunAStar(WorldState currentWorldState)
{
AStarNode startNode = new AStarNode();
AStarNode endNode = new AStarNode();
startNode.SetWorldState(goalWorldState);
endNode.SetWorldState(currentWorldState);
AStar star = new AStar();
List<AStarNode> plan = star.Run(startNode, endNode);
//används under utvecklingsfasen
/*Debug.Log("HÄR ÄR PLANEN!!!!!!!!: " + plan.Count);
foreach(AStarNode node in plan)
{
Debug.Log(node.name);
}*/
//----------------------------
/*foreach(AStarNode node in plan)
{
//TODO: dubbelkolla att returnerande action faktiskt finns
blackBoard.setCurrentAction(node.name);
}*/
//blackBoard.setCurrentAction("");
return plan;
}
public void InvokeHeuristic(AStarNode callAStarNode)
{
if (this.OnHeuristic != null)
{
this.OnHeuristic(callAStarNode);
}
}
public override bool isSameState(AStarNode node)
{
if (node == null) {
return false;
}
AStarNode2D node2d = (AStarNode2D) node;
return node2d.x == _x && node2d.y == _y;
}
public void CheckTarget()
{
AStarNode currentNode = new AStarNode(m_planer.GetNode(), m_planer.Direction, 0, null);
if (currentNode.EqualWithRandomRotation(m_target))
{
m_target = null;
}
}
// Update is called once per frame
public void Reset()
{
closed = false;
parent = null;
gScore = 0;
fScore = 0;
gScore = 0;
renderer.material.color = Color.grey;
}
public float HeuristicCost(AStarNode currentNeighbour, AStarNode endNode)
{
// See list of heuristics: http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
/*var d1 = Math.abs (pos1.x - pos0.x);
var d2 = Math.abs (pos1.y - pos0.y);
return d1 + d2;*/
float cost = ActionManager.Instance.getAction(currentNeighbour.GetName()).cost + Random.Range(-3, 3); //could it be dangerous to use negative values?
return cost;
}
//Converts AStarNodes to TreeNodes
public void ConvertNodes(AStarNode node, TreeNode parent2)
{
TreeNode parent = new TreeNode();
parent = new TreeNode(new Vector3(), node.GetName(), parent2, goalIndex);
planList.Add(parent);
foreach(AStarNode child in node.GetChildren(plan))
{
ConvertNodes(child, parent);
}
}
private void GenerateNodeMapFromIntMap()
{
nodeMap = new AStarNode[xSize, zSize];
for (int x = 0; x < xSize; x++)
{
for (int y = 0; y < zSize; y++)
{
nodeMap[x, y] = new AStarNode(new MapPosition(x, y, map[x, y] > 0), 0f, 0f);
}
}
}
public AStarComponent(GameObject parent)
: base(parent)
{
this.Path = new Stack<AStarNode>();
this.Follow = false;
this.EntityToFollow = null;
this.EntityToFollowPosBuffer = Vector2.Zero;
this.GoalPosition = Vector2.Zero;
this.SwitchedEntity = false;
this.DoSearch = false;
this.CurrentNode = null;
}
/// <summary>
/// Finds the shortest path from the start node to the goal node
/// </summary>
/// <param name="AStartNode">Start node</param>
/// <param name="AGoalNode">Goal node</param>
public void FindPath(AStarNode AStartNode,AStarNode AGoalNode)
{
FStartNode = AStartNode;
FGoalNode = AGoalNode;
FOpenList.Add(FStartNode);
while(FOpenList.Count > 0)
{
// Get the node with the lowest TotalCost
AStarNode NodeCurrent = (AStarNode)FOpenList.Pop();
// If the node is the goal copy the path to the solution array
if(NodeCurrent.IsGoal()) {
while(NodeCurrent != null) {
FSolution.Insert(0,NodeCurrent);
NodeCurrent = NodeCurrent.Parent;
}
break;
}
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach(AStarNode NodeSuccessor in FSuccessors)
{
// Test if the currect successor node is on the open list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeOpen = null;
if(FOpenList.Contains(NodeSuccessor))
NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)];
if((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost))
continue;
// Test if the currect successor node is on the closed list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeClosed = null;
if(FClosedList.Contains(NodeSuccessor))
NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)];
if((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost))
continue;
// Remove the old successor from the open list
FOpenList.Remove(NodeOpen);
// Remove the old successor from the closed list
FClosedList.Remove(NodeClosed);
// Add the current successor to the open list
FOpenList.Push(NodeSuccessor);
}
// Add the current node to the closed list
FClosedList.Add(NodeCurrent);
}
}
public AStarNode findClosed( AStarNode node )
{
for( var i = 0; i < _numClosed; i++ )
{
long care = node.worldState.dontCare ^ -1L;
if( ( node.worldState.values & care ) == ( _closed[i].worldState.values & care ) )
{
_lastFoundClosed = i;
return _closed[i];
}
}
return null;
}
/// <summary>
/// Add node to priority queue
/// </summary>
/// <param name="node"></param>
public void addNode(AStarNode node)
{
if (this.isEmpty())
{
this.queue.Insert(this.binarySearch(node.getCost()), node);
}
else
{
this.queue.Add(node);
}
// reduce size
++this.size;
}
public void SetTarget(GraphNode target, int direction, int maxDistance)
{
if (target == null)
{
m_target = null;
return;
}
m_maxDistance = maxDistance;
m_target = new AStarNode(target, direction, float.MaxValue, null);
m_target.IsTarget = true;
if (!AStarSearch())
{
Debug.Log("FAIL");
m_target = null;
}
}
public List<AStarNode> CreatePath(AStarNode currentNode, AStarNode endNode)
{
while(!(ActionManager.Instance.getAction(currentNode.GetName()).postConditions.contains(endNode.GetWorldState())))
{
if(Object.ReferenceEquals(currentNode.GetParent().GetName(), null)){
break;
}
else
{
pathList.Add(currentNode);
currentNode = currentNode.GetParent();
}
}
pathList.Add(currentNode);
return pathList;
}
public IList<Point> Plan(Point start, Point destination)
{
Rectangle map = new Rectangle(0, 0, this.columnCount, this.lineCount);
if ((!map.Contains(start)) || (!map.Contains(destination)))
{
throw new Exception("StartPoint or Destination not in the current map!");
}
RoutePlanData routePlanData = new RoutePlanData(map, destination);
AStarNode startNode = new AStarNode(start, null, 0, 0);
routePlanData.OpenedList.Add(startNode);
AStarNode currenNode = startNode;
//从起始节点开始进行递归调用
return DoPlan(routePlanData, currenNode);
}
/// <summary>
/// 获得一个节点
/// </summary>
/// <returns>The node.</returns>
/// <param name="data">Data.</param>
/// <param name="parentNode">Parent node.</param>
private BinaryHeapNode GetNode(AStarNode data, BinaryHeapNode parentNode)
{
BinaryHeapNode binaryHeapNode = null;
if(this.cacheNodes.Count > 0)
{
binaryHeapNode = this.cacheNodes[this.cacheNodes.Count - 1];
binaryHeapNode.data = data;
binaryHeapNode.parentNode = parentNode;
this.cacheNodes.RemoveAt(this.cacheNodes.Count - 1);
}
else
{
binaryHeapNode = new BinaryHeapNode(data, parentNode);
}
return binaryHeapNode;
}
private void FindPath(int x, int y)
{
AStarNode endNode = this.aStarUtils.GetNode(x, y);
if (this.beginNode == null)
{
this.beginNode = endNode;
return;
}
if (this.pathList != null && this.pathList.Count > 0)
{
foreach (GameObject xxObject in this.pathList)
{
Destroy(xxObject);
}
}
if(endNode != null && endNode.walkable)
{
System.DateTime dateTime = System.DateTime.Now;
IList<AStarNode> pathList = this.aStarUtils.FindPath(this.beginNode, endNode);
System.DateTime currentTime = System.DateTime.Now;
System.TimeSpan timeSpan = currentTime.Subtract(dateTime);
Debug.Log(timeSpan.Seconds + "秒" + timeSpan.Milliseconds + "毫秒");
if(pathList != null && pathList.Count > 0)
{
foreach(AStarNode nodeItem in pathList)
{
GameObject gameObject = (GameObject)Instantiate(this.pathObject);
this.pathList.Add(gameObject);
gameObject.transform.localPosition = new Vector3(nodeItem.nodeX - this.cols * 0.5f + 0.5f, 0f, nodeItem.nodeY - this.cols * 0.5f + 0.5f);
}
}
this.beginNode = endNode;
}
}
private WorldState goalWorldState; //the goal of the planner
#endregion Fields
#region Methods
public List<AStarNode> RunAStar(WorldState currentWorldState)
{
//get a plan to get from goal to current state
AStarNode startNode = new AStarNode();
AStarNode endNode = new AStarNode();
//Debug!!!!
foreach(KeyValuePair<string, WorldStateValue> pair in goalWorldState.GetProperties())
{
startNode.SetName(pair.Key);
}
startNode.SetWorldState(goalWorldState); //startnode is the goal
endNode.SetWorldState(currentWorldState); //end node is the current WorldState
AStar star = new AStar();
List<AStarNode> plan = star.Run(startNode, endNode); //run the AStar to get a plan
return plan;
}
private void calculateCost(AStarNode node)
{
node.gCost = getManhattanDistance(node.c, startNode.c);
node.hCost = getManhattanDistance(node.c, endNode.c);
node.fCost = node.hCost + node.gCost;
}
public List <AStarNode> AStar(Cell start, Cell end)
{
if (start.Equals(end))
{
return(new List <AStarNode>());
}
open = new List <AStarNode>();
closed = new List <AStarNode>();
bool finished = false;
startNode = new AStarNode(start);
startNode.parent = startNode;
endNode = new AStarNode(end);
open.Add(startNode);
while (!finished)
{
if (open.Count == 0)
{
return(null);
}
AStarNode current = getNodeWithLowestF();
open.Remove(current);
closed.Add(current);
if (current.Equals(endNode))
{
endNode = current;
finished = true;
break;
}
List <AStarNode> neighbours = getPossibleNeighbours(current);
foreach (AStarNode n in neighbours)
{
calculateCost(n);
if (closed.Contains(n))
{
continue;
}
if (!open.Contains(n))
{
open.Add(n);
}
AStarNode n1 = open.Find(x => x.c.Equals(n.c));
if (n1.fCost > n.fCost ||
(n1.fCost == n.fCost && n1.hCost > n.hCost) ||
(n1.fCost == n.fCost && n1.hCost == n.hCost && n1.gCost > n.gCost))
{
n1.fCost = n.fCost;
n1.gCost = n.gCost;
n1.hCost = n.hCost;
n1.parent = n.parent;
n1.c = n.c;
}
}
}
return(gereatePath());
}
public void Remove(AStarNode node)
{
AStarNodeList.Remove(node);
AStarNodeList.Sort();
}
private void expandNodeBoundaries(float[,] g, AStarNode thisNode)
{
// test to expand boundaries
// (1) at [top, bottom, left, right]
// check [above, below, left, right]
// of each square at each edge
// (2) if (unpathable) isExtendable = false;
// (3) if (point is inside another node) isExtendable = false;
// (4) if (isExtendable) extend node by 1 row in current direction
AStarNode an = thisNode;
// check to the right
bool isExtendable = true;
while (isExtendable)
{
// scan over the height of the node
for (int m = an.getYCorner(); m <= (an.getYCorner() + an.getHeight()); m++)
{
int x = an.getXCorner() + an.getWidth() + 1;
// make sure this edge isnt off the map
if (pointIsInBounds(x, m))
{
// check if the spot is unpathable
if (g [x, m] == 1)
{
isExtendable = false;
}
if (ANYaStarNodesContainTestPoint(x, m))
{
isExtendable = false;
}
}
else
{
isExtendable = false;
}
}
if (isExtendable)
{
int w = an.getWidth() + 1;
an = new AStarNode(an.getXCorner(), an.getYCorner(), w, an.getHeight());
}
}
// check to the left
isExtendable = true;
while (isExtendable)
{
// scan over the height of the node
for (int m = an.getYCorner(); m <= (an.getYCorner() + an.getHeight()); m++)
{
int x = an.getXCorner() - 1;
// make sure this edge isnt off the map
if (pointIsInBounds(x, m))
{
// check if the spot is unpathable
if (g [x, m] == 1)
{
isExtendable = false;
}
if (ANYaStarNodesContainTestPoint(x, m))
{
isExtendable = false;
}
}
else
{
isExtendable = false;
}
}
if (isExtendable)
{
int xc = an.getXCorner() - 1;
int w = an.getWidth() + 1;
an = new AStarNode(xc, an.getYCorner(), w, an.getHeight());
}
}
// check up
isExtendable = true;
while (isExtendable)
{
// scan over the height of the node
for (int n = an.getXCorner(); n <= (an.getXCorner() + an.getWidth()); n++)
{
int y = an.getYCorner() + an.getHeight() + 1;
// make sure this edge isnt off the map
if (pointIsInBounds(n, y))
{
// check if the spot is unpathable
if (g [n, y] == 1)
{
isExtendable = false;
}
if (ANYaStarNodesContainTestPoint(n, y))
{
isExtendable = false;
}
}
else
{
isExtendable = false;
}
}
if (isExtendable)
{
int h = an.getHeight() + 1;
an = new AStarNode(an.getXCorner(), an.getYCorner(), an.getWidth(), h);
}
}
// check down
isExtendable = true;
while (isExtendable)
{
// scan over the height of the node
for (int n = an.getXCorner(); n <= (an.getXCorner() + an.getWidth()); n++)
{
int y = an.getYCorner() - 1;
// make sure this edge isnt off the map
if (pointIsInBounds(n, y))
{
// check if the spot is unpathable
if (g [n, y] == 1)
{
isExtendable = false;
}
if (ANYaStarNodesContainTestPoint(n, y))
{
isExtendable = false;
}
}
else
{
isExtendable = false;
}
}
if (isExtendable)
{
int yc = an.getYCorner() - 1;
int h = an.getHeight() + 1;
an = new AStarNode(an.getXCorner(), yc, an.getWidth(), h);
}
}
if (an != thisNode)
{
int i = nodes.IndexOf(thisNode);
nodes [i] = an;
}
}
/// <summary>
/// Equality comparer
/// </summary>
/// <param name="other"> The object to compare to. </param>
/// <returns> True if their positions are equal; false otherwise. </returns>
protected bool Equals(AStarNode other)
{
return(object.Equals(this.Position, other.Position));
}
private void Update()
{
this.GetComponent <AStarPathfinding>().target = randPos;
path = GetComponent <AStarPathfinding>().seekPath;
AStarNode pathNode = path[0];
if ((crossedLocation == false || strideEnded == false) && firingAnimation == false)
{
periodBetweenMoves += Time.deltaTime;
if (periodBetweenMoves >= .6f)
{
strideEnded = false;
//setting travel direction based on whether the ship is close to the enemy
if (pickedAngle == false)
{
pickedAngle = true;
Vector3 targetPos = pathNode.nodePosition;
travelAngle = cardinalizeDirections((360 + Mathf.Atan2(targetPos.y - (transform.position.y + 0.4f), targetPos.x - transform.position.x) * Mathf.Rad2Deg) % 360);
pickSprite(travelAngle);
}
//acceleration and deacceleration
moveTimer += Time.deltaTime;
if (moveTimer < 0.2f)
{
updateSpeed(3);
}
else if (moveTimer <= 0.5f && moveTimer >= 0.2f)
{
updateSpeed(3 - 3 * (3 * (moveTimer - 0.2f)));
}
else
{
pickedAngle = false;
strideEnded = true;
updateSpeed(0);
periodBetweenMoves = 0;
moveTimer = 0;
}
}
moveDirection(travelAngle, speed);
if (Vector2.Distance(transform.position, randPos) < 1.5f)
{
//checks if the guy has swam over the targeted position
crossedLocation = true;
}
}
else
{
if (Vector2.Distance(transform.position, playerShip.transform.position) < 3)
{
//reinitializes movement and sets new random position to go to
strideEnded = false;
crossedLocation = false;
randPos = pickRandPos();
periodBetweenMoves = 0.6f;
}
attackTimer += Time.deltaTime;
if (attackTimer > 3f && stopAttacking == false)
{
//cooldown for attacking and starting to spawn attacks
firingAnimation = true; //<-- so the enemy doesn't start moving while attacking
attackTimer = 0;
StartCoroutine(lightningAnim());
}
float angleToShip = (360 + Mathf.Atan2(playerShip.transform.position.y - transform.position.y, playerShip.transform.position.x - transform.position.x) * Mathf.Rad2Deg) % 360;
pickSprite(angleToShip); //makes it so the enemy faces the ship
}
spawnFoam();
pickRendererLayer();
}
public override float distanceBetweenNodes(AStarNode node, AStarNode node2)
{
return(distanceBetweenParams(node.parameters, node2.parameters));
}
/**
* Returns the neighbours action converted to a node
* @return a neighbour
*/
public override short GetAStarNeighbour(AStarNode AStarNode, short neighbourCount)
{
return((short)m_Neighbours[neighbourCount].Type);
}
private static AStarNode aStarSearch(Location start, Location end, Level level)
{
HashSet<AStarNode> open = new HashSet<AStarNode>();
HashSet<AStarNode> closed = new HashSet<AStarNode>();
AStarNode startNode = new AStarNode(start);
startNode.g = 0;
startNode.h = start.distance(end);
open.Add(startNode);
while (open.Count > 0)
{
AStarNode q = new AStarNode(new Location(0, 0));
double min = 10000;
foreach (AStarNode node in open)
{
if (node.f() < min)
{
min = node.f();
q = node;
}
}
open.Remove(q);
Direction[] directions = {Direction.North, Direction.South, Direction.East, Direction.West};
for (int i = 0; i < 4; i++)
{
Location adj = q.location.getAdjLocation(directions[i]);
if (level.getTile(adj).blocksMovement)
continue;
AStarNode node = new AStarNode(adj);
if (closed.Contains(node))
continue;
node.setParent(q);
if (adj.Equals(end))
return node;
node.g = q.g + 1;
node.h = adj.distance(end);
bool add = true;
foreach (AStarNode openNode in open)
{
if (openNode.location.Equals(node.location) && openNode.f() < node.f())
add = false;
}
foreach (AStarNode closedNode in closed)
{
if (closedNode.location.Equals(node.location) && closedNode.f() < node.f())
add = false;
}
if (add)
{
open.Add(node);
}
}
closed.Add(q);
}
return startNode;
}
public void FindShortestPath(GameObject from, GameObject to)
{
foreach (var item in crossMetaObject)
{
//A* -hoz szükséges számítások
//csúcspontok súlyozása G-H-F értékek
item.CallGCalculationOnFromElements(this.gameObject);
item.CallHCalculationOnExitElements(this.gameObject, to);
}
calculatedRoute.Add(from);
bool straightRoad = true;
List <GameObject> exitsFromStart = new List <GameObject>();
while (straightRoad)
{
GameObject nextElement = calculatedRoute[calculatedRoute.Count - 1].GetComponent <RoadElementModel>().nextElement;
if (GameObject.ReferenceEquals(nextElement, to))
{
//Debug.Log("megvagyunk");
calculatedRoute.Add(nextElement);
routeReady = true;
this.gameObject.GetComponent <TweenHelper>().test = calculatedRoute.ToArray();
return;
}
Component nextComp = nextElement.GetComponent <CrossRoadModel>();
if (nextComp != null)
{
exitsFromStart = nextComp.GetComponentInParent <CrossRoadMeta>().GetOptions(nextElement);
straightRoad = false;
foreach (var item in exitsFromStart)
{
openList.Enqueue(new AStarNode(item, null, calculatedRoute.Count + item.GetComponent <CrossRoadModel>().GetH(this.gameObject)));
openList = new Queue <AStarNode>(openList.OrderBy(x => x.F));
}
}
calculatedRoute.Add(nextElement);
}
//ha megvan az elem akkor már kiléptünk
//ha megvan a legközelebbi kereszteződés akkor ide jutottunk, a lehetséges kimenetelek a exits változóban
bool searchNearestCrossToEnd = true;
GameObject prevUntilClosestCross = to;
while (searchNearestCrossToEnd)
{
GameObject prevElement = prevUntilClosestCross.GetComponent <RoadElementModel>().previousElement;
Component prevComp = prevElement.GetComponent <CrossRoadModel>();
if (prevComp != null)
{
searchNearestCrossToEnd = false;
}
prevUntilClosestCross = prevElement;
}
//megvannak a CÉL legközelebbi entrance pontjai
//innen kezdődik maga az algoritmus
/*
* Debug.Log("Open list :\n " + String.Join("",
* new List<AStarNode>(openList)
* .ConvertAll(i => String.Concat(i.node.ToString(), i.node.transform.parent.ToString(), "\t", i.F, "\n"))
* .ToArray()));
*/
do
{
closedList.Enqueue(openList.Dequeue());
if (GameObject.ReferenceEquals(prevUntilClosestCross, closedList.ToList().Last().node))
{
//path found
//Debug.Log("Found the exit point, from where we will reach the end object");
break;
}
GameObject adjacentEntrance = closedList.ToList().Last().node.GetComponent <CrossRoadModel>().canConnectToFromExit;
List <GameObject> adjacentExits = adjacentEntrance.GetComponentInParent <CrossRoadMeta>().GetOptions(adjacentEntrance);
List <AStarNode> adjacentAStarNodes = new List <AStarNode>();
foreach (var item in adjacentExits)
{
adjacentAStarNodes.Add(new AStarNode(item, closedList.ToList().Last(), closedList.ToList().Last().F + adjacentEntrance.GetComponent <CrossRoadModel>().GetG(this.gameObject)
+ item.GetComponent <CrossRoadModel>().GetH(this.gameObject)));
}
foreach (var aSquare in adjacentAStarNodes)
{
//megnézzük hogy benne van e már a closedlistben
bool flag = false;
foreach (var closed in closedList)
{
if (GameObject.ReferenceEquals(closed.node, aSquare.node))
{
flag = true;
break;
}
}
if (flag)
{
continue;
}
AStarNode oldOpen = null;
foreach (var open in openList)
{
if (GameObject.ReferenceEquals(open.node, aSquare.node))
{
oldOpen = open;
}
}
if (oldOpen == null)
{
openList.Enqueue(aSquare);
}
else
{
//Recalculate the parent and the F value!!!
float oldF = oldOpen.F;
float newF = aSquare.F;
//Debug.Log("oldF: " + oldF + " ------ newF: " + newF);
if (newF < oldF)
{
//Debug.Log("oldF: " + oldF + " ------ newF: " + newF + " -----whoami- " + String.Concat(oldOpen.node.name, oldOpen.node.transform.parent.name));
//Debug.Log("bejöttünk a newF kisebb oldF ágba ---- oldparent : " + oldOpen.parent.node.name + " ---- " + oldOpen.parent.node.transform.parent + " --- newparent: " + closedList.ToList().Last().node + " ----- " + closedList.ToList().Last().node.transform.parent);
oldOpen.F = newF;
oldOpen.parent = closedList.ToList().Last();
}
}
openList = new Queue <AStarNode>(openList.OrderBy(x => x.F));
}
/*
* https://www.raywenderlich.com/3016-introduction-to-a-pathfinding
*
* */
} while (openList.Any());
//kész az útkeresés, ez a csúcsok közti kapcsolat térképe
AStarNode nextReverse = closedList.ToList().Last();
do
{
aStarResult.Enqueue(nextReverse.node);
nextReverse = nextReverse.parent;
} while (nextReverse != null);
aStarResult = new Queue <GameObject>(aStarResult.Reverse());
/*
* Debug.Log("REVERSE A* pathfinding result :\n " + String.Join("",
* new List<GameObject>(aStarResult)
* .ConvertAll(i => String.Concat(i.ToString(), i.transform.parent.ToString(), "\n"))
* .ToArray()));
*/
bool endNotReached = true;
while (endNotReached)
{
GameObject nextcross = aStarResult.Dequeue();
calculatedRoute.AddRange(calculatedRoute.Last().GetComponentInParent <CrossRoadMeta>().GetDirectionObjects(calculatedRoute.Last(), nextcross));
calculatedRoute = calculatedRoute.Distinct().ToList();
/*
* Debug.Log("útvonal DISTINCT :\n " + String.Join("",
* new List<GameObject>(calculatedRoute)
* .ConvertAll(i => String.Concat(i.ToString(), i.transform.parent.ToString(), "\n"))
* .ToArray()));
*/
calculatedRoute.Add(calculatedRoute.Last().GetComponent <CrossRoadModel>().closestRoad);
bool insideRoads = true;
do
{
if (GameObject.ReferenceEquals(calculatedRoute.Last(), to))
{
endNotReached = false;
}
else if (calculatedRoute.Last().GetComponent <CrossRoadModel>() != null)
{
insideRoads = false;
}
else
{
calculatedRoute.Add(calculatedRoute.Last().GetComponent <RoadElementModel>().nextElement);
}
} while (endNotReached && insideRoads);
}
/*
* Debug.Log("útvonal DISTINCT a ciklus után :\n " + String.Join("",
* new List<GameObject>(calculatedRoute)
* .ConvertAll(i => String.Concat(i.ToString(), i.transform.parent.ToString(), "\n"))
* .ToArray()));
*/
routeReady = true;
this.gameObject.GetComponent <TweenHelper>().test = calculatedRoute.ToArray();
}
/// <summary>
/// Finds a path from a start and end node
/// </summary>
/// <param name="start">The start node in the path</param>
/// <param name="end">The end node in the path</param>
/// <returns>The path from the start to the end node</returns>
List <Vector2> FindPath(AStarNode start, AStarNode end)
{
for (int i = 0; i < nodes.Count; i++)
{
nodes[i].Reset();
}
start.hCost = Vector2.Distance(start.position, end.position);
start.gCost = 0;
List <AStarNode> openList = new List <AStarNode>();
AStarNode current = start;
openList.Add(current);
while (openList.Count != 0)
{
// get the node with the lowest f cost
int lowestInd = 0;
float lowestF = float.MaxValue;
for (int i = 0; i < openList.Count; i++)
{
float curF = openList[i].fCost;
if (curF < lowestF)
{
lowestInd = i;
lowestF = curF;
}
}
current = openList[lowestInd];
// if current is goal, end
if (current == end)
{
List <Vector2> output = new List <Vector2>()
{
current.position
};
while (current.parent != null)
{
current = current.parent;
output.Add(current.position);
}
output.RemoveAt(output.Count - 1);
if (nodeLinks.Count > 0)
{
for (int i = 0; i < nodeLinks.Count; i++)
{
nodeLinks[i].Reset();
}
nodeLinks.Clear();
}
return(output);
}
// remove current from the open list
openList.Remove(current);
// go through the current neighbours
foreach (AStarNode neighbour in current.neigbours)
{
float tentative_gCost = current.gCost + Vector2.Distance(neighbour.position, current.position);
if (tentative_gCost < neighbour.gCost)
{
neighbour.parent = current;
neighbour.gCost = tentative_gCost;
neighbour.hCost = Vector2.Distance(neighbour.position, end.position);
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
}
}
}
return(null);
}
public bool Contanis(AStarNode node)
{
return(AStarNodeList.Contains(node));
}
public override int GetNumAStarNeighbours(AStarNode aStarNode)
{
if (aStarNode == null) //If the node is invalid then there's no neighbours
{
return(0);
}
//New search about to occur, clear the previous neighbour records
m_Neighbours.Clear();
/**
* Go through each world state property and test if it is in the goal and current state
* If it isn't then we look through the actionEffects table and see if the actions for this effect can solve the goal state
*/
AStarGOAPNode node = (AStarGOAPNode)aStarNode;
WorldStateProp currProp;
WorldStateProp goalProp;
GOAPAction action;
for (E_PropKey i = 0; i < E_PropKey.Count; i++)
{
// First test if the effect isn't in both the goal and the current state
if (!(node.CurrentState.IsWSPropertySet(i) && node.GoalState.IsWSPropertySet(i)))
{
continue; //If not skip this effect
}
/**
* Now test if the two world state properties are the same
* If not we continue
*/
currProp = node.CurrentState.GetWSProperty(i);
goalProp = node.GoalState.GetWSProperty(i);
if (currProp != null && goalProp != null)
{
if (!(currProp == goalProp))
{
for (int j = 0; j < m_EffectsTable[(int)i].Count; j++)
{
action = m_EffectsTable[(int)i][j];
//Are the context preconditions valid?
if (!action.ValidateContextPreconditions(node.CurrentState, true))
{
continue;
}
// UnityEngine.Debug.Log(action.ToString() + " adding to sousedi");
m_Neighbours.Add(action);
}
}
}
}
//Sort the returned neighbour actions by precedence
PrecedenceComparer c = new PrecedenceComparer();
m_Neighbours.Sort(c);
return(m_Neighbours.Count);
}
/// <summary>
/// 寻路方法 提供给外部使用
/// </summary>
/// <param name="startPos"></param>
/// <param name="endPos"></param>
/// <returns></returns>
public List <AStarNode> FindPath(Vector2 startPos, Vector2 endPos)
{
//实际项目中 传入的点往往是 坐标系中的位置
//我们这里省略换算的过程 直接认为传入的是格子坐标
//首先判断传入的两个点是否合法(1 在地图范围内 2是不是阻挡)
//1 先判断在不在地图内
if (startPos.x < 0 || startPos.x >= mapW ||
startPos.y < 0 || startPos.y >= mapH ||
endPos.x < 0 || endPos.x >= mapW ||
endPos.y < 0 || endPos.y >= mapH)
{
Debug.LogError("开始或者结束点在地图格子范围之外!");
//如果不合法直接返回
return(null);
}
//2 再判断是否为阻挡
//应该得到起点和终点对应的格子
AStarNode start = nodes[(int)startPos.x, (int)startPos.y];
AStarNode end = nodes[(int)endPos.x, (int)endPos.y];
if (start.type == E_Node_Type.Stop ||
end.type == E_Node_Type.Stop)
{
Debug.LogError("开始或者结束点是阻挡!");
//如果不合法直接返回
return(null);
}
//清空上一次相关的数据 避免他们影响这一次的寻路计算
//清空开启和关闭列表
openList.Clear();
openList.Clear();
//把开始点放入关闭列表里面
start.father = null;
start.f = 0;
start.g = 0;
start.h = 0;
closeList.Add(start);
while (true)
{
//左上 x-1 y-1
FindNearlyNodeToOpenList(start.x - 1, start.y - 1, 1.4f, start, end);
//上 x y-1
FindNearlyNodeToOpenList(start.x, start.y - 1, 1f, start, end);
//右上 x+1 y-1
FindNearlyNodeToOpenList(start.x + 1, start.y - 1, 1.4f, start, end);
//左 x-1 y
FindNearlyNodeToOpenList(start.x - 1, start.y, 1f, start, end);
//右 x+1 y
FindNearlyNodeToOpenList(start.x + 1, start.y, 1f, start, end);
//左下 x-1 y+1
FindNearlyNodeToOpenList(start.x - 1, start.y + 1, 1.4f, start, end);
//下 x y+1
FindNearlyNodeToOpenList(start.x, start.y + 1, 1f, start, end);
//右下 x+1 y+1
FindNearlyNodeToOpenList(start.x + 1, start.y + 1, 1.4f, start, end);
//死路判断 开启列表里面为空 都还没找到终点
if (openList.Count == 0)
{
Debug.Log("死路!");
return(null);
}
//选出开启列表中 寻路消耗最小的点
openList.Sort(SortOpenList);
//放入关闭列表中
closeList.Add(openList[0]);
//找到的这个点又变成了新的起点 进行下一次计算
start = openList[0];
//再从开启列表中移除
openList.RemoveAt(0);
//如果这个点已经是终点了 就直接得到结果返回
if (start == end)
{
//找完了
List <AStarNode> path = new List <AStarNode>();
path.Add(end);
while (end.father != null)
{
path.Add(end.father);
end = end.father;
}
//列表翻转的API
path.Reverse();
return(path);
}
//如果不是 那以这个点为起点 继续寻路
}
}
public static AStarReturnObject findPath(AStarMap map, int max)
{
if (map.startNode == null)
{
return(null);
}
BinaryHeap heap = new BinaryHeap(map.startNode);
ArrayList closed = new ArrayList();
AStarNode node = null;
AStarNode closest = null;
while (!heap.isEmpty())
{
node = heap.remove();
closed.Add(node.parameters);
if (map.nodeIsCloseEnough(node))
{
return(new AStarReturnObject(node, closed, heap, map));
}
if (max == 0)
{
return(new AStarReturnObject(closest, closed, heap, map));
}
float d = node.distance();
ArrayList next = map.nextNodesFrom(node);
foreach (AStarNode nextNode in next)
{
if (!closed.Contains(nextNode.parameters))
{
AStarNode nextNode2 = heap.remove(nextNode.parameters);
float newD = d + map.distanceBetweenNodes(node, nextNode);
if (nextNode2 != null)
{
// Debug.Log("NewD: " + newD + " OldD: " + nextNode2.distance());
if (newD < nextNode2.distance())
{
nextNode2.setDistance(newD);
nextNode2.setPrev(node);
}
}
else
{
// Debug.Log("NewD: " + newD);
nextNode2 = nextNode;
nextNode2.setDistance(newD);
nextNode2.setPrev(node);
}
if (!nextNode2.isValidNode(map))
{
closed.Add(nextNode2.parameters);
}
else
{
heap.add(nextNode2);
}
}
}
if (closest == null)
{
closest = node;
}
else if (node.heuristic() < closest.heuristic())
{
closest = node;
}
max--;
}
return(new AStarReturnObject(closest, closed, heap, map));
}
public AStarNode(GameObject node, AStarNode parent, float f)
{
this.node = node;
this.parent = parent;
F = f;
}
//public int getCostBetweenPoints(Vector2Int pointOne, Vector2Int pointTwo)
//{
//}
private List <PathNode> AStar(MapTile[,] mapTiles, Vector2Int start, Vector2Int goal, float maxCost, bool pathMode, MapUnit c)//True: return path, False, return all possibilities
{
List <PathNode> nodesInRange = new List <PathNode>();
aStarNodeArray = new AStarNode[mapTiles.GetLength(0), mapTiles.GetLength(1)];
for (int x = 0; x < mapTiles.GetLength(0); x++)
{
for (int y = 0; y < mapTiles.GetLength(1); y++)
{
aStarNodeArray[x, y] = new AStarNode(new Vector2Int(x, y));
aStarNodeArray[x, y].gScore = float.PositiveInfinity;
if (pathMode)
{
aStarNodeArray[x, y].hScore = Distance(new Vector2Int(x, y), goal);
}
else
{
aStarNodeArray[x, y].hScore = 0;
}
}
}
aStarNodeArray[start.x, start.y].gScore = 0;
FastPriorityQueue <AStarNode> openSet = new FastPriorityQueue <AStarNode>(aStarNodeArray.GetLength(0) * aStarNodeArray.GetLength(1));
openSet.Enqueue(aStarNodeArray[start.x, start.y], aStarNodeArray[start.x, start.y].fScore);
//worldMap.SetDebugTile(start, Color.cyan);
//worldMap.SetDebugTile(start, Color.red);
if (pathMode && (!mapTiles[start.x, start.y].GetPassable(c) || !mapTiles[goal.x, goal.y].GetPassable(c)))
{
return(ReconstructPath(start, goal, aStarNodeArray, maxCost));
}
while (openSet.Count != 0)
{
AStarNode currentNode = openSet.Dequeue();
if (currentNode.gScore > maxCost && !pathMode)
{
return(nodesInRange);
}
if (pathMode && currentNode.pos == goal)
{
//worldMap.SetDebugTile(goal, Color.green);
return(ReconstructPath(start, goal, aStarNodeArray, maxCost));
}
currentNode.closed = true;
// worldMap.SetDebugTile(currentNode.pos, Color.blue);
if (!pathMode)
{
nodesInRange.Add(new PathNode(currentNode.pos, currentNode.gScore));
}
foreach (Vector2Int v in GetNeighbors(currentNode, mapTiles, c))
{
AStarNode currentNeighbor = aStarNodeArray[v.x, v.y];
if (currentNeighbor.closed)
{
continue;
}
if (!openSet.Contains(currentNeighbor))
{
openSet.Enqueue(currentNeighbor, currentNeighbor.fScore);
//worldMap.SetDebugTile(currentNeighbor.pos, Color.magenta);
currentNeighbor.prevPos = currentNode.pos;
}
float tentative_gScore = currentNode.gScore + Cost(mapTiles, currentNode.pos, currentNeighbor.pos, c);
if (tentative_gScore < currentNeighbor.gScore)
{
// This path to neighbor is better than any previous one. Record it!
currentNeighbor.prevPos = currentNode.pos;
currentNeighbor.gScore = tentative_gScore;
openSet.UpdatePriority(currentNeighbor, currentNeighbor.fScore);
}
}
}
if (pathMode)
{
return(ReconstructPath(start, goal, aStarNodeArray, maxCost));
}
else
{
return(nodesInRange);
}
}
/// <summary> /// 应用规则 /// </summary> public abstract void Apply(AStarNode node);
public abstract void SetDestNode(AStarNode destNode);
public override ArrayList nextNodesFrom(AStarNode node)
{
return(nextNodesFrom(node, null));
}
public abstract float GetHeuristicDistance(AgentHuman ai, AStarNode pAStarNode, bool firstRun);
/// <summary>
/// 添加穿越代价被修改后的回调函数
/// </summary>
/// <param name="callback">Callback.</param>
/// <param name="aStarNodeParam">A star node parameter.</param>
public void AddHeuristic(AStarCallback.HeuristicCallback callback, AStarNode aStarNodeParam)
{
this.aStarNodeParam = aStarNodeParam;
this.aStarCallback.OnHeuristic += callback;
}
public abstract float GetActualCost(AStarNode nodeOne, AStarNode nodeTwo);
/// <summary>
/// Finds the shortest path on a map using A*.
/// </summary>
/// <param name="mapHeightLevels"> The height levels of the map. </param>
/// <param name="startPosition"> The position to start the pathfinding from. </param>
/// <param name="endPosition"> The position to find a path to. </param>
/// <returns> A list of the positions that make up the path. </returns>
public static List <Position> FindPathFromTo(Enums.HeightLevel[,] mapHeightLevels, Position startPosition, Position endPosition)
{
// The lists used.
var closedList = new List <AStarNode>();
var openList = new List <AStarNode>();
// The initial node.
var startNode = new AStarNode
{
Position = startPosition,
CameFrom = null,
GScore = 0d,
FScore = 0d + CostEstimate(startPosition, endPosition)
};
openList.Add(startNode);
// Loops as long as there are non-visited nodes left.
while (openList.Count > 0)
{
// Grab the first node in the list.
var current = openList[0];
// If we have found the end, reconstruct the path and return it.
if (current.Position.Equals(endPosition))
{
return(ReconstructPath(current));
}
// Remove the node from the list and add it to the closed.
openList.Remove(current);
closedList.Add(current);
// Find neighbours and iterate over them.
var neighbours = Neighbours(mapHeightLevels, current.Position);
foreach (var neighbourPos in neighbours)
{
var neighbour = new AStarNode {
Position = neighbourPos,
};
// If the neighbour has been visited already, ignore it.
if (closedList.Contains(neighbour))
{
continue;
}
var tentGScore = current.GScore + CostEstimate(current.Position, neighbourPos);
if (!openList.Contains(neighbour))
{
// If open list does not contain the neighbour, add it.
neighbour.CameFrom = current;
neighbour.GScore = tentGScore;
neighbour.FScore = neighbour.GScore + CostEstimate(neighbour.Position, endPosition);
openList.Add(neighbour);
}
else if (openList.Contains(neighbour))
{
// If the open list contains the neighbour and the tentGScore is lower than the GScore of the already-existing
// neighbour, update the neighbour in the list.
var neighbourIndex = openList.IndexOf(neighbour);
if (tentGScore < openList[neighbourIndex].GScore)
{
openList[neighbourIndex].CameFrom = current;
openList[neighbourIndex].GScore = tentGScore;
openList[neighbourIndex].FScore = neighbour.GScore
+ CostEstimate(neighbour.Position, endPosition);
}
}
}
openList = openList.OrderBy(o => o.FScore).ToList();
}
var list = new List <Position>();
return(list);
}
public abstract bool IsAStarFinished(AStarNode currNode);
private bool aStarNodeContainsTestPoint(AStarNode an, int x, int y)
{
return(aStarNodeContainsTestNode(an, x, y, 0));
}
public void Push(AStarNode node)
{
AStarNodeList.Add(node);
AStarNodeList.Sort();
}
//the AStar algorithm
public List<AStarNode> Run(AStarNode startNode, AStarNode endNode)
{
AStarNode currentNode = startNode;
//special case for the first node since it is NOT an action but a postCondition. We are counting on that it will never be a goal!
List<AStarNode> neighbourList = currentNode.GetNeighbours(true); //gives a list with actions that has startNodes preCondition
foreach(AStarNode node in neighbourList) //for each of these neighbours
{
node.SetF(ActionManager.Instance.GetAction(node.GetName()).cost + HeuristicCost(node, endNode)); //calculate F of the neigbour
node.SetParent(startNode); //Set the startNode as the neighbours parent
openList.Add(node); //Add the neighbours of the startNode to the openList
}
//As long as we still have nodes in the openList
while(openList.Count > 0)
{
//Take out the node with the lowest value for F in the openList
int lowInd = 0;
for(int i = 0; i < openList.Count; i++)
{
if(openList[i].GetF() < openList[lowInd].GetF())
{
lowInd = i;
}
}
currentNode = openList[lowInd];
openList.Remove(currentNode);
closedList.Add(currentNode);
//Check if we have reached the target
if(endNode.GetWorldState().Contains(ActionManager.Instance.GetAction(currentNode.GetName()).preConditions))
{
return CreatePath(currentNode, startNode);
}
//if action with preCondition with amount or several preConditions
WorldState preConditions = ActionManager.Instance.GetAction(currentNode.GetName()).preConditions;
if(preConditions.ContainsAmount() || preConditions.GetProperties().Count > 1)
{
List<List<AStarNode>> lists = new List<List<AStarNode>>();
//for each of the preConditions of the currentNodes action we need to start new AStars
foreach(KeyValuePair<string, WorldStateValue> pair in preConditions.GetProperties())
{
//if an action contains a preCondition that has amount then we need to run the algorithm several times
for(int j = 0; j < (int)pair.Value.propertyValues["amount"]; j++)
{
AStar astar2 = new AStar();
AStarNode tempNode = new AStarNode();
WorldState tempWorldState = new WorldState();
tempWorldState.SetProperty(pair.Key, pair.Value);
tempNode.SetWorldState(tempWorldState);
List<AStarNode> tempList = new List<AStarNode>();
tempList.AddRange(astar2.Run(tempNode, endNode)); //run the AStar for the preCondition
tempList[tempList.Count-1].SetParent(currentNode); //set parent to action before fork
if(tempList.Count > 0)
{
lists.Add(tempList); //add new plan to main plan
}
else
{
if(!tempWorldState.Contains(endNode.GetWorldState()))
{
return new List<AStarNode>(); //unreachable goal for the algorithm, cant find goal
}
}
}
}
//Sort by cost to make the plan prioritize actions with small costs
lists.Sort((a, b) => ActionManager.Instance.GetAction(a[0].GetName()).cost.CompareTo(ActionManager.Instance.GetAction(b[0].GetName()).cost));
foreach(List<AStarNode> list in lists)
{
pathList.AddRange(list); //add all branches to final path
}
return CreatePath(currentNode, startNode);
}
neighbourList = currentNode.GetNeighbours(false); //get neighbouring nodes
for(int i = 0; i < neighbourList.Count; i++)
{
AStarNode currentNeighbour = neighbourList[i];
if(closedList.Contains(currentNeighbour)) //We have already looked at this node
{
//not a valid node
continue;
}
if(!openList.Contains(currentNeighbour)) //it can be an interesting node
{
currentNeighbour.SetH(HeuristicCost(currentNeighbour, endNode));
currentNeighbour.SetG(currentNode.GetG() + currentNeighbour.GetTime());
currentNeighbour.SetF(currentNeighbour.GetG() + currentNeighbour.GetH());
openList.Add(currentNeighbour); //add it to the openList
}
}
}
return new List<AStarNode>();
}
private void addOpenList(AStarNode node)
{
openList.Add(node);
node.setInspected();
}
public void setParent(AStarNode node)
{
parent = node;
}
private void removeOpenList(AStarNode node)
{
openList.Remove(node);
node.setVisited();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
/// <summary> Performs the A* algorithm to find minimum distance path. </summary>
///
/// <remarks> Darrell Plank, 5/28/2020. </remarks>
///
/// <returns>
/// A list of T objects which minimizes the distance between _start and one of the _targets.
/// </returns>
////////////////////////////////////////////////////////////////////////////////////////////////////
public List <T> Solve()
{
// Given an IState we need to be able to locate the corresponding
// AStarNode in the Fibonacci heap and since we are letting Pqt
// handle the messy details, we actually are looking for the
// Pqt<AStarNode<T>> which is in the heap...
var dictOpen = new Dictionary <T, Pqt <AStarNode <T> > >();
// While there are elements in Open
while (_openSet.Count != 0)
{
// Get the most likely candidate in Open
var curOpenNode = (AStarNode <T>)_openSet.Pop();
#if PRINTASTAR
Debug.WriteLine($"Opening {curOpenNode.StateData.ToString()}");
#endif
// Is it a target node?
if (_targets.Contains(curOpenNode.StateData))
{
// We're here! Reconstruct the path and return it to the user.
return(ReconstructPath(curOpenNode));
}
// Move this node to Closed
_closed.Add(curOpenNode.StateData);
// For each of this node's successor nodes
foreach (var nodeState in curOpenNode.StateData.Successors())
{
// Set neighbor to successor
var neighbor = (T)nodeState;
// Is neighbor in Closed?
if (_closed.Contains(neighbor))
{
// TODO: if the dist est is inadmissable we may need to move this node to Open
// This is because we can't be guaranteed of finding the absolute minimal
// distance to nodes in _closed and so the new distance we found to our
// closed neighbor may be smaller than it's original distance at which point
// we need to reintroduce the node into the _open mix.
// We've already handled this neighbor so continue on to next one
continue;
}
// Is the neighbor in Open?
if (dictOpen.ContainsKey(neighbor))
{
// Get the Open node for the neighbor
var nodeInOpen = dictOpen[neighbor];
// Make a proposed replacement using current node as the back link
var nodeCandidate = new AStarNode <T>(neighbor, _targets, curOpenNode);
// Is our proposed replacement a better solution?
if (((AStarNode <T>)nodeInOpen).CompareTo(nodeCandidate) > 0)
{
// Then replace old neighbor node with our replacement
_openSet.DecreaseKeyTyped(nodeInOpen, nodeCandidate);
}
}
else
{
// Neighbor is outside both Closed and Open sets so add it to Open
dictOpen[neighbor] = _openSet.AddTyped(new AStarNode <T>(neighbor, _targets, curOpenNode));
}
}
}
return(null);
}
private void addClosedList(AStarNode node)
{
closedList.Add(node);
node.setVisited();
}
