private void UpdateNextWaypoint()
{
if (!(contatore != 0 && contatore < listaWaypoint.Count))
{
nextWaypointIndex = currentWaypointIndex + 1;
if (nextWaypointIndex >= path.pathNodes.Count)
{
nextWaypointIndex = 0;
}
}
if (nextWaypointIndex > 872 && nextWaypointIndex < 885 && contatore < listaWaypoint.Count) //877
{
nextNode = path.pathNodes[nextWaypointIndex];
nextWaypoint = listaWaypoint[contatore];
contatore++;
return;
}
if (contatore == listaWaypoint.Count)
{
nextWaypointIndex = 884;
contatore = 0;
}
nextNode = path.pathNodes[nextWaypointIndex];
nextWaypoint = nextNode.position;
}
public void Spawn()
{
int spot = Random.Range(1, maxSteps);
int currentIndex = spot;
RoadPathNode currentNode = path.pathNodes[spot];
Vector3 position = currentNode.position;
if (!Physics.CheckSphere(position, checkRadius, 1 << LayerMask.NameToLayer("Traffic")))
{
GameObject go = GameObject.Instantiate(prefabs[Random.Range(0, prefabs.Length)], position, Quaternion.identity) as GameObject;
currentNode = path.pathNodes[currentIndex++];
var motor = go.GetComponent <TrafPCH>();
motor.currentNode = currentNode;
motor.currentWaypointIndex = currentIndex;
// Debug.Log("IDX " + currentIndex);
motor.path = path;
go.transform.LookAt(currentNode.position);
motor.Init();
}
else
{
// Debug.Log("taken: " + position);
}
}
private void UpdateNextWaypoint()
{
nextWaypointIndex = currentWaypointIndex + 1;
if (nextWaypointIndex >= path.pathNodes.Count)
{
nextWaypointIndex = 0;
}
nextNode = path.pathNodes[nextWaypointIndex];
nextWaypoint = nextNode.position;
}
public void Init()
{
vehicleController = GetComponent <VehicleController>();
maxSteer = vehicleController.maxSteeringAngle;
vehicleController.maxSteeringAngle = 45f;
currentWaypointIndex = 0;
currentNode = path.pathNodes[currentWaypointIndex++];
currentWaypoint = currentNode.position;
UpdateNextWaypoint();
targetSpeed = maxSpeed;
currentThrottle = 0f;
}
public void Init()
{
vehicleController = GetComponent <VehicleController>();
maxSteer = vehicleController.maxSteeringAngle;
vehicleController.maxSteeringAngle = 45f;
currentWaypointIndex = waypoint;
currentNode = path.pathNodes[currentWaypointIndex++];
currentWaypoint = currentNode.position;
UpdateNextWaypoint();
targetSpeed = maxSpeed;
currentThrottle = 0f;
if (_pidPars == null)
{
_pidPars = Resources.Load <PIDPars>("PIDPars_steeringWheel");
}
PIDControllerSterzata = new PID(_pidPars.p_sterzataPCH, _pidPars.i_sterzataPCH, _pidPars.d_sterzataPCH);
PIDControllerAccelerazione = new PID(_pidPars.p_accelerazione, _pidPars.i_accelerazione, _pidPars.d_accelerazione);
if (raggioDestra != null)
{
return;
}
///Inizializzazione posizione partenza raggio raycast
raggioDestra = new GameObject("raggioDestra");
raggioDestra.transform.SetParent(this.transform);
raggioDestra.transform.localPosition = new Vector3(raycastOrigin.localPosition.x + 0.92f, raycastOrigin.localPosition.y, raycastOrigin.localPosition.z);
raggioDestra.transform.localRotation = Quaternion.identity;
raggioDestra.transform.localScale = Vector3.zero;
raggioSinistra = new GameObject("raggioSinistra");
raggioSinistra.transform.SetParent(this.transform);
raggioSinistra.transform.localPosition = new Vector3(raycastOrigin.localPosition.x - 0.92f, raycastOrigin.localPosition.y, raycastOrigin.localPosition.z);
raggioSinistra.transform.localRotation = Quaternion.identity;
raggioSinistra.transform.localScale = Vector3.zero;
}
public GameObject CreaMacchinaTrafficoSorpasso(int indice)
{
int currentIndex = indice;
RoadPathNode currentNode = carAutoPath.pathNodes[currentIndex];
Vector3 position = currentNode.position;
if (!Physics.CheckSphere(position, checkRadius, 1 << LayerMask.NameToLayer("Traffic")))
{
GameObject go = GameObject.Instantiate(prefabs[UnityEngine.Random.Range(0, prefabs.Length)], position, Quaternion.identity) as GameObject;
currentNode = carAutoPath.pathNodes[currentIndex++];
var motor = go.AddComponent <MacchinaSorpassoPCH>();
motor.currentNode = currentNode;
motor.currentWaypointIndex = currentIndex;
motor.path = carAutoPath;
go.transform.LookAt(currentNode.position);
motor.Init();
go.tag = "DangerousCar";
return(go);
}
return(null);
}
public void Init()
{
currentWaypoint = currentNode.position;
nextWaypointIndex = currentWaypointIndex;
UpdateNextWaypoint();
prevWaypoint = currentWaypoint;
prevNode = currentNode;
currentWaypoint = nextWaypoint;
currentNode = nextNode;
currentWaypointIndex = nextWaypointIndex;
UpdateNextWaypoint();
if (prevNode.tangent == Vector3.zero)
{
prevTangent = (currentWaypoint - prevWaypoint).normalized;
}
else
{
prevTangent = (prevNode.tangent - prevWaypoint).normalized;
}
if (currentNode.tangent == Vector3.zero)
{
currentTangent = (nextWaypoint - currentWaypoint).normalized;
}
else
{
currentTangent = (currentNode.tangent - currentWaypoint).normalized;
}
targetSpeed = maxSpeed;
}
void FixedUpdate()
{
var predicted = GetPredictedPoint();
var normal = GetNormalPoint(predicted, currentWaypoint, nextWaypoint);
if (evitare && hitInfo.collider != null && hitInfo.collider.gameObject.layer == 12)
{
evita();
return;
}
if (inchiodare && hitInfo.collider != null && hitInfo.collider.gameObject.layer == 12)
{
inchioda();
return;
}
if (sosta)
{
sostaDopoPericolo();
return;
}
//check if we are heading past the current waypoint
if (Vector3.Dot(normal - nextWaypoint, nextWaypoint - currentWaypoint) >= 0)
{
contatore++;
currentWaypoint = nextWaypoint;
currentNode = nextNode;
currentWaypointIndex = nextWaypointIndex;
UpdateNextWaypoint();
}
if (Vector3.Distance(transform.position, new Vector3(target.x, transform.position.y, target.z)) <= 10f || target == Vector3.zero)
{
RoadPathNode prossimoNodo5 = path.pathNodes[currentWaypointIndex + 2];
target = prossimoNodo5.position;
}
//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.name.Equals("RocciaTest"))
{
vehicleController.accellInput = -0.18f;
if (hitInfo.distance < 15f)
{
vehicleController.steerInput = -8f / 45f;
}
return;
}
if (hitInfo.rigidbody != null && ((hitInfo.rigidbody.tag.Equals("TrafficCar")) || hitInfo.rigidbody.tag.Equals("DangerousCar") || hitInfo.rigidbody.gameObject.layer.Equals(12)))
{
if (hitInfo.rigidbody.tag.Equals("TrafficCar") || hitInfo.rigidbody.tag.Equals("DangerousCar"))
{
Debug.DrawLine(this.transform.position, hitInfo.transform.position);
if (hitInfo.distance <= 35f)
{
TrafPCH macchinaDavanti = hitInfo.rigidbody.GetComponent <TrafPCH>();
float frontSpeed = macchinaDavanti.currentSpeed;
float velocitaTarget = Mathf.Min(targetSpeed, (frontSpeed - 0.25f));
float miaVelocita = this.GetComponent <Rigidbody>().velocity.magnitude;
double distanzaSicurezzaUpdate = (Math.Pow((this.GetComponent <Rigidbody>().velocity.magnitude * 3.6f / 10f), 2) + 5f); //+ questo valore perchè la distanza viene calcolata dal centro dell'auto del traffico
bool sottoDistanzaSicurezza = (distanzaSicurezzaUpdate - hitInfo.distance) > 1f;
bool piuVeloceDelTarget = (miaVelocita - velocitaTarget) >= 1f;
if ((piuVeloceDelTarget || sottoDistanzaSicurezza))
{
if (autoDavanti == false)
{
if (sottoDistanzaSicurezza)
{
distanzaSicurezza = distanzaSicurezzaUpdate;
}
else
{
distanzaSicurezza = (Math.Pow((frontSpeed * 3.6f / 10f), 2) + 5f);
}
autoDavanti = true;
distanzaInizialeSicurezza = hitInfo.distance;
velocitàInizialeSicurezza = this.GetComponent <Rigidbody>().velocity.magnitude;
}
}
else
{
autoDavanti = false;
}
if (Math.Abs(hitInfo.distance - distanzaSicurezza) >= 1f && autoDavanti && miaVelocita > 0.1f)
{
/* se Vf è la velocita finale, Vi quella iniziale, Df la distanza finale, Di la distanza iniziale, Dc la distanza corrente
* allora Vx = (Vf + Vi) / ((Df + Di)/Dc)*/
targetSpeed = (velocitaTarget + velocitàInizialeSicurezza) / (((float)distanzaSicurezza + distanzaInizialeSicurezza) / hitInfo.distance);
}
else
{
autoDavanti = false;
targetSpeed = velocitaTarget;
}
}
}
if (hitInfo.rigidbody.gameObject.layer.Equals(12))
{
float distanza = Vector3.Distance(transform.position, hitInfo.rigidbody.transform.position);
double spazioFrenata = (Math.Pow(GetComponent <Rigidbody>().velocity.magnitude, 2)) / (2 * 2.3f);
if (spazioFrenata > distanza)
{
evitare = true;
}
else
{
inchiodare = true;
}
}
}
else
{
evitare = inchiodare = false;
if (currentNode.isInintersection)
{
targetSpeed = 4f;
}
else
{
targetSpeed = maxSpeed;
}
}
}
else
{
evitare = inchiodare = false;
if (currentNode.isInintersection)
{
targetSpeed = 4f;
}
else
{
targetSpeed = maxSpeed;
}
}
Debug.DrawLine(transform.position, target);
//STEER CAR
Vector3 steerVector = new Vector3(normal.x, transform.position.y, normal.z) - transform.position;
m_targetSteer = Vector3.SignedAngle(transform.forward, normal - transform.position, Vector3.up);
float differenza = Math.Abs(m_targetSteer) - Math.Abs(m_targetSteer_precedente);
float differenzaTarget = Math.Abs(m_targetSteer - m_targetSteer_target);
if (differenza <= 0.2f || (m_targetSteer_target != 0 && differenzaTarget > 0.2f))
{
if ((m_targetSteer_precedente < 0 && m_targetSteer > 0) || (m_targetSteer_precedente > 0 && m_targetSteer < 0) || m_targetSteer_target != 0)
{
if (m_targetSteer_target == 0)
{
m_targetSteer_target = m_targetSteer;
}
else
{
}
m_targetSteer = Mathf.Lerp(m_targetSteer_precedente, m_targetSteer_target, steerSpeed * Time.fixedDeltaTime);
m_targetSteer_precedente = m_targetSteer;
}
else
{
m_targetSteer = m_targetSteer_precedente;
m_targetSteer_precedente = m_targetSteer;
}
}
PIDControllerSterzata.pFactor = _pidPars.p_sterzataPCH;
PIDControllerSterzata.iFactor = _pidPars.i_sterzataPCH;
PIDControllerSterzata.dFactor = _pidPars.d_sterzataPCH;
if (sbacchettamentoEvitabile)
{
PIDControllerSterzata.dFactor = 0f;
}
else
{
PIDControllerSterzata.dFactor = 0.3f;
//if (sbacchettamentoForte)
//{
// PIDControllerSterzata.dFactor = 0.3f;
//}
//else
//{
// PIDControllerSterzata.dFactor = 0.3f;
//}
}
float turnPrecedente = vehicleController.steerInput * 45f;
float steeringAngle = PIDControllerSterzata.UpdatePars(m_targetSteer, turnPrecedente, Time.fixedDeltaTime);
//float steeringAngle = m_targetSteer;
//Limit the steering angle
steeringAngle = Mathf.Clamp(steeringAngle, -45, 45);
float differenzaTurn = Math.Abs(steeringAngle - turnPrecedente);
averageSteeringAngle = averageSteeringAngle + ((steeringAngle - averageSteeringAngle) / _pidPars.indiceSterzataCurva);
if (Mathf.Abs(averageSteeringAngle - turnPrecedente) <= _pidPars.puntoMortoSterzata)
{
averageSteeringAngle = turnPrecedente;
}
averageSteeringAngle = Mathf.Clamp(Mathf.Lerp(turnPrecedente, averageSteeringAngle, steerSpeed * Time.fixedDeltaTime), -45f, 45f);
vehicleController.steerInput = averageSteeringAngle / 45f;
//vehicleController.steerInput = steeringAngle / 45f;
//FINE STEERCAR
if (averageSteeringAngle > 5f || averageSteeringAngle < -5f)
{
//targetSpeed = targetSpeed * Mathf.Clamp(1 - (currentTurn / maxTurn), 0.1f, 1f);
targetSpeed = targetSpeed * Mathf.Clamp(1 - (Math.Abs(averageSteeringAngle) / 45), 0.2f, 1f);
}
MoveCarUtenteAccelerazione();
}
void MoveCar()
{
float thisDist = Vector3.Distance(currentWaypoint, prevWaypoint);
currentPc += currentSpeed / thisDist * Time.fixedDeltaTime;
if (currentPc > 1f)
{
prevWaypoint = currentWaypoint;
prevNode = currentNode;
prevTangent = currentTangent;
currentWaypoint = nextWaypoint;
currentNode = nextNode;
currentWaypointIndex = nextWaypointIndex;
UpdateNextWaypoint();
if (currentNode.tangent == Vector3.zero)
{
currentTangent = (nextWaypoint - currentWaypoint).normalized;
}
else
{
currentTangent = (currentNode.tangent - currentWaypoint).normalized;
}
currentPc -= 1f;
currentPc = (currentPc * thisDist) / Vector3.Distance(currentWaypoint, prevWaypoint);
}
Vector3 currentSpot = HermiteMath.HermiteVal(prevWaypoint, currentWaypoint, prevTangent, currentTangent, currentPc);
//transform.Rotate(0f, currentTurn * Time.deltaTime, 0f);
RaycastHit hit;
Physics.Raycast(transform.position + Vector3.up * 5, -transform.up, out hit, 100f, ~(1 << LayerMask.NameToLayer("Traffic")));
Vector3 hTarget = new Vector3(currentSpot.x, hit.point.y, currentSpot.z);
Vector3 tangent;
if (currentPc < 0.95f)
{
tangent = HermiteMath.HermiteVal(prevWaypoint, currentWaypoint, prevTangent, currentTangent, currentPc + 0.05f) - currentSpot;
}
else
{
tangent = currentSpot - HermiteMath.HermiteVal(prevWaypoint, currentWaypoint, prevTangent, currentTangent, currentPc - 0.05f);
}
tangent.y = 0f;
tangent = tangent.normalized;
GetComponent <Rigidbody>().MoveRotation(Quaternion.FromToRotation(Vector3.up, hit.normal) * Quaternion.LookRotation(tangent));
GetComponent <Rigidbody>().MovePosition(hTarget);
//transform.Translate(Vector3.forward * currentSpeed * Time.deltaTime);
}
void Update()
{
var predicted = GetPredictedPoint();
var normal = GetNormalPoint(predicted, currentWaypoint, nextWaypoint);
//check if we are heading past the current waypoint
if (Vector3.Dot(normal - nextWaypoint, nextWaypoint - currentWaypoint) >= 0)
{
currentWaypoint = nextWaypoint;
currentNode = nextNode;
currentWaypointIndex = nextWaypointIndex;
UpdateNextWaypoint();
predicted = GetPredictedPoint();
normal = GetNormalPoint(predicted, currentWaypoint, nextWaypoint);
}
if (currentNode.isInintersection)
{
targetSpeed = 4f;
}
else
{
targetSpeed = maxSpeed;
}
Vector3 steerVector = new Vector3(normal.x, transform.position.y, normal.z) - transform.position;
//Vector3 desired = seek(new Vector3(normal.x, transform.position.y, normal.z), transform.position);
//Vector3 steers = desired - rigidbody.velocity;
//steers.limit(maxforce);
//applyForce(steers);
float steer = Vector3.Angle(transform.forward, steerVector);
m_targetSteer = (Vector3.Cross(transform.forward, steerVector).y < 0 ? -steer : steer) / vehicleController.maxSteeringAngle;
if (Vector3.Distance(predicted, normal) < pathRadius)
{
m_targetSteer = 0f;
}
float speedDifference = targetSpeed - GetComponent <Rigidbody>().velocity.magnitude;
if (speedDifference < 0)
{
//m_targetThrottle = 0f;
// m_CarControl.motorInput = 0f;
// m_targetBrake = (rigidbody.velocity.magnitude / targetSpeed) * maxBrake;
}
else
{
m_targetThrottle = maxThrottle * Mathf.Clamp(1 - Mathf.Abs(m_targetSteer), 0.2f, 1f);
speedDifference *= m_targetThrottle;
}
currentThrottle = Mathf.Clamp(Mathf.MoveTowards(currentThrottle, speedDifference / 2f, throttleSpeed * Time.deltaTime), -maxBrake, maxThrottle);
// m_CarControl.steerInput = Mathf.MoveTowards(m_CarControl.steerInput, m_targetSteer, steerSpeed * Time.deltaTime);
vehicleController.steerInput = Mathf.Lerp(vehicleController.steerInput, m_targetSteer, steerSpeed * Time.deltaTime);
vehicleController.accellInput = currentThrottle;
//vehicleController.motorInput = Mathf.Clamp(currentThrottle, 0f, m_targetThrottle);
//vehicleController.brakeInput = Mathf.Abs(Mathf.Clamp(currentThrottle, -maxBrake, 0f));
}
public void Init()
{
/*wp0 = GameObject.Find("wp0").transform.position;
* wp1 = GameObject.Find("wp1").transform.position;
* wp2 = GameObject.Find("wp2").transform.position;
* wp3 = GameObject.Find("wp3").transform.position;
* wp4 = GameObject.Find("wp4").transform.position;
* wp5 = GameObject.Find("wp5").transform.position;
* wp6 = GameObject.Find("wp6").transform.position;
* wp7 = GameObject.Find("wp7").transform.position;
* wp8 = GameObject.Find("wp8").transform.position;
* wp9 = GameObject.Find("wp9").transform.position;*/
wp0 = new Vector3(-1283.71f, 142.58f, 1387.47f);
wp1 = new Vector3(-1285.2f, 142.58f, 1388.12f);
wp2 = new Vector3(-1293.8f, 142.7f, 1391.3f);
wp3 = new Vector3(-1302.41f, 142.535f, 1392.83f);
wp4 = new Vector3(-1310.52f, 142.535f, 1395.57f);
wp5 = new Vector3(-1318.09f, 142.535f, 1398.64f);
wp6 = new Vector3(-1325.74f, 142.82f, 1402.11f);
wp7 = new Vector3(-1331.6f, 142.92f, 1406.4f);
wp8 = new Vector3(-1336.1f, 142.85f, 1411.1f);
wp9 = new Vector3(-1341.94f, 143.16f, 1415.09f);
GetComponent <TrafPCH>().enabled = false;
Vector3[] lisWayp = new Vector3[10];
lisWayp[0] = wp0;
lisWayp[1] = wp1;
lisWayp[2] = wp2;
lisWayp[3] = wp3;
lisWayp[4] = wp4;
lisWayp[5] = wp5;
lisWayp[6] = wp6;
lisWayp[7] = wp7;
lisWayp[8] = wp8;
lisWayp[9] = wp9;
listaWaypoint = ChaikinCurve(lisWayp, 3);
int index = 0;
foreach (Vector3 punto in listaWaypoint)
{
GameObject cubo = GameObject.CreatePrimitive(PrimitiveType.Cube);
cubo.transform.position = punto;
cubo.GetComponent <Collider>().isTrigger = true;
RaycastHit hit;
Physics.Raycast(cubo.transform.position + Vector3.up * 5, -transform.up, out hit, 100f, (1 << LayerMask.NameToLayer("Roads")));
cubo.transform.position = new Vector3(punto.x, hit.point.y, punto.z);
cubo.SetActive(false);
listaWaypoint[index] = cubo.transform.position;
Destroy(cubo);
index++;
}
currentWaypoint = currentNode.position;
nextWaypointIndex = currentWaypointIndex;
UpdateNextWaypoint();
prevWaypoint = currentWaypoint;
prevNode = currentNode;
currentWaypoint = nextWaypoint;
currentNode = nextNode;
currentWaypointIndex = nextWaypointIndex;
UpdateNextWaypoint();
if (prevNode.tangent == Vector3.zero)
{
prevTangent = (currentWaypoint - prevWaypoint).normalized;
}
else
{
prevTangent = (prevNode.tangent - prevWaypoint).normalized;
}
if (currentNode.tangent == Vector3.zero)
{
currentTangent = (nextWaypoint - currentWaypoint).normalized;
}
else
{
currentTangent = (currentNode.tangent - currentWaypoint).normalized;
}
targetSpeed = maxSpeed;
}
