private void RemoveVisit(GraphNodeNavigatorVisit <T> to_remove)
{
visits.Remove(to_remove);
closed_set.Add(to_remove.GetGraphNode());
open_set.Remove(to_remove.GetGraphNode());
}
private void AddVisit(GraphNodeNavigatorVisit <T> to_add)
{
visits.Remove(to_add);
visits.InsertBefore(to_add, v => to_add.GetTotalScore() < v.GetTotalScore());
open_set[to_add.GetGraphNode()] = to_add;
}
private GraphNodeNavigatorVisit(T g, double d, double h)
{
graph_node = g;
parent_graph_node_visit = null;
total_distance = d;
heuristic_cost_estimate = h;
}
public override bool IsGoal(GraphNodeNavigatorVisit <T> visit)
{
if (visit.GetTotalDistance() >= minimum_distance)
{
return(true);
}
return(false);
}
public override bool IsGoal(GraphNodeNavigatorVisit <T> visit)
{
if (visit.GetGraphNode().Equals(goal_node))
{
return(true);
}
return(false);
}
public GraphNodeNavigator(T s, GraphNodeNavigatorGoal <T> g)
{
start_node = s;
goal = g;
visits = new LinkedList <GraphNodeNavigatorVisit <T> >();
closed_set = new HashSet <T>();
open_set = new Dictionary <T, GraphNodeNavigatorVisit <T> >();
AddVisit(GraphNodeNavigatorVisit <T> .CreateInitial(start_node, goal.CalculateHeuristicCostEstimate(start_node)));
}
public bool Update(GraphNodeNavigatorVisit <T> p, double d)
{
if (d < total_distance)
{
parent_graph_node_visit = p;
total_distance = d;
return(true);
}
return(false);
}
private void Grow(GraphNodeNavigatorVisit <T> parent, T node)
{
if (closed_set.Contains(node) == false)
{
double total_distance = parent.GetTentativeTotalDistance(node);
GraphNodeNavigatorVisit <T> visit = open_set.GetOrCreateValue(node,
n => GraphNodeNavigatorVisit <T> .CreateNormal(n, goal.CalculateHeuristicCostEstimate(n))
);
if (visit.Update(parent, total_distance))
{
AddVisit(visit);
}
}
}
protected override bool WorkInternal()
{
if (open_set.IsNotEmpty())
{
GraphNodeNavigatorVisit <T> current_visit = visits.GetFirst();
RemoveVisit(current_visit);
foreach (T sub_node in current_visit.GetConnectedGraphNodes())
{
Grow(current_visit, sub_node);
}
return(true);
}
return(false);
}
public abstract bool IsGoal(GraphNodeNavigatorVisit <T> visit);
