public static bool ShouldRecalculatePath(ushort vehicleId, ref Vehicle vehicleData, int maxBlockCounter)
{
if (vehicleData.m_leadingVehicle != 0)
{
return(false);
}
if ((vehicleData.m_flags & Vehicle.Flags.Emergency2) == Vehicle.Flags.None)
{
return(false);
}
if (!Options.dynamicPathRecalculation)
{
return(false);
}
if (TrafficPriority.GetVehiclePosition(vehicleId).LastPathRecalculation >= GetVehiclePathRecalculationFrame())
{
return(false);
}
if (vehicleData.m_path != 0)
{
PathUnit.Position pos = Singleton <PathManager> .instance.m_pathUnits.m_buffer[vehicleData.m_path].GetPosition(vehicleData.m_pathPositionIndex >> 1);
if (pos.m_segment != 0)
{
bool isHighway = CustomRoadAI.GetSegmentGeometry(pos.m_segment).IsHighway();
if (isHighway)
{
return(false); // no recalculation on highways
}
}
}
return(vehicleData.m_blockCounter >= MIN_BLOCK_COUNTER_PATH_RECALC_VALUE);
/*float recalcDecisionValue = Math.Max(0.005f, ((float)vehicleData.m_blockCounter - (float)MIN_BLOCK_RECALC_VALUE) / ((float)maxBlockCounter - (float)MIN_BLOCK_RECALC_VALUE));
* float bias = 1f;
* switch (Options.simAccuracy) {
* case 1:
* bias = 1.25f;
* break;
* case 2:
* bias = 1.5f;
* break;
* case 3:
* bias = 2f;
* break;
* case 4:
* bias = 3f;
* break;
* }
* //Log._Debug($"Path recalculation for vehicle {vehicleId}: recalcDecisionValue={recalcDecisionValue} bias={bias}");
* recalcDecisionValue *= bias;
* return UnityEngine.Random.Range(0f, 1f) < recalcDecisionValue;*/
}
/// <summary>
/// Checks for traffic lights and priority signs when changing segments (for road & rail vehicles).
/// Sets the maximum allowed speed <paramref name="maxSpeed"/> if segment change is not allowed (otherwise <paramref name="maxSpeed"/> has to be set by the calling method).
/// </summary>
/// <param name="vehicleId">vehicle id</param>
/// <param name="vehicleData">vehicle data</param>
/// <param name="lastFrameData">last frame data of vehicle</param>
/// <param name="isRecklessDriver">if true, this vehicle ignores red traffic lights and priority signs</param>
/// <param name="prevPos">previous path position</param>
/// <param name="prevTargetNodeId">previous target node</param>
/// <param name="prevLaneID">previous lane</param>
/// <param name="position">current path position</param>
/// <param name="targetNodeId">transit node</param>
/// <param name="laneID">current lane</param>
/// <param name="nextPosition">next path position</param>
/// <param name="nextTargetNodeId">next target node</param>
/// <param name="maxSpeed">maximum allowed speed (only valid if method returns false)</param>
/// <returns>true, if the vehicle may change segments, false otherwise.</returns>
internal static bool MayChangeSegment(ushort vehicleId, ref Vehicle vehicleData, ref Vehicle.Frame lastFrameData, bool isRecklessDriver, ref PathUnit.Position prevPos, ushort prevTargetNodeId, uint prevLaneID, ref PathUnit.Position position, ushort targetNodeId, uint laneID, ref PathUnit.Position nextPosition, ushort nextTargetNodeId, out float maxSpeed, bool debug = false)
{
debug = false;
if (prevTargetNodeId != targetNodeId)
{
// method should only be called if targetNodeId == prevTargetNode
maxSpeed = 0f;
return(true);
}
bool forceUpdatePos = false;
VehicleState vehicleState = null;
try {
vehicleState = VehicleStateManager.GetVehicleState(vehicleId);
if (vehicleState == null)
{
VehicleStateManager.OnPathFindReady(vehicleId, ref vehicleData);
vehicleState = VehicleStateManager.GetVehicleState(vehicleId);
if (vehicleState == null)
{
#if DEBUG
Log._Debug($"Could not get vehicle state of {vehicleId}!");
#endif
}
else
{
forceUpdatePos = true;
}
}
} catch (Exception e) {
Log.Error("VehicleAI MayChangeSegment vehicle state error: " + e.ToString());
}
if (forceUpdatePos || Options.simAccuracy >= 2)
{
try {
VehicleStateManager.UpdateVehiclePos(vehicleId, ref vehicleData, ref prevPos, ref position);
} catch (Exception e) {
Log.Error("VehicleAI MayChangeSegment Error: " + e.ToString());
}
}
var netManager = Singleton <NetManager> .instance;
uint currentFrameIndex = Singleton <SimulationManager> .instance.m_currentFrameIndex;
uint prevTargetNodeLower8Bits = (uint)((prevTargetNodeId << 8) / 32768);
uint random = currentFrameIndex - prevTargetNodeLower8Bits & 255u;
bool isRailVehicle = (vehicleData.Info.m_vehicleType & (VehicleInfo.VehicleType.Train | VehicleInfo.VehicleType.Metro)) != VehicleInfo.VehicleType.None;
NetNode.Flags targetNodeFlags = netManager.m_nodes.m_buffer[targetNodeId].m_flags;
bool hasTrafficLight = (targetNodeFlags & NetNode.Flags.TrafficLights) != NetNode.Flags.None;
bool checkTrafficLights = false;
if (!isRailVehicle)
{
// check if to check space
#if DEBUG
if (debug)
{
Log._Debug($"CustomVehicleAI.MayChangeSegment: Vehicle {vehicleId} is not a train.");
}
#endif
var prevLaneFlags = (NetLane.Flags)netManager.m_lanes.m_buffer[(int)((UIntPtr)prevLaneID)].m_flags;
var hasCrossing = (targetNodeFlags & NetNode.Flags.LevelCrossing) != NetNode.Flags.None;
var isJoinedJunction = (prevLaneFlags & NetLane.Flags.JoinedJunction) != NetLane.Flags.None;
bool checkSpace = !Flags.getEnterWhenBlockedAllowed(prevPos.m_segment, netManager.m_segments.m_buffer[prevPos.m_segment].m_startNode == targetNodeId) && !isRecklessDriver;
//TrafficLightSimulation nodeSim = TrafficLightSimulation.GetNodeSimulation(destinationNodeId);
//if (timedNode != null && timedNode.vehiclesMayEnterBlockedJunctions) {
// checkSpace = false;
//}
if (checkSpace)
{
// check if there is enough space
if ((targetNodeFlags & (NetNode.Flags.Junction | NetNode.Flags.OneWayOut | NetNode.Flags.OneWayIn)) == NetNode.Flags.Junction &&
netManager.m_nodes.m_buffer[targetNodeId].CountSegments() != 2)
{
var len = vehicleData.CalculateTotalLength(vehicleId) + 2f;
if (!netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].CheckSpace(len))
{
var sufficientSpace = false;
if (nextPosition.m_segment != 0 && netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_length < 30f)
{
NetNode.Flags nextTargetNodeFlags = netManager.m_nodes.m_buffer[nextTargetNodeId].m_flags;
if ((nextTargetNodeFlags & (NetNode.Flags.Junction | NetNode.Flags.OneWayOut | NetNode.Flags.OneWayIn)) != NetNode.Flags.Junction ||
netManager.m_nodes.m_buffer[nextTargetNodeId].CountSegments() == 2)
{
uint nextLaneId = PathManager.GetLaneID(nextPosition);
if (nextLaneId != 0u)
{
sufficientSpace = netManager.m_lanes.m_buffer[(int)((UIntPtr)nextLaneId)].CheckSpace(len);
}
}
}
if (!sufficientSpace)
{
maxSpeed = 0f;
try {
if (vehicleState != null)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to BLOCKED");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Blocked;
}
} catch (Exception e) {
Log.Error("VehicleAI MayChangeSegment error while setting junction state to BLOCKED: " + e.ToString());
}
return(false);
}
}
}
}
checkTrafficLights = (!isJoinedJunction || hasCrossing);
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"CustomVehicleAI.MayChangeSegment: Vehicle {vehicleId} is a train.");
}
#endif
checkTrafficLights = true;
}
try {
if (vehicleState != null && vehicleState.JunctionTransitState == VehicleJunctionTransitState.Blocked)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState from BLOCKED to ENTER");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Enter;
}
if ((vehicleData.m_flags & Vehicle.Flags.Emergency2) == 0)
{
if (hasTrafficLight && checkTrafficLights)
{
#if DEBUG
if (debug)
{
Log._Debug($"CustomVehicleAI.MayChangeSegment: Node {targetNodeId} has a traffic light.");
}
#endif
var destinationInfo = netManager.m_nodes.m_buffer[targetNodeId].Info;
if (vehicleState != null && vehicleState.JunctionTransitState == VehicleJunctionTransitState.None)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to ENTER (1)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Enter;
}
RoadBaseAI.TrafficLightState vehicleLightState;
RoadBaseAI.TrafficLightState pedestrianLightState;
bool vehicles;
bool pedestrians;
CustomRoadAI.GetTrafficLightState(vehicleId, ref vehicleData, targetNodeId, prevPos.m_segment, prevPos.m_lane, position.m_segment, ref netManager.m_segments.m_buffer[prevPos.m_segment], currentFrameIndex - prevTargetNodeLower8Bits, out vehicleLightState, out pedestrianLightState, out vehicles, out pedestrians);
if (vehicleData.Info.m_vehicleType == VehicleInfo.VehicleType.Car && isRecklessDriver) // no reckless driving at railroad crossings
{
vehicleLightState = RoadBaseAI.TrafficLightState.Green;
}
#if DEBUG
if (debug)
{
Log._Debug($"CustomVehicleAI.MayChangeSegment: Vehicle {vehicleId} has {vehicleLightState} at node {targetNodeId}");
}
#endif
if (!vehicles && random >= 196u)
{
vehicles = true;
RoadBaseAI.SetTrafficLightState(targetNodeId, ref netManager.m_segments.m_buffer[prevPos.m_segment], currentFrameIndex - prevTargetNodeLower8Bits, vehicleLightState, pedestrianLightState, vehicles, pedestrians);
}
var stopCar = false;
switch (vehicleLightState)
{
case RoadBaseAI.TrafficLightState.RedToGreen:
if (random < 60u)
{
stopCar = true;
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (RedToGreen)");
}
#endif
if (vehicleState != null)
{
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
}
break;
case RoadBaseAI.TrafficLightState.Red:
stopCar = true;
break;
case RoadBaseAI.TrafficLightState.GreenToRed:
if (random >= 30u)
{
stopCar = true;
}
else if (vehicleState != null)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (GreenToRed)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
break;
}
/*if ((vehicleLightState == RoadBaseAI.TrafficLightState.Green || vehicleLightState == RoadBaseAI.TrafficLightState.RedToGreen) && !Flags.getEnterWhenBlockedAllowed(prevPos.m_segment, netManager.m_segments.m_buffer[prevPos.m_segment].m_startNode == targetNodeId)) {
* var hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, targetNodeId);
*
* if (hasIncomingCars) {
* // green light but other cars are incoming and they have priority: stop
* stopCar = true;
* }
* }*/
if (stopCar)
{
if (vehicleState != null)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to STOP");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Stop;
}
maxSpeed = 0f;
return(false);
}
}
else if (vehicleState != null)
{
#if DEBUG
//bool debug = destinationNodeId == 10864;
//bool debug = destinationNodeId == 13531;
//bool debug = false;// targetNodeId == 5027;
#endif
//bool debug = false;
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId} is arriving @ seg. {prevPos.m_segment} ({position.m_segment}, {nextPosition.m_segment}), node {targetNodeId} which is not a traffic light.");
}
#endif
var prioritySegment = TrafficPriority.GetPrioritySegment(targetNodeId, prevPos.m_segment);
if (prioritySegment != null)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId} is arriving @ seg. {prevPos.m_segment} ({position.m_segment}, {nextPosition.m_segment}), node {targetNodeId} which is not a traffic light and is a priority segment.");
}
#endif
//if (prioritySegment.HasVehicle(vehicleId)) {
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: segment target position found");
}
#endif
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: global target position found. carState = {vehicleState.JunctionTransitState.ToString()}");
}
#endif
var currentFrameIndex2 = Singleton <SimulationManager> .instance.m_currentFrameIndex;
var frame = currentFrameIndex2 >> 4;
float speed = lastFrameData.m_velocity.magnitude;
if (vehicleState.JunctionTransitState == VehicleJunctionTransitState.None)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to ENTER (prio)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Enter;
}
if (vehicleState.JunctionTransitState != VehicleJunctionTransitState.Leave)
{
bool hasIncomingCars;
switch (prioritySegment.Type)
{
case SegmentEnd.PriorityType.Stop:
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: STOP sign. waittime={vehicleState.WaitTime}, vel={speed}");
}
#endif
if (Options.simAccuracy <= 2 || (Options.simAccuracy >= 3 && vehicleState.WaitTime < MaxPriorityWaitTime))
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to STOP (wait)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Stop;
if (speed <= TrafficPriority.maxStopVelocity)
{
vehicleState.WaitTime++;
float minStopWaitTime = UnityEngine.Random.Range(0f, 3f);
if (vehicleState.WaitTime >= minStopWaitTime)
{
if (Options.simAccuracy >= 4)
{
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
else
{
hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, ref vehicleData, ref prevPos, ref position);
#if DEBUG
if (debug)
{
Log._Debug($"hasIncomingCars: {hasIncomingCars}");
}
#endif
if (hasIncomingCars)
{
maxSpeed = 0f;
return(false);
}
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (min wait timeout)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
}
else
{
maxSpeed = 0;
return(false);
}
}
else
{
vehicleState.WaitTime = 0;
maxSpeed = 0f;
return(false);
}
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (max wait timeout)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
break;
case SegmentEnd.PriorityType.Yield:
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: YIELD sign. waittime={vehicleState.WaitTime}");
}
#endif
if (Options.simAccuracy <= 2 || (Options.simAccuracy >= 3 && vehicleState.WaitTime < MaxPriorityWaitTime))
{
vehicleState.WaitTime++;
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to STOP (wait)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Stop;
if (speed <= TrafficPriority.maxYieldVelocity || Options.simAccuracy <= 2)
{
if (Options.simAccuracy >= 4)
{
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
else
{
hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, ref vehicleData, ref prevPos, ref position);
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: hasIncomingCars: {hasIncomingCars}");
}
#endif
if (hasIncomingCars)
{
maxSpeed = 0f;
return(false);
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (no incoming cars)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
}
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Vehicle has not yet reached yield speed (reduce {speed} by {vehicleState.ReduceSpeedByValueToYield})");
}
#endif
// vehicle has not yet reached yield speed
maxSpeed = TrafficPriority.maxYieldVelocity;
return(false);
}
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (max wait timeout)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
break;
case SegmentEnd.PriorityType.Main:
case SegmentEnd.PriorityType.None:
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: MAIN sign. waittime={vehicleState.WaitTime}");
}
#endif
maxSpeed = 0f;
if (Options.simAccuracy == 4)
{
return(true);
}
if (Options.simAccuracy <= 2 || (Options.simAccuracy == 3 && vehicleState.WaitTime < MaxPriorityWaitTime))
{
vehicleState.WaitTime++;
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to STOP (wait)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Stop;
hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, ref vehicleData, ref prevPos, ref position);
#if DEBUG
if (debug)
{
Log._Debug($"hasIncomingCars: {hasIncomingCars}");
}
#endif
if (hasIncomingCars)
{
return(false);
}
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState to LEAVE (no conflicting car)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Leave;
}
return(true);
}
}
else if (speed <= TrafficPriority.maxStopVelocity)
{
// vehicle is not moving. reset allowance to leave junction
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: Setting JunctionTransitState from LEAVE to BLOCKED (speed to low)");
}
#endif
vehicleState.JunctionTransitState = VehicleJunctionTransitState.Blocked;
maxSpeed = 0f;
return(false);
}
}
}
}
} catch (Exception e) {
Log.Error($"Error occured in MayChangeSegment: {e.ToString()}");
}
maxSpeed = 0f; // maxSpeed should be set by caller
return(true);
}
/// <summary>
/// Handles vehicle path information in order to manage special nodes (nodes with priority signs or traffic lights).
/// Data like "vehicle X is on segment S0 and is going to segment S1" is collected.
/// </summary>
/// <param name="vehicleId"></param>
/// <param name="vehicleData"></param>
internal static void HandleVehicle(ushort vehicleId, ref Vehicle vehicleData, bool addTraffic, bool realTraffic, byte maxUpcomingPathPositions, bool debug = false)
{
if (maxUpcomingPathPositions <= 0)
{
maxUpcomingPathPositions = 1; // we need at least one upcoming path position
}
var netManager = Singleton <NetManager> .instance;
var lastFrameData = vehicleData.GetLastFrameData();
var lastFrameVehiclePos = lastFrameData.m_position;
#if DEBUGV
var camPos = Camera.main.transform.position;
//debug = (lastFrameVehiclePos - camPos).sqrMagnitude < CloseLod;
debug = false;
List <String> logBuffer = new List <String>();
bool logme = false;
#endif
if ((vehicleData.m_flags & Vehicle.Flags.Created) == 0)
{
TrafficPriority.RemoveVehicleFromSegments(vehicleId);
return;
}
if (vehicleData.Info.m_vehicleType != VehicleInfo.VehicleType.Car &&
vehicleData.Info.m_vehicleType != VehicleInfo.VehicleType.Train &&
vehicleData.Info.m_vehicleType != VehicleInfo.VehicleType.Tram)
{
//Log._Debug($"HandleVehicle does not handle vehicles of type {vehicleData.Info.m_vehicleType}");
return;
}
#if DEBUGV
logBuffer.Add("Calculating prio info for vehicleId " + vehicleId);
#endif
ExtVehicleType?vehicleType = CustomVehicleAI.DetermineVehicleTypeFromVehicle(vehicleId, ref vehicleData);
if (vehicleType == null)
{
Log.Warning($"Could not determine vehicle type of vehicle {vehicleId}!");
}
if (vehicleType == null || vehicleType == ExtVehicleType.None)
{
return;
}
// add vehicle to our vehicle list
VehiclePosition vehiclePos = TrafficPriority.GetVehiclePosition(vehicleId);
// we extract the segment information directly from the vehicle
var currentPathUnitId = vehicleData.m_path;
List <ushort> realTimeDestinationNodes = new List <ushort>(); // current and upcoming node ids
List <PathUnit.Position> realTimePositions = new List <PathUnit.Position>(); // current and upcoming vehicle positions
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". currentPathId: " + currentPathUnitId + " pathPositionIndex: " + vehicleData.m_pathPositionIndex);
#endif
if (currentPathUnitId > 0)
{
// vehicle has a path...
if ((Singleton <PathManager> .instance.m_pathUnits.m_buffer[currentPathUnitId].m_pathFindFlags & PathUnit.FLAG_READY) != 0)
{
// The path(unit) is established and is ready for use: get the vehicle's current position in terms of segment and lane
realTimePositions.Add(Singleton <PathManager> .instance.m_pathUnits.m_buffer[currentPathUnitId].GetPosition(vehicleData.m_pathPositionIndex >> 1));
if (realTimePositions[0].m_offset == 0)
{
realTimeDestinationNodes.Add(netManager.m_segments.m_buffer[realTimePositions[0].m_segment].m_startNode);
}
else
{
realTimeDestinationNodes.Add(netManager.m_segments.m_buffer[realTimePositions[0].m_segment].m_endNode);
}
if (maxUpcomingPathPositions > 0)
{
// evaluate upcoming path units
byte i = 0;
uint pathUnitId = currentPathUnitId;
int pathPos = (byte)((vehicleData.m_pathPositionIndex >> 1) + 1);
while (true)
{
if (pathPos > 11)
{
// go to next path unit
pathPos = 0;
pathUnitId = Singleton <PathManager> .instance.m_pathUnits.m_buffer[pathUnitId].m_nextPathUnit;
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". Going to next path unit (1). pathUnitId=" + pathUnitId);
#endif
if (pathUnitId <= 0)
{
break;
}
}
PathUnit.Position nextRealTimePosition = default(PathUnit.Position);
if (!Singleton <PathManager> .instance.m_pathUnits.m_buffer[pathUnitId].GetPosition(pathPos, out nextRealTimePosition)) // if this returns false, there is no next path unit
{
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". No next path unit! pathPos=" + pathPos + ", pathUnitId=" + pathUnitId);
#endif
break;
}
ushort destNodeId = 0;
if (nextRealTimePosition.m_segment > 0)
{
if (nextRealTimePosition.m_offset == 0)
{
destNodeId = netManager.m_segments.m_buffer[nextRealTimePosition.m_segment].m_startNode;
}
else
{
destNodeId = netManager.m_segments.m_buffer[nextRealTimePosition.m_segment].m_endNode;
}
}
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". Next path unit! node " + destNodeId + ", seg. " + nextRealTimePosition.m_segment + ", pathUnitId=" + pathUnitId + ", pathPos: " + pathPos);
#endif
realTimePositions.Add(nextRealTimePosition);
realTimeDestinationNodes.Add(destNodeId);
if (i >= maxUpcomingPathPositions - 1)
{
break; // we calculate up to 2 upcoming path units at the moment
}
++pathPos;
++i;
}
}
// please don't ask why we use "m_pathPositionIndex >> 1" (which equals to "m_pathPositionIndex / 2") here (Though it would
// be interesting to know why they used such an ugly indexing scheme!!). I assume the oddness of m_pathPositionIndex relates
// to the car's position on the segment. If it is even the car might be in the segment's first half and if it is odd, it might
// be in the segment's second half.
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". *INFO* rtPos.seg=" + realTimePositions[0].m_segment + " nrtPos.seg=" + (realTimePositions.Count > 1 ? "" + realTimePositions[1].m_segment : "n/a"));
#endif
}
}
// we have seen the car!
vehiclePos.LastFrame = Singleton <SimulationManager> .instance.m_currentFrameIndex;
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". ToNode: " + vehiclePos.ToNode + ". FromSegment: " + vehiclePos.FromSegment /* + ". FromLaneId: " + TrafficPriority.Vehicles[vehicleId].FromLaneId*/);
#endif
if (addTraffic && vehicleData.m_leadingVehicle == 0 && realTimePositions.Count > 0)
{
// add traffic to lane
uint laneId = PathManager.GetLaneID(realTimePositions[0]);
CustomRoadAI.AddTraffic(laneId, (ushort)Mathf.RoundToInt(vehicleData.CalculateTotalLength(vehicleId)), (ushort)Mathf.RoundToInt(lastFrameData.m_velocity.magnitude), realTraffic);
}
#if DEBUGV
logBuffer.Add("* vehicleId " + vehicleId + ". Real time positions: " + realTimePositions.Count + ", Destination nodes: " + realTimeDestinationNodes.Count);
#endif
if (realTimePositions.Count >= 1)
{
// we found a valid path unit
var sourceLaneIndex = realTimePositions[0].m_lane;
if (
!vehiclePos.Valid ||
vehiclePos.ToNode != realTimeDestinationNodes[0] ||
vehiclePos.FromSegment != realTimePositions[0].m_segment ||
vehiclePos.FromLaneIndex != sourceLaneIndex)
{
// vehicle information is not up-to-date. remove the car from old priority segments (if existing)...
TrafficPriority.RemoveVehicleFromSegments(vehicleId);
if (realTimePositions.Count >= 2)
{
// save vehicle information for priority rule handling
vehiclePos.Valid = true;
vehiclePos.CarState = VehicleJunctionTransitState.None;
vehiclePos.WaitTime = 0;
vehiclePos.Stopped = false;
vehiclePos.ToNode = realTimeDestinationNodes[0];
vehiclePos.FromSegment = realTimePositions[0].m_segment;
vehiclePos.FromLaneIndex = realTimePositions[0].m_lane;
vehiclePos.ToSegment = realTimePositions[1].m_segment;
vehiclePos.ToLaneIndex = realTimePositions[1].m_lane;
vehiclePos.ReduceSpeedByValueToYield = UnityEngine.Random.Range(16f, 28f);
vehiclePos.OnEmergency = (vehicleData.m_flags & Vehicle.Flags.Emergency2) != 0;
vehiclePos.VehicleType = (ExtVehicleType)vehicleType;
#if DEBUGV
logBuffer.Add($"* vehicleId {vehicleId}. Setting current position to: from {vehiclePos.FromSegment} (lane {vehiclePos.FromLaneIndex}), going over {vehiclePos.ToNode}, to {vehiclePos.ToSegment} (lane {vehiclePos.ToLaneIndex})");
#endif
//if (!Options.disableSomething) {
// add the vehicle to upcoming priority segments that have timed traffic lights
for (int i = 0; i < realTimePositions.Count - 1; ++i)
{
var prioritySegment = TrafficPriority.GetPrioritySegment(realTimeDestinationNodes[i], realTimePositions[i].m_segment);
if (prioritySegment == null)
{
continue;
}
// add upcoming segments only if there is a timed traffic light
TrafficLightSimulation nodeSim = TrafficLightSimulation.GetNodeSimulation(realTimeDestinationNodes[i]);
if (i > 0 && (nodeSim == null || !nodeSim.IsTimedLight() || !nodeSim.IsTimedLightActive()))
{
continue;
}
VehiclePosition upcomingVehiclePos = new VehiclePosition();
upcomingVehiclePos.Valid = true;
upcomingVehiclePos.CarState = VehicleJunctionTransitState.None;
upcomingVehiclePos.LastFrame = vehiclePos.LastFrame;
upcomingVehiclePos.ToNode = realTimeDestinationNodes[i];
upcomingVehiclePos.FromSegment = realTimePositions[i].m_segment;
upcomingVehiclePos.FromLaneIndex = realTimePositions[i].m_lane;
upcomingVehiclePos.ToSegment = realTimePositions[i + 1].m_segment;
upcomingVehiclePos.ToLaneIndex = realTimePositions[i + 1].m_lane;
upcomingVehiclePos.ReduceSpeedByValueToYield = UnityEngine.Random.Range(16f, 28f);
upcomingVehiclePos.OnEmergency = (vehicleData.m_flags & Vehicle.Flags.Emergency2) != 0;
upcomingVehiclePos.VehicleType = (ExtVehicleType)vehicleType;
#if DEBUGV
logBuffer.Add($"* vehicleId {vehicleId}. Adding future position: from {upcomingVehiclePos.FromSegment} (lane {upcomingVehiclePos.FromLaneIndex}), going over {upcomingVehiclePos.ToNode}, to {upcomingVehiclePos.ToSegment} (lane {upcomingVehiclePos.ToLaneIndex})");
#endif
prioritySegment.AddVehicle(vehicleId, upcomingVehiclePos);
}
}
//}
}
else
{
#if DEBUGV
logBuffer.Add($"* vehicleId {vehicleId}. Nothing has changed. from {vehiclePos.FromSegment} (lane {vehiclePos.FromLaneIndex}), going over {vehiclePos.ToNode}, to {vehiclePos.ToSegment} (lane {vehiclePos.ToLaneIndex})");
logme = false;
#endif
}
}
else
{
#if DEBUGV
logBuffer.Add($"* vehicleId {vehicleId}. Insufficient path unit positions.");
#endif
TrafficPriority.RemoveVehicleFromSegments(vehicleId);
}
#if DEBUGV
if (logme)
{
Log._Debug("vehicleId: " + vehicleId + " ============================================");
foreach (String logBuf in logBuffer)
{
Log._Debug(logBuf);
}
Log._Debug("vehicleId: " + vehicleId + " ============================================");
}
#endif
}
public void CustomCalculateSegmentPosition(ushort vehicleId, ref Vehicle vehicleData, PathUnit.Position nextPosition,
PathUnit.Position position, uint laneID, byte offset, PathUnit.Position prevPos, uint prevLaneID,
byte prevOffset, int index, out Vector3 pos, out Vector3 dir, out float maxSpeed)
{
var netManager = Singleton <NetManager> .instance;
//var vehicleManager = Singleton<VehicleManager>.instance;
netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].CalculatePositionAndDirection(offset * 0.003921569f, out pos, out dir);
bool isRecklessDriver = IsRecklessDriver(vehicleId, ref vehicleData);
var lastFrameData = vehicleData.GetLastFrameData();
var lastFrameVehiclePos = lastFrameData.m_position;
var camPos = Camera.main.transform.position;
bool simulatePrioritySigns = (lastFrameVehiclePos - camPos).sqrMagnitude < FarLod && !isRecklessDriver;
if (Options.simAccuracy <= 0)
{
if (vehicleData.Info.m_vehicleType == VehicleInfo.VehicleType.Car)
{
VehiclePosition vehiclePos = TrafficPriority.GetVehiclePosition(vehicleId);
if (vehiclePos.Valid && simulatePrioritySigns) // TODO check if this should be !vehiclePos.Valid
{
try {
CustomVehicleAI.HandleVehicle(vehicleId, ref Singleton <VehicleManager> .instance.m_vehicles.m_buffer[vehicleId], false, false);
} catch (Exception e) {
Log.Error("CarAI TmCalculateSegmentPosition Error: " + e.ToString());
}
}
}
else
{
//Log._Debug($"TmCalculateSegmentPosition does not handle vehicles of type {vehicleData.Info.m_vehicleType}");
}
}
// I think this is supposed to be the lane position?
// [VN, 12/23/2015] It's the 3D car position on the Bezier curve of the lane.
// This crazy 0.003921569f equals to 1f/255 and prevOffset is the byte value (0..255) of the car position.
var vehiclePosOnBezier = netManager.m_lanes.m_buffer[(int)((UIntPtr)prevLaneID)].CalculatePosition(prevOffset * 0.003921569f);
//ushort currentSegmentId = netManager.m_lanes.m_buffer[(int)((UIntPtr)prevLaneID)].m_segment;
ushort destinationNodeId;
ushort sourceNodeId;
if (offset < position.m_offset)
{
destinationNodeId = netManager.m_segments.m_buffer[position.m_segment].m_startNode;
sourceNodeId = netManager.m_segments.m_buffer[position.m_segment].m_endNode;
}
else
{
destinationNodeId = netManager.m_segments.m_buffer[position.m_segment].m_endNode;
sourceNodeId = netManager.m_segments.m_buffer[position.m_segment].m_startNode;
}
var previousDestinationNode = prevOffset == 0 ? netManager.m_segments.m_buffer[prevPos.m_segment].m_startNode : netManager.m_segments.m_buffer[prevPos.m_segment].m_endNode;
// this seems to be like the required braking force in order to stop the vehicle within its half length.
var crazyValue = 0.5f * lastFrameData.m_velocity.sqrMagnitude / m_info.m_braking + m_info.m_generatedInfo.m_size.z * 0.5f;
// Essentially, this is true if the car has enough time and space to brake (e.g. for a red traffic light)
if (destinationNodeId == previousDestinationNode)
{
if (Vector3.Distance(lastFrameVehiclePos, vehiclePosOnBezier) >= crazyValue - 1f)
{
var currentFrameIndex = Singleton <SimulationManager> .instance.m_currentFrameIndex;
var num5 = (uint)((previousDestinationNode << 8) / 32768);
var num6 = currentFrameIndex - num5 & 255u;
var nodeFlags = netManager.m_nodes.m_buffer[destinationNodeId].m_flags;
var prevLaneFlags = (NetLane.Flags)netManager.m_lanes.m_buffer[(int)((UIntPtr)prevLaneID)].m_flags;
var hasTrafficLight = (nodeFlags & NetNode.Flags.TrafficLights) != NetNode.Flags.None;
var hasCrossing = (nodeFlags & NetNode.Flags.LevelCrossing) != NetNode.Flags.None;
var isJoinedJunction = (prevLaneFlags & NetLane.Flags.JoinedJunction) != NetLane.Flags.None;
bool checkSpace = !Flags.getEnterWhenBlockedAllowed(prevPos.m_segment, netManager.m_segments.m_buffer[prevPos.m_segment].m_startNode == destinationNodeId) && !isRecklessDriver;
//TrafficLightSimulation nodeSim = TrafficLightSimulation.GetNodeSimulation(destinationNodeId);
/*if (timedNode != null && timedNode.vehiclesMayEnterBlockedJunctions) {
* checkSpace = false;
* }*/
if (checkSpace)
{
// check if there is enough space
if ((nodeFlags & (NetNode.Flags.Junction | NetNode.Flags.OneWayOut | NetNode.Flags.OneWayIn)) == NetNode.Flags.Junction &&
netManager.m_nodes.m_buffer[destinationNodeId].CountSegments() != 2)
{
var len = vehicleData.CalculateTotalLength(vehicleId) + 2f;
if (!netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].CheckSpace(len))
{
var sufficientSpace = false;
if (nextPosition.m_segment != 0 &&
netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_length < 30f)
{
var flags3 = netManager.m_nodes.m_buffer[sourceNodeId].m_flags;
if ((flags3 &
(NetNode.Flags.Junction | NetNode.Flags.OneWayOut | NetNode.Flags.OneWayIn)) !=
NetNode.Flags.Junction || netManager.m_nodes.m_buffer[sourceNodeId].CountSegments() == 2)
{
var laneId2 = PathManager.GetLaneID(nextPosition);
if (laneId2 != 0u)
{
sufficientSpace = netManager.m_lanes.m_buffer[(int)((UIntPtr)laneId2)].CheckSpace(len);
}
}
}
if (!sufficientSpace)
{
maxSpeed = 0f;
return;
}
}
}
}
try {
VehiclePosition globalTargetPos = TrafficPriority.GetVehiclePosition(vehicleId);
if ((vehicleData.m_flags & Vehicle.Flags.Emergency2) == 0)
{
if (vehicleData.Info.m_vehicleType == VehicleInfo.VehicleType.Car)
{
if (hasTrafficLight && (!isJoinedJunction || hasCrossing))
{
var destinationInfo = netManager.m_nodes.m_buffer[destinationNodeId].Info;
if (globalTargetPos.CarState == VehicleJunctionTransitState.None)
{
globalTargetPos.CarState = VehicleJunctionTransitState.Enter;
}
RoadBaseAI.TrafficLightState vehicleLightState;
RoadBaseAI.TrafficLightState pedestrianLightState;
bool vehicles;
bool pedestrians;
CustomRoadAI.GetTrafficLightState(vehicleId, ref vehicleData, destinationNodeId, prevPos.m_segment, position.m_segment, ref netManager.m_segments.m_buffer[prevPos.m_segment], currentFrameIndex - num5, out vehicleLightState, out pedestrianLightState, out vehicles, out pedestrians);
if (isRecklessDriver && (destinationInfo.GetConnectionClass().m_service & ItemClass.Service.PublicTransport) == ItemClass.Service.None) // no reckless driving at railroad crossings
{
vehicleLightState = RoadBaseAI.TrafficLightState.Green;
}
if (!vehicles && num6 >= 196u)
{
vehicles = true;
RoadBaseAI.SetTrafficLightState(destinationNodeId, ref netManager.m_segments.m_buffer[prevPos.m_segment], currentFrameIndex - num5, vehicleLightState, pedestrianLightState, vehicles, pedestrians);
}
var stopCar = false;
switch (vehicleLightState)
{
case RoadBaseAI.TrafficLightState.RedToGreen:
if (num6 < 60u)
{
stopCar = true;
}
else
{
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
break;
case RoadBaseAI.TrafficLightState.Red:
stopCar = true;
break;
case RoadBaseAI.TrafficLightState.GreenToRed:
if (num6 >= 30u)
{
stopCar = true;
}
else
{
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
break;
}
if ((vehicleLightState == RoadBaseAI.TrafficLightState.Green || vehicleLightState == RoadBaseAI.TrafficLightState.RedToGreen) && !Flags.getEnterWhenBlockedAllowed(prevPos.m_segment, netManager.m_segments.m_buffer[prevPos.m_segment].m_startNode == destinationNodeId))
{
var hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, destinationNodeId);
if (hasIncomingCars)
{
// green light but other cars are incoming and they have priority: stop
stopCar = true;
}
}
if (stopCar)
{
globalTargetPos.CarState = VehicleJunctionTransitState.Stop;
maxSpeed = 0f;
return;
}
}
else if (simulatePrioritySigns)
{
#if DEBUG
//bool debug = destinationNodeId == 10864;
//bool debug = destinationNodeId == 13531;
bool debug = false;
#endif
//bool debug = false;
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId} is arriving @ seg. {prevPos.m_segment} ({position.m_segment}, {nextPosition.m_segment}), node {destinationNodeId} which is not a traffic light.");
}
#endif
var prioritySegment = TrafficPriority.GetPrioritySegment(destinationNodeId, prevPos.m_segment);
if (prioritySegment != null)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId} is arriving @ seg. {prevPos.m_segment} ({position.m_segment}, {nextPosition.m_segment}), node {destinationNodeId} which is not a traffic light and is a priority segment.");
}
#endif
if (prioritySegment.HasVehicle(vehicleId))
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: segment target position found");
}
#endif
if (globalTargetPos.Valid)
{
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: global target position found. carState = {globalTargetPos.CarState.ToString()}");
}
#endif
var currentFrameIndex2 = Singleton <SimulationManager> .instance.m_currentFrameIndex;
var frame = currentFrameIndex2 >> 4;
if (globalTargetPos.CarState == VehicleJunctionTransitState.None)
{
globalTargetPos.CarState = VehicleJunctionTransitState.Enter;
}
if (globalTargetPos.CarState != VehicleJunctionTransitState.Leave)
{
bool hasIncomingCars;
switch (prioritySegment.Type)
{
case SegmentEnd.PriorityType.Stop:
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: STOP sign. waittime={globalTargetPos.WaitTime}, vel={lastFrameData.m_velocity.magnitude}");
}
#endif
if (globalTargetPos.WaitTime < MaxPriorityWaitTime)
{
globalTargetPos.CarState = VehicleJunctionTransitState.Stop;
if (lastFrameData.m_velocity.magnitude < 0.5f ||
globalTargetPos.Stopped)
{
globalTargetPos.Stopped = true;
globalTargetPos.WaitTime++;
float minStopWaitTime = Random.Range(0f, 3f);
if (globalTargetPos.WaitTime >= minStopWaitTime)
{
hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, destinationNodeId);
#if DEBUG
if (debug)
{
Log._Debug($"hasIncomingCars: {hasIncomingCars}");
}
#endif
if (hasIncomingCars)
{
maxSpeed = 0f;
return;
}
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
else
{
maxSpeed = 0;
return;
}
}
else
{
maxSpeed = 0f;
return;
}
}
else
{
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
break;
case SegmentEnd.PriorityType.Yield:
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: YIELD sign. waittime={globalTargetPos.WaitTime}");
}
#endif
if (globalTargetPos.WaitTime < MaxPriorityWaitTime)
{
globalTargetPos.WaitTime++;
globalTargetPos.CarState = VehicleJunctionTransitState.Stop;
hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, destinationNodeId);
#if DEBUG
if (debug)
{
Log._Debug($"hasIncomingCars: {hasIncomingCars}");
}
#endif
if (hasIncomingCars)
{
if (lastFrameData.m_velocity.magnitude > 0)
{
maxSpeed = Math.Max(0f, lastFrameData.m_velocity.magnitude - globalTargetPos.ReduceSpeedByValueToYield);
}
else
{
maxSpeed = 0;
}
#if DEBUG
/*if (TrafficPriority.Vehicles[vehicleId].ToNode == 8621)
* Log.Message($"Vehicle {vehicleId} is yielding at node {destinationNodeId}. Speed: {maxSpeed}, Waiting time: {TrafficPriority.Vehicles[vehicleId].WaitTime}");*/
#endif
return;
}
else
{
#if DEBUG
/*if (TrafficPriority.Vehicles[vehicleId].ToNode == 8621)
* Log.Message($"Vehicle {vehicleId} is NOT yielding at node {destinationNodeId}.");*/
#endif
if (lastFrameData.m_velocity.magnitude > 0)
{
maxSpeed = Math.Max(1f, lastFrameData.m_velocity.magnitude - globalTargetPos.ReduceSpeedByValueToYield * 0.5f);
}
}
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
else
{
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
break;
case SegmentEnd.PriorityType.Main:
#if DEBUG
if (debug)
{
Log._Debug($"Vehicle {vehicleId}: MAIN sign. waittime={globalTargetPos.WaitTime}");
}
#endif
if (globalTargetPos.WaitTime < MaxPriorityWaitTime)
{
globalTargetPos.WaitTime++;
globalTargetPos.CarState = VehicleJunctionTransitState.Stop;
maxSpeed = 0f;
hasIncomingCars = TrafficPriority.HasIncomingVehiclesWithHigherPriority(vehicleId, destinationNodeId);
#if DEBUG
if (debug)
{
Log._Debug($"hasIncomingCars: {hasIncomingCars}");
}
#endif
if (hasIncomingCars)
{
globalTargetPos.Stopped = true;
return;
}
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
globalTargetPos.Stopped = false;
}
var info3 = netManager.m_segments.m_buffer[position.m_segment].Info;
if (info3.m_lanes != null && info3.m_lanes.Length > position.m_lane)
{
//maxSpeed = CalculateTargetSpeed(vehicleId, ref vehicleData, info3.m_lanes[position.m_lane].m_speedLimit, netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_curve) * 0.8f;
maxSpeed = CalculateTargetSpeed(vehicleId, ref vehicleData, SpeedLimitManager.GetLockFreeGameSpeedLimit(position.m_segment, position.m_lane, laneID, info3.m_lanes[position.m_lane]), netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_curve);
}
else
{
maxSpeed = CalculateTargetSpeed(vehicleId, ref vehicleData, 1f, 0f);
}
return;
}
}
else
{
globalTargetPos.CarState = VehicleJunctionTransitState.Leave;
}
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"globalTargetPos is null! {vehicleId} @ seg. {prevPos.m_segment} @ node {destinationNodeId}");
}
#endif
}
}
else
{
#if DEBUG
if (debug)
{
Log._Debug($"targetPos is null! {vehicleId} @ seg. {prevPos.m_segment} @ node {destinationNodeId}");
}
#endif
}
}
}
}
}
} catch (Exception e) {
Log.Error($"Error occured in TmCalculateSegmentPosition: {e.ToString()}");
}
}
}
var info2 = netManager.m_segments.m_buffer[position.m_segment].Info;
if (info2.m_lanes != null && info2.m_lanes.Length > position.m_lane)
{
var laneSpeedLimit = SpeedLimitManager.GetLockFreeGameSpeedLimit(position.m_segment, position.m_lane, laneID, info2.m_lanes[position.m_lane]); // info2.m_lanes[position.m_lane].m_speedLimit;
#if DEBUG
/*if (position.m_segment == 275) {
* Log._Debug($"Applying lane speed limit of {laneSpeedLimit} to lane {laneID} @ seg. {position.m_segment}");
* }*/
#endif
/*if (TrafficRoadRestrictions.IsSegment(position.m_segment)) {
* var restrictionSegment = TrafficRoadRestrictions.GetSegment(position.m_segment);
*
* if (restrictionSegment.SpeedLimits[position.m_lane] > 0.1f) {
* laneSpeedLimit = restrictionSegment.SpeedLimits[position.m_lane];
* }
* }*/
maxSpeed = CalculateTargetSpeed(vehicleId, ref vehicleData, laneSpeedLimit, netManager.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_curve);
}
else
{
maxSpeed = CalculateTargetSpeed(vehicleId, ref vehicleData, 1f, 0f);
}
maxSpeed = CalcMaxSpeed(vehicleId, ref vehicleData, position, pos, maxSpeed, isRecklessDriver);
}
public void CustomCheckNextLane(ushort vehicleID, ref Vehicle vehicleData, ref float maxSpeed, PathUnit.Position position, uint laneID, byte offset, PathUnit.Position prevPos, uint prevLaneID, byte prevOffset, Bezier3 bezier)
{
NetManager instance = Singleton <NetManager> .instance;
Vehicle.Frame lastFrameData = vehicleData.GetLastFrameData();
Vector3 a = lastFrameData.m_position;
Vector3 a2 = lastFrameData.m_position;
Vector3 b = lastFrameData.m_rotation * new Vector3(0f, 0f, this.m_info.m_generatedInfo.m_wheelBase * 0.5f);
a += b;
a2 -= b;
float num = 0.5f * lastFrameData.m_velocity.sqrMagnitude / this.m_info.m_braking;
float a3 = Vector3.Distance(a, bezier.a);
float b2 = Vector3.Distance(a2, bezier.a);
if (Mathf.Min(a3, b2) >= num - 5f)
{
if (!instance.m_lanes.m_buffer[(int)((UIntPtr)laneID)].CheckSpace(1000f, vehicleID))
{
maxSpeed = 0f;
return;
}
Vector3 vector = bezier.Position(0.5f);
Segment3 segment;
if (Vector3.SqrMagnitude(vehicleData.m_segment.a - vector) < Vector3.SqrMagnitude(bezier.a - vector))
{
segment = new Segment3(vehicleData.m_segment.a, vector);
}
else
{
segment = new Segment3(bezier.a, vector);
}
if (segment.LengthSqr() >= 3f)
{
segment.a += (segment.b - segment.a).normalized * 2.5f;
if (CustomTrainAI.CheckOverlap(vehicleID, ref vehicleData, segment, vehicleID))
{
maxSpeed = 0f;
return;
}
}
segment = new Segment3(vector, bezier.d);
if (segment.LengthSqr() >= 1f && CustomTrainAI.CheckOverlap(vehicleID, ref vehicleData, segment, vehicleID))
{
maxSpeed = 0f;
return;
}
ushort num2;
if (offset < position.m_offset)
{
num2 = instance.m_segments.m_buffer[(int)position.m_segment].m_startNode;
}
else
{
num2 = instance.m_segments.m_buffer[(int)position.m_segment].m_endNode;
}
ushort num3;
if (prevOffset == 0)
{
num3 = instance.m_segments.m_buffer[(int)prevPos.m_segment].m_startNode;
}
else
{
num3 = instance.m_segments.m_buffer[(int)prevPos.m_segment].m_endNode;
}
if (num2 == num3)
{
NetNode.Flags flags = instance.m_nodes.m_buffer[(int)num2].m_flags;
if ((flags & NetNode.Flags.TrafficLights) != NetNode.Flags.None)
{
uint currentFrameIndex = Singleton <SimulationManager> .instance.m_currentFrameIndex;
uint num4 = (uint)(((int)num3 << 8) / 32768);
uint num5 = currentFrameIndex - num4 & 255u;
RoadBaseAI.TrafficLightState vehicleLightState;
RoadBaseAI.TrafficLightState pedestrianLightState;
bool flag;
bool pedestrians;
/// NON-STOCK CODE START ///
CustomRoadAI.GetTrafficLightState(vehicleID, ref vehicleData, num3, prevPos.m_segment, position.m_segment, ref instance.m_segments.m_buffer[(int)prevPos.m_segment], currentFrameIndex - num4, out vehicleLightState, out pedestrianLightState, out flag, out pedestrians);
/// NON-STOCK CODE END ///
//RoadBaseAI.GetTrafficLightState(num3, ref instance.m_segments.m_buffer[(int)prevPos.m_segment], currentFrameIndex - num4, out vehicleLightState, out pedestrianLightState, out flag, out pedestrians);
if (!flag && num5 >= 196u)
{
flag = true;
RoadBaseAI.SetTrafficLightState(num3, ref instance.m_segments.m_buffer[(int)prevPos.m_segment], currentFrameIndex - num4, vehicleLightState, pedestrianLightState, flag, pedestrians);
}
switch (vehicleLightState)
{
case RoadBaseAI.TrafficLightState.RedToGreen:
if (num5 < 60u)
{
maxSpeed = 0f;
return;
}
break;
case RoadBaseAI.TrafficLightState.Red:
maxSpeed = 0f;
return;
case RoadBaseAI.TrafficLightState.GreenToRed:
if (num5 >= 30u)
{
maxSpeed = 0f;
return;
}
break;
}
}
}
}
}
private static void GetCustomTrafficLightState(ushort vehicleId, ref Vehicle vehicleData, ushort nodeId, ushort fromSegmentId, ushort toSegmentId, out RoadBaseAI.TrafficLightState vehicleLightState, out RoadBaseAI.TrafficLightState pedestrianLightState, TrafficLightSimulation nodeSim = null)
{
if (nodeSim == null)
{
nodeSim = TrafficLightSimulation.GetNodeSimulation(nodeId);
if (nodeSim == null)
{
Log.Error($"GetCustomTrafficLightState: node traffic light simulation not found at node {nodeId}! Vehicle {vehicleId} comes from segment {fromSegmentId} and goes to node {nodeId}");
throw new ApplicationException($"GetCustomTrafficLightState: node traffic light simulation not found at node {nodeId}! Vehicle {vehicleId} comes from segment {fromSegmentId} and goes to node {nodeId}");
}
}
// get vehicle position
/*VehiclePosition vehiclePos = TrafficPriority.GetVehiclePosition(vehicleId);
* if (!vehiclePos.Valid || vehiclePos.FromSegment != fromSegmentId || vehiclePos.ToNode != nodeId) {
* Log._Debug($"GetTrafficLightState: Recalculating position for vehicle {vehicleId}! FromSegment={vehiclePos.FromSegment} Valid={vehiclePos.Valid}");
* try {
* HandleVehicle(vehicleId, ref Singleton<VehicleManager>.instance.m_vehicles.m_buffer[vehicleId], false, false);
* } catch (Exception e) {
* Log.Error("VehicleAI GetTrafficLightState Error: " + e.ToString());
* }
* }
*
* if (!vehiclePos.Valid || vehiclePos.FromSegment != fromSegmentId || vehiclePos.ToNode != nodeId) {
* Log.Warning($"GetTrafficLightState: Vehicle {vehicleId} is not moving at segment {fromSegmentId} to node {nodeId}! FromSegment={vehiclePos.FromSegment} ToNode={vehiclePos.ToNode} Valid={vehiclePos.Valid}");
* vehicleLightState = RoadBaseAI.TrafficLightState.Red;
* pedestrianLightState = RoadBaseAI.TrafficLightState.Red;
* return;
* }*/
// get vehicle type
ExtVehicleType?vehicleType = CustomVehicleAI.DetermineVehicleTypeFromVehicle(vehicleId, ref vehicleData);
if (vehicleData.Info.m_vehicleType == VehicleInfo.VehicleType.Tram && vehicleType != ExtVehicleType.Tram)
{
Log.Warning($"vehicleType={vehicleType} ({(int)vehicleType}) for Tram");
}
//Log._Debug($"GetCustomTrafficLightState: Vehicle {vehicleId} is a {vehicleType}");
if (vehicleType == null)
{
Log.Warning($"GetTrafficLightState: Could not determine vehicle type of vehicle {vehicleId}!");
vehicleLightState = RoadBaseAI.TrafficLightState.Red;
pedestrianLightState = RoadBaseAI.TrafficLightState.Red;
return;
}
// get responsible traffic light
CustomSegmentLights lights = CustomTrafficLights.GetSegmentLights(nodeId, fromSegmentId);
CustomSegmentLight light = lights == null ? null : lights.GetCustomLight((ExtVehicleType)vehicleType);
if (lights == null || light == null)
{
Log.Warning($"GetTrafficLightState: No custom light for vehicleType {vehicleType} @ node {nodeId}, segment {fromSegmentId} found. lights null? {lights == null} light null? {light == null}");
vehicleLightState = RoadBaseAI.TrafficLightState.Red;
pedestrianLightState = RoadBaseAI.TrafficLightState.Red;
return;
}
SegmentGeometry geometry = CustomRoadAI.GetSegmentGeometry(fromSegmentId);
// get traffic light state from responsible traffic light
if (geometry.IsLeftSegment(toSegmentId, nodeId))
{
vehicleLightState = light.GetLightLeft();
}
else if (geometry.IsRightSegment(toSegmentId, nodeId))
{
vehicleLightState = light.GetLightRight();
}
else
{
vehicleLightState = light.GetLightMain();
}
// get traffic lights state for pedestrians
pedestrianLightState = (lights.PedestrianLightState != null) ? (RoadBaseAI.TrafficLightState)lights.PedestrianLightState : RoadBaseAI.TrafficLightState.Red;
}
public void CustomSegmentSimulationStep(ushort segmentID, ref NetSegment data)
{
if (initDone)
{
CustomRoadAI.GetSegmentGeometry(segmentID).VerifySegmentsByCount();
try {
TrafficPriority.segmentHousekeeping(segmentID);
} catch (Exception e) {
Log.Error($"Error occured while housekeeping segment {segmentID}: " + e.ToString());
}
if (!Options.isStockLaneChangerUsed())
{
try {
InStartupPhase = simStartFrame == 0 || simStartFrame >> 14 >= Singleton <SimulationManager> .instance.m_currentFrameIndex >> 14; // approx. 3 minutes
// calculate traffic density
uint curLaneId = data.m_lanes;
int nextNumLanes = data.Info.m_lanes.Length;
uint laneIndex = 0;
bool resetDensity = false;
uint maxDensity = 0u;
uint densitySum = 0u;
while (laneIndex < nextNumLanes && curLaneId != 0u)
{
uint currentDensity = currentLaneDensities[curLaneId];
if (maxDensity == 0 || currentDensity > maxDensity)
{
maxDensity = currentDensity;
}
densitySum += currentDensity;
laneIndex++;
curLaneId = Singleton <NetManager> .instance.m_lanes.m_buffer[curLaneId].m_nextLane;
}
if (maxDensity > 250)
{
resetDensity = true;
}
curLaneId = data.m_lanes;
laneIndex = 0;
while (laneIndex < nextNumLanes && curLaneId != 0u)
{
uint buf = currentLaneTrafficBuffer[curLaneId];
uint currentDensity = currentLaneDensities[curLaneId];
//currentMeanDensity = (byte)Math.Min(100u, (uint)((currentDensities * 100u) / Math.Max(1u, maxDens))); // 0 .. 100
byte currentMeanSpeed = 25;
// we use integer division here because it's faster
if (buf > 0)
{
uint currentSpeeds = currentLaneSpeeds[curLaneId];
if (!InStartupPhase)
{
currentMeanSpeed = (byte)Math.Min(100u, ((currentSpeeds * 100u) / buf) / ((uint)(Math.Max(SpeedLimitManager.GetLockFreeGameSpeedLimit(segmentID, laneIndex, curLaneId, data.Info.m_lanes[laneIndex]) * 8f, 1f)))); // 0 .. 100, m_speedLimit of highway is 2, actual max. vehicle speed on highway is 16, that's why we use x*8 == x<<3 (don't ask why CO uses different units for velocity)
}
}
else
{
if (!InStartupPhase)
{
currentMeanSpeed = 100;
}
}
/*if (segmentID == 22980) {
* Log._Debug($"Lane {curLaneId}: currentMeanSpeed={currentMeanSpeed} currentMeanDensity={currentMeanDensity}");
* }*/
if (currentMeanSpeed >= laneMeanSpeeds[curLaneId])
{
laneMeanSpeeds[curLaneId] = (byte)Math.Min((int)laneMeanSpeeds[curLaneId] + 10, currentMeanSpeed);
}
else
{
laneMeanSpeeds[curLaneId] = (byte)Math.Max((int)laneMeanSpeeds[curLaneId] - 10, 0);
}
if (densitySum > 0)
{
laneMeanDensities[curLaneId] = (byte)Math.Min(100u, (currentDensity * 100u) / densitySum);
}
else
{
laneMeanDensities[curLaneId] = (byte)0;
}
currentLaneTrafficBuffer[curLaneId] = 0;
currentLaneSpeeds[curLaneId] = 0;
if (resetDensity)
{
currentLaneDensities[curLaneId] /= 10u;
}
laneIndex++;
curLaneId = Singleton <NetManager> .instance.m_lanes.m_buffer[curLaneId].m_nextLane;
}
} catch (Exception e) {
Log.Error("Error occured while calculating lane traffic density: " + e.ToString());
}
}
}
try {
OriginalSimulationStep(segmentID, ref data);
} catch (Exception ex) {
Log.Error("Error in CustomRoadAI.SimulationStep: " + ex.ToString());
}
}
public void CustomCalculateSegmentPosition(ushort vehicleID, ref Vehicle vehicleData, PathUnit.Position nextPosition, PathUnit.Position position, uint laneID, byte offset, PathUnit.Position prevPos, uint prevLaneID, byte prevOffset, int index, out Vector3 pos, out Vector3 dir, out float maxSpeed)
{
NetManager instance = Singleton <NetManager> .instance;
instance.m_lanes.m_buffer[(int)((UIntPtr)laneID)].CalculatePositionAndDirection((float)offset * 0.003921569f, out pos, out dir);
Vector3 b = instance.m_lanes.m_buffer[(int)((UIntPtr)prevLaneID)].CalculatePosition((float)prevOffset * 0.003921569f);
Vehicle.Frame lastFrameData = vehicleData.GetLastFrameData();
Vector3 a = lastFrameData.m_position;
Vector3 a2 = lastFrameData.m_position;
Vector3 b2 = lastFrameData.m_rotation * new Vector3(0f, 0f, this.m_info.m_generatedInfo.m_wheelBase * 0.5f);
a += b2;
a2 -= b2;
float num = 0.5f * lastFrameData.m_velocity.sqrMagnitude / this.m_info.m_braking;
float a3 = Vector3.Distance(a, b);
float b3 = Vector3.Distance(a2, b);
if (Mathf.Min(a3, b3) >= num - 1f)
{
Segment3 segment;
segment.a = pos;
ushort num2;
ushort num3;
if (offset < position.m_offset)
{
segment.b = pos + dir.normalized * this.m_info.m_generatedInfo.m_size.z;
num2 = instance.m_segments.m_buffer[(int)position.m_segment].m_startNode;
num3 = instance.m_segments.m_buffer[(int)position.m_segment].m_endNode;
}
else
{
segment.b = pos - dir.normalized * this.m_info.m_generatedInfo.m_size.z;
num2 = instance.m_segments.m_buffer[(int)position.m_segment].m_endNode;
num3 = instance.m_segments.m_buffer[(int)position.m_segment].m_startNode;
}
ushort num4;
if (prevOffset == 0)
{
num4 = instance.m_segments.m_buffer[(int)prevPos.m_segment].m_startNode;
}
else
{
num4 = instance.m_segments.m_buffer[(int)prevPos.m_segment].m_endNode;
}
if (num2 == num4)
{
NetNode.Flags flags = instance.m_nodes.m_buffer[(int)num2].m_flags;
NetLane.Flags flags2 = (NetLane.Flags)instance.m_lanes.m_buffer[(int)((UIntPtr)prevLaneID)].m_flags;
bool flag = (flags & NetNode.Flags.TrafficLights) != NetNode.Flags.None;
bool flag2 = (flags & NetNode.Flags.LevelCrossing) != NetNode.Flags.None;
bool flag3 = (flags2 & NetLane.Flags.JoinedJunction) != NetLane.Flags.None;
bool checkSpace = !Options.allowEnterBlockedJunctions; // NON-STOCK CODE
if (checkSpace && (flags & (NetNode.Flags.Junction | NetNode.Flags.OneWayOut | NetNode.Flags.OneWayIn)) == NetNode.Flags.Junction && instance.m_nodes.m_buffer[(int)num2].CountSegments() != 2)
{
float len = vehicleData.CalculateTotalLength(vehicleID) + 2f;
if (!instance.m_lanes.m_buffer[(int)((UIntPtr)laneID)].CheckSpace(len))
{
bool flag4 = false;
if (nextPosition.m_segment != 0 && instance.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_length < 30f)
{
NetNode.Flags flags3 = instance.m_nodes.m_buffer[(int)num3].m_flags;
if ((flags3 & (NetNode.Flags.Junction | NetNode.Flags.OneWayOut | NetNode.Flags.OneWayIn)) != NetNode.Flags.Junction || instance.m_nodes.m_buffer[(int)num3].CountSegments() == 2)
{
uint laneID2 = PathManager.GetLaneID(nextPosition);
if (laneID2 != 0u)
{
flag4 = instance.m_lanes.m_buffer[(int)((UIntPtr)laneID2)].CheckSpace(len);
}
}
}
if (!flag4)
{
maxSpeed = 0f;
return;
}
}
}
if (flag && (!flag3 || flag2))
{
uint currentFrameIndex = Singleton <SimulationManager> .instance.m_currentFrameIndex;
uint num5 = (uint)(((int)num4 << 8) / 32768);
uint num6 = currentFrameIndex - num5 & 255u;
RoadBaseAI.TrafficLightState vehicleLightState;
RoadBaseAI.TrafficLightState pedestrianLightState;
bool flag5;
bool pedestrians;
/// NON-STOCK CODE START ///
CustomRoadAI.GetTrafficLightState(vehicleID, ref vehicleData, num4, prevPos.m_segment, position.m_segment, ref instance.m_segments.m_buffer[(int)prevPos.m_segment], currentFrameIndex - num5, out vehicleLightState, out pedestrianLightState, out flag5, out pedestrians);
/// NON-STOCK CODE END ///
//RoadBaseAI.GetTrafficLightState(num4, ref instance.m_segments.m_buffer[(int)prevPos.m_segment], currentFrameIndex - num5, out vehicleLightState, out pedestrianLightState, out flag5, out pedestrians);
if (!flag5 && num6 >= 196u)
{
flag5 = true;
RoadBaseAI.SetTrafficLightState(num4, ref instance.m_segments.m_buffer[(int)prevPos.m_segment], currentFrameIndex - num5, vehicleLightState, pedestrianLightState, flag5, pedestrians);
}
switch (vehicleLightState)
{
case RoadBaseAI.TrafficLightState.RedToGreen:
if (num6 < 60u)
{
maxSpeed = 0f;
return;
}
break;
case RoadBaseAI.TrafficLightState.Red:
maxSpeed = 0f;
return;
case RoadBaseAI.TrafficLightState.GreenToRed:
if (num6 >= 30u)
{
maxSpeed = 0f;
return;
}
break;
}
}
}
}
NetInfo info = instance.m_segments.m_buffer[(int)position.m_segment].Info;
if (info.m_lanes != null && info.m_lanes.Length > (int)position.m_lane)
{
//maxSpeed = this.CalculateTargetSpeed(vehicleID, ref vehicleData, info.m_lanes[(int)position.m_lane].m_speedLimit, instance.m_lanes.m_buffer[(int)((UIntPtr)laneID)].m_curve);
maxSpeed = CalculateTargetSpeed(vehicleID, ref vehicleData, SpeedLimitManager.GetLockFreeGameSpeedLimit(position.m_segment, position.m_lane, laneID, info.m_lanes[position.m_lane]), instance.m_lanes.m_buffer[laneID].m_curve);
}
else
{
maxSpeed = this.CalculateTargetSpeed(vehicleID, ref vehicleData, 1f, 0f);
}
}