/// <summary>
/// 取出最小值
/// </summary>
/// <returns>The node.</returns>
public SimpleAStarNode PopNode()
{
SimpleAStarNode minValue = this.headNode.data;
BinaryHeapNode lastNode = this.nodes[--this.nodeLength];
if (lastNode != this.headNode)
{
BinaryHeapNode parentNode = lastNode.parentNode;
if (parentNode.leftNode == lastNode)
{
parentNode.leftNode = null;
}
else
{
parentNode.rightNode = null;
}
this.headNode.data = lastNode.data;
this.headNode.data.mBinaryHeapNode = this.headNode;
this.ModifyToLeaf(this.headNode);
}
else
{
this.headNode = null;
}
this.CacheNode(this.nodes[this.nodeLength]);
this.nodes[this.nodeLength] = null;
return(minValue);
}
/// <summary>
/// 向上修正节点(向树根方向修正节点)
/// </summary>
/// <returns>The to root.</returns>
/// <param name="node">Node.</param>
private BinaryHeapNode ModifyToRoot(BinaryHeapNode node)
{
SimpleAStarNode currentNodeData = node.data;
int currentNodeValue = currentNodeData.f;
BinaryHeapNode parentNode = node.parentNode;
while (parentNode != null)
{
if (currentNodeValue < parentNode.data.f)
{
node.data = parentNode.data;
node.data.mBinaryHeapNode = node;
node = node.parentNode;
parentNode = node.parentNode;
}
else
{
break;
}
}
node.data = currentNodeData;
node.data.mBinaryHeapNode = node;
return(node);
}
/// <summary>
/// 添加新节点
/// </summary>
/// <returns>The node.</returns>
/// <param name="data">Data.</param>
public BinaryHeapNode InsertNode(SimpleAStarNode data)
{
if (this.headNode != null)
{
BinaryHeapNode parentNode = this.nodes[this.nodeLength >> 1];
BinaryHeapNode node = this.GetNode(data, parentNode);
node.data.mBinaryHeapNode = 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.mBinaryHeapNode = this.headNode;
this.nodeLength = 2;
return(this.headNode);
}
}
/// <summary>
/// 刷新节点的相邻节点的 links 属性
/// </summary>
/// <param name="node">Node.</param>
private void RefreshLinksOfAdjacentNodes(SimpleAStarNode node)
{
IList <SimpleAStarNode> adjacentNodes = GetAdjacentNodes(node);
SimpleAStarNode tmpNode = null;
for (int i = 0; i < adjacentNodes.Count; ++i)
{
tmpNode = adjacentNodes[i];
if (tmpNode == null)
{
continue;
}
RefreshNodeLinks(tmpNode);
}
}
/// <summary>
/// 获得一个节点
/// </summary>
/// <returns>The node.</returns>
/// <param name="data">Data.</param>
/// <param name="parentNode">Parent node.</param>
private BinaryHeapNode GetNode(SimpleAStarNode 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);
}
/// <summary>
/// 获得一个节点
/// </summary>
/// <returns>The node.</returns>
/// <param name="data">Data.</param>
/// <param name="parentNode">Parent node.</param>
private BinaryHeapNode GetNode(SimpleAStarNode 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;
}
/// <summary>
/// 寻路
/// </summary>
public IList <SimpleAStarNode> FindPath(SimpleAStarNode startNode, SimpleAStarNode endNode)
{
mSearchPathCheckNum += 2;
if (SearchBaseBinaryHeap(startNode, endNode, mSearchPathCheckNum))
{
SimpleAStarNode currentNode = endNode;
IList <SimpleAStarNode> pathList = new List <SimpleAStarNode>()
{
endNode
};
while (currentNode != startNode)
{
currentNode = currentNode.mParentNode;
pathList.Add(currentNode);
}
return(pathList);
}
return(null);
}
/// <summary>
/// 向下修正节点(向树叶方向修正节点)
/// </summary>
/// <returns>The to leaf.</returns>
/// <param name="node">Node.</param>
private BinaryHeapNode ModifyToLeaf(BinaryHeapNode node)
{
SimpleAStarNode currentNodeData = node.data;
int currentNodeValue = currentNodeData.f;
BinaryHeapNode leftNode = null;
BinaryHeapNode rightNode = null;
while (true)
{
leftNode = node.leftNode;
rightNode = node.rightNode;
if (rightNode != null && leftNode != null && rightNode.data.f < leftNode.data.f)
{
if (currentNodeValue > rightNode.data.f)
{
node.data = rightNode.data;
node.data.mBinaryHeapNode = node;
node = rightNode;
}
else
{
break;
}
}
else if (leftNode != null && leftNode.data.f < currentNodeValue)
{
node.data = leftNode.data;
node.data.mBinaryHeapNode = node;
node = leftNode;
}
else
{
break;
}
}
node.data = currentNodeData;
node.data.mBinaryHeapNode = node;
return(node);
}
/// <summary>
/// 刷新节点的 links 属性
/// </summary>
private void RefreshNodeLinks(SimpleAStarNode node)
{
IList <SimpleAStarNode> adjacentNodes = GetAdjacentNodes(node);
int cost = 0;
List <SimpleAStarLinkNode> linkNodes = new List <SimpleAStarLinkNode>();
SimpleAStarNode tmpNode = null;
for (int i = 0; i < adjacentNodes.Count; ++i)
{
tmpNode = adjacentNodes[i];
if (tmpNode == null)
{
continue;
}
if (tmpNode.mWalkable)
{
if (node.mNodeX != tmpNode.mNodeX && node.mNodeY != tmpNode.mNodeY)
{
if (!mNodes[GetNodeKey(node.mNodeX, tmpNode.mNodeY)].mWalkable ||
!mNodes[GetNodeKey(tmpNode.mNodeX, node.mNodeY)].mWalkable)
{
continue;
}
else
{
cost = DIAG_COST;
}
}
else
{
cost = STRAIGHT_COST;
}
linkNodes.Add(new SimpleAStarLinkNode(tmpNode, cost));
}
}
node.mLinks = linkNodes;
}
/// <summary>
/// 获取节点的相邻节点
/// </summary>
private IList <SimpleAStarNode> GetAdjacentNodes(SimpleAStarNode node)
{
IList <SimpleAStarNode> adjacentNodes = new List <SimpleAStarNode>();
int startX = 0;
int endX = 0;
int startY = 0;
int endY = 0;
startX = Mathf.Max(0, node.mNodeX - 1);
endX = Mathf.Min(mNumCols - 1, node.mNodeX + 1);
startY = Mathf.Max(0, node.mNodeY - 1);
endY = Mathf.Min(mNumRows - 1, node.mNodeY + 1);
SimpleAStarNode varNode = null;
for (int i = startX; i <= endX; ++i)
{
for (int j = startY; j <= endY; ++j)
{
string nodeKey = GetNodeKey(i, j);
if (mNodes.TryGetValue(nodeKey, out varNode))
{
if (varNode != null && varNode != node)
{
if (mIsFourWay)
{
if (i != node.mNodeX && j != node.mNodeY)
{
continue;
}
}
adjacentNodes.Add(varNode);
}
}
}
}
return(adjacentNodes);
}
public SimpleAStarManager(int numCols, int numRows, bool isFourWay = false)
{
mNumCols = numCols;
mNumRows = numRows;
mIsFourWay = isFourWay;
//mAStarHeuristic = new SimpleAStarManhattanHeuristic();
mAStarHeuristic = new SimpleAStarDiagonalHeuristic();
SimpleAStarNode node = null;
mNodes = new Dictionary <string, SimpleAStarNode>();
for (int i = 0; i < mNumCols; i++)
{
for (int j = 0; j < mNumRows; j++)
{
node = new SimpleAStarNode(i, j);
node.AddCostMultiplierCallback(RefreshLinksOfAdjacentNodes);
mNodes.Add(GetNodeKey(i, j), node);
}
}
RefreshLinksOfAllNodes();
mBinaryHeapUtils = new BinaryHeapUtils(numCols * numRows / 2);
}
/// <summary>
/// 返回节点在指定的代价内可移动的范围
/// </summary>
/// <returns>The range.</returns>
/// <param name="startNode">Start node.</param>
/// <param name="costLimit">Cost limit.</param>
public IList <SimpleAStarNode> WalkableRange(SimpleAStarNode startNode, int costLimit)
{
mWalkableRangeCheckNum++;
int maxStep = (int)(costLimit / STRAIGHT_COST);
int startX = Mathf.Max(startNode.mNodeX - maxStep, 0);
int endX = Mathf.Min(startNode.mNodeX + maxStep, mNumCols - 1);
int startY = Mathf.Max(startNode.mNodeY - maxStep, 0);
int endY = Mathf.Min(startNode.mNodeY + maxStep, mNumRows - 1);
IList <SimpleAStarNode> rangeList = new List <SimpleAStarNode>();
for (int i = startX; i <= endX; i++)
{
for (int j = startY; j <= endY; j++)
{
SimpleAStarNode nodeItem = mNodes[GetNodeKey(i, j)];
if (nodeItem.mWalkable && nodeItem.mWalkableRangeCheckNum != mWalkableRangeCheckNum)
{
IList <SimpleAStarNode> pathList = FindPath(startNode, nodeItem);
if (pathList != null && pathList[pathList.Count - 1].f <= costLimit)
{
foreach (SimpleAStarNode node in pathList)
{
if (node.mWalkableRangeCheckNum != mWalkableRangeCheckNum)
{
node.mWalkableRangeCheckNum = mWalkableRangeCheckNum;
rangeList.Add(node);
}
}
}
}
}
}
return(rangeList);
}
/// <summary>
/// 返回节点在指定的代价内可移动的范围
/// </summary>
/// <returns>The range.</returns>
/// <param name="startNode">Start node.</param>
/// <param name="costLimit">Cost limit.</param>
public IList<SimpleAStarNode> WalkableRange(SimpleAStarNode startNode, int costLimit)
{
mWalkableRangeCheckNum++;
int maxStep = (int)(costLimit / STRAIGHT_COST);
int startX = Mathf.Max(startNode.mNodeX - maxStep, 0);
int endX = Mathf.Min(startNode.mNodeX + maxStep, mNumCols - 1);
int startY = Mathf.Max(startNode.mNodeY - maxStep, 0);
int endY = Mathf.Min(startNode.mNodeY + maxStep, mNumRows - 1);
IList<SimpleAStarNode> rangeList = new List<SimpleAStarNode>();
for (int i = startX; i <= endX; i++)
{
for (int j = startY; j <= endY; j++)
{
SimpleAStarNode nodeItem = mNodes[GetNodeKey(i, j)];
if (nodeItem.mWalkable && nodeItem.mWalkableRangeCheckNum != mWalkableRangeCheckNum)
{
IList<SimpleAStarNode> pathList = FindPath(startNode, nodeItem);
if (pathList != null && pathList[pathList.Count - 1].f <= costLimit)
{
foreach (SimpleAStarNode node in pathList)
{
if (node.mWalkableRangeCheckNum != mWalkableRangeCheckNum)
{
node.mWalkableRangeCheckNum = mWalkableRangeCheckNum;
rangeList.Add(node);
}
}
}
}
}
}
return rangeList;
}
public SimpleAStarManager(int numCols, int numRows, bool isFourWay = false)
{
mNumCols = numCols;
mNumRows = numRows;
mIsFourWay = isFourWay;
//mAStarHeuristic = new SimpleAStarManhattanHeuristic();
mAStarHeuristic = new SimpleAStarDiagonalHeuristic();
SimpleAStarNode node = null;
mNodes = new Dictionary<string, SimpleAStarNode>();
for (int i = 0; i < mNumCols; i++)
{
for (int j = 0; j < mNumRows; j++)
{
node = new SimpleAStarNode(i, j);
node.AddCostMultiplierCallback(RefreshLinksOfAdjacentNodes);
mNodes.Add(GetNodeKey(i, j), node);
}
}
RefreshLinksOfAllNodes();
mBinaryHeapUtils = new BinaryHeapUtils(numCols * numRows / 2);
}
/// <summary>
/// 搜索路径
/// </summary>
private bool SearchBaseBinaryHeap(SimpleAStarNode startNode, SimpleAStarNode endNode, int nowCheckNum)
{
/**已进入关闭列表(从openList中移除即进入关闭列表)*/
int STATUS_CLOSED = nowCheckNum + 1;
mBinaryHeapUtils.Reset();
startNode.g = 0;
startNode.f = startNode.g + mAStarHeuristic.Heuristic(startNode.mNodeX, startNode.mNodeY, endNode.mNodeX, endNode.mNodeY);
startNode.mSearchPathCheckNum = STATUS_CLOSED;
int g = 0;
SimpleAStarNode node = startNode;
SimpleAStarNode tmpNode = null;
while (node != endNode)
{
IList<SimpleAStarLinkNode> linkNodes = node.mLinks;
for (int i = 0; i < linkNodes.Count; ++i)
{
tmpNode = linkNodes[i].mNode;
if (tmpNode == null) continue;
g = node.g + linkNodes[i].mCost;
// 如果已被检查过
if (tmpNode.mSearchPathCheckNum >= nowCheckNum)
{
if (tmpNode.g > g)
{
tmpNode.f = g + mAStarHeuristic.Heuristic(tmpNode.mNodeX, tmpNode.mNodeY, endNode.mNodeX, endNode.mNodeY);
tmpNode.g = g;
tmpNode.mParentNode = node;
if (tmpNode.mSearchPathCheckNum == nowCheckNum)
{
mBinaryHeapUtils.ModifyNode(tmpNode.mBinaryHeapNode);
}
}
}
else
{
tmpNode.f = g + mAStarHeuristic.Heuristic(tmpNode.mNodeX, tmpNode.mNodeY, endNode.mNodeX, endNode.mNodeY);
tmpNode.g = g;
tmpNode.mParentNode = node;
tmpNode.mBinaryHeapNode = mBinaryHeapUtils.InsertNode(tmpNode);
tmpNode.mSearchPathCheckNum = nowCheckNum;
}
}
if (mBinaryHeapUtils.headNode != null)
{
node = mBinaryHeapUtils.PopNode();
node.mSearchPathCheckNum = STATUS_CLOSED;
}
else
{
return false;
}
}
return true;
}
/// <summary>
/// 寻路
/// </summary>
public IList<SimpleAStarNode> FindPath(SimpleAStarNode startNode, SimpleAStarNode endNode)
{
mSearchPathCheckNum += 2;
if (SearchBaseBinaryHeap(startNode, endNode, mSearchPathCheckNum))
{
SimpleAStarNode currentNode = endNode;
IList<SimpleAStarNode> pathList = new List<SimpleAStarNode>() { endNode };
while (currentNode != startNode)
{
currentNode = currentNode.mParentNode;
pathList.Add(currentNode);
}
return pathList;
}
return null;
}
/// <summary>
/// 刷新节点的 links 属性
/// </summary>
private void RefreshNodeLinks(SimpleAStarNode node)
{
IList<SimpleAStarNode> adjacentNodes = GetAdjacentNodes(node);
int cost = 0;
List<SimpleAStarLinkNode> linkNodes = new List<SimpleAStarLinkNode>();
SimpleAStarNode tmpNode = null;
for (int i = 0; i < adjacentNodes.Count; ++i)
{
tmpNode = adjacentNodes[i];
if (tmpNode == null) continue;
if (tmpNode.mWalkable)
{
if (node.mNodeX != tmpNode.mNodeX && node.mNodeY != tmpNode.mNodeY)
{
if (!mNodes[GetNodeKey(node.mNodeX, tmpNode.mNodeY)].mWalkable
|| !mNodes[GetNodeKey(tmpNode.mNodeX, node.mNodeY)].mWalkable)
{
continue;
}
else
{
cost = DIAG_COST;
}
}
else
{
cost = STRAIGHT_COST;
}
linkNodes.Add(new SimpleAStarLinkNode(tmpNode, cost));
}
}
node.mLinks = linkNodes;
}
/// <summary>
/// 刷新节点的相邻节点的 links 属性
/// </summary>
/// <param name="node">Node.</param>
private void RefreshLinksOfAdjacentNodes(SimpleAStarNode node)
{
IList<SimpleAStarNode> adjacentNodes = GetAdjacentNodes(node);
SimpleAStarNode tmpNode = null;
for (int i = 0; i < adjacentNodes.Count; ++i)
{
tmpNode = adjacentNodes[i];
if (tmpNode == null) continue;
RefreshNodeLinks(tmpNode);
}
}
public BinaryHeapNode(SimpleAStarNode data, BinaryHeapNode parentNode)
{
this.data = data;
this.parentNode = parentNode;
}
/// <summary>
/// 获取节点的相邻节点
/// </summary>
private IList<SimpleAStarNode> GetAdjacentNodes(SimpleAStarNode node)
{
IList<SimpleAStarNode> adjacentNodes = new List<SimpleAStarNode>();
int startX = 0;
int endX = 0;
int startY = 0;
int endY = 0;
startX = Mathf.Max(0, node.mNodeX - 1);
endX = Mathf.Min(mNumCols - 1, node.mNodeX + 1);
startY = Mathf.Max(0, node.mNodeY - 1);
endY = Mathf.Min(mNumRows - 1, node.mNodeY + 1);
SimpleAStarNode varNode = null;
for (int i = startX; i <= endX; ++i)
{
for (int j = startY; j <= endY; ++j)
{
string nodeKey = GetNodeKey(i, j);
if (mNodes.TryGetValue(nodeKey, out varNode))
{
if (varNode != null && varNode != node)
{
if (mIsFourWay)
{
if (i != node.mNodeX && j != node.mNodeY)
{
continue;
}
}
adjacentNodes.Add(varNode);
}
}
}
}
return adjacentNodes;
}
public SimpleAStarLinkNode(SimpleAStarNode node, int cost)
{
mNode = node;
mCost = cost;
}
public BinaryHeapNode(SimpleAStarNode data, BinaryHeapNode parentNode)
{
this.data = data;
this.parentNode = parentNode;
}
/// <summary>
/// 添加新节点
/// </summary>
/// <returns>The node.</returns>
/// <param name="data">Data.</param>
public BinaryHeapNode InsertNode(SimpleAStarNode data)
{
if (this.headNode != null)
{
BinaryHeapNode parentNode = this.nodes[this.nodeLength >> 1];
BinaryHeapNode node = this.GetNode(data, parentNode);
node.data.mBinaryHeapNode = 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.mBinaryHeapNode = this.headNode;
this.nodeLength = 2;
return this.headNode;
}
}
public SimpleAStarLinkNode(SimpleAStarNode node, int cost)
{
mNode = node;
mCost = cost;
}
/// <summary>
/// 搜索路径
/// </summary>
private bool SearchBaseBinaryHeap(SimpleAStarNode startNode, SimpleAStarNode endNode, int nowCheckNum)
{
/**已进入关闭列表(从openList中移除即进入关闭列表)*/
int STATUS_CLOSED = nowCheckNum + 1;
mBinaryHeapUtils.Reset();
startNode.g = 0;
startNode.f = startNode.g + mAStarHeuristic.Heuristic(startNode.mNodeX, startNode.mNodeY, endNode.mNodeX, endNode.mNodeY);
startNode.mSearchPathCheckNum = STATUS_CLOSED;
int g = 0;
SimpleAStarNode node = startNode;
SimpleAStarNode tmpNode = null;
while (node != endNode)
{
IList <SimpleAStarLinkNode> linkNodes = node.mLinks;
for (int i = 0; i < linkNodes.Count; ++i)
{
tmpNode = linkNodes[i].mNode;
if (tmpNode == null)
{
continue;
}
g = node.g + linkNodes[i].mCost;
// 如果已被检查过
if (tmpNode.mSearchPathCheckNum >= nowCheckNum)
{
if (tmpNode.g > g)
{
tmpNode.f = g + mAStarHeuristic.Heuristic(tmpNode.mNodeX, tmpNode.mNodeY, endNode.mNodeX, endNode.mNodeY);
tmpNode.g = g;
tmpNode.mParentNode = node;
if (tmpNode.mSearchPathCheckNum == nowCheckNum)
{
mBinaryHeapUtils.ModifyNode(tmpNode.mBinaryHeapNode);
}
}
}
else
{
tmpNode.f = g + mAStarHeuristic.Heuristic(tmpNode.mNodeX, tmpNode.mNodeY, endNode.mNodeX, endNode.mNodeY);
tmpNode.g = g;
tmpNode.mParentNode = node;
tmpNode.mBinaryHeapNode = mBinaryHeapUtils.InsertNode(tmpNode);
tmpNode.mSearchPathCheckNum = nowCheckNum;
}
}
if (mBinaryHeapUtils.headNode != null)
{
node = mBinaryHeapUtils.PopNode();
node.mSearchPathCheckNum = STATUS_CLOSED;
}
else
{
return(false);
}
}
return(true);
}