//Try to merge the last node of the tree with the first possible node of the OTHER tree public ArrayList Merge(rrTree other) { ArrayList nodeList = other.nodes; bool find = false; rrtNode conn = null; foreach (rrtNode checkNode in nodeList) { if (freePath(last, checkNode)) { conn = checkNode; find = true; break; } } if (!find) { return(null); } ArrayList fwdPath = getPath(); ArrayList bwdPath = other.getPath(conn); bwdPath.Reverse(); foreach (Vector3 v in bwdPath) { fwdPath.Add(v); } return(fwdPath); }
public ArrayList findPath(Vector3 source, Vector3 dest, int numPoints, int dim) { rrTree tree = new rrTree(); rrtNode s = new rrtNode(source, null); bool end = false; tree.addNode(s); rrtNode lastPoint = s; bool doCheck = true; for (int i = 0; i < numPoints && !end; i++) { if (doCheck) { if (freePath(lastPoint.point, dest)) { rrtNode d = new rrtNode(dest, lastPoint); lastPoint.addConnection(d); tree.addNode(d); //Debug.Log ("Point: "+point.point); end = true; } } if (!end) { //pick up a random point //Vector3 newPoint = new Vector3 (myRnd (-dim, dim), 0, myRnd (-dim, dim)); //Vector3 newPoint = new Vector3 ( myRnd (s.point.x, 1, dim), y, myRnd (s.point.z, 1, dim)); Vector3 newPoint = myRndVector(source, dest, dim, 30f, 0.3f); rrtNode point = tree.findClosestNode(newPoint); if (point != null && freePath(newPoint, point.point)) { //if it is possible to reach the point from the previous point //add the connection rrtNode p2 = new rrtNode(newPoint, point); point.addConnection(p2); tree.addNode(p2); //from the next iteration consider the new point lastPoint = p2; createPoint(p2.point); doCheck = true; } //else doCheck = false; } } return(tree.getPath()); }
private void RRT() { if (tree.getCount() > numPoints) { calculating = false; return; } if (freePath(tree.last.point, destination)) { Debug.DrawLine(tree.last.point, destination, Color.blue, timeDebug, false); rrtNode d = new rrtNode(destination, tree.last); tree.last.addConnection(d); tree.addNode(d); calculating = false; path = rrTree.optimizePath(tree.getPath()); printPath(path); } else { addRandomNode(tree, dim); } }