public void Connect(PlanningNode from, PlanningNode to)
{
PlanningEdge edge = new PlanningEdge(from, to);
mEdges.Add(edge);
edge.parent = this;
}
public void RemovePlanningNode(PlanningNode node, int t)
{
if (t > 0)
{
for (int i = _timeLevels[t - 1].mEdges.Count - 1; i >= 0; i--)
{
if (_timeLevels[t - 1].mEdges[i].to == node)
{
_timeLevels[t - 1].mEdges.RemoveAt(i);
}
}
}
for (int i = _timeLevels[t].mEdges.Count - 1; i >= 0; i--)
{
if (_timeLevels[t].mEdges[i].from == node)
{
_timeLevels[t].mEdges.RemoveAt(i);
}
}
for (int i = _timeLevels[t].mNodes.Count - 1; i >= 0; i--)
{
if (_timeLevels[t].mNodes[i] == node)
{
_timeLevels[t].mNodes.RemoveAt(i);
}
}
}
public int GetIndex(PlanningNode node)
{
for (int i = 0; i < mNodes.Count; i++)
{
if (mNodes[i] == node)
{
return(i);
}
}
return(-1);
}
public ExpandingNode(PlanningNode node)
{
planningNode = node;
parentNode = null;
childrenNodes = new List <ExpandingNode>();
minVal = 0.0;
maxVal = Double.MaxValue;
level = -1;
instRwd = 0.0;
futrRwd = 0.0;
state = STATE.NEW;
}
public bool hasOut(PlanningNode node, int level)
{
if (level < 0 || level >= _planningLength - 1)
{
return(false);
}
List <PlanningEdge> .Enumerator e = _timeLevels[level].mEdges.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.from == node)
{
return(true);
}
}
return(true);
}
public List <PlanningEdge> GetEdges(PlanningNode node)
{
List <PlanningEdge> edges = new List <PlanningEdge>();
List <PlanningEdge> .Enumerator e = mEdges.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.from.pos.X == node.pos.X &&
e.Current.from.pos.Y == node.pos.Y)
{
edges.Add(e.Current);
}
}
return(edges);
}
public bool hasEdge(PlanningNode from, PlanningNode to)
{
if (to.parent.Level != from.parent.Level + 1)
{
return(false);
}
List <PlanningEdge> .Enumerator e = from.parent.mEdges.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.from == from && e.Current.to == to)
{
return(true);
}
}
return(false);
}
public PlanningEdge(PlanningNode from, PlanningNode to)
{
_from = from;
_to = to;
}
public void AddPlanningNode(PlanningNode node, int t)
{
_timeLevels[t].AddPlanningNode(node);
}
public void AddPlanningNode(PlanningNode node)
{
mNodes.Add(node);
node.parent = this;
}
