private void UpdateNode(NodeWrapperEx <T> nodeWrapper)
{
if (nodeWrapper.node.Equals(endingNode))
{
}
else
{
// 更新这些节点的 previous
var neighbors = context.GetGraphData().CollectNeighbor(nodeWrapper.node).Select(n => context.GetOrCreateNode(n));
nodeWrapper.rhs = CalculateRHS(nodeWrapper);
}
if (nodeWrapper.GetConsistency() == NodeConsistency.Consistent)
{
context.openList.Remove(nodeWrapper);
}
else
{
nodeWrapper.h = CalculateH(nodeWrapper);
nodeWrapper.key = CalculateKey(nodeWrapper);
if (context.openList.Contains(nodeWrapper))
{
context.openList.Update(nodeWrapper);
}
else
{
context.openList.Enqueue(nodeWrapper);
}
}
}
public void NotifyNodeDataModified(T nodeData)
{
if (state == PathfindingState.Found || state == PathfindingState.Failed)
{
state = PathfindingState.Finding;
//NodeWrapperEx<T> endingNodeWrapper = context.GetOrCreateNode(this.endingNode);
//endingNodeWrapper.g = float.PositiveInfinity;
//endingNodeWrapper.rhs = float.PositiveInfinity;
//endingNodeWrapper.h = 0;
//endingNodeWrapper.key = CalculateKey(endingNodeWrapper);
//if (context.openList.Contains(endingNodeWrapper))
//{
// context.openList.Remove(endingNodeWrapper);
//}
}
NodeWrapperEx <T> nodeWrapper = this.context.GetOrCreateNode(nodeData);
nodeWrapper.rhs = CalculateRHS(nodeWrapper);
if (nodeWrapper.GetConsistency() == NodeConsistency.Overconsistent)
{
nodeWrapper.g = nodeWrapper.rhs;
}
else
{
nodeWrapper.g = float.PositiveInfinity;
UpdateNode(nodeWrapper);
}
var neighbors = context.GetGraphData().CollectNeighbor(nodeData).Select(n => context.GetOrCreateNode(n));
foreach (var nei in neighbors)
{
UpdateNode(nei);
}
}
public PathfindingState Step()
{
if (state == PathfindingState.Initialized)
{
state = PathfindingState.Finding;
NodeWrapperEx <T> startingNodeWrapper = context.GetOrCreateNode(this.startingNode);
startingNodeWrapper.g = float.PositiveInfinity;
startingNodeWrapper.rhs = float.PositiveInfinity;
startingNodeWrapper.h = 0;
startingNodeWrapper.key = CalculateKey(startingNodeWrapper);
NodeWrapperEx <T> endingNodeWrapper = context.GetOrCreateNode(this.endingNode);
endingNodeWrapper.g = float.PositiveInfinity;
endingNodeWrapper.rhs = 0;
endingNodeWrapper.h = 0;
endingNodeWrapper.key = CalculateKey(endingNodeWrapper);
context.openList.Enqueue(endingNodeWrapper);
return(state);
}
if (state != PathfindingState.Finding)
{
throw new Exception($"Unexpected state {state}");
}
NodeWrapperEx <T> first = context.openList.TryDequeue();
if (first == null)
{
// 寻路失败
state = PathfindingState.Failed;
TraceBackForPath(null);
return(state);
}
NodeWrapperEx <T> startingNodeWrapper0 = context.GetOrCreateNode(startingNode);
startingNodeWrapper0.key = CalculateKey(startingNodeWrapper0);
if (startingNodeWrapper0.GetConsistency() == NodeConsistency.Consistent &&
first.CompareTo(startingNodeWrapper0) >= 0)
{
// 终点局部一致
// 此时有一条有效路径
// state = PathfindingState.Found;
// TraceBackForPath(startingNodeWrapper0);
if (MoveAgent())
{
state = PathfindingState.Found;
}
return(state);
}
Vector2 keyOld = first.key;
Vector2 key = CalculateKey(first);
if (keyOld.x < key.x && keyOld.y < key.y)
{
first.key = key;
context.openList.Enqueue(first);
}
else
{
if (first.GetConsistency() == NodeConsistency.Overconsistent)
{
// 障碍被清除 或者cost变低
first.g = first.rhs;
}
else
{
first.g = float.PositiveInfinity;
UpdateNode(first);
}
var neighbors = context.GetGraphData().CollectNeighbor(first.node).Select(n => context.GetOrCreateNode(n));
foreach (NodeWrapperEx <T> nei in neighbors)
{
UpdateNode(nei);
}
}
return(state);
}