rrt tree

package com.bim.rrt_20190529;

import static java.lang.Math.pow;
import static java.lang.Math.sqrt;

import java.util.ArrayList;

public class Tree {
Node root;//起点位置
　　ArrayList<Node> nodeArrayList;

　　
　　public Tree(Node node) {
　　　　root = new Node(node, 0, Double.POSITIVE_INFINITY, null);
　　　　nodeArrayList = new ArrayList<Node>();
　　　　nodeArrayList.add(root);
　　}

　　/**
　　 * 存入节点
　　 * @param parent
　　 * @param child
　　 */
　　public void add(Node parent, Node child) {
　　　　parent.addChild(child);
　　　　nodeArrayList.add(child);
　　　　child.setParent(parent);
　　}

　　/**
　　 * 移除点
　　 * @param node
　　 */
　　public void remove(Node node) {
　　　　node.getParent().removeChild(node);
　　　　nodeArrayList.remove(node);
　　}

　　/**
　　 * 点是否存在
　　 * @param nodeReq
　　 * @return
　　 */
　　public boolean contains(Node nodeReq) {
　　　　return nodeArrayList.contains(nodeReq);
　　}

　 /**
　　* 选择离随机点最近的点
　　* @param randomNode
　　* @return
　　*/
　　public Node nearestNode(Node randomNode) {
　　 //System.out.println("计算距离：x="+randomNode.getX()+"　y="+randomNode.getX()+"　z="+randomNode.getZ());
　　 //System.out.println("计算距离：x="+root.getX()+"　y="+root.getX()+"　z="+root.getZ());
　　　　double minDistance = distance(root, randomNode);//随机点和起点之间的距离
　　　　Node nearestNode = root;
　　　　for (Node node : nodeArrayList) {//遍历树上所有节点
　　　　　　double currentDistance = distance(node, randomNode);//随机点和遍历点之间的距离
　　　　　　if (currentDistance < minDistance) {
　　　　　　　　minDistance = currentDistance;
　　　　　　　　nearestNode = node;
　　　　　　}
　　　　}
　　　　return nearestNode;
　　}

　 /**
　　* 计算两个点之间的距离
　　* @param node1
　　* @param node2
　　* @return
　　*/
　　private double distance(Node node1,Node node2) {
　　　　return sqrt(pow(node1.getX() - node2.getX(), 2) + pow(node1.getY() - node2.getY(), 2));
　　}

}