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