C# Tree.findClosest方法代码示例

    public void generateRRTObstaclesWithPath(int numNodes, OccupancyGrid occupancy, int pathSearchDepth)
    {
        // Create a new tree which we will populate with nodes, with the root of the tree
        // set to the initial configuration
        tree = new Tree(initialConfiguration);
        // Loop until the graph is the requested size
        for (int i = 0; i < numNodes; i++) {
            bool clearConf = false;
            StateConfig randConf = null;
            StateConfig newConf = null;
            TreeNode<StateConfig> closestNode = null;
            // Generate a random configuration within the configuration space until one is
            // found which does not fall in any occupied cell in the occupancy grid
            while (!clearConf){
                randConf = getRandomConfiguration(limits);
                Tuple<float,float> pos = randConf.getPosition();
                // If the random configuration is occupied, generate a new one.
                if(occupancy.isOccupied(pos.first, pos.second))
                    continue;
                // Find the closest node on the current graph to the point that was generated
                closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS);
                StateConfig closestConf = closestNode.content;
                // Generate a new configuration by moving along the hyper-edge connecting
                // the closest configuration and the randomly generated one.
                newConf = closestConf + (randConf - closestConf) * increment;
                Tuple<float,float> newPos = newConf.getPosition();
                if (occupancy.isOccupied(newPos.first, newPos.second))
                    continue;

                if (!occupancy.obstacleFreeStraightPath(closestConf.getPosition(), newConf.getPosition(), pathSearchDepth))
                    continue;

                clearConf = true;
            }
            // Add the new configuration as a child of the closest node.
            TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode);
            // If we are within range of the goal with the new node, stop expanding the tree.
            if (checkGoalDist(newNode))
                break;
        }
    }

 /*
  * Generates an RRT with the specified number of nodes. The goalBias
  * parameter defines how biased the RRT should be towards choosing the
  * goal as the configuration to go towards. It should be a value between
  * 0 and 1.
  */
 public void generateRRTBasic(int numNodes)
 {
     // Create a new tree which we will populate with nodes, with the
     // initial configuration as the root of the tree.
     tree = new Tree(initialConfiguration);
     // Loop until the tree has the requested number of nodes
     for (int i = 0; i < numNodes; i++) {
         // Generate a random configuration within the configuration space
         StateConfig randConf = getRandomConfiguration(limits);
         // Find the closest node on the current graph to the point that was generated
         TreeNode<StateConfig> closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS);
         StateConfig closestConf = closestNode.content;
         // Generate a new configuration by moving along the hyper-edge connecting
         // the closest configuration and the randomly generated one.
         StateConfig newConf = closestConf + (randConf - closestConf) * increment;
         // Add the new configuration as a child of the closest node.
         TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode);
         // If we are within range of the goal with the new node, stop expanding the tree.
         if (checkGoalDist(newNode))
             break;
     }
 }