protected bool TraceBackForPath(INodeWrapper <T> startingNode)
{
result.Clear();
NodeWrapperEx <T> nodeWrapper = startingNode as NodeWrapperEx <T>;
if (nodeWrapper == null || nodeWrapper.g == float.PositiveInfinity)
{
return(false);
}
while (!nodeWrapper.node.Equals(endingNode))
{
NodeWrapperEx <T> previous = default;
foreach (var nei in context.GetGraphData().CollectNeighbor(nodeWrapper.node).Select(n => context.GetOrCreateNode(n)))
{
if (previous == null || nei.g < previous.g)
{
previous = nei;
}
}
result.Add(nodeWrapper.node);
nodeWrapper = previous;
}
result.Add(nodeWrapper.node);
return(true);
}
protected Vector2 CalculateKey(NodeWrapperEx <T> nodeWrapper)
{
float k2 = Mathf.Min(nodeWrapper.g, nodeWrapper.rhs);
float k1 = k2 + nodeWrapper.h + km;
return(new Vector2(k1, k2));
}
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 NodeWrapperEx <T> GetOrCreateNode(T nodeData)
{
if (!nodeMapping.TryGetValue(nodeData, out NodeWrapperEx <T> nodeWrapperEx))
{
nodeWrapperEx = new NodeWrapperEx <T>(nodeData);
nodeMapping.Add(nodeData, nodeWrapperEx);
}
return(nodeWrapperEx);
}
public NodeWrapperEx <T> TryGetNode(T nodeData)
{
NodeWrapperEx <T> node = null;
if (this.nodeMapping.TryGetValue(nodeData, out node))
{
return(node);
}
return(null);
}
public int CompareTo(NodeWrapperEx <T> other)
{
Vector2 ko = other.key;
if (key.x != ko.x)
{
return(key.x.CompareTo(ko.x));
}
if (key.y != ko.y)
{
return(key.y.CompareTo(ko.y));
}
return(Comparer <T> .Default.Compare(node, other.node));
}
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 void NotifyNodeDataModified(T nodeData)
{
if (state != PathfindingState.Finding)
{
throw new InvalidOperationException();
}
km = km + HeuristicFunc(oldStartingNode, currentNode);
oldStartingNode = currentNode;
NodeWrapperEx <T> nodeWrapper = this.context.GetOrCreateNode(nodeData);
nodeWrapper.g = float.PositiveInfinity;
nodeWrapper.rhs = 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);
}
protected float CalculateRHS(NodeWrapperEx <T> nodeWrapper)
{
var neighbors = context.GetGraphData().CollectNeighbor(nodeWrapper.node).Select(n => context.GetOrCreateNode(n));
return(neighbors.Select(nei => nei.g + HeuristicFunc(nei.node, nodeWrapper.node) * (1 + nodeWrapper.node.cost)).Min());
}
protected float CalculateH(NodeWrapperEx <T> nodeWrapper)
{
return(HeuristicFunc(nodeWrapper.node, this.startingNode));
}