// Update is called once per frame
void Update()
{
Bus b = hud.getSceneManager().getFocusedBus();
if (b)
{
txt_busName.text = "Bus: " + b.vehicleName;
txt_currentPos.text = "Current position: " + b.transform.position;
WaypointOld w = b.currentWayPoint;
if (w)
{
txt_waypointPos.text = "Waypoint position: " + b.currentWayPoint.transform.position;
}
else
{
txt_waypointPos.text = "No waypoint to go";
}
float speed = b.currentSpeed * 3.6F;
txt_currentSpeed.text = "Current speed (KM/H): " + (speed >= 0.01 ? speed : 0);
}
else
{
txt_busName.text = "No bus found";
txt_currentPos.text = "";
txt_waypointPos.text = "";
txt_currentSpeed.text = "";
}
}
public WaypointOld ResolveCurrentWaypointToGo(WaypointOld fromDefaultRoute, WaypointOld fromRecordedRoute)
{
if (!fromDefaultRoute && !fromRecordedRoute)
{
return(null);
}
if (fromDefaultRoute && !fromRecordedRoute)
{
return(fromDefaultRoute);
}
if (!fromDefaultRoute && fromRecordedRoute)
{
return(fromRecordedRoute);
}
float distToFDR = Vector3.Distance(vehicle.transform.position, fromDefaultRoute.transform.position);
float distToFRR = Vector3.Distance(vehicle.transform.position, fromRecordedRoute.transform.position);
if (distToFDR < distToFRR)
{
return(fromDefaultRoute);
}
else
{
return(fromRecordedRoute);
}
}
public WaypointOld getNextWayPoint(WaypointOld current)
{
if (waypoints.Count > 1)
{
if (current)
{
foreach (WaypointOld wp in waypoints)
{
if (current == wp)
{
int aux = waypoints.IndexOf(wp);
if (aux + 1 >= waypoints.Count)
{
aux = 0; //Volta para o primeiro waypoint da lista.
}
else
{
aux += 1; //Vai para o próximo waypoint da lista.
}
return(waypoints[aux]);
}
}
}
//By default, return the first Waypoint
return(waypoints[0]);
}
else
{
return(null);
}
}
public float calculateSteeringThreshold()
{
WaypointOld currentWP = ResolveCurrentWaypointToGo(vehicle.currentWayPoint, vehicle.recordedWayPoint);
if (!currentWP)
{
return(0);
}
float result;
if (!vehicle.keepStopped)
{
// calculate the local-relative position of the target, to steer towards
Vector3 targetPos = transform.InverseTransformPoint(currentWP.transform.position);
// work out the local angle towards the target
float targetAngle = Mathf.Atan2(targetPos.x, targetPos.z) * Mathf.Rad2Deg;
// get the amount of steering needed to aim the car towards the target
result = Mathf.Clamp(targetAngle, -1, 1); // * Mathf.Sign(m_CarController.CurrentSpeed);
//if (result > -.5F && result < .5F) result = 0F;
// feed input to the car controller.
//m_CarController.Move(steer, accel, accel, 0f);
}
else
{
result = 0;
}
return(result);
}
public bool addRecordedWaypoint(Vector3 position)
{
SimulationResources sr = FindObjectOfType <SimulationResources>();
WaypointOld wp = Instantiate(sr.waypoint_prefab, position, Quaternion.identity);
wp.GetComponent <Renderer>().material = sr.waypointRecorded_material;
recordedWaypoints.Add(wp);
return(true);
}
private void Start_InitializeRoute()
{
if (!busRoute)
{
busRoute = GetComponentInParent <BusRoute>();
}
currentWayPoint = busRoute.getNextWayPoint(null);
nextWayPoint = busRoute.getNextWayPoint(currentWayPoint);
}
private void Update_CheckWaypoints()
{
if (!currentWayPoint)
{
currentWayPoint = busRoute.getNextWayPoint(null);
if (!currentWayPoint)
{
Debug.LogWarning("A rota " + busRoute.routeName + " não possui Waypoints suficientes!");
canRunSimulation = false;
return;
}
nextWayPoint = busRoute.getNextWayPoint(currentWayPoint);
}
}
public void defineNextWaypoints(bool calledFromRecordedWP)
{
if (!calledFromRecordedWP)
{
WaypointOld wp = currentWayPoint;
currentWayPoint = busRoute.getNextWayPoint(wp);
nextWayPoint = busRoute.getNextWayPoint(currentWayPoint);
}
else
{
waypointRecorder.removeRecordedWaypoint(recordedWayPoint);
recordedWayPoint = null;
}
}
private void initializeWaypoints()
{
for (int i = 0; i < db_BusRoutes.Count; i++)
{
for (int j = 0; j < db_Waypoints[i].Count; j++)
{
WaypointOld w = Instantiate(simResources.waypoint_prefab);
w.transform.position = db_Waypoints[i][j];
w.transform.parent = busRoutes[i].transform;
busRoutes[i].addWaypoint(w);
//TODO habilitar isso somente quando houver detecção de outros ônibus como obstáculos!
//if (j == 3 || j == 8)
// w.isStopPoint = true;
}
}
}
public WaypointOld askNextWaypoint()
{
//TODO Mudar esta função para realmente contactar o webservice ou outro repositório de dados.
if (recordedCoordinates.Count > 0)
{
Vector3 position = recordedCoordinates[0];
usedCoordinates.Add(position);
recordedCoordinates.RemoveAt(0);
SimulationResources sr = FindObjectOfType <SimulationResources>();
WaypointOld wp = Instantiate(sr.waypoint_prefab, position, Quaternion.identity);
wp.GetComponent <Renderer>().material = sr.waypointRecorded_material;
wp.isRecordedWP = true;
return(wp);
}
return(null);
}
// Update is called once per frame
void Update()
{
if (queryTimeCurrent <= 0)
{
WaypointOld wp = askNextWaypoint();
if (wp != null)
{
addRecordedWaypoint(wp);
}
queryTimeCurrent = queryTimeMax;
}
queryTimeCurrent -= Time.deltaTime;
if (recordedWaypoints.Count > 0 && vehicle.recordedWayPoint != recordedWaypoints[0])
{
vehicle.recordedWayPoint = recordedWaypoints[0];
}
}
public bool removeRecordedWaypoint(WaypointOld waypoint)
{
return(recordedWaypoints.Remove(waypoint));
}
public void addWaypoint(WaypointOld w)
{
waypoints.Add(w);
w.busRoute = this;
}
public float calculateMotorThreshold()
{
WaypointOld currentWP = ResolveCurrentWaypointToGo(vehicle.currentWayPoint, vehicle.recordedWayPoint);
if (!currentWP)
{
return(0);
}
Vector3 myPos = transform.position;
Vector3 currentWPPos = currentWP.transform.position;
Vector3 nextWPPos = vehicle.nextWayPoint.transform.position;
myPos.y = 0;
currentWPPos.y = 0;
nextWPPos.y = 0;
Vector3 directionToCurrentWP = currentWPPos - myPos;
float distanceToCurrentWP = Vector3.Distance(myPos, currentWPPos);
float angleToCurrentWP = Vector3.Angle(directionToCurrentWP, transform.forward);
Vector3 directionToNextWP = nextWPPos - myPos;
float distanceToNextWP = Vector3.Distance(myPos, nextWPPos);
float angleToNextWP = Vector3.Angle(directionToNextWP, transform.forward);
Vector3 directionToObstacle = Vector3.zero;
float distanceToObstacle = float.NaN;
float angleToObstacle = float.NaN;
if (closestObstacle)
{
obstaclePosition.transform.position = closestObstacle.transform.position;
obstaclePosition.transform.rotation = closestObstacle.transform.rotation;
obstaclePositionOffset.transform.position = closestObstacle.transform.position;
obstaclePositionOffset.transform.rotation = closestObstacle.transform.rotation;
Vector3 offset = new Vector3();
Semaphore s = closestObstacle.GetComponent <Semaphore>();
if (s)
{
offset = HelperFunctions.OffsetBetweenTransforms(transform, obstaclePosition.transform);
offset.y = 0;
offset.z = vehicle.bounds.size.z * 2;
obstaclePositionOffset.transform.position -= offset;
}
obstaclePosition.transform.Rotate(closestObstacle.transform.rotation.eulerAngles, Space.World);
directionToObstacle = obstaclePositionOffset.transform.position - myPos;
distanceToObstacle = Vector3.Distance(myPos, obstaclePositionOffset.transform.position);
angleToObstacle = Vector3.Angle(directionToObstacle, transform.forward);
}
else
{
obstaclePosition.transform.position = transform.position;
obstaclePositionOffset.transform.position = transform.position;
obstaclePosition.transform.rotation = transform.rotation;
obstaclePositionOffset.transform.rotation = transform.rotation;
}
dynStopDist_WP = BusCalculations.calculate_DynamicStoppingDistance_Waypoint(vehicle, stoppingDistance, currentWP, vehicle.nextWayPoint);
dynStopDist_Obs = BusCalculations.calculate_DynamicStoppingDistance_Obstacle(vehicle, stoppingDistance, closestObstacle);
dynBraking_WP = BusCalculations.calculateDynamicBrakingThresholdWP(
vehicle.currentSpeed,
vehicle.maneuverSpeed,
angleToNextWP,
vehicle.maxSteeringAngle);
dynBraking_SP = BusCalculations.calculateDynamicBrakingThresholdSP(
vehicle.currentSpeed,
vehicle.maneuverSpeed,
distanceToCurrentWP,
dynStopDist_WP
);
dynBraking_Obs = BusCalculations.calculateDynamicBrakingThresholdObstacle(vehicle,
vehicle.currentSpeed,
vehicle.maneuverSpeed,
closestObstacle,
dynStopDist_Obs
);
float dynamicStoppingDistance;
if (closestObstacle && distanceToObstacle <= distanceToCurrentWP)
{
distanceToTarget = distanceToObstacle;
dynamicStoppingDistance = dynStopDist_Obs;
}
else
{
distanceToTarget = distanceToCurrentWP;
dynamicStoppingDistance = dynStopDist_WP;
}
float result;
if (distanceToTarget > dynamicStoppingDistance)
{
if (vehicle.currentSpeed >= vehicle.maximumSpeed)
{
//Cease the positive torque since we achieved maximumSpeed.
result = 0;
}
else
{
result = 1;
}
}
else
{
if (!closestObstacle)
{
if (!currentWP.isStopPoint)
{
result = -dynBraking_WP;
}
else
{
if (!vehicle.almostStopped)
{
result = -dynBraking_SP;
}
else
{
result = 0;
}
}
}
else
{
if (!vehicle.almostStopped)
{
result = -dynBraking_Obs;
}
else
{
result = 0;
}
}
}
return(result);
}
public void addRecordedWaypoint(WaypointOld waypoint)
{
recordedWaypoints.Add(waypoint);
}
public static float calculate_DynamicStoppingDistance_Waypoint(Vehicle vehicle, float stoppingDistance, WaypointOld currentWP, WaypointOld nextWP)
{
//Initial stopping distance is used, since we know the bus can effectively brake at full speed at this distance from the target point.
float result = stoppingDistance;
//But if the bus is not at its maximum speed, then the distance needed will be proportional to its current speed.
result *= (vehicle.currentSpeed / vehicle.maximumSpeed);
//However, since waypoints can be in the same line, we don't need to brake if thats the case.
//Here we define the distance based on how steep is the angle between the bus and the next waypoint (after the current one).
if (!currentWP.isStopPoint)
{
Vector3 directionToNextWP = nextWP.transform.position - vehicle.transform.position;
float angleToNextWP = Vector3.Angle(directionToNextWP, vehicle.transform.forward);
//TODO Change the '0' to some static variable higher than 0 (like MINIMUM_ANGLE_REQUIRED), to stop the 'trembling'
result *= (Mathf.Clamp(angleToNextWP, 0, vehicle.maxSteeringAngle) / vehicle.maxSteeringAngle);
}
return(result);
}
