public static AStarPath GetPath(Vector3 vStartPosition, Vector3 vEndPosition)
{
AStarPath path = new AStarPath();
AStarNode pStartNode = GetClosestNode(vStartPosition);
AStarNode pEndNode = GetClosestNode(vEndPosition);
Vector3 vEndNodePosition = pEndNode.transform.position;
Dictionary <AStarNode, AStarNodeInfo> nodeInfos = new Dictionary <AStarNode, AStarNodeInfo>();
List <AStarNodeInfo> openList = new List <AStarNodeInfo>();
List <AStarNode> closedList = new List <AStarNode>();
// Add start node to open list
AddNodeToOpenList(pStartNode, null, nodeInfos, openList, closedList, vEndNodePosition);
AStarNodeInfo pFinalNode = null;
while (openList.Count > 0)
{
// Examine node
AStarNodeInfo pCurrentNode = openList[0];
openList.RemoveAt(0);
if (pCurrentNode.mNode == pEndNode)
{
pFinalNode = pCurrentNode;
break;
}
else
{
foreach (AStarNode pNode in pCurrentNode.mNode.Links)
{
AddNodeToOpenList(pNode, pCurrentNode, nodeInfos, openList, closedList, vEndNodePosition);
}
}
}
// Retrace path
if (pFinalNode != null)
{
AStarNodeInfo pNode = pFinalNode;
while (pNode != null)
{
path.InsertNode(pNode.mNode);
pNode = pNode.mPreviousNode;
}
}
return(path);
}
static void AddNodeToOpenList(AStarNode pNode, AStarNodeInfo parent, Dictionary <AStarNode, AStarNodeInfo> nodeInfos, List <AStarNodeInfo> openList, List <AStarNode> closedList, Vector3 vEndPosition)
{
AStarNodeInfo info = null;
if (!nodeInfos.TryGetValue(pNode, out info))
{
info = new AStarNodeInfo();
info.mNode = pNode;
info.mCostH = Vector3.Distance(pNode.transform.position, vEndPosition);
}
float fGCost = 0.0f;
if (parent != null)
{
fGCost = parent.mCostG + Vector3.Distance(pNode.transform.position, parent.mNode.transform.position);
}
if (fGCost < info.mCostG)
{
info.mPreviousNode = parent;
info.mCostG = fGCost;
info.mCostF = info.mCostG + info.mCostH;
int iIndex = openList.Count;
for (int i = 0; i < iIndex; ++i)
{
if (info.mCostF < openList[i].mCostF)
{
iIndex = i;
break;
}
}
openList.Remove(info);
openList.Insert(iIndex, info);
}
}
//===================== map base function ================================
//Astar
public List <MapCoordinates> FindPath(MapCoordinates start, MapCoordinates end)
{
List <AStarNodeInfo> openList = new List <AStarNodeInfo>();
bool[,] closeArray = new bool[mapGrid.width, mapGrid.height];
var keyNode = new AStarNodeInfo();
keyNode.pos = start;
keyNode.gWeight = 0;
keyNode.hWeight = MapCoordinates.Distance(start, end);
openList.Add(keyNode);
Vector2Int[] neighbourOffsets = new Vector2Int[4]
{
new Vector2Int(-1, 0),
new Vector2Int(1, 0),
new Vector2Int(0, 1),
new Vector2Int(0, -1)
};
while (openList.Count > 0)
{
AStarNodeInfo curNode = openList[0];
openList.RemoveAt(0);
//check if curNode is end
if (curNode.pos.Equal(end))
{
keyNode = curNode;
break;
}
//insert neighbours
for (int i = 0; i < neighbourOffsets.Length; ++i)
{
MapCoordinates neighbourPos = new MapCoordinates(curNode.pos.X + neighbourOffsets[i].x, curNode.pos.Z + neighbourOffsets[i].y);
if (!IsMapCoordCanMove(neighbourPos) || closeArray[neighbourPos.X, neighbourPos.Z])
{
continue;
}
//check if openList has contained same node
AStarNodeInfo sameNode = openList.Find((AStarNodeInfo node) => { return(node.pos.Equal(neighbourPos)); });
if (sameNode != null)
{
if (sameNode.gWeight > curNode.gWeight + 1)
{
sameNode.parent = curNode;
sameNode.gWeight = curNode.gWeight + 1;
}
continue;
}
AStarNodeInfo newNode = new AStarNodeInfo();
newNode.pos = neighbourPos;
newNode.gWeight = curNode.gWeight + 1;
newNode.hWeight = MapCoordinates.Distance(neighbourPos, end);
newNode.parent = curNode;
openList.Add(newNode);
}
closeArray[curNode.pos.X, curNode.pos.Z] = true;
openList.Sort((AStarNodeInfo a, AStarNodeInfo b) =>
{
return((a.gWeight + a.hWeight) - (b.gWeight + b.hWeight));
});
}
//return astar result-path
List <MapCoordinates> path = new List <MapCoordinates>();
while (keyNode != null)
{
path.Insert(0, keyNode.pos);
keyNode = keyNode.parent;
}
return(path);
}
