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