public int CompareTo(NodeWrapper <T> other)
{
float f1 = h + g;
float f2 = other.h + other.g;
if (f1 != f2)
{
return(f1.CompareTo(f2));
}
if (h != other.h)
{
return(h.CompareTo(other.h));
}
return(Comparer <T> .Default.Compare(node, other.node));
}
public PathfindingState Step()
{
if (state == PathfindingState.Initialized)
{
state = PathfindingState.Finding;
var startingNodeWrapper = context.GetOrCreateNode(this.startingNode);
startingNodeWrapper.state = NodeState.Open;
context.openList.Enqueue(startingNodeWrapper);
return(state);
}
if (state != PathfindingState.Finding)
{
throw new Exception($"Unexpected state {state}");
}
NodeWrapper <T> first = context.openList.TryDequeue();
if (first != null)
{
// 把周围点 加入open
var neighbors = context.GetGraphData().CollectNeighbor(first.node);
foreach (T p in neighbors)
{
Collect(p, first);
}
first.state = NodeState.Closed;
if (first.node.Equals(this.endingNode))
{
state = PathfindingState.Found;
TraceBackForPath(first);
return(state);
}
}
else
{
state = PathfindingState.Failed;
TraceBackForPath(null);
return(state);
}
return(state);
}
protected bool TraceBackForPath(INodeWrapper <T> endingNode)
{
result.Clear();
if (endingNode == null)
{
return(false);
}
NodeWrapper <T> node = endingNode as NodeWrapper <T>;
while (node.previous != null)
{
result.Add(node.node);
node = node.previous;
}
result.Add(node.node);
return(true);
}
protected bool Collect(T node, NodeWrapper <T> parentNode)
{
NodeWrapper <T> oldNodeWrapper = context.TryGetNode(node);
if (oldNodeWrapper != null && oldNodeWrapper.state == NodeState.Closed)
{
return(false);
}
bool changed = false;
NodeWrapper <T> nodeWrapper = context.GetOrCreateNode(node);
nodeWrapper.previous = parentNode;
nodeWrapper.g = CalculateG(nodeWrapper);
nodeWrapper.h = CalculateH(nodeWrapper);
if (parentNode.previous != null && context.GetGraphData().LineOfSight(node, parentNode.previous.node))
{
nodeWrapper.previous = parentNode.previous;
nodeWrapper.g = CalculateG(nodeWrapper);
}
if (oldNodeWrapper == null)
{
changed = true;
nodeWrapper.state = NodeState.Open;
this.context.openList.Enqueue(nodeWrapper);
}
else if (nodeWrapper.g < oldNodeWrapper.g)
{
changed = true;
oldNodeWrapper.g = nodeWrapper.g;
this.context.openList.Update(oldNodeWrapper);
}
return(changed);
}
protected virtual float CalculateH(NodeWrapper <T> nodeWrapper)
{
return(HeuristicFunc(nodeWrapper.node, this.endingNode));
}
protected override float CalculateH(NodeWrapper <T> nodeWrapper)
{
return(0f);
}
