internal ExplorationTree(CSpace cSpace, Node rootNode, GrowConnectionType connectionType, bool useKDtree, double maxEdgeSize)
{
this.cSpace = cSpace;
this.nodeList = new NodeList(cSpace, growConnectionType, useKDtree);
this.growConnectionType = connectionType;
this.maxEdgeSize = maxEdgeSize;
nodeList.AddNode(rootNode);
}
public NodeList(CSpace cSpace, GrowConnectionType connectionType, bool useKdTree)
{
this.cSpace = cSpace;
this.useKdTree = useKdTree;
this.connectionType = connectionType;
if (useKdTree)
{
kdTree = new KdTree(cSpace);
}
this.nodeList = new List<Node>();
if (connectionType == GrowConnectionType.Edge)
{
this.edgeList = new List<Edge>();
}
}
public int generatePath(double[] origin, double[] dest,
out Node originNode, out Node destNode, GrowConnectionType connectionType, bool useKdTree, double maxEdgeSize, EventWaitHandle stopSignal)
{
originNode = new Node(origin);
startTree = new ExplorationTree(cSpace, originNode, connectionType, useKdTree, maxEdgeSize);
destNode = new Node(dest);
goalTree = new ExplorationTree(cSpace, destNode, connectionType, useKdTree, maxEdgeSize);
Node qs;
Node qs2;
ExplorationTree T1 = startTree;
ExplorationTree T2 = goalTree;
int i;
for (i = 0; i < k; i++)
{
qs = T1.Grow();
qs2 = T2.Grow(qs);
if (qs == qs2)
break;
if (stopSignal.WaitOne(0))
{
break;
}
if (T1.Size > T2.Size)
{
ExplorationTree temp = T1;
T1 = T2;
T2 = temp;
}
}
CSpace.A_Star(originNode, destNode);
T1 = null;
T2 = null;
return i;
}
