private void ResetState()
{
this.Interesting.Clear();
this.Nodes.Clear();
this.PathReconstructor.Clear();
this.NodeClosestToGoal = null;
}
private void UpdateNodeClosestToGoal(PathFinderNode current)
{
if (current.ExpectedRemainingTime < this.NodeClosestToGoal.ExpectedRemainingTime)
{
this.NodeClosestToGoal = current;
}
}
private void AddFirstNode(INode start, INode goal, Velocity maximumVelocity)
{
var head = new PathFinderNode(start, Duration.Zero, ExpectedDuration(start, goal, maximumVelocity));
this.Interesting.Insert(head);
this.Nodes.Add(head.Node, head);
this.NodeClosestToGoal = head;
}
private void UpdateExistingNode(INode goal, Velocity maximumVelocity, PathFinderNode current, IEdge edge, INode oppositeNode, Duration costSoFar, PathFinderNode node)
{
if (node.DurationSoFar > costSoFar)
{
this.Interesting.Remove(node);
this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity);
}
}
private void InsertNode(INode current, IEdge via, INode goal, Duration costSoFar, Velocity maximumVelocity)
{
this.PathReconstructor.SetCameFrom(current, via); // 修改前驱节点
var node = new PathFinderNode(current, costSoFar, ExpectedDuration(current, goal, maximumVelocity));
this.Interesting.Insert(node);
this.Nodes[current] = node;
}
/// <summary>
/// 修改到现有节点所需的最少时间方案
/// </summary>
/// <param name="goal"></param>
/// <param name="maximumVelocity"></param>
/// <param name="current"></param>
/// <param name="edge"></param>
/// <param name="oppositeNode"></param>
/// <param name="costSoFar"></param>
/// <param name="node"></param>
private void UpdateExistingNode(INode goal, Velocity maximumVelocity, PathFinderNode current, IEdge edge, INode oppositeNode, Duration costSoFar, PathFinderNode node)
{
if (node.DurationSoFar > costSoFar)
{
this.Interesting.Remove(node);// 删除到达该点原有的路线方案，PathFinderNode 中记录了到达该点的位置信息，以及到达该点所有的时间，以及走完到终点大概所需的总时间
// 上面的只是删除可能感兴趣的PathFinderNode， 删除这个pathFinderNode,后面这个InsertNode还会继续增加增加该点信息，
// 1， 上面的删除其实也只是修改到达该点的信息，
// 2. InsertNode中的PathReconstructor 中的SetCameFrom中的容器会修改这个路径来源，其实没有修改点Node,而是修改点的value值， Value值其实是路径值
// 路径值表示出发点是哪一个点，结束点是当前点
this.InsertNode(oppositeNode, edge, goal, costSoFar, maximumVelocity);
}
}
private static bool GoalReached(INode goal, PathFinderNode current) => current.Node == goal;