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;
}
Vector3 FindADirection(MapIntersection mapIntersection)
{
var direction = Vector3.right;
var maxProduct = 0f;
foreach (Transform child in mapIntersection.transform)
{
var mapLane = child.GetComponent <MapLane>();
if (mapLane != null)
{
// Check if the lane is a straight lane
var worldPositions = mapLane.mapWorldPositions;
var firstTwoPointsDir = (worldPositions[1] - worldPositions[0]).normalized;
var lastTwoPointsDir = (worldPositions[worldPositions.Count - 1] - worldPositions[worldPositions.Count - 2]).normalized;
var product = Vector3.Dot(firstTwoPointsDir, lastTwoPointsDir);
if (product > 0.9 && product > maxProduct)
{
direction = firstTwoPointsDir;
maxProduct = product;
}
}
}
return(direction);
}
void CreateStopSignMesh(string id, MapIntersection intersection, MapSign mapSign)
{
GameObject stopSignPrefab = Settings.MapStopSignPrefab;
var stopSignObj = UnityEngine.Object.Instantiate(stopSignPrefab, mapSign.transform.position + mapSign.boundOffsets, mapSign.transform.rotation);
stopSignObj.transform.parent = intersection.transform;
stopSignObj.name = "MapStopSignMesh_" + id;
}
Renderer GetSignalMesh(string id, MapIntersection intersection, MapSignal mapSignal)
{
var mapSignalMesh = UnityEngine.Object.Instantiate(Settings.MapTrafficSignalPrefab, mapSignal.transform.position, mapSignal.transform.rotation);
mapSignalMesh.transform.parent = intersection.transform;
mapSignalMesh.name = "MapSignalMeshVertical_" + id;
var mapSignalMeshRenderer = mapSignalMesh.AddComponent <SignalLight>().GetComponent <Renderer>();
return(mapSignalMeshRenderer);
}
private void ResetData()
{
StopNPCCoroutines();
ResetLights();
rb.angularVelocity = Vector3.zero;
rb.velocity = Vector3.zero;
isLeftTurn = false;
isRightTurn = false;
isForcedStop = false;
simpleVelocity = Vector3.zero;
simpleAngularVelocity = Vector3.zero;
lastRBPosition = transform.position;
lastRBRotation = transform.rotation;
currentIntersection = null;
}
static void ComputeProjectedMinMaxPoints(MapIntersection mapIntersection, Vector3 direction, out Vector3 minPoint, out Vector3 maxPoint)
{
var min = float.MaxValue;
var max = float.MinValue;
minPoint = Vector3.zero;
maxPoint = Vector3.zero;
// Get bounds from all intersection lines's end points
foreach (Transform child in mapIntersection.transform)
{
var mapLine = child.GetComponent <MapLine>();
if (mapLine != null)
{
var firstPt = mapLine.mapWorldPositions.First();
var lastPt = mapLine.mapWorldPositions.Last();
var projectedFirstPt = Vector3.Dot(direction, firstPt);
var projectedLastPt = Vector3.Dot(direction, lastPt);
Vector3 tempMinPoint, tempMaxPoint;
float tempMin, tempMax;
if (projectedFirstPt < projectedLastPt)
{
tempMinPoint = firstPt;
tempMin = projectedFirstPt;
tempMaxPoint = lastPt;
tempMax = projectedLastPt;
}
else
{
tempMinPoint = lastPt;
tempMin = projectedLastPt;
tempMaxPoint = firstPt;
tempMax = projectedFirstPt;
}
if (tempMin < min)
{
min = tempMin;
minPoint = tempMinPoint;
}
if (tempMax > max)
{
max = tempMax;
maxPoint = tempMaxPoint;
}
}
}
}
static void GetBoundsPoints(MapIntersection mapIntersection, Vector3 direction, out Vector3 leftBottom, out Vector3 rightBottom, out Vector3 rightTop, out Vector3 leftTop)
{
// use dot product to get min/max values
var perpendicularDir = Vector3.Cross(direction, Vector3.up);
ComputeProjectedMinMaxPoints(mapIntersection, direction, out Vector3 minPoint, out Vector3 maxPoint);
ComputeProjectedMinMaxPoints(mapIntersection, perpendicularDir, out Vector3 minPerpendicularPoint, out Vector3 maxPerpendicularPoint);
leftBottom = GetIntersectingPoint(perpendicularDir, direction, minPoint, minPerpendicularPoint);
leftBottom.y = minPoint.y;
rightBottom = GetIntersectingPoint(perpendicularDir, direction, minPoint, maxPerpendicularPoint);
rightBottom.y = minPoint.y;
rightTop = GetIntersectingPoint(perpendicularDir, direction, maxPoint, maxPerpendicularPoint);
rightTop.y = maxPoint.y;
leftTop = GetIntersectingPoint(perpendicularDir, direction, maxPoint, minPerpendicularPoint);
leftTop.y = maxPoint.y;
}
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);
}
void StartStoppingCoroutine()
{
if (currentMapLane?.stopLine != null) // check if stopline is connected to current path
{
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
{
Coroutines.Add(FixedUpdateManager.StartCoroutine(WaitStopSign()));
}
else
{
Coroutines.Add(FixedUpdateManager.StartCoroutine(WaitTrafficLight()));
}
}
}
}
