//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);
}
