//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); }
private void RRT() { if (tree.getCount() > numPoints) { calculating = false; return; } ArrayList carPath = freePath(tree.last.point, destination, tree.last.forward); if (carPath.Count > 0) { Debug.DrawLine(tree.last.point, destination, Color.blue, timeDebug, false); rrtNode d = new rrtNode(destination, tree.last); //tree.last.addConnection (d); d.forward = ((Vector3)carPath[carPath.Count - 1] - (Vector3)carPath[carPath.Count - 2]).normalized; tree.addNode(d); calculating = false; //path = rrTree.optimizePath(tree.getPath()); path = tree.getCarPath(); printPath(path); } else { addRandomNode(tree, dim); } }
private void RRT() { if (tree.getCount() > numPoints) { calculating = false; return; } ArrayList dubinConn = freePath(tree.last.point, destination); if (dubinConn.Count > 0) { Debug.DrawLine(tree.last.point, destination, Color.blue, timeDebug, false); rrtNode d = new rrtNode(destination, tree.last); //d.connection = dubinConn; tree.addNode(d); calculating = false; //path = rrTree.optimizePath(tree.getPath()); path = tree.getPath(); //printPath (path); } else { addRandomNode(tree, dim); } }
private void RRT() { //if(onePoint) { onePoint=false; addFixedPoint(new Vector3(-12f, 1.5f, 3f)); return;} //if(twoPoint) { twoPoint=false; addFixedPoint(new Vector3(-14f, 1.5f, -18f)); return;} ArrayList dubinWay = freeCarPath(tree.last.point, tree.last.forward, destination); draw.drawLine(tree.last.point, destination, Color.blue); if (dubinWay.Count > 0) { rrtNode d = new rrtNode(destination, tree.last); dubinWay.Reverse(); d.connection = dubinWay; //draw.drawMultipleLines (d.connection, Color.magenta); d.forward = ((Vector3)dubinWay[dubinWay.Count - 1] - (Vector3)dubinWay[dubinWay.Count - 2]).normalized; tree.addNode(d); calculating = false; path = tree.getPath(); draw.drawMultipleLines(path, Color.magenta); } else { //if(onePoint) { onePoint=false; addRandomNode (tree, dimX, dimZ); } //addRandomNode (tree, dimX, dimZ); } }
private void inizializeTree(Vector3 dest) { tree = new rrTree(radius); rrtNode node = new rrtNode(transform.position, null); node.forward = transform.forward; tree.addNode(node); }
// private float turnSpeed = 10000; // private bool onePoint = true; // private bool twoPoint = true; private void inizializeTree(Vector3 source, Vector3 dest) { tree = new rrDubinTree(); rrtNode node = new rrtNode(source, null); node.forward = transform.forward; tree.addNode(node); }
public ArrayList findBidirectionalPath(Vector3 source, Vector3 dest, int numPoints, int dim) { y = source.y; rrTree treeForward = new rrTree(); rrtNode s = new rrtNode(source, null); treeForward.addNode(s); rrTree treeBackward = new rrTree(); rrtNode d = new rrtNode(dest, null); treeBackward.addNode(d); bool end = false; createPoint(s.point); createPoint(d.point); // try to merge the two tree ArrayList path = treeForward.Merge(treeBackward); if (path != null) { return(path); } //ArrayList path2 = treeBackward.Merge (treeForward); //if (path2 != null ) return path2; for (int i = 0; i < numPoints && !end; i++) { //forward if (addRandomNode(source, dest, treeForward, dim)) { // try to merge the two tree ArrayList path3 = treeForward.Merge(treeBackward); if (path3 != null) { return(path3); } } //backward if (addRandomNode(dest, source, treeBackward, dim)) { // try to merge the two tree ArrayList path4 = treeBackward.Merge(treeForward); if (path4 != null) { path4.Reverse(); return(path4); } } } return(null); }
// static private float margin = 1f; public void addNode(rrtNode node) { if (nodes.Count == 0) { source = node; } nodes.Add(node); last = node; }
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()); }
public ArrayList getPath() { ArrayList path = new ArrayList(); rrtNode node = last; path.Add(node.point); while (node.back != null) { node = node.back; path.Add(node.point); } path.Reverse(); return(path); }
/* * static public ArrayList optimizePath (ArrayList pathOld) { * ArrayList path = new ArrayList (pathOld); * //eliminate intermediate points * for (int i=0; i<path.Count-1; i++) { * for (int j=i+1; j<path.Count; j++) { * if (freePath((Vector3)path[i], (Vector3)path[j])){ * path.RemoveRange(i+1, j-i-1); * } * } * } * return path; * } */ public ArrayList getPath(rrtNode node) { if (node == null) { return(null); } ArrayList path = new ArrayList(); path.Add(node.point); while (node.back != null) { node = node.back; path.Add(node.point); } path.Reverse(); return(path); }
private void addFixedPoint(Vector3 newPoint) { draw.drawLine(newPoint, new Vector3(newPoint.x + 1, newPoint.y, newPoint.z + 1), Color.green); rrtNode point = tree.findClosestNode(newPoint); ArrayList dubinWay = freeCarPath(point.point, point.forward, newPoint); if (point != null && dubinWay.Count > 0) { //if it is possible to reach the point from the previous point //add the connection rrtNode p2 = new rrtNode(newPoint, point); p2.forward = ((Vector3)dubinWay[dubinWay.Count - 1] - (Vector3)dubinWay[dubinWay.Count - 2]).normalized; dubinWay.Reverse(); p2.connection = dubinWay; tree.addNode(p2); draw.drawMultipleLines(p2.connection, Color.magenta); } }
private bool addRandomNode(rrTree tree, int dim) { Vector3 newPoint = new Vector3(myRnd(dim), y, myRnd(dim)); createPoint(newPoint); rrtNode point = tree.findClosestNode(newPoint); if (point != null && freePath(point.point, newPoint)) { //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); myDrawLine(point.point, newPoint, Color.blue); return(true); } return(true); }
public rrtNode findClosestNode(Vector3 point) { //ArrayList reachble = getAllReachbleNode (point); ArrayList reachble = nodes; float min = 100000; rrtNode closest = null; foreach (rrtNode node in reachble) { float d = Vector3.Distance(node.point, point); if (d < min) { min = d; closest = node; } } return(closest); }
private bool addRandomNode(rrDubinTree tree, int dim) { Vector3 newPoint = new Vector3(myRnd(dim), y, myRnd(dim)); createPoint(newPoint); rrtNode point = tree.findClosestNode(newPoint); ArrayList dubinConn = freePath(point.point, newPoint); if (point != null && dubinConn.Count > 0) { //if it is possible to reach the point from the previous point //add the connection rrtNode p2 = new rrtNode(newPoint, point); //p2.connection = dubinConn; tree.addNode(p2); //myDrawLine (point.point, newPoint, Color.blue); return(true); } return(true); }
private bool addRandomNode(rrTree tree, int dim) { Vector3 newPoint = new Vector3(myRnd(dim), y, myRnd(dim)); createPoint(newPoint); rrtNode point = tree.findClosestNode(newPoint); ArrayList carPath = freePath(point.point, newPoint, point.forward); if (point != null && carPath.Count > 0) { //if it is possible to reach the point from the previous point //add the connection rrtNode p2 = new rrtNode(newPoint, point); p2.forward = ((Vector3)carPath[carPath.Count - 1] - (Vector3)carPath[carPath.Count - 2]).normalized; //point.addConnection(p2); tree.addNode(p2); myDrawLine(point.point, newPoint, Color.blue); return(true); } return(true); }
public ArrayList getCarPath() { ArrayList path = new ArrayList(); rrtNode node = last; path.Add(node.point); while (node.back != null) { ArrayList carPath = freeCarPath(node.back.point, node.point, node.back.forward); node = node.back; //path.Add (node.point); carPath.Reverse(); foreach (Vector3 v in carPath) { path.Add(v); } } path.Reverse(); return(path); }
private bool addRandomNode(rrDubinTree tree, int dimX, int dimZ) { Vector3 newPoint = new Vector3(myRnd(dimX), mainY, myRnd(dimZ)); draw.drawLine(newPoint, new Vector3(newPoint.x + 1, newPoint.y, newPoint.z + 1), Color.green); rrtNode point = tree.findClosestNode(newPoint); ArrayList dubinWay = freeCarPath(point.point, point.forward, newPoint); if (point != null && dubinWay.Count > 0) { //if it is possible to reach the point from the previous point //add the connection rrtNode p2 = new rrtNode(newPoint, point); p2.forward = ((Vector3)dubinWay[dubinWay.Count - 1] - (Vector3)dubinWay[dubinWay.Count - 2]).normalized; //conn.Reverse(); p2.connection = dubinWay; tree.addNode(p2); draw.drawMultipleLines(p2.connection, Color.magenta); return(true); } return(true); }
private bool addRandomNode(Vector3 source, Vector3 dest, rrTree tree, int dim) { //Vector3 newPoint = new Vector3 (myRnd (-dim, dim), 0, myRnd (-dim, dim)); //Vector3 newPoint = new Vector3 ( myRnd (tree.source.point.x, 1, dim), y, myRnd (tree.source.point.z, 1, dim)); Vector3 newPoint = myRndVector(source, dest, dim, 30f, 0.3f); createPoint(newPoint); Debug.Log("Rand point: " + newPoint); 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); return(true); } return(true); }
public ArrayList getPath() { ArrayList path = new ArrayList(); rrtNode node = last; path.Add(node.point); foreach (Vector3 n in node.connection) { path.Add(n); } while (node.back != null) { node = node.back; foreach (Vector3 n in node.connection) { path.Add(n); } } path.Reverse(); return(path); }
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); } }
static public bool freePath(rrtNode n1, rrtNode n2) { return(freePath(n1.point, n2.point)); }
public rrtNode(Vector3 p, rrtNode father) { this.point = p; this.back = father; }
public void addConnection(rrtNode point2) { connection.Add(point2); }