public void CustomSimulationStep(ushort vehicleID, ref Vehicle vehicleData, ref Vehicle.Frame frameData, ushort leaderID, ref Vehicle leaderData, int lodPhysics)
{
#if DEBUG
bool debug = GlobalConfig.Instance.Debug.Switches[16] && GlobalConfig.Instance.Debug.NodeId == vehicleID;
#endif
ushort leadingVehicle = vehicleData.m_leadingVehicle;
uint currentFrameIndex = Singleton <SimulationManager> .instance.m_currentFrameIndex;
VehicleInfo leaderInfo;
if (leaderID != vehicleID)
{
leaderInfo = leaderData.Info;
}
else
{
leaderInfo = this.m_info;
}
TramBaseAI tramBaseAI = leaderInfo.m_vehicleAI as TramBaseAI;
if (leadingVehicle != 0)
{
frameData.m_position += frameData.m_velocity * 0.4f;
}
else
{
frameData.m_position += frameData.m_velocity * 0.5f;
}
frameData.m_swayPosition += frameData.m_swayVelocity * 0.5f;
Vector3 wheelBaseRot = frameData.m_rotation * new Vector3(0f, 0f, this.m_info.m_generatedInfo.m_wheelBase * 0.5f);
Vector3 posAfterWheelRot = frameData.m_position + wheelBaseRot;
Vector3 posBeforeWheelRot = frameData.m_position - wheelBaseRot;
float acceleration = this.m_info.m_acceleration;
float braking = this.m_info.m_braking;
float curSpeed = frameData.m_velocity.magnitude;
Vector3 afterRotToTargetPos1Diff = (Vector3)vehicleData.m_targetPos1 - posAfterWheelRot;
float afterRotToTargetPos1DiffSqrMag = afterRotToTargetPos1Diff.sqrMagnitude;
Quaternion curInvRot = Quaternion.Inverse(frameData.m_rotation);
Vector3 curveTangent = curInvRot * frameData.m_velocity;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): ================================================");
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): leadingVehicle={leadingVehicle} frameData.m_position={frameData.m_position} frameData.m_swayPosition={frameData.m_swayPosition} wheelBaseRot={wheelBaseRot} posAfterWheelRot={posAfterWheelRot} posBeforeWheelRot={posBeforeWheelRot} acceleration={acceleration} braking={braking} curSpeed={curSpeed} afterRotToTargetPos1Diff={afterRotToTargetPos1Diff} afterRotToTargetPos1DiffSqrMag={afterRotToTargetPos1DiffSqrMag} curInvRot={curInvRot} curveTangent={curveTangent} this.m_info.m_generatedInfo.m_wheelBase={this.m_info.m_generatedInfo.m_wheelBase}");
}
#endif
Vector3 forward = Vector3.forward;
Vector3 targetMotion = Vector3.zero;
float targetSpeed = 0f;
float motionFactor = 0.5f;
float turnAngle = 0f;
if (leadingVehicle != 0)
{
VehicleManager vehMan = Singleton <VehicleManager> .instance;
Vehicle.Frame leadingVehLastFrameData = vehMan.m_vehicles.m_buffer[(int)leadingVehicle].GetLastFrameData();
VehicleInfo leadingVehInfo = vehMan.m_vehicles.m_buffer[(int)leadingVehicle].Info;
float attachOffset;
if ((vehicleData.m_flags & Vehicle.Flags.Inverted) != (Vehicle.Flags) 0)
{
attachOffset = this.m_info.m_attachOffsetBack - this.m_info.m_generatedInfo.m_size.z * 0.5f;
}
else
{
attachOffset = this.m_info.m_attachOffsetFront - this.m_info.m_generatedInfo.m_size.z * 0.5f;
}
float leadingAttachOffset;
if ((vehMan.m_vehicles.m_buffer[(int)leadingVehicle].m_flags & Vehicle.Flags.Inverted) != (Vehicle.Flags) 0)
{
leadingAttachOffset = leadingVehInfo.m_attachOffsetFront - leadingVehInfo.m_generatedInfo.m_size.z * 0.5f;
}
else
{
leadingAttachOffset = leadingVehInfo.m_attachOffsetBack - leadingVehInfo.m_generatedInfo.m_size.z * 0.5f;
}
Vector3 curPosMinusRotAttachOffset = frameData.m_position - frameData.m_rotation * new Vector3(0f, 0f, attachOffset);
Vector3 leadingPosPlusRotAttachOffset = leadingVehLastFrameData.m_position + leadingVehLastFrameData.m_rotation * new Vector3(0f, 0f, leadingAttachOffset);
wheelBaseRot = leadingVehLastFrameData.m_rotation * new Vector3(0f, 0f, leadingVehInfo.m_generatedInfo.m_wheelBase * 0.5f);
Vector3 leadingPosBeforeWheelRot = leadingVehLastFrameData.m_position - wheelBaseRot;
if (Vector3.Dot(vehicleData.m_targetPos1 - vehicleData.m_targetPos0, (Vector3)vehicleData.m_targetPos0 - posBeforeWheelRot) < 0f && vehicleData.m_path != 0u && (leaderData.m_flags & Vehicle.Flags.WaitingPath) == (Vehicle.Flags) 0)
{
int someIndex = -1;
InvokeUpdatePathTargetPositions(tramBaseAI, vehicleID, ref vehicleData, vehicleData.m_targetPos0, posBeforeWheelRot, 0, ref leaderData, ref someIndex, 0, 0, Vector3.SqrMagnitude(posBeforeWheelRot - (Vector3)vehicleData.m_targetPos0) + 1f, 1f);
afterRotToTargetPos1DiffSqrMag = 0f;
}
float attachRotDist = Mathf.Max(Vector3.Distance(curPosMinusRotAttachOffset, leadingPosPlusRotAttachOffset), 2f);
float one = 1f;
float attachRotSqrDist = attachRotDist * attachRotDist;
float oneSqr = one * one;
int i = 0;
if (afterRotToTargetPos1DiffSqrMag < attachRotSqrDist)
{
if (vehicleData.m_path != 0u && (leaderData.m_flags & Vehicle.Flags.WaitingPath) == (Vehicle.Flags) 0)
{
InvokeUpdatePathTargetPositions(tramBaseAI, vehicleID, ref vehicleData, posBeforeWheelRot, posAfterWheelRot, 0, ref leaderData, ref i, 1, 2, attachRotSqrDist, oneSqr);
}
while (i < 4)
{
vehicleData.SetTargetPos(i, vehicleData.GetTargetPos(i - 1));
i++;
}
afterRotToTargetPos1Diff = (Vector3)vehicleData.m_targetPos1 - posAfterWheelRot;
afterRotToTargetPos1DiffSqrMag = afterRotToTargetPos1Diff.sqrMagnitude;
}
afterRotToTargetPos1Diff = curInvRot * afterRotToTargetPos1Diff;
float negTotalAttachLen = -((this.m_info.m_generatedInfo.m_wheelBase + leadingVehInfo.m_generatedInfo.m_wheelBase) * 0.5f + attachOffset + leadingAttachOffset);
bool hasPath = false;
if (vehicleData.m_path != 0u && (leaderData.m_flags & Vehicle.Flags.WaitingPath) == (Vehicle.Flags) 0)
{
float u1;
float u2;
if (Line3.Intersect(posAfterWheelRot, vehicleData.m_targetPos1, leadingPosBeforeWheelRot, negTotalAttachLen, out u1, out u2))
{
targetMotion = afterRotToTargetPos1Diff * Mathf.Clamp(Mathf.Min(u1, u2) / 0.6f, 0f, 2f);
}
else
{
Line3.DistanceSqr(posAfterWheelRot, vehicleData.m_targetPos1, leadingPosBeforeWheelRot, out u1);
targetMotion = afterRotToTargetPos1Diff * Mathf.Clamp(u1 / 0.6f, 0f, 2f);
}
hasPath = true;
}
if (hasPath)
{
if (Vector3.Dot(leadingPosBeforeWheelRot - posAfterWheelRot, posAfterWheelRot - posBeforeWheelRot) < 0f)
{
motionFactor = 0f;
}
}
else
{
float leadingPosBeforeToAfterWheelRotDist = Vector3.Distance(leadingPosBeforeWheelRot, posAfterWheelRot);
motionFactor = 0f;
targetMotion = curInvRot * ((leadingPosBeforeWheelRot - posAfterWheelRot) * (Mathf.Max(0f, leadingPosBeforeToAfterWheelRotDist - negTotalAttachLen) / Mathf.Max(1f, leadingPosBeforeToAfterWheelRotDist * 0.6f)));
}
}
else
{
float estimatedFrameDist = (curSpeed + acceleration) * (0.5f + 0.5f * (curSpeed + acceleration) / braking) + (this.m_info.m_generatedInfo.m_size.z - this.m_info.m_generatedInfo.m_wheelBase) * 0.5f;
float maxSpeedAdd = Mathf.Max(curSpeed + acceleration, 2f);
float meanSpeedAdd = Mathf.Max((estimatedFrameDist - maxSpeedAdd) / 2f, 2f);
float maxSpeedAddSqr = maxSpeedAdd * maxSpeedAdd;
float meanSpeedAddSqr = meanSpeedAdd * meanSpeedAdd;
if (Vector3.Dot(vehicleData.m_targetPos1 - vehicleData.m_targetPos0, (Vector3)vehicleData.m_targetPos0 - posBeforeWheelRot) < 0f && vehicleData.m_path != 0u && (leaderData.m_flags & (Vehicle.Flags.WaitingPath | Vehicle.Flags.Stopped)) == (Vehicle.Flags) 0)
{
int someIndex = -1;
InvokeUpdatePathTargetPositions(tramBaseAI, vehicleID, ref vehicleData, vehicleData.m_targetPos0, posBeforeWheelRot, leaderID, ref leaderData, ref someIndex, 0, 0, Vector3.SqrMagnitude(posBeforeWheelRot - (Vector3)vehicleData.m_targetPos0) + 1f, 1f);
afterRotToTargetPos1DiffSqrMag = 0f;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): dot < 0");
}
#endif
}
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Leading vehicle is 0. vehicleData.m_targetPos0={vehicleData.m_targetPos0} vehicleData.m_targetPos1={vehicleData.m_targetPos1} posBeforeWheelRot={posBeforeWheelRot} posBeforeWheelRot={posAfterWheelRot} estimatedFrameDist={estimatedFrameDist} maxSpeedAdd={maxSpeedAdd} meanSpeedAdd={meanSpeedAdd} maxSpeedAddSqr={maxSpeedAddSqr} meanSpeedAddSqr={meanSpeedAddSqr} afterRotToTargetPos1DiffSqrMag={afterRotToTargetPos1DiffSqrMag}");
}
#endif
int posIndex = 0;
bool hasValidPathTargetPos = false;
if ((afterRotToTargetPos1DiffSqrMag < maxSpeedAddSqr || vehicleData.m_targetPos3.w < 0.01f) && (leaderData.m_flags & (Vehicle.Flags.WaitingPath | Vehicle.Flags.Stopped)) == (Vehicle.Flags) 0)
{
if (vehicleData.m_path != 0u)
{
InvokeUpdatePathTargetPositions(tramBaseAI, vehicleID, ref vehicleData, posBeforeWheelRot, posAfterWheelRot, leaderID, ref leaderData, ref posIndex, 1, 4, maxSpeedAddSqr, meanSpeedAddSqr);
}
if (posIndex < 4)
{
hasValidPathTargetPos = true;
while (posIndex < 4)
{
vehicleData.SetTargetPos(posIndex, vehicleData.GetTargetPos(posIndex - 1));
posIndex++;
}
}
afterRotToTargetPos1Diff = (Vector3)vehicleData.m_targetPos1 - posAfterWheelRot;
afterRotToTargetPos1DiffSqrMag = afterRotToTargetPos1Diff.sqrMagnitude;
}
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): posIndex={posIndex} hasValidPathTargetPos={hasValidPathTargetPos}");
}
#endif
if (leaderData.m_path != 0u && (leaderData.m_flags & Vehicle.Flags.WaitingPath) == (Vehicle.Flags) 0)
{
NetManager netMan = Singleton <NetManager> .instance;
byte leaderPathPosIndex = leaderData.m_pathPositionIndex;
byte leaderLastPathOffset = leaderData.m_lastPathOffset;
if (leaderPathPosIndex == 255)
{
leaderPathPosIndex = 0;
}
float leaderLen = 1f + leaderData.CalculateTotalLength(leaderID);
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): leaderPathPosIndex={leaderPathPosIndex} leaderLastPathOffset={leaderLastPathOffset} leaderPathPosIndex={leaderPathPosIndex} leaderLen={leaderLen}");
}
#endif
// reserve space / add traffic
PathManager pathMan = Singleton <PathManager> .instance;
PathUnit.Position pathPos;
if (pathMan.m_pathUnits.m_buffer[leaderData.m_path].GetPosition(leaderPathPosIndex >> 1, out pathPos))
{
netMan.m_segments.m_buffer[(int)pathPos.m_segment].AddTraffic(Mathf.RoundToInt(leaderLen * 2.5f));
bool reservedSpaceOnCurrentLane = false;
if ((leaderPathPosIndex & 1) == 0 || leaderLastPathOffset == 0)
{
uint laneId = PathManager.GetLaneID(pathPos);
if (laneId != 0u)
{
Vector3 curPathOffsetPos = netMan.m_lanes.m_buffer[laneId].CalculatePosition((float)pathPos.m_offset * 0.003921569f);
float speedAdd = 0.5f * curSpeed * curSpeed / this.m_info.m_braking;
float afterWheelRotCurPathOffsetDist = Vector3.Distance(posAfterWheelRot, curPathOffsetPos);
float beforeWheelRotCurPathOffsetDist = Vector3.Distance(posBeforeWheelRot, curPathOffsetPos);
if (Mathf.Min(afterWheelRotCurPathOffsetDist, beforeWheelRotCurPathOffsetDist) >= speedAdd - 1f)
{
netMan.m_lanes.m_buffer[laneId].ReserveSpace(leaderLen);
reservedSpaceOnCurrentLane = true;
}
}
}
if (!reservedSpaceOnCurrentLane && pathMan.m_pathUnits.m_buffer[leaderData.m_path].GetNextPosition(leaderPathPosIndex >> 1, out pathPos))
{
uint nextLaneId = PathManager.GetLaneID(pathPos);
if (nextLaneId != 0u)
{
netMan.m_lanes.m_buffer[nextLaneId].ReserveSpace(leaderLen);
}
}
}
if ((ulong)(currentFrameIndex >> 4 & 15u) == (ulong)((long)(leaderID & 15)))
{
// check if vehicle can proceed to next path position
bool canProceeed = false;
uint curLeaderPathId = leaderData.m_path;
int curLeaderPathPosIndex = leaderPathPosIndex >> 1;
int k = 0;
while (k < 5)
{
bool invalidPos;
if (PathUnit.GetNextPosition(ref curLeaderPathId, ref curLeaderPathPosIndex, out pathPos, out invalidPos))
{
uint laneId = PathManager.GetLaneID(pathPos);
if (laneId != 0u && !netMan.m_lanes.m_buffer[laneId].CheckSpace(leaderLen))
{
k++;
continue;
}
}
if (invalidPos)
{
this.InvalidPath(vehicleID, ref vehicleData, leaderID, ref leaderData);
}
canProceeed = true;
break;
}
if (!canProceeed)
{
leaderData.m_flags |= Vehicle.Flags.Congestion;
}
}
}
float maxSpeed;
if ((leaderData.m_flags & Vehicle.Flags.Stopped) != (Vehicle.Flags) 0)
{
maxSpeed = 0f;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Vehicle is stopped. maxSpeed={maxSpeed}");
}
#endif
}
else
{
maxSpeed = Mathf.Min(vehicleData.m_targetPos1.w, GetMaxSpeed(leaderID, ref leaderData));
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Vehicle is not stopped. maxSpeed={maxSpeed}");
}
#endif
}
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Start of second part. curSpeed={curSpeed} curInvRot={curInvRot}");
}
#endif
afterRotToTargetPos1Diff = curInvRot * afterRotToTargetPos1Diff;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): afterRotToTargetPos1Diff={afterRotToTargetPos1Diff} (old afterRotToTargetPos1DiffSqrMag={afterRotToTargetPos1DiffSqrMag})");
}
#endif
Vector3 zero = Vector3.zero;
bool blocked = false;
float forwardLen = 0f;
if (afterRotToTargetPos1DiffSqrMag > 1f) // TODO why is this not recalculated?
{
forward = VectorUtils.NormalizeXZ(afterRotToTargetPos1Diff, out forwardLen);
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): afterRotToTargetPos1DiffSqrMag > 1f. forward={forward} forwardLen={forwardLen}");
}
#endif
if (forwardLen > 1f)
{
Vector3 fwd = afterRotToTargetPos1Diff;
maxSpeedAdd = Mathf.Max(curSpeed, 2f);
maxSpeedAddSqr = maxSpeedAdd * maxSpeedAdd;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): forwardLen > 1f. fwd={fwd} maxSpeedAdd={maxSpeedAdd} maxSpeedAddSqr={maxSpeedAddSqr}");
}
#endif
if (afterRotToTargetPos1DiffSqrMag > maxSpeedAddSqr)
{
float fwdLimiter = maxSpeedAdd / Mathf.Sqrt(afterRotToTargetPos1DiffSqrMag);
fwd.x *= fwdLimiter;
fwd.y *= fwdLimiter;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): afterRotToTargetPos1DiffSqrMag > maxSpeedAddSqr. afterRotToTargetPos1DiffSqrMag={afterRotToTargetPos1DiffSqrMag} maxSpeedAddSqr={maxSpeedAddSqr} fwdLimiter={fwdLimiter} fwd={fwd}");
}
#endif
}
if (fwd.z < -1f) // !!!
{
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): fwd.z < -1f. fwd={fwd}");
}
#endif
if (vehicleData.m_path != 0u && (leaderData.m_flags & Vehicle.Flags.WaitingPath) == (Vehicle.Flags) 0)
{
Vector3 targetPos0TargetPos1Diff = vehicleData.m_targetPos1 - vehicleData.m_targetPos0;
if ((curInvRot * targetPos0TargetPos1Diff).z < -0.01f) // !!!
{
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): (curInvRot * targetPos0TargetPos1Diff).z < -0.01f. curInvRot={curInvRot} targetPos0TargetPos1Diff={targetPos0TargetPos1Diff}");
}
#endif
if (afterRotToTargetPos1Diff.z < Mathf.Abs(afterRotToTargetPos1Diff.x) * -10f) // !!!
{
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): afterRotToTargetPos1Diff.z < Mathf.Abs(afterRotToTargetPos1Diff.x) * -10f. fwd={fwd} targetPos0TargetPos1Diff={targetPos0TargetPos1Diff} afterRotToTargetPos1Diff={afterRotToTargetPos1Diff}");
}
#endif
/*fwd.z = 0f;
* afterRotToTargetPos1Diff = Vector3.zero;*/
maxSpeed = 0.5f; // NON-STOCK CODE
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): (1) set maxSpeed={maxSpeed}");
}
#endif
}
else
{
posAfterWheelRot = posBeforeWheelRot + Vector3.Normalize(vehicleData.m_targetPos1 - vehicleData.m_targetPos0) * this.m_info.m_generatedInfo.m_wheelBase;
posIndex = -1;
InvokeUpdatePathTargetPositions(tramBaseAI, vehicleID, ref vehicleData, vehicleData.m_targetPos0, vehicleData.m_targetPos1, leaderID, ref leaderData, ref posIndex, 0, 0, Vector3.SqrMagnitude(vehicleData.m_targetPos1 - vehicleData.m_targetPos0) + 1f, 1f);
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): afterRotToTargetPos1Diff.z >= Mathf.Abs(afterRotToTargetPos1Diff.x) * -10f. Invoked UpdatePathTargetPositions. posAfterWheelRot={posAfterWheelRot} posBeforeWheelRot={posBeforeWheelRot} this.m_info.m_generatedInfo.m_wheelBase={this.m_info.m_generatedInfo.m_wheelBase}");
}
#endif
}
}
else
{
posIndex = -1;
InvokeUpdatePathTargetPositions(tramBaseAI, vehicleID, ref vehicleData, vehicleData.m_targetPos0, posBeforeWheelRot, leaderID, ref leaderData, ref posIndex, 0, 0, Vector3.SqrMagnitude(posBeforeWheelRot - (Vector3)vehicleData.m_targetPos0) + 1f, 1f);
vehicleData.m_targetPos1 = posAfterWheelRot;
fwd.z = 0f;
afterRotToTargetPos1Diff = Vector3.zero;
maxSpeed = 0f;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Vehicle is waiting for a path. posIndex={posIndex} vehicleData.m_targetPos1={vehicleData.m_targetPos1} fwd={fwd} afterRotToTargetPos1Diff={afterRotToTargetPos1Diff} maxSpeed={maxSpeed}");
}
#endif
}
}
motionFactor = 0f;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Reset motion factor. motionFactor={motionFactor}");
}
#endif
}
forward = VectorUtils.NormalizeXZ(fwd, out forwardLen);
float curve = Mathf.PI / 2f /* 1.57079637f*/ * (1f - forward.z); // <- constant: a bit inaccurate PI/2
if (forwardLen > 1f)
{
curve /= forwardLen;
}
float targetDist = forwardLen;
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): targetDist={targetDist} fwd={fwd} curve={curve} maxSpeed={maxSpeed}");
}
#endif
if (vehicleData.m_targetPos1.w < 0.1f)
{
maxSpeed = this.CalculateTargetSpeed(vehicleID, ref vehicleData, 1000f, curve);
maxSpeed = Mathf.Min(maxSpeed, CalculateMaxSpeed(targetDist, vehicleData.m_targetPos1.w, braking * 0.9f));
}
else
{
maxSpeed = Mathf.Min(maxSpeed, this.CalculateTargetSpeed(vehicleID, ref vehicleData, 1000f, curve));
}
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): [1] maxSpeed={maxSpeed}");
}
#endif
maxSpeed = Mathf.Min(maxSpeed, CalculateMaxSpeed(targetDist, vehicleData.m_targetPos2.w, braking * 0.9f));
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): [2] maxSpeed={maxSpeed}");
}
#endif
targetDist += VectorUtils.LengthXZ(vehicleData.m_targetPos2 - vehicleData.m_targetPos1);
maxSpeed = Mathf.Min(maxSpeed, CalculateMaxSpeed(targetDist, vehicleData.m_targetPos3.w, braking * 0.9f));
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): [3] maxSpeed={maxSpeed}");
}
#endif
targetDist += VectorUtils.LengthXZ(vehicleData.m_targetPos3 - vehicleData.m_targetPos2);
if (vehicleData.m_targetPos3.w < 0.01f)
{
targetDist = Mathf.Max(0f, targetDist + (this.m_info.m_generatedInfo.m_wheelBase - this.m_info.m_generatedInfo.m_size.z) * 0.5f);
}
maxSpeed = Mathf.Min(maxSpeed, CalculateMaxSpeed(targetDist, 0f, braking * 0.9f));
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): [4] maxSpeed={maxSpeed}");
}
#endif
CarAI.CheckOtherVehicles(vehicleID, ref vehicleData, ref frameData, ref maxSpeed, ref blocked, ref zero, estimatedFrameDist, braking * 0.9f, lodPhysics);
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): CheckOtherVehicles finished. blocked={blocked}");
}
#endif
if (maxSpeed < curSpeed)
{
float brake = Mathf.Max(acceleration, Mathf.Min(braking, curSpeed));
targetSpeed = Mathf.Max(maxSpeed, curSpeed - brake);
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): maxSpeed < curSpeed. maxSpeed={maxSpeed} curSpeed={curSpeed} brake={brake} targetSpeed={targetSpeed}");
}
#endif
}
else
{
float accel = Mathf.Max(acceleration, Mathf.Min(braking, -curSpeed));
targetSpeed = Mathf.Min(maxSpeed, curSpeed + accel);
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): maxSpeed >= curSpeed. maxSpeed={maxSpeed} curSpeed={curSpeed} accel={accel} targetSpeed={targetSpeed}");
}
#endif
}
}
}
else if (curSpeed < 0.1f && hasValidPathTargetPos && leaderInfo.m_vehicleAI.ArriveAtDestination(leaderID, ref leaderData))
{
leaderData.Unspawn(leaderID);
return;
}
if ((leaderData.m_flags & Vehicle.Flags.Stopped) == (Vehicle.Flags) 0 && maxSpeed < 0.1f)
{
#if DEBUG
if (debug)
{
Log._Debug($"CustomTramBaseAI.SimulationStep({vehicleID}): Vehicle is not stopped but maxSpeed < 0.1. maxSpeed={maxSpeed}");
}
#endif
blocked = true;
}
if (blocked)
{
leaderData.m_blockCounter = (byte)Mathf.Min((int)(leaderData.m_blockCounter + 1), 255);
}
else
{
leaderData.m_blockCounter = 0;
}
if (forwardLen > 1f)
{
turnAngle = Mathf.Asin(forward.x) * Mathf.Sign(targetSpeed);
targetMotion = forward * targetSpeed;
}
else
{
Vector3 vel = Vector3.ClampMagnitude(afterRotToTargetPos1Diff * 0.5f - curveTangent, braking);
targetMotion = curveTangent + vel;
}
}
bool mayBlink = (currentFrameIndex + (uint)leaderID & 16u) != 0u;
Vector3 springs = targetMotion - curveTangent;
Vector3 targetAfterWheelRotMotion = frameData.m_rotation * targetMotion;
Vector3 targetBeforeWheelRotMotion = Vector3.Normalize((Vector3)vehicleData.m_targetPos0 - posBeforeWheelRot) * (targetMotion.magnitude * motionFactor);
targetBeforeWheelRotMotion -= targetAfterWheelRotMotion * (Vector3.Dot(targetAfterWheelRotMotion, targetBeforeWheelRotMotion) / Mathf.Max(1f, targetAfterWheelRotMotion.sqrMagnitude));
posAfterWheelRot += targetAfterWheelRotMotion;
posBeforeWheelRot += targetBeforeWheelRotMotion;
frameData.m_rotation = Quaternion.LookRotation(posAfterWheelRot - posBeforeWheelRot);
Vector3 targetPos = posAfterWheelRot - frameData.m_rotation * new Vector3(0f, 0f, this.m_info.m_generatedInfo.m_wheelBase * 0.5f);
frameData.m_velocity = targetPos - frameData.m_position;
if (leadingVehicle != 0)
{
frameData.m_position += frameData.m_velocity * 0.6f;
}
else
{
frameData.m_position += frameData.m_velocity * 0.5f;
}
frameData.m_swayVelocity = frameData.m_swayVelocity * (1f - this.m_info.m_dampers) - springs * (1f - this.m_info.m_springs) - frameData.m_swayPosition * this.m_info.m_springs;
frameData.m_swayPosition += frameData.m_swayVelocity * 0.5f;
frameData.m_steerAngle = 0f;
frameData.m_travelDistance += targetMotion.z;
if (leadingVehicle != 0)
{
frameData.m_lightIntensity = Singleton <VehicleManager> .instance.m_vehicles.m_buffer[(int)leaderID].GetLastFrameData().m_lightIntensity;
}
else
{
frameData.m_lightIntensity.x = 5f;
frameData.m_lightIntensity.y = ((springs.z >= -0.1f) ? 0.5f : 5f);
frameData.m_lightIntensity.z = ((turnAngle >= -0.1f || !mayBlink) ? 0f : 5f);
frameData.m_lightIntensity.w = ((turnAngle <= 0.1f || !mayBlink) ? 0f : 5f);
}
frameData.m_underground = ((vehicleData.m_flags & Vehicle.Flags.Underground) != (Vehicle.Flags) 0);
frameData.m_transition = ((vehicleData.m_flags & Vehicle.Flags.Transition) != (Vehicle.Flags) 0);
//base.SimulationStep(vehicleID, ref vehicleData, ref frameData, leaderID, ref leaderData, lodPhysics);
}
public override void SimulationStep(ushort vehicleID, ref Vehicle vehicleData, ref Vehicle.Frame frameData, ushort leaderID, ref Vehicle leaderData, int lodPhysics)
{
if ((leaderData.m_flags2 & Vehicle.Flags2.Blown) != (Vehicle.Flags2) 0)
{
this.SimulationStepBlown(vehicleID, ref vehicleData, ref frameData, leaderID, ref leaderData, lodPhysics);
return;
}
if ((leaderData.m_flags2 & Vehicle.Flags2.Floating) != (Vehicle.Flags2) 0)
{
this.SimulationStepFloating(vehicleID, ref vehicleData, ref frameData, leaderID, ref leaderData, lodPhysics);
return;
}
uint currentFrameIndex = Singleton <SimulationManager> .instance.m_currentFrameIndex;
frameData.m_position += frameData.m_velocity * 0.5f;
frameData.m_swayPosition += frameData.m_swayVelocity * 0.5f;
float accelerationAbility = this.m_info.m_acceleration;
float brakingAbility = this.m_info.m_braking;
if (vehicleData.m_flags.IsFlagSet(Vehicle.Flags.Emergency2))
{
accelerationAbility *= 2f;
brakingAbility *= 2f;
}
float velocity = frameData.m_velocity.magnitude;
Vector3 posdiff0 = (Vector3)vehicleData.m_targetPos0 - frameData.m_position;
float postdiff0_len_pow2 = posdiff0.sqrMagnitude;
float carLength = this.m_info.m_generatedInfo.m_size.z;
float velocity_max_new2 = Mathf.Max(velocity + accelerationAbility, 5f); // maximum new velocity the car is capble of.
if (lodPhysics >= 2 && (ulong)(currentFrameIndex >> 4 & 3u) == (ulong)(vehicleID & 3))
{
velocity_max_new2 *= 2f;
}
float num3 = (velocity + accelerationAbility) * (0.5f + 0.5f * (velocity + accelerationAbility) / brakingAbility) + carLength * 0.5f;
float num5 = Mathf.Max((num3 - velocity_max_new2) / 3f, 1f);
float velocity_max_new2_pow2 = velocity_max_new2 * velocity_max_new2;
float num7 = num5 * num5;
int i = 0;
bool shortTravelDistanceButStillMoving = false;
if ((postdiff0_len_pow2 < velocity_max_new2_pow2 || vehicleData.m_targetPos3.w < 0.01f) && !leaderData.m_flags.IsFlagSet(Vehicle.Flags.WaitingPath | Vehicle.Flags.Stopped))
{
if (leaderData.m_path != 0u)
{
base.UpdatePathTargetPositions(vehicleID, ref vehicleData, frameData.m_position, ref i, 4, velocity_max_new2_pow2, num7);
if (!leaderData.m_flags.IsFlagSet(Vehicle.Flags.Spawned))
{
frameData = vehicleData.m_frame0;
return;
}
}
if (!leaderData.m_flags.IsFlagSet(Vehicle.Flags.WaitingPath))
{
while (i < 4)
{
float minSqrDistance;
Vector3 refPos;
if (i == 0)
{
minSqrDistance = velocity_max_new2_pow2;
refPos = frameData.m_position;
shortTravelDistanceButStillMoving = true;
}
else
{
minSqrDistance = num7;
refPos = vehicleData.GetTargetPos(i - 1);
}
int i0 = i;
this.UpdateBuildingTargetPositions(vehicleID, ref vehicleData, refPos, leaderID, ref leaderData, ref i, minSqrDistance);
if (i == i0)
{
break;
}
}
if (i != 0)
{
Vector4 targetPos = vehicleData.GetTargetPos(i - 1);
while (i < 4)
{
vehicleData.SetTargetPos(i++, targetPos);
}
}
}
posdiff0 = (Vector3)vehicleData.m_targetPos0 - frameData.m_position;
postdiff0_len_pow2 = posdiff0.sqrMagnitude;
}
if (leaderData.m_path != 0u && !leaderData.m_flags.IsFlagSet(Vehicle.Flags.WaitingPath))
{
byte b = leaderData.m_pathPositionIndex;
byte lastPathOffset = leaderData.m_lastPathOffset;
if (b == 255)
{
b = 0;
}
int noise;
float num9 = 1f + leaderData.CalculateTotalLength(leaderID, out noise);
PathManager instance2 = Singleton <PathManager> .instance;
PathUnit.Position pathPos;
if (instance2.m_pathUnits.m_buffer[leaderData.m_path].GetPosition(b >> 1, out pathPos))
{
if (SegmentBuffer[pathPos.m_segment].m_flags.IsFlagSet(NetSegment.Flags.Flooded) &&
Singleton <TerrainManager> .instance.HasWater(VectorUtils.XZ(frameData.m_position)))
{
leaderData.m_flags2 |= Vehicle.Flags2.Floating;
}
SegmentBuffer[pathPos.m_segment].AddTraffic(Mathf.RoundToInt(num9 * 2.5f), noise);
bool flag2 = false;
if ((b & 1) == 0 || lastPathOffset == 0)
{
uint laneID = PathManager.GetLaneID(pathPos);
if (laneID != 0u)
{
Vector3 b2 = LaneBuffer[laneID].CalculatePosition(pathPos.m_offset * (1f / 255));
float num10 = 0.5f * velocity * velocity / brakingAbility + carLength * 0.5f;
if (Vector3.Distance(frameData.m_position, b2) >= num10 - 1f)
{
LaneBuffer[laneID].ReserveSpace(num9);
flag2 = true;
}
}
}
if (!flag2 && instance2.m_pathUnits.m_buffer[leaderData.m_path].GetNextPosition(b >> 1, out pathPos))
{
uint laneID2 = PathManager.GetLaneID(pathPos);
if (laneID2 != 0u)
{
LaneBuffer[laneID2].ReserveSpace(num9);
}
}
}
if ((ulong)(currentFrameIndex >> 4 & 15u) == (ulong)((long)(leaderID & 15)))
{
bool flag3 = false;
uint path = leaderData.m_path;
int num11 = b >> 1;
int j = 0;
while (j < 5)
{
bool flag4;
if (PathUnit.GetNextPosition(ref path, ref num11, out pathPos, out flag4))
{
uint laneID3 = PathManager.GetLaneID(pathPos);
if (laneID3 != 0u && !LaneBuffer[laneID3].CheckSpace(num9))
{
j++;
continue;
}
}
if (flag4)
{
this.InvalidPath(vehicleID, ref vehicleData, leaderID, ref leaderData);
}
flag3 = true;
break;
}
if (!flag3)
{
leaderData.m_flags |= Vehicle.Flags.Congestion;
}
}
}
float num12;
if (leaderData.m_flags.IsFlagSet(Vehicle.Flags.Stopped))
{
num12 = 0f;
}
else
{
num12 = vehicleData.m_targetPos0.w;
if (!leaderData.m_flags.IsFlagSet(Vehicle.Flags.DummyTraffic))
{
VehicleManager instance3 = Singleton <VehicleManager> .instance;
float f = velocity * 100f / Mathf.Max(1f, vehicleData.m_targetPos0.w);
instance3.m_totalTrafficFlow += (uint)Mathf.RoundToInt(f);
instance3.m_maxTrafficFlow += 100u;
}
}
Quaternion rotation = Quaternion.Inverse(frameData.m_rotation);
posdiff0 = rotation * posdiff0;
Vector3 vector2 = rotation * frameData.m_velocity;
Vector3 forwardDir0 = Vector3.forward;
Vector3 vector3;
Vector3 zero = Vector3.zero;
float num13 = 0f;
float num14 = 0f;
bool flag5 = false;
float posdiff0_lenxz = 0f;
if (postdiff0_len_pow2 > 1f)
{
forwardDir0 = VectorUtils.NormalizeXZ(posdiff0, out posdiff0_lenxz);
if (posdiff0_lenxz > 1f)
{
Vector3 vector4 = posdiff0;
velocity_max_new2 = Mathf.Max(velocity, 2f);
velocity_max_new2_pow2 = velocity_max_new2 * velocity_max_new2;
if (postdiff0_len_pow2 > velocity_max_new2_pow2)
{
vector4 *= velocity_max_new2 / Mathf.Sqrt(postdiff0_len_pow2);
}
bool flag6 = false;
if (vector4.z < Mathf.Abs(vector4.x))
{
if (vector4.z < 0f)
{
flag6 = true;
}
float num16 = Mathf.Abs(vector4.x);
if (num16 < 1f)
{
vector4.x = Mathf.Sign(vector4.x);
if (vector4.x == 0f)
{
vector4.x = 1f;
}
num16 = 1f;
}
vector4.z = num16;
}
float b3;
forwardDir0 = VectorUtils.NormalizeXZ(vector4, out b3);
posdiff0_lenxz = Mathf.Min(posdiff0_lenxz, b3);
float num17 = 1.57079637f * (1f - forwardDir0.z);
if (posdiff0_lenxz > 1f)
{
num17 /= posdiff0_lenxz;
}
float num18 = posdiff0_lenxz;
if (vehicleData.m_targetPos0.w < 0.1f)
{
num12 = this.CalculateTargetSpeed(vehicleID, ref vehicleData, 1000f, num17);
num12 = Mathf.Min(num12, CalculateMaxSpeed(num18, Mathf.Min(vehicleData.m_targetPos0.w, vehicleData.m_targetPos1.w), brakingAbility * 0.9f));
}
else
{
num12 = Mathf.Min(num12, this.CalculateTargetSpeed(vehicleID, ref vehicleData, 1000f, num17));
num12 = Mathf.Min(num12, CalculateMaxSpeed(num18, vehicleData.m_targetPos1.w, brakingAbility * 0.9f));
}
num18 += VectorUtils.LengthXZ(vehicleData.m_targetPos1 - vehicleData.m_targetPos0);
num12 = Mathf.Min(num12, CalculateMaxSpeed(num18, vehicleData.m_targetPos2.w, brakingAbility * 0.9f));
num18 += VectorUtils.LengthXZ(vehicleData.m_targetPos2 - vehicleData.m_targetPos1);
num12 = Mathf.Min(num12, CalculateMaxSpeed(num18, vehicleData.m_targetPos3.w, brakingAbility * 0.9f));
num18 += VectorUtils.LengthXZ(vehicleData.m_targetPos3 - vehicleData.m_targetPos2);
if (vehicleData.m_targetPos3.w < 0.01f)
{
num18 = Mathf.Max(0f, num18 - carLength * 0.5f);
}
num12 = Mathf.Min(num12, CalculateMaxSpeed(num18, 0f, brakingAbility * 0.9f));
if (!DisableCollisionCheck(leaderID, ref leaderData))
{
CarAI.CheckOtherVehicles(vehicleID, ref vehicleData, ref frameData, ref num12, ref flag5, ref zero, num3, brakingAbility * 0.9f, lodPhysics);
}
if (flag6)
{
num12 = -num12;
}
if (num12 < velocity)
{
float num19 = Mathf.Max(accelerationAbility, Mathf.Min(brakingAbility, velocity));
num13 = Mathf.Max(num12, velocity - num19);
}
else
{
float num20 = Mathf.Max(accelerationAbility, Mathf.Min(brakingAbility, -velocity));
num13 = Mathf.Min(num12, velocity + num20);
}
}
}
else if (velocity < 0.1f && shortTravelDistanceButStillMoving && this.ArriveAtDestination(leaderID, ref leaderData))
{
leaderData.Unspawn(leaderID);
if (leaderID == vehicleID)
{
frameData = leaderData.m_frame0;
}
return;
}
if (!leaderData.m_flags.IsFlagSet(Vehicle.Flags.Stopped) && num12 < 0.1f)
{
flag5 = true;
}
if (flag5)
{
vehicleData.m_blockCounter = (byte)Mathf.Min((vehicleData.m_blockCounter + 1), 255);
}
else
{
vehicleData.m_blockCounter = 0;
}
if (posdiff0_lenxz > 1f)
{
num14 = Mathf.Asin(forwardDir0.x) * Mathf.Sign(num13);
vector3 = forwardDir0 * num13;
}
else
{
num13 = 0f;
Vector3 b4 = Vector3.ClampMagnitude(posdiff0 * 0.5f - vector2, brakingAbility);
vector3 = vector2 + b4;
}
bool flag7 = (currentFrameIndex + (uint)leaderID & 16u) != 0u;
Vector3 a2 = vector3 - vector2;
Vector3 vector5 = frameData.m_rotation * vector3;
frameData.m_velocity = vector5 + zero;
frameData.m_position += frameData.m_velocity * 0.5f;
frameData.m_swayVelocity = frameData.m_swayVelocity * (1f - this.m_info.m_dampers) - a2 * (1f - this.m_info.m_springs) - frameData.m_swayPosition * this.m_info.m_springs;
frameData.m_swayPosition += frameData.m_swayVelocity * 0.5f;
frameData.m_steerAngle = num14;
frameData.m_travelDistance += vector3.z;
frameData.m_lightIntensity.x = 5f;
frameData.m_lightIntensity.y = ((a2.z >= -0.1f) ? 0.5f : 5f);
frameData.m_lightIntensity.z = ((num14 >= -0.1f || !flag7) ? 0f : 5f);
frameData.m_lightIntensity.w = ((num14 <= 0.1f || !flag7) ? 0f : 5f);
frameData.m_underground = (vehicleData.m_flags.IsFlagSet(Vehicle.Flags.Underground));
frameData.m_transition = (vehicleData.m_flags.IsFlagSet(Vehicle.Flags.Transition));
if (vehicleData.m_flags.IsFlagSet(Vehicle.Flags.Parking) && posdiff0_lenxz <= 1f && shortTravelDistanceButStillMoving)
{
Vector3 forward = vehicleData.m_targetPos1 - vehicleData.m_targetPos0;
if (forward.sqrMagnitude > 0.01f)
{
frameData.m_rotation = Quaternion.LookRotation(forward);
}
}
else if (num13 > 0.1f)
{
if (vector5.sqrMagnitude > 0.01f)
{
frameData.m_rotation = Quaternion.LookRotation(vector5);
}
}
else if (num13 < -0.1f && vector5.sqrMagnitude > 0.01f)
{
frameData.m_rotation = Quaternion.LookRotation(-vector5);
}
base.SimulationStep(vehicleID, ref vehicleData, ref frameData, leaderID, ref leaderData, lodPhysics);
}