Example #1
0
        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;
        }
Example #2
0
        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;
                    }
                }
            }
        }