/// <summary>
/// Finds the root node of the selected node and sets it
/// </summary>
/// <param name="node">Node to have the root of found</param>
/// <param name="leftBias">True if the scan should go left-right, false if right-left</param>
/// <param name="lookUpwards">True if the scan should look at the parents, false if it will look at children</param>
/// <param name="cutoffIndex">The index that is the cutoff for a valid root</param>
private void FindRootNode(GraphNode node, bool leftBias, bool lookUpwards, ref int cutoffIndex)
{
//System.Diagnostics.Debug.WriteLine(node.ToString());
List<GraphNode> canidates = new List<GraphNode>();
if (lookUpwards)
{
if (node.ParentNodes.Count > 0)
{
int upperBound = node.ParentNodes.Count / 2;
int lowerBound = (int)(node.ParentNodes.Count / 2.0 - .5);
int count = 0;
foreach (GraphNode g in RobotGraph[node.GraphLevel - 1])
{
if (node.ParentNodes.Contains(g))
{
if (count <= upperBound && count >= lowerBound)
{
canidates.Add(g);
}
count++;
}
}
if (canidates.Count > 0)
{
if (leftBias)
{
foreach (GraphNode g in canidates)
{
if (g.LevelIndex > cutoffIndex)
{
node.RootNode = g.RootNode;
node.RootNode.SubNodes[node.GraphLevel] = node;
cutoffIndex=g.LevelIndex;
return;
}
}
}
else
{
for (int i = canidates.Count - 1; i >= 0; i--)
{
if (canidates[i].LevelIndex < cutoffIndex)
{
node.RootNode = canidates[i].RootNode;
node.RootNode.SubNodes[node.GraphLevel] = node;
cutoffIndex = canidates[i].LevelIndex;
return;
}
}
}
}
}
node.RootNode = node;
node.SubNodes = new GraphNode[RobotGraph.Count];
node.SubNodes[node.GraphLevel] = node;
return;
}
else
{
if (node.RootNode == null)
{
node.RootNode = node;
node.SubNodes = new GraphNode[RobotGraph.Count];
node.SubNodes[node.GraphLevel] = node;
}
if (node.ChildNodes.Count > 0)
{
int upperBound = node.ChildNodes.Count / 2;
int lowerBound = (int)(node.ChildNodes.Count / 2.0 - .5);
int count = 0;
foreach (GraphNode g in RobotGraph[node.GraphLevel + 1])
{
if (node.ChildNodes.Contains(g))
{
if (count <= upperBound && count >= lowerBound)
{
canidates.Add(g);
}
count++;
}
}
if (canidates.Count > 0)
{
if (leftBias)
{
foreach (GraphNode g in canidates)
{
if (g.LevelIndex > cutoffIndex)
{
g.RootNode = node.RootNode;
g.RootNode.SubNodes[g.GraphLevel] = g;
cutoffIndex = g.LevelIndex;
return;
}
}
}
else
{
for (int i = canidates.Count - 1; i >= 0; i--)
{
if (canidates[i].LevelIndex < cutoffIndex)
{
canidates[i].RootNode = node.RootNode;
canidates[i].RootNode.SubNodes[canidates[i].GraphLevel] = canidates[i];
cutoffIndex = canidates[i].LevelIndex;
return;
}
}
}
}
}
}
}
/// <summary>
/// Flips the joint that is connected to the specified node. This node must be the child of the joint
/// </summary>
/// <param name="g"> parent node that needs to be flipped</param>
public void FlipJoint(GraphNode g)
{
if (ParentNodes.Contains(g) && !FlippedNodes.Contains(g))
{
ParentNodes.Remove(g);
ChildNodes.Add(g);
g.ChildNodes.Remove(this);
g.ParentNodes.Add(this);
FlippedNodes.Add(g);
}
else if (ChildNodes.Contains(g) && FlippedNodes.Contains(g))
{
ChildNodes.Remove(g);
ParentNodes.Add(g);
g.ParentNodes.Remove(this);
g.ChildNodes.Add(this);
FlippedNodes.Remove(g);
}
else
{
return;
}
}
/// <summary>
/// Creates a new node and inserts it inbetween this node and the given parent
/// </summary>
/// <param name="parent">The node that is a parent of this node and the new node will be inserted before</param>
/// <returns>THe newly created node</returns>
public GraphNode InsertNode(GraphNode parent)
{
GraphNode node = new GraphNode(null);
node.ParentNodes.Add(parent);
node.ChildNodes.Add(this);
parent.ChildNodes.Remove(this);
parent.ChildNodes.Add(node);
this.ParentNodes.Remove(parent);
this.ParentNodes.Add(node);
if (parent.FlippedNodes.Contains(this))
{
parent.FlippedNodes.Remove(this);
parent.FlippedNodes.Add(node);
node.FlippedNodes.Add(this);
}
return node;
}
/// <summary>
/// finds all nodes that are connected to the base node by joints that are of lower level than the base node
/// </summary>
/// <param name="set">The set that all connected nodes should be added to</param>
/// <param name="baseNode">The base node for testing the tree</param>
/// <param name="previousNode">The previous node to be tested</param>
public void FindConnectedNodes(HashSet<GraphNode> set, GraphNode baseNode, GraphNode previousNode)
{
if (GraphLevel <= baseNode.GraphLevel && previousNode.GraphLevel > GraphLevel)
return;
if (!set.Contains(this))
set.Add(this);
else
return;
if (!previousNode.Equals(baseNode))
foreach (GraphNode n in ParentNodes)
{
if (!n.Equals(previousNode))
n.FindConnectedNodes(set,baseNode,this);
}
foreach (GraphNode n in ChildNodes)
{
if (!n.Equals(previousNode))
n.FindConnectedNodes(set,baseNode,this);
}
if(!set.Contains(this))
set.Add(this);
}