Ejemplo n.º 1
0
    void Update()
    {
        if (!inited)
        {
            return;
        }

        if (!currentEntry.isIntersection() && currentIndex > 0 && !hasNextEntry)
        {
            //last waypoint in this entry, grab the next path when we are in range
            if (Vector3.Distance(nose.transform.position, currentEntry.waypoints[currentEntry.waypoints.Count - 1]) <= giveWayRegisterDistance)
            {
                var node = system.roadGraph.GetNode(currentEntry.identifier, currentEntry.subIdentifier);

                RoadGraphEdge newNode;

                if (fixedRoute)
                {
                    if (++currentFixedNode >= fixedPath.Count)
                    {
                        currentFixedNode = 0;
                    }
                    newNode = system.FindJoiningIntersection(node, fixedPath[currentFixedNode]);
                }
                else
                {
                    newNode = node.SelectRandom();
                }

                if (newNode == null)
                {
                    Debug.Log("no edges on " + currentEntry.identifier + "_" + currentEntry.subIdentifier);
                    Destroy(gameObject);
                    inited = false;
                    return;
                }

                nextEntry    = system.GetEntry(newNode.id, newNode.subId);
                nextTarget   = nextEntry.waypoints[0];
                hasNextEntry = true;

                nextEntry.RegisterInterest(this);

                //see if we need to slow down for this intersection
                intersectionCornerSpeed = Mathf.Clamp(1 - Vector3.Angle(nextEntry.path.start.transform.forward, nextEntry.path.end.transform.forward) / 90f, 0.4f, 1f);
            }
        }

        //check if we have reached the target waypoint
        if (Vector3.Distance(nose.transform.position, target) <= waypointThreshold)// && !hasStopTarget && !hasGiveWayTarget)
        {
            if (++currentIndex >= currentEntry.waypoints.Count)
            {
                if (currentEntry.isIntersection())
                {
                    currentEntry.DeregisterInterest();
                    var node    = system.roadGraph.GetNode(currentEntry.identifier, currentEntry.subIdentifier);
                    var newNode = node.SelectRandom();

                    if (newNode == null)
                    {
                        Debug.Log("no edges on " + currentEntry.identifier + "_" + currentEntry.subIdentifier);
                        Destroy(gameObject);
                        inited = false;
                        return;
                    }

                    currentEntry = system.GetEntry(newNode.id, newNode.subId);
                    nextEntry    = null;
                    hasNextEntry = false;

                    targetTangent = (currentEntry.waypoints[1] - currentEntry.waypoints[0]).normalized;
                }
                else
                {
                    if (hasStopTarget || hasGiveWayTarget)
                    {
                        target = nextEntry.waypoints[0];
                    }
                    else
                    {
                        currentEntry  = nextEntry;
                        nextEntry     = null;
                        hasNextEntry  = false;
                        targetTangent = Vector3.zero;
                    }
                }

                if (!hasStopTarget && !hasGiveWayTarget)
                {
                    currentIndex = 0;
                }
            }
            if (currentIndex > 1)
            {
                targetTangent = Vector3.zero;
            }

            if (!hasStopTarget && !hasGiveWayTarget)
            {
                target = currentEntry.waypoints[currentIndex];
            }
        }



        SteerCar();

        if (hasNextEntry && nextEntry.isIntersection() && nextEntry.intersection.stopSign)
        {
            if (stopEnd == 0f)
            {
                hasStopTarget = true;
                stopTarget    = nextTarget;
                stopEnd       = Time.time + stopLength;
            }
            else if (Time.time > stopEnd)
            {
                if (nextEntry.intersection.stopQueue.Peek() == this)
                {
                    hasGiveWayTarget = false;
                    hasStopTarget    = false;
                    stopEnd          = 0f;
                }
            }
        }


        if (hasNextEntry && nextEntry.isIntersection() && !nextEntry.intersection.stopSign)
        {
            //check next entry for stop needed
            if (nextEntry.MustGiveWay())
            {
                hasGiveWayTarget = true;
                stopTarget       = target;
            }
            else
            {
                hasGiveWayTarget = false;
            }
        }

        if (!hasGiveWayTarget && hasNextEntry && nextEntry.light != null)
        {
            if (!hasStopTarget && nextEntry.light.State == TrafLightState.RED)
            {
                //light is red, stop here
                hasStopTarget = true;
                stopTarget    = nextTarget;
            }
            else if (hasStopTarget && nextEntry.light.State == TrafLightState.GREEN)
            {
                //green light, go!
                hasStopTarget = false;
                return;
            }
            else if (!hasStopTarget && nextEntry.light.State == TrafLightState.YELLOW)
            {
                //yellow, stop if we aren't zooming on through
                //TODO: carry on if we are too fast/close

                if (Vector3.Distance(nextTarget, nose.transform.position) > yellowLightGoDistance)
                {
                    hasStopTarget = true;
                    stopTarget    = nextTarget;
                }
            }
        }

        float targetSpeed = maxSpeed;

        //check in front of us
        if (Time.time > nextRaycast)
        {
            hitInfo          = new RaycastHit();
            somethingInFront = CheckFrontBlocked(out hitInfo);
            nextRaycast      = NextRaycastTime();
        }

        if (somethingInFront)
        {
            float frontSpeed = Mathf.Clamp((hitInfo.distance - 1f) / 3f, 0f, maxSpeed);
            if (frontSpeed < 0.2f)
            {
                frontSpeed = 0f;
            }

            targetSpeed = Mathf.Min(targetSpeed, frontSpeed);
        }

        if (hasStopTarget || hasGiveWayTarget)
        {
            Vector3 targetVec = (stopTarget - nose.transform.position);

            float stopSpeed = Mathf.Clamp(targetVec.magnitude * (Vector3.Dot(targetVec, nose.transform.forward) > 0 ? 1f : 0f) / 3f, 0f, maxSpeed);
            if (stopSpeed < 0.24f)
            {
                stopSpeed = 0f;
            }

            targetSpeed = Mathf.Min(targetSpeed, stopSpeed);
        }

        //slow down if we need to turn
        if (currentEntry.isIntersection() || hasNextEntry)
        {
            targetSpeed = targetSpeed * intersectionCornerSpeed;
        }
        else
        {
            targetSpeed = targetSpeed * Mathf.Clamp(1 - (currentTurn / maxTurn), 0.1f, 1f);
        }

        if (targetSpeed > currentSpeed)
        {
            currentSpeed += Mathf.Min(maxAccell * Time.deltaTime, targetSpeed - currentSpeed);
        }
        else
        {
            currentSpeed -= Mathf.Min(maxBrake * Time.deltaTime, currentSpeed - targetSpeed);
            if (currentSpeed < 0)
            {
                currentSpeed = 0;
            }
        }
    }
Ejemplo n.º 2
0
    void Guida()
    {
        if (!inited)
        {
            return;
        }

        if (inchiodata)
        {
            inchioda();
            return;
        }

        if (evitare)
        {
            evita();
            return;
        }

        if (this.tag.Equals("Player"))
        {
            velocitaAttuale = GetComponent <Rigidbody>().velocity.magnitude;
        }
        else
        {
            velocitaAttuale = currentSpeed;
        }

        accelerazione      = (velocitaAttuale - velocitaPrecedente) / Time.deltaTime;
        velocitaPrecedente = velocitaAttuale;

        TrafAIMotor motor = GetComponent <TrafAIMotor>();

        motor.currentSpeed  = currentSpeed;
        motor.hasStopTarget = hasStopTarget;
        motor.frenata       = frenata;
        motor.luceStop      = luceStop;
        motor.hasNextEntry  = hasNextEntry;
        motor.currentEntry  = currentEntry;
        motor.nextEntry     = nextEntry;



        if (!currentEntry.isIntersection() && currentIndex > 0 && !hasNextEntry)
        {
            if (Vector3.Distance(nose.transform.position, currentEntry.waypoints[currentEntry.waypoints.Count - 1]) <= 60f && this.tag.Equals("Player")) //piu è alto e piu inizia prima la valutazione dell'intersezione (es. semaforo)
            {
                if (semaforo != currentEntry.identifier)
                {
                    Semaforo = "" + currentEntry.identifier; //Setto la proprietà quando inizio a valutare il semaforo
                }
            }


            //last waypoint in this entry, grab the next path when we are in range
            //ANTONELLO
            //if(Vector3.Distance(nose.transform.position, currentEntry.waypoints[currentEntry.waypoints.Count - 1]) <= giveWayRegisterDistance)
            if (Vector3.Distance(nose.transform.position, currentEntry.waypoints[currentEntry.waypoints.Count - 1]) <= 40f) //era a 20
            {
                var node = system.roadGraph.GetNode(currentEntry.identifier, currentEntry.subIdentifier);

                RoadGraphEdge newNode;

                if (fixedRoute)
                {
                    if (++currentFixedNode >= fixedPath.Count)
                    {
                        currentFixedNode = 0;
                    }
                    newNode = system.FindJoiningIntersection(node, fixedPath[currentFixedNode]);
                }
                else
                {
                    newNode = node.SelectRandom();
                }

                if (newNode == null)
                {
                    Debug.Log("no edges on " + currentEntry.identifier + "_" + currentEntry.subIdentifier);
                    //ANTONELLO
                    if (!this.tag.Equals("Player"))
                    {
                        Destroy(gameObject);
                    }
                    else
                    {
                        Property = "9999";
                        inchioda();
                        return;
                    }

                    inited = false;
                    return;
                }

                nextEntry    = system.GetEntry(newNode.id, newNode.subId);
                nextTarget   = nextEntry.waypoints[0];
                hasNextEntry = true;


                nextEntry.RegisterInterest(gameObject.GetComponent <TrafAIMotor>());

                //see if we need to slow down for this intersection
                intersectionCornerSpeed = Mathf.Clamp(1 - Vector3.Angle(nextEntry.path.start.transform.forward, nextEntry.path.end.transform.forward) / 90f, 0.4f, 1f);
            }
        }
        if (hasNextEntry && !inizioValutazioneSemaforo && Vector3.Distance(nose.transform.position, nextTarget) <= 30f)
        {
            inizioValutazioneSemaforo = true;
        }

        float distanzaWaypointFramePrecedente = distanzaWaypoint;

        distanzaWaypoint = Vector3.Distance(nose.transform.position, target);

        //check if we have reached the target waypoint
        if (distanzaWaypoint <= waypointThreshold)// && !hasStopTarget && !hasGiveWayTarget)
        {
            distanzaWaypoint = 0;
            modificaTarget(false);
            //Debug.Log("Target = " + target.x + " " + target.y + "  " + target.z);
            numeroWaypointSaltati = 0;
        }
        else
        {
            if (distanzaWaypointFramePrecedente != 0 && (distanzaWaypoint - distanzaWaypointFramePrecedente) > 0)
            {
                contatore++;
                if (contatore >= 3)
                {
                    //abbiamo saltato il waypoint
                    if ((numeroWaypointSaltati++) >= 7)
                    {
                        Destroy(gameObject);
                    }
                    Debug.Log("waypoint saltato");
                    distanzaWaypoint = 0;
                    contatore        = 0;
                    modificaTarget(true);
                    //Debug.Log("Target = " + target.x + " " + target.y + "  " + target.z);
                    //hasStopTarget = false;
                    //hasGiveWayTarget = false;
                }
            }
            else
            {
                contatore = 0;
            }
        }

        Debug.DrawLine(this.transform.position, target);

        //SteerCar();

        if (hasNextEntry && inizioValutazioneSemaforo && nextEntry.isIntersection() && nextEntry.intersection.stopSign && !autoScorretta)
        {
            if (stopEnd == 0f)
            {
                hasStopTarget           = true;
                velocitaInizialeFrenata = velocitaAttuale;
                stopTarget = nextTarget;
                calcolaDistanzaIniziale();
                stopEnd = Time.time + stopLength;
            }
            else if (Time.time > stopEnd)
            {
                if (nextEntry.intersection.stopQueue.Peek() == this)
                {
                    hasGiveWayTarget = false;
                    hasStopTarget    = false;
                    stopEnd          = 0f;
                    float distanza = Vector3.Distance(this.transform.position, stopTarget);
                    if (distanza < 10f)
                    {
                        //evita che quando ci fermiamo a uno stop (un pochino prima dello stopTarget), riparte e si riferma allo stopTarget effettivo
                        //nextEntry.intersection.stopSign = false;
                        currentEntry = nextEntry;
                        nextEntry    = null;
                        hasNextEntry = false;
                        inizioValutazioneSemaforo = false;
                    }
                }
            }
        }


        if (hasNextEntry && !hasGiveWayTarget && inizioValutazioneSemaforo && nextEntry.isIntersection() && !nextEntry.intersection.stopSign)
        {
            //check next entry for stop needed
            if (nextEntry.MustGiveWay())
            {
                hasGiveWayTarget        = true;
                velocitaInizialeFrenata = velocitaAttuale;
                stopTarget = nextTarget;
                calcolaDistanzaIniziale();
            }
            else
            {
                hasGiveWayTarget = false;
            }
        }
        else
        {
            if (hasGiveWayTarget && !nextEntry.MustGiveWay())
            {
                hasGiveWayTarget = false;
            }
        }


        if (!hasGiveWayTarget && hasNextEntry && inizioValutazioneSemaforo && nextEntry.light != null)
        {
            if (!hasStopTarget && nextEntry.light.State == TrafLightState.RED)
            {
                //light is red, stop here
                //ANTONELLO
                if (!autoScorretta)
                {
                    hasStopTarget           = true;
                    velocitaInizialeFrenata = velocitaAttuale;
                    stopTarget = nextTarget;

                    calcolaDistanzaIniziale();
                } //else autoScorretta = true, non rispetta i semafori rossi e passacomunque
            }
            else if (hasStopTarget && nextEntry.light.State == TrafLightState.GREEN)
            {
                //green light, go!
                //ANTONELLO
                hasStopTarget = false;
            }
            else if (!hasStopTarget && nextEntry.light.State == TrafLightState.YELLOW)
            {
                //yellow, stop if we aren't zooming on through
                //TODO: carry on if we are too fast/close

                if (Vector3.Distance(nextTarget, nose.transform.position) > yellowLightGoDistance)
                {
                    //ANTONELLO
                    hasStopTarget           = true;
                    velocitaInizialeFrenata = velocitaAttuale;
                    stopTarget = nextTarget;

                    calcolaDistanzaIniziale();
                } //altrimenti non riesco a fermarmi, continuo
            }
        }

        float targetSpeed = maxSpeed;

        //float targetSpeed = Mathf.MoveTowards(currentSpeed, maxSpeed, 100f * Time.deltaTime);

        //lancio il raggio per vedere cosa ho davanti
        if (Time.time > nextRaycast)
        {
            hitInfo          = new RaycastHit();
            somethingInFront = CheckFrontBlocked(out hitInfo);
            nextRaycast      = NextRaycastTime();
        }

        if (somethingInFront)
        {
            if (hitInfo.collider.gameObject.tag.Equals("Player"))
            {
                currentSpeed = 0;
                return;
            }
        }


        if (frenata)
        {
            //ANTONELLO
            //se ho come target quello di fermarmi, diminuisco la velocità proporzionalmente fino al punto in cui devo fermarmi
            Vector3 vettoreDifferenza = frenataTarget - transform.position;
            float   distanzaCorrente  = 0;
            if (Math.Abs(vettoreDifferenza.x) > Math.Abs(vettoreDifferenza.z))
            {
                distanzaCorrente = Math.Abs(vettoreDifferenza.x);
            }
            else
            {
                distanzaCorrente = Math.Abs(vettoreDifferenza.z);
            }
            if (this.tag.Equals("Player"))
            {
                distanzaCorrente -= 5f;
            }
            else
            {
                distanzaCorrente -= 6f;
            }

            Debug.DrawLine(frenataTarget, transform.position);
            //if (velocitaInizialeFrenata >= 6f)
            //{
            targetSpeed = velocitaInizialeFrenata * distanzaCorrente / distanzaInizialeInchiodata;

            /*} else
             * {
             *  targetSpeed = 6f * distanzaCorrente / distanzaInizialeInchiodata;
             * }*/

            if (targetSpeed <= _pidPars.sogliaFermata)
            {
                targetSpeed = 0;
                //Debug.Log("target < 2");
            }
            //else
            //Debug.Log("distanza Iniziale Inchiodata = " + distanzaInizialeInchiodata + "; distanzaCorrente = " + distanzaCorrente + "; targetSpeed: " + targetSpeed);
        }

        if (!frenata && !autoDavantiFrenata && (hasStopTarget || hasGiveWayTarget))
        {
            /*Vector3 targetVec = (stopTarget - nose.transform.position);
             *
             * float stopSpeed = Mathf.Clamp(targetVec.magnitude * (Vector3.Dot(targetVec, nose.transform.forward) > 0 ? 1f : 0f) / 3f, 0f, maxSpeed);
             * if(stopSpeed < 0.24f)
             *  stopSpeed = 0f;
             *
             * targetSpeed = Mathf.Min(targetSpeed, stopSpeed);
             * //Debug.Log("hasStopTarget true; targetSpeed: " + targetSpeed);*/

            //ANTONELLO
            //se ho come target quello di fermarmi, diminuisco la velocità proporzionalmente fino al punto in cui devo fermarmi
            Vector3 vettoreDifferenza = stopTarget - transform.position;
            float   distanzaCorrente  = 0;
            if (Math.Abs(vettoreDifferenza.x) > Math.Abs(vettoreDifferenza.z))
            {
                distanzaCorrente = Math.Abs(vettoreDifferenza.x);
            }
            else
            {
                distanzaCorrente = Math.Abs(vettoreDifferenza.z);
            }
            distanzaCorrente -= 2f;
            Debug.DrawLine(stopTarget, transform.position);
            targetSpeed = velocitaInizialeFrenata * distanzaCorrente / distanzaIniziale;

            /*if (velocitaInizialeFrenata >= 6f)
             * {
             *  targetSpeed = velocitaInizialeFrenata * distanzaCorrente / distanzaIniziale;
             * }
             * else
             * {
             *  targetSpeed = 6f * distanzaCorrente / distanzaIniziale;
             * }*/
            if (targetSpeed <= _pidPars.sogliaFermata)
            {
                targetSpeed = 0;
                //Debug.Log("target < 2");
            } //else
              //Debug.Log("distanza Iniziale = " + distanzaIniziale + "; distanzaCorrente = " + distanzaCorrente + "; targetSpeed: " + targetSpeed);
        }

        //slow down if we need to turn
        //if(currentEntry.isIntersection() || hasNextEntry)
        //ANTONELLO
        //if ((currentEntry.isIntersection() || hasNextEntry) && !hasStopTarget && !frenata)
        if ((currentEntry.isIntersection() || inizioValutazioneSemaforo) && !hasStopTarget && !hasGiveWayTarget && !frenata)
        {
            float min = 4;
            if (targetSpeed <= 4)
            {
                min = targetSpeed;
            }
            targetSpeed = Mathf.Clamp(targetSpeed * intersectionCornerSpeed, min, maxSpeed);

            /*targetSpeed = Mathf.MoveTowards(targetSpeed, targetSpeed * intersectionCornerSpeed, Time.fixedDeltaTime * 5f);
             * if (this.tag.Equals("Player"))
             * {
             *  Debug.Log("IntersectionCornerSpeed: " + intersectionCornerSpeed + "; targetSpeed: " + targetSpeed);
             * }*/
        }
        else
        {
            //DA TESTARE - ANTONELLO
            if (currentTurn > 5f || currentTurn < -5f)
            {
                //targetSpeed = targetSpeed * Mathf.Clamp(1 - (currentTurn / maxTurn), 0.1f, 1f);
                targetSpeed = targetSpeed * Mathf.Clamp(1 - (Math.Abs(currentTurn) / maxTurn), 0.2f, 1f);
            }
        }
        float targetSpeedIniziale = targetSpeed;

        if (targetSpeed > currentSpeed)
        {
            float distanzaCorrente = 0;
            if (hasStopTarget || hasGiveWayTarget)
            {
                Vector3 vettoreDifferenza = stopTarget - transform.position;
                if (Math.Abs(vettoreDifferenza.x) > Math.Abs(vettoreDifferenza.z))
                {
                    distanzaCorrente = Math.Abs(vettoreDifferenza.x);
                }
                else
                {
                    distanzaCorrente = Math.Abs(vettoreDifferenza.z);
                }
            }
            //Debug.Log("distanza corrente ripartenza: " + distanzaCorrente + "; targetSpee:" +targetSpeed);
            //if ((targetSpeed - currentSpeed) < 1 && (hasStopTarget || frenata) && distanzaCorrente < 3f)
            if ((hasStopTarget || hasGiveWayTarget) && distanzaCorrente < 6f)
            {
                //fa si che quando ci fermiamo allo stop o per via di una inchiodata la macchina rimanga ferma
                //currentSpeed = 0;
            }
            else
            {
                //currentSpeed += Mathf.Min(maxAccell * Time.deltaTime, targetSpeed - currentSpeed);
                currentSpeed = targetSpeed;
            }
            //currentSpeed = Mathf.MoveTowards(currentSpeed, targetSpeed, Time.fixedDeltaTime * _pidPars.velocitaFrenata);
        }
        else
        {
            //ANTONELLO
            if (hasStopTarget || hasGiveWayTarget || frenata || autoDavanti || autoDavantiFrenata)
            {
                if (targetSpeed > currentSpeed)
                {
                    //serve questa condizione perchè evita di accelerare quando sono in fase di fermata
                    targetSpeed = currentSpeed;
                }
                //currentSpeed = targetSpeed;
            }
            else
            {
                //targetSpeed -= Mathf.Min(maxBrake * Time.deltaTime, currentSpeed - targetSpeed);
                //Per fare in modo che arriva a targetSpeed dalla velocità corrente non troppo immediatamente
                targetSpeed = Mathf.MoveTowards(currentSpeed, targetSpeed, Time.fixedDeltaTime * _pidPars.velocitaFrenata);
            }

            if (targetSpeed < _pidPars.sogliaFermata)
            {
                currentSpeed = 0;
            }
            else
            {
                currentSpeed = targetSpeed;
            }
            //Debug.Log("CurrentSpeed: " + currentSpeed);


            //if (this.tag.Equals("Player"))
            //{
            //    Debug.Log("TargetSpeedIniziale: " + targetSpeedIniziale + "; currentSpeed impsotato: " + currentSpeed);
            //}
        }

        controllaAccensioneLuceStop(targetSpeedIniziale);
    }