Esempio n. 1
0
    public void addRootNode(RRTNode node)
    {
        int x = (int)node.X() - 25;
        int y = 400 - (int)node.Z();

        Cv2.Circle(pic, x, y, 2, Scalar.Blue, -4, LineTypes.Link8);
    }
Esempio n. 2
0
 public RRTNode(float theX, float theZ, RRTNode theFather)
 {
     x                = theX;
     z                = theZ;
     father           = theFather;
     distanceToFather = Mathf.Sqrt(Mathf.Pow(father.X() - this.x, 2) + Mathf.Pow(father.Z() - this.z, 2));
     nodeID           = z * 500 + x;
 }
    private List <RRTNode> findPathOnRRT(RRTNode curr, RRTNode dest) // curr: current position, dest: destination
    {
        if (curr == dest)
        {
            return(new List <RRTNode>());
        }

        List <RRTNode> ancestors = new List <RRTNode>();
        RRTNode        r         = dest;
        bool           b         = true;

        ancestors.Add(r);
        while (b) // dont directly use "true", because visual studio will report an unreasonable error
        {
            if (r == root)
            {
                break;
            }
            r = r.Father();
            if (r == curr)
            {
                ancestors.Reverse();
                return(ancestors);
            }
            ancestors.Add(r);
        }

        List <RRTNode> path = new List <RRTNode>();

        r = curr;
        while (b)
        {
            r = r.Father();
            path.Add(r);
            List <RRTNode> commonParentToDest = new List <RRTNode>();
            for (int i = 0; i < ancestors.Count; i++)
            {
                if (ancestors[i] == r)
                {
                    for (int j = i - 1; j >= 0; j--)
                    {
                        commonParentToDest.Add(ancestors[j]);
                    }
                    foreach (RRTNode node in commonParentToDest)
                    {
                        path.Add(node);
                    }
                    b = false; // dont directly use "break", because visual studio will report an unreasonable error
                }
                else
                {
                    commonParentToDest.Clear();
                }
            }
        }
        return(path);
    }
Esempio n. 4
0
    public void addNode(RRTNode node)
    {
        int x = (int)node.X() - 25;
        int y = 400 - (int)node.Z();

        Cv2.Circle(pic, x, y, 2, Scalar.Blue, -1, LineTypes.Link8);
        int fatherX = (int)node.Father().X() - 25;
        int fatherZ = 400 - (int)node.Father().Z();

        Cv2.Line(pic, x, y, fatherX, fatherZ, Scalar.Green);
    }
    // Start is called before the first frame update
    void Start()
    {
        distanceCounter = this.gameObject.GetComponent <DistanceCounter>();
        rotation        = this.gameObject.GetComponent <RotationSimulator>();
        demoGraph       = this.gameObject.GetComponent <RRTDrawer>();
        map             = this.gameObject.GetComponent <MapBuilder>();
        drone           = this.gameObject.GetComponent <PowerfulEngine>();
        root            = new RRTNode(50, 75);
        theRRT          = new List <RRTNode>();
        theRRT.Add(root);
        demoGraph.addRootNode(root);

        minDisNode  = root;
        requestList = new LinkedList <(RRTNode, Vector3)>();
        b           = true;
    }
    void FixedUpdate()
    {
        if (uiManager.getLock()) // if locked, do nothing
        {
            return;
        }
        timer += Time.deltaTime;
        if (timer >= 0.5) // run per 0.5 second
        {
            timer = 0;

            float[] distances = new float[8];
            for (int i = 1; i <= 8; i++)
            {
                distances[i - 1] = distanceCounter.getDistance(i);
            }
            State currState = distanceCounter.getCurrentStateWhileFindingDistance();
            map.updateMap(distances, currState.getCurrentOrientation(), currState.getCurrentPosition() + new Vector3(0, 0, 1));

            if (uiManager.getGameMode() == InterfaceManager.SELF_DRIVING_MODE)
            {
                if (b)
                {
                    b = false;
                    StartCoroutine("buildingRRT");
                }
                if (drone.getIfIdle())
                {
                    if (requestList.Count != 0)
                    {
                        RRTNode nearestNode = findNearestNode();
                        RRTNode nearestNodeInRequestList = requestList.First.Value.Item1;
                        float   minDistance = nearestNodeInRequestList.distanceTo(this.transform.position.x, this.transform.position.z);
                        //foreach ((RRTNode before, Vector3 after) in requestList)
                        var node = requestList.First;
                        while (node != null)
                        {
                            (RRTNode before, Vector3 after) = node.Value;
                            var next = node.Next;
                            if (map.ifThisPointIsChecked(after))
                            {
                                requestList.Remove(node);
                            }
                            node = next;
                        }
                        for (var item = requestList.First; item != null; item = item.Next)
                        {
                            (RRTNode before, Vector3 after) = item.Value;
                            float distance = before.distanceTo(this.transform.position.x, this.transform.position.z);
                            if (distance < minDistance)
                            {
                                minDistance = distance;
                                nearestNodeInRequestList = before;
                            }
                        }
                        requestList.Clear();
                        letDroneFly(nearestNode, nearestNodeInRequestList);
                        //rotation.setRotatedAngle(90 - flyAngle);
                    }
                }
            }
        }
    }
Esempio n. 7
0
 public void addChild(RRTNode child)
 {
     children.Add(child);
 }
Esempio n. 8
0
    public float distanceTo(RRTNode other)
    {
        float distanceSquare = Mathf.Pow(other.X() - x, 2) + Mathf.Pow(other.Z() - z, 2);

        return(Mathf.Sqrt(distanceSquare));
    }