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;
}
private void calc(CSpace cSpace)
{
CSpaceRRT RRT = new CSpaceRRT(cSpace, maxIterations);
Node originNode, destNode;
int iterations = RRT.generatePath(origin, dest, out originNode, out destNode, GrowConnectionType.Node, true, 10.0, stopEvent);
double distance = destNode.aTotalDist;
if (distance != 0)
{
noResultCount = 0;
lock (sw)
{
if (distance < minDist)
{
minDist = distance;
//maxIterations = iterations;
bestDestNode = destNode;
t1 = RRT.startTree;
t2 = RRT.goalTree;
//sw.WriteLine(iterations.ToString() + ";" + distance.ToString());
//sw.Flush();
}
//results.Add(new Result(iterations, distance));
}
}
else
{
lock (sw)
{
noResultCount++;
if (noResultCount > 50)
{
//maxIterations *= 2;
noResultCount = 0;
}
}
}
}
