protected void ResetData()
{
controller.StopNPCCoroutines();
currentMapLane = null;
laneSpeedLimit = 0f;
foreach (var intersection in SimulatorManager.Instance.MapManager.intersections)
{
intersection.ExitStopSignQueue(controller);
intersection.ExitIntersectionList(controller);
}
prevMapLane = null;
controller.ResetLights();
currentSpeed = 0f;
currentStopTime = 0f;
rb.angularVelocity = Vector3.zero;
rb.velocity = Vector3.zero;
isCurve = false;
isLeftTurn = false;
isRightTurn = false;
isWaitingToDodge = false;
isDodge = false;
laneChange = true;
isStopLight = false;
isStopSign = false;
hasReachedStopSign = false;
isLaneDataSet = false;
isForcedStop = false;
controller.SetLastPosRot(transform.position, transform.rotation);
}
// npc and api traffic lanes only
public MapTrafficLane GetClosestLane(Vector3 position)
{
MapTrafficLane result = null;
float minDist = float.PositiveInfinity;
// TODO: this should be optimized
foreach (var lane in trafficLanes)
{
if (lane.mapWorldPositions.Count >= 2)
{
for (int i = 0; i < lane.mapWorldPositions.Count - 1; i++)
{
var p0 = lane.mapWorldPositions[i];
var p1 = lane.mapWorldPositions[i + 1];
float d = Utility.SqrDistanceToSegment(p0, p1, position);
if (d < minDist)
{
minDist = d;
result = lane;
}
}
}
}
return(result);
}
public void ForceLaneChange(bool isLeft)
{
if (isLeft)
{
if (currentMapLane.leftLaneForward != null)
{
if (!isFrontLeftDetect)
{
currentMapLane = currentMapLane.leftLaneForward;
laneSpeedLimit = currentMapLane.speedLimit;
aggressionAdjustRate = laneSpeedLimit / 11.176f; // 11.176 m/s corresponds to 25 mph
SetChangeLaneData(currentMapLane.mapWorldPositions);
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(DelayOffTurnSignals()));
ApiManager.Instance?.AddLaneChange(gameObject);
}
}
}
else
{
if (currentMapLane.rightLaneForward != null)
{
if (!isFrontRightDetect)
{
currentMapLane = currentMapLane.rightLaneForward;
laneSpeedLimit = currentMapLane.speedLimit;
aggressionAdjustRate = laneSpeedLimit / 11.176f; // 11.176 m/s corresponds to 25 mph
SetChangeLaneData(currentMapLane.mapWorldPositions);
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(DelayOffTurnSignals()));
ApiManager.Instance?.AddLaneChange(gameObject);
}
}
}
}
public void InitLaneData(MapTrafficLane lane)
{
if (_ActiveBehaviour)
{
_ActiveBehaviour.InitLaneData(lane);
}
}
private void InitLaneData()
{
ResetData();
var lane = SimulatorManager.Instance.MapManager.GetClosestLane(transform.position);
laneSpeedLimit = lane.speedLimit;
if (laneSpeedLimit > 0)
{
aggressionAdjustRate = laneSpeedLimit / 11.176f; // give more space at faster speeds
stopHitDistance = 12 / aggression * aggressionAdjustRate;
}
normalSpeed = RandomGenerator.NextFloat(laneSpeedLimit, laneSpeedLimit + 1 + aggression);
currentMapLane = lane;
int index = -1;
float minDist = float.PositiveInfinity;
Vector3 closest = Vector3.zero;
// choose closest waypoint
for (int i = 0; i < lane.mapWorldPositions.Count - 1; i++)
{
var p0 = lane.mapWorldPositions[i];
var p1 = lane.mapWorldPositions[i + 1];
var p = Utility.ClosetPointOnSegment(p0, p1, transform.position);
float d = Vector3.SqrMagnitude(transform.position - p);
if (d < minDist)
{
minDist = d;
index = i;
closest = p;
}
}
if (closest != lane.mapWorldPositions[index])
{
index++;
}
laneData = new List <Vector3>(lane.mapWorldPositions);
isDodge = false;
currentTarget = lane.mapWorldPositions[index];
currentIndex = index;
stopTarget = lane.mapWorldPositions[lane.mapWorldPositions.Count - 1];
currentIntersection = lane.stopLine?.intersection;
distanceToCurrentTarget = Vector3.Distance(new Vector3(frontCenter.position.x, 0f, frontCenter.position.z), new Vector3(currentTarget.x, 0f, currentTarget.z));
distanceToStopTarget = Vector3.Distance(new Vector3(frontCenter.position.x, 0f, frontCenter.position.z), new Vector3(stopTarget.x, 0f, stopTarget.z));
if (currentIndex >= laneData.Count - 2)
{
StartStoppingCoroutine();
}
SetLastPosRot(transform.position, transform.rotation);
isLaneDataSet = true;
}
protected virtual void SetLaneChange()
{
if (currentMapLane == null) // Prevent null if despawned during wait
{
return;
}
ApiManager.Instance?.AddLaneChange(gameObject);
if (currentMapLane.leftLaneForward != null)
{
if (!isFrontLeftDetect)
{
currentMapLane = currentMapLane.leftLaneForward;
laneSpeedLimit = currentMapLane.speedLimit;
aggressionAdjustRate = laneSpeedLimit / 11.176f; // 11.176 m/s corresponds to 25 mph
SetChangeLaneData(currentMapLane.mapWorldPositions);
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(DelayOffTurnSignals()));
}
}
else if (currentMapLane.rightLaneForward != null)
{
if (!isFrontRightDetect)
{
currentMapLane = currentMapLane.rightLaneForward;
laneSpeedLimit = currentMapLane.speedLimit;
aggressionAdjustRate = laneSpeedLimit / 11.176f; // 11.176 m/s corresponds to 25 mph
SetChangeLaneData(currentMapLane.mapWorldPositions);
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(DelayOffTurnSignals()));
}
}
}
private void OnTriggerStay(Collider other)
{
if (other.gameObject.layer == LaneLayer)
{
CurrentMapLane = other.GetComponentInParent <MapTrafficLane>();
}
}
private void OnTriggerExit(Collider other)
{
if (other.gameObject.layer == LaneLayer)
{
CurrentMapLane = null;
}
}
private void GetCinematicFollowMapLane()
{
var lanesNear = new List <MapTrafficLane>();
for (int i = 0; i < SimulatorManager.Instance.MapManager.trafficLanes.Count; i++)
{
float dist = Vector3.Distance(SimulatorManager.Instance.MapManager.trafficLanes[i].mapWorldPositions[0], targetObject.position);
if (dist < 50)
{
lanesNear.Add(SimulatorManager.Instance.MapManager.trafficLanes[i]);
}
}
if (lanesNear.Count != 0)
{
var randIndex = Random.Range(0, lanesNear.Count);
currentMapLane = lanesNear[randIndex];
cinematicStart = currentMapLane.mapWorldPositions[0];
cinematicStart += cinematicOffset;
cinematicEnd = currentMapLane.mapWorldPositions[currentMapLane.mapWorldPositions.Count - 1];
cinematicEnd += cinematicOffset;
transform.position = cinematicStart;
}
else
{
RandomCinematicState(); // no close lanes just use a different cinematic state
}
}
public void copyFromMapLane(MapTrafficLane lane)
{
this.MapWorldPositions.Clear();
this.MapWorldPositions.AddRange(lane.mapWorldPositions);
this.LaneCount = lane.laneCount;
this.LaneNumber = lane.laneNumber;
this.SpeedLimit = lane.speedLimit;
}
public void setStartLane(MapTrafficLane lane)
{
this.Q.Clear(); // ensure there is nothing left over in the queue (this is the FIRST lane)
this.currentLane = lane;
this.previousLane = lane; // to avoid having a null previousLane at the start.
firstLaneSegmentInit();
fetchStopTargetFromLane();
}
public List <Vector3> GetLaneLineData(MapTrafficLane lane, bool isLeft)
{
if (dict == null)
{
return(null);
}
if (dict.ContainsKey(lane))
{
return(isLeft ? dict[lane].leftLineWorldPositions : dict[lane].rightLineWorldPositions);
}
return(null);
}
protected virtual void OnSceneGUI()
{
MapTrafficLane vmMapLane = (MapTrafficLane)target;
if (vmMapLane.mapLocalPositions.Count < 1)
{
return;
}
if (vmMapLane.DisplayHandles)
{
for (int i = 0; i < vmMapLane.mapLocalPositions.Count; i++)
{
EditorGUI.BeginChangeCheck();
Vector3 newTargetPosition = Handles.PositionHandle(vmMapLane.transform.TransformPoint(vmMapLane.mapLocalPositions[i]), vmMapLane.transform.rotation);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(vmMapLane, "Line points change");
vmMapLane.mapLocalPositions[i] = vmMapLane.transform.InverseTransformPoint(newTargetPosition);
}
}
}
if (vmMapLane.displayLane)
{
var left = new Vector3[vmMapLane.mapLocalPositions.Count];
var right = new Vector3[vmMapLane.mapLocalPositions.Count];
var halfLaneWidth = vmMapLane.displayLaneWidth / 2;
var laneTransform = vmMapLane.transform;
var a = laneTransform.TransformPoint(vmMapLane.mapLocalPositions[0]);
for (int i = 1; i < vmMapLane.mapLocalPositions.Count; i++)
{
var b = laneTransform.TransformPoint(vmMapLane.mapLocalPositions[i]);
left[i - 1] = (GetPerpPoint(a, b, halfLaneWidth));
right[i - 1] = (GetPerpPoint(a, b, -halfLaneWidth));
if (i == vmMapLane.mapLocalPositions.Count - 1)
{
left[i] = (GetPerpPoint(b, a, -halfLaneWidth));
right[i] = (GetPerpPoint(b, a, halfLaneWidth));
}
a = b;
}
Handles.DrawAAPolyLine(left);
Handles.DrawAAPolyLine(right);
}
}
public override void OnInspectorGUI()
{
DrawDefaultInspector();
MapTrafficLane mapLane = (MapTrafficLane)target;
if (mapLane.leftLineBoundry != null)
{
mapLane.leftLineBoundry.lineType = (MapData.LineType)EditorGUILayout.EnumPopup("Left Boundary Type: ", mapLane.leftLineBoundry?.lineType);
}
if (mapLane.rightLineBoundry != null)
{
mapLane.rightLineBoundry.lineType = (MapData.LineType)EditorGUILayout.EnumPopup("Right Boundary Type: ", mapLane.rightLineBoundry?.lineType);
}
EditorGUILayout.LabelField("", GUI.skin.horizontalSlider);
if (GUILayout.Button("Reverse Lane"))
{
Undo.RecordObject(mapLane, "change builder");
mapLane.ReversePoints();
}
EditorGUILayout.BeginHorizontal();
if (GUILayout.Button("Add yield lanes"))
{
if (Selection.gameObjects != null)
{
foreach (var obj in Selection.gameObjects)
{
var lane = obj.GetComponent <MapTrafficLane>();
if (lane == mapLane)
{
continue;
}
if (lane != null)
{
mapLane.yieldToLanes.Add(lane);
}
}
}
}
if (GUILayout.Button("Clear yield lanes"))
{
mapLane.yieldToLanes.Clear();
}
EditorGUILayout.EndHorizontal();
}
public override void InitLaneData(MapTrafficLane lane)
{
ResetData();
laneSpeedLimit = lane.speedLimit;
if (laneSpeedLimit > 0)
{
aggressionAdjustRate = laneSpeedLimit / 11.176f; // give more space at faster speeds
stopHitDistance = 12 / aggression * aggressionAdjustRate;
}
normalSpeed = RandomGenerator.NextFloat(laneSpeedLimit - 3 + aggression, laneSpeedLimit + 1 + aggression);
currentMapLane = lane;
SetLaneData(currentMapLane.mapWorldPositions);
controller.SetLastPosRot(transform.position, transform.rotation);
isLaneDataSet = true;
}
private void RandomCinematicState()
{
SimulatorManager.Instance?.UIManager?.FadeOutIn(1f);
currentMapLane = null;
thisCamera.transform.localRotation = Quaternion.identity;
thisCamera.transform.localPosition = Vector3.zero;
var temp = (CinematicStateType)UnityEngine.Random.Range(0, System.Enum.GetValues(typeof(CinematicStateType)).Length);
while (temp == CurrentCinematicState)
{
temp = (CinematicStateType)UnityEngine.Random.Range(0, System.Enum.GetValues(typeof(CinematicStateType)).Length);
}
CurrentCinematicState = temp;
switch (CurrentCinematicState)
{
case CinematicStateType.Static:
transform.SetParent(SimulatorManager.Instance?.CameraManager.transform);
transform.position = (UnityEngine.Random.insideUnitSphere * (BoundsZ * 3f)) + targetObject.transform.position;
transform.position = new Vector3(transform.position.x, targetObject.position.y + (BoundsY * 2f), transform.position.z);
break;
case CinematicStateType.Follow:
transform.SetParent(SimulatorManager.Instance?.CameraManager.transform);
GetCinematicFollowMapLane();
break;
case CinematicStateType.Rotate:
transform.position = targetObject.position + offset;
transform.rotation = targetObject.transform.rotation;
transform.SetParent(targetObject);
break;
case CinematicStateType.Stuck:
if (cinematicCameraTransforms.Count == 0)
{
//Reroll state as Stuck type is not supported in the active agent
RandomCinematicState();
break;
}
var rando = Random.Range(0, cinematicCameraTransforms.Count);
transform.position = cinematicCameraTransforms[rando].transform.position;
transform.rotation = cinematicCameraTransforms[rando].transform.rotation;
transform.SetParent(cinematicCameraTransforms[rando].transform);
break;
}
}
private void SpeedViolationEvent(uint id, MapTrafficLane laneData)
{
Hashtable data = new Hashtable
{
{ "Id", id },
{ "Type", "SpeedViolation" },
{ "Time", SimulatorManager.Instance.GetSessionElapsedTimeSpan().ToString() },
{ "Location", transform.position },
{ "SpeedLimit", laneData.speedLimit },
{ "MaxSpeed", SpeedViolationMax },
{ "Duration", TimeSpan.FromSeconds(SpeedViolationCount).ToString() },
{ "Status", AnalysisManager.AnalysisStatusType.Failed },
};
SimulatorManager.Instance.AnalysisManager.AddEvent(data);
}
public void Update()
{
// distance analysis
Distance += Vector3.Distance(transform.position, PrevPos) / 1000;
PrevPos = transform.position;
// traffic speed violation
float speed = Dynamics.Velocity.magnitude;
if (speed > Lane?.CurrentMapLane?.speedLimit)
{
SpeedViolationLane = Lane.CurrentMapLane;
SpeedViolationCount += Time.fixedDeltaTime;
UpdateMinMax(speed, ref SpeedViolationMin, ref SpeedViolationMax);
}
else
{
if (SpeedViolationCount > 0 && SpeedViolationLane != null)
{
SpeedViolationEvent(Controller.GTID, SpeedViolationLane);
}
SpeedViolationCount = 0f;
SpeedViolationMax = 0f;
SpeedViolationLane = null;
}
msg = new VehicleOdometryData()
{
Time = SimulatorManager.Instance.CurrentTime,
Speed = speed,
SteeringAngleFront = Dynamics.WheelAngle,
SteeringAngleBack = 0f,
};
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(msg);
}
}
private void ResetData()
{
if (FixedUpdateManager != null)
{
foreach (Coroutine coroutine in Coroutines)
{
if (coroutine != null)
{
FixedUpdateManager.StopCoroutine(coroutine);
}
}
}
StopAllCoroutines();
RB.angularVelocity = Vector3.zero;
RB.velocity = Vector3.zero;
simpleVelocity = Vector3.zero;
simpleAngularVelocity = Vector3.zero;
currentMapLane = null;
prevMapLane = null;
currentIntersection = null;
currentIndex = 0;
laneSpeedLimit = 0f;
currentSpeed = 0f;
currentStopTime = 0f;
foreach (var intersection in SimulatorManager.Instance.MapManager.intersections)
{
//intersection.ExitStopSignQueue(controller); // TODO Agent enter queues
//intersection.ExitIntersectionList(controller);
}
//ResetLights();
isLeftTurn = false;
isRightTurn = false;
isForcedStop = false;
isCurve = false;
isWaitingToDodge = false;
isDodge = false;
laneChange = true;
isStopLight = false;
isStopSign = false;
hasReachedStopSign = false;
isLaneDataSet = false;
SetLastPosRot(transform.position, transform.rotation);
}
protected void GetNextLane()
{
// last index of current lane data
if (currentMapLane?.nextConnectedLanes.Count >= 1) // choose next path and set waypoints
{
currentMapLane = currentMapLane.nextConnectedLanes[RandomGenerator.Next(currentMapLane.nextConnectedLanes.Count)];
laneSpeedLimit = currentMapLane.speedLimit;
aggressionAdjustRate = laneSpeedLimit / 11.176f; // 11.176 m/s corresponds to 25 mph
normalSpeed = APIMaxSpeed > 0 ?
Mathf.Min(APIMaxSpeed, laneSpeedLimit) :
RandomGenerator.NextFloat(laneSpeedLimit - 3 + aggression, laneSpeedLimit + 1 + aggression); // API set max speed or lane speed limit
SetLaneData(currentMapLane.mapWorldPositions);
SetTurnSignal();
}
else
{
Despawn(); // issue getting new waypoints so despawn
}
}
public Vector3 Dequeue()
{
if (this.Q.Count == 0)
{
if (this.currentLane)
{
// enqueue waypoints from next lane, then dequeue
this.previousLane = this.currentLane;
// fetch next lane
this.currentLane = this.currentLane.nextConnectedLanes[RandomGenerator.Next(this.currentLane.nextConnectedLanes.Count)];
fetchWaypointsFromLane();
}
else
{
Debug.LogError("currentLane is not set for the waypoint queue.");
}
}
return(this.Q.Dequeue());
}
void StartStoppingCoroutine()
{
if (currentMapLane?.stopLine != null) // check if stopline is connected to current path
{
controller.currentIntersection = currentMapLane.stopLine?.intersection;
stopTarget = currentMapLane.mapWorldPositions[currentMapLane.mapWorldPositions.Count - 1];
prevMapLane = currentMapLane;
if (prevMapLane.stopLine.intersection != null) // null if map not setup right TODO add check to report missing stopline
{
if (prevMapLane.stopLine.isStopSign) // stop sign
{
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(WaitStopSign()));
}
else
{
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(WaitTrafficLight()));
}
}
}
}
private void CheckFollow()
{
if (Vector3.Distance(transform.position, cinematicEnd) < 0.001f)
{
if (currentMapLane.nextConnectedLanes == null || currentMapLane.nextConnectedLanes.Count == 0)
{
RandomCinematicState();
}
else
{
int rand = UnityEngine.Random.Range(0, currentMapLane.nextConnectedLanes.Count - 1);
currentMapLane = currentMapLane.nextConnectedLanes[rand];
cinematicStart = currentMapLane.mapWorldPositions[0];
cinematicStart += cinematicOffset;
cinematicEnd = currentMapLane.mapWorldPositions[currentMapLane.mapWorldPositions.Count - 1];
cinematicEnd += cinematicOffset;
transform.position = cinematicStart;
}
}
}
public int GetLaneNextIndex(Vector3 position, MapTrafficLane lane)
{
float minDist = float.PositiveInfinity;
int index = -1;
for (int i = 0; i < lane.mapWorldPositions.Count - 1; i++)
{
var p0 = lane.mapWorldPositions[i];
var p1 = lane.mapWorldPositions[i + 1];
var p = Utility.ClosetPointOnSegment(p0, p1, position);
float d = Vector3.SqrMagnitude(position - p);
if (d < minDist)
{
minDist = d;
index = i + 1;
}
}
return(index);
}
public override void InitLaneData(MapTrafficLane lane)
{
}
public override void InitLaneData(MapTrafficLane lane)
{
// Maplane data void in waypoint behavior
}
private void Start()
{
LaneLayer = LayerMask.NameToLayer("Lane");
CurrentMapLane = null;
}
private void OnDisable()
{
CurrentMapLane = null;
}
public IEnumerable <LineVert> Enumerate(MapTrafficLane lane)
{
cIndex = -1;
cLane = lane;
return(this);
}
protected virtual void EvaluateTarget()
{
distanceToCurrentTarget = Vector3.Distance(new Vector3(controller.frontCenter.position.x, 0f, controller.frontCenter.position.z), new Vector3(currentTarget.x, 0f, currentTarget.z));
distanceToStopTarget = Vector3.Distance(new Vector3(controller.frontCenter.position.x, 0f, controller.frontCenter.position.z), new Vector3(stopTarget.x, 0f, stopTarget.z));
if (distanceToStopTarget < 1f)
{
if (!atStopTarget)
{
ApiManager.Instance?.AddStopLine(gameObject);
atStopTarget = true;
}
}
else
{
atStopTarget = false;
}
// check if we are past the target or reached current target
if (Vector3.Dot(controller.frontCenter.forward, (currentTarget - controller.frontCenter.position).normalized) < 0 || distanceToCurrentTarget < 1f)
{
if (currentIndex == laneData.Count - 2) // reached 2nd to last target index see if stop line is present
{
StartStoppingCoroutine();
}
if (currentIndex < laneData.Count - 1) // reached target dist and is not at last index of lane data
{
currentIndex++;
currentTarget = laneData[currentIndex];
controller.Coroutines.Add(FixedUpdateManager.StartCoroutine(DelayChangeLane()));
}
else
{
// GetNextLane
// last index of current lane data
if (currentMapLane?.nextConnectedLanes.Count >= 1) // choose next path and set waypoints
{
currentMapLane = currentMapLane.nextConnectedLanes[RandomGenerator.Next(currentMapLane.nextConnectedLanes.Count)];
laneSpeedLimit = currentMapLane.speedLimit;
aggressionAdjustRate = laneSpeedLimit / 11.176f; // 11.176 m/s corresponds to 25 mph
normalSpeed = APIMaxSpeed > 0 ?
Mathf.Min(APIMaxSpeed, laneSpeedLimit) :
RandomGenerator.NextFloat(laneSpeedLimit, laneSpeedLimit + 1 + aggression); // API set max speed or lane speed limit
SetLaneData(currentMapLane.mapWorldPositions);
SetTurnSignal();
}
else
{
Despawn(); // issue getting new waypoints so despawn
}
}
}
// isTurn
if (currentMapLane == null)
{
return;
}
var path = transform.InverseTransformPoint(currentTarget).x;
isCurve = path <-1f || path> 1f ? true : false;
}
