private void AddToOpenSet(TSearchState state, double gValue, double fValue, SearchStateAndAction parent)
{
f[state] = fValue;
g[state] = gValue;
came_from[state] = parent;
openSet.Enqueue(f[state], state);
}
public override SearchPath Run(TSearchState initialState, OpertorsMethodType releventOpertorsFunc,
GoalMethodType goalFunc, HuristicMethodType huristicFunc)
{
init(initialState);
while (!openSet.IsEmpty && !toStop())
{
var current = openSet.DequeueItem();
if (goalFunc(current))
{
return(reconstruct_path(current));
}
closedSet.Add(current);
++Expansations;
var operators = releventOpertorsFunc(current);
if (operators == null)
{
operators = new ActionsCollection();
}
foreach (var opertor in operators)
{
var neighboorStateAndCost = opertor(current);
var neighboor = neighboorStateAndCost.State;
//if the state exists at the closedSet it means that is have a better f already
// so no need to calculate the huristic
if (closedSet.Contains(neighboor))
{
continue;
}
double gValue = neighboorStateAndCost.Cost + g[current];
double fValue = gValue + huristicFunc(neighboor);
SearchStateAndAction stateAndAction = new SearchStateAndAction()
{
state = current, action = opertor
};
HandleDuplicatesInOpenSet(neighboor, gValue, fValue, stateAndAction);
}
}
return(null);
}
private void HandleDuplicatesInOpenSet(TSearchState state, double gValue, double fValue, SearchStateAndAction parent)
{
var duplicates = openSet.Where(x => x.Value.Equals(state));
if (duplicates.Count() > 0)
{
//supposed to be only one duplicate because we check before any insertion
var same_f = duplicates.First();
var same = duplicates.First().Value;
if (fValue < f[same])
{
RemoveFromOpenSet(same_f);
AddToOpenSet(state, gValue, fValue, parent);
}
}
else
{
AddToOpenSet(state, gValue, fValue, parent);
}
}