public gxtPathPlanNode AddToOpenSet(gxtPathNode node, gxtPathPlanNode prevNode)
{
//int index = nodes.Keys.GetEnumerator().
gxtPathNode searchNode = node;
gxtPathPlanNode thisNode = null;
if (nodes.ContainsKey(node))
{
thisNode = new gxtPathPlanNode(node, prevNode, goal);
nodes.Add(node, thisNode);
}
else
{
thisNode = nodes[searchNode];
thisNode.IsClosed = false;
}
InsertNode(thisNode);
return thisNode;
}
public void AddToClosedSet(gxtPathPlanNode node)
{
node.IsClosed = true;
}
public gxtPathPlan RebuildPath(gxtPathPlanNode pathGoalNode)
{
gxtPathPlan plan = new gxtPathPlan();
gxtPathPlanNode tmpGoal = pathGoalNode;
while (tmpGoal != null)
{
plan.AddNode(tmpGoal.Node);
tmpGoal = tmpGoal.Prev;
}
return plan;
}
public void ReinsertNode(gxtPathPlanNode planNode)
{
for (int i = 0; i < openSet.Count; i++)
{
if (openSet[i] == planNode)
{
openSet.RemoveAt(i);
InsertNode(planNode);
return;
}
}
InsertNode(planNode);
}
public void InsertNode(gxtPathPlanNode planNode)
{
// just add the node if the open set is empty
if (openSet.Count == 0)
{
openSet.Add(planNode);
return;
}
// otherwise, perform an insertion sort
int index = 0;
gxtPathPlanNode compareNode = openSet[index];
while (compareNode < planNode)
{
++index;
if (index != openSet.Count)
compareNode = openSet[index];
else
break;
}
openSet.Insert(index, planNode);
}