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; } } } }