Exemplo n.º 1
0
    //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);
    }
Exemplo n.º 2
0
    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);
        }
    }
Exemplo n.º 3
0
    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);
        }
    }
Exemplo n.º 4
0
    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);
        }
    }
Exemplo n.º 5
0
    private void inizializeTree(Vector3 dest)
    {
        tree = new rrTree(radius);
        rrtNode node = new rrtNode(transform.position, null);

        node.forward = transform.forward;
        tree.addNode(node);
    }
Exemplo n.º 6
0
//	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);
    }
Exemplo n.º 7
0
    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);
    }
Exemplo n.º 8
0
//	static private float margin = 1f;

    public void addNode(rrtNode node)
    {
        if (nodes.Count == 0)
        {
            source = node;
        }

        nodes.Add(node);
        last = node;
    }
Exemplo n.º 9
0
    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());
    }
Exemplo n.º 10
0
    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);
    }
Exemplo n.º 11
0
    /*
     * 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);
    }
Exemplo n.º 12
0
    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);
        }
    }
Exemplo n.º 13
0
    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);
    }
Exemplo n.º 14
0
    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);
    }
Exemplo n.º 15
0
    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);
    }
Exemplo n.º 16
0
    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);
    }
Exemplo n.º 17
0
    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);
    }
Exemplo n.º 18
0
    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);
    }
Exemplo n.º 19
0
    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);
    }
Exemplo n.º 20
0
    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);
    }
Exemplo n.º 21
0
    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);
        }
    }
Exemplo n.º 22
0
 static public bool freePath(rrtNode n1, rrtNode n2)
 {
     return(freePath(n1.point, n2.point));
 }
Exemplo n.º 23
0
 public rrtNode(Vector3 p, rrtNode father)
 {
     this.point = p;
     this.back  = father;
 }
Exemplo n.º 24
0
 public void addConnection(rrtNode point2)
 {
     connection.Add(point2);
 }