public async Task SaveVehicleStateAsync(VehicleState vehicleState)
{
await _daprClient.SaveStateAsync <VehicleState>(
DAPR_STORE_NAME, vehicleState.LicenseNumber, vehicleState);
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, RoadPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
List <ITacticalBlockage> blockages, double vehicleSpeed)
{
// assign vehicles to their lanes
this.roadMonitor.Assign(vehicles);
// navigation tasks
this.taskReasoning.navigationPlan = navigationalPlan;
#region Stay in lane
// maneuver given we are in a lane
if (planningState is StayInLaneState)
{
// get state
StayInLaneState sils = (StayInLaneState)planningState;
// check reasoning if needs to be different
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sils.Lane))
{
if (sils.Lane.LaneOnLeft == null)
{
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
}
else if (sils.Lane.LaneOnLeft.Way.Equals(sils.Lane.Way))
{
this.leftLateralReasoning = new LateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
}
else
{
this.leftLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
}
if (sils.Lane.LaneOnRight == null)
{
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
}
else if (sils.Lane.LaneOnRight.Way.Equals(sils.Lane.Way))
{
this.rightLateralReasoning = new LateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
}
else
{
this.rightLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
}
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sils.Lane);
}
// populate navigation with road plan
taskReasoning.SetRoadPlan(navigationalPlan, sils.Lane);
// as penalties for lane changes already taken into account, can just look at
// best lane plan to figure out what to do
TypeOfTasks bestTask = taskReasoning.Best;
// get the forward lane plan
Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore);
// get the secondary
Maneuver?secondaryManeuver = forwardReasoning.AdvancedSecondary(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask); //forwardReasoning.SecondaryManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask);
// check behavior type for uturn
if (secondaryManeuver.HasValue && secondaryManeuver.Value.PrimaryBehavior is UTurnBehavior)
{
return(secondaryManeuver.Value);
}
// check if we wish to change lanes here
if (bestTask != TypeOfTasks.Straight)
{
// parameters
LaneChangeParameters parameters;
secondaryManeuver = this.forwardReasoning.AdvancedDesiredLaneChangeManeuver(sils.Lane, bestTask == TypeOfTasks.Left ? true : false, navigationalPlan.BestPlan.laneWaypointOfInterest.PointOfInterest,
navigationalPlan, vehicleState, blockages, sils.WaypointsToIgnore, new LaneChangeInformation(LaneChangeReason.Navigation, this.forwardReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle), secondaryManeuver, out parameters);
}
// final maneuver
Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;
// set opposing vehicle flag
if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is StayInLaneBehavior)
{
StayInLaneBehavior silb = (StayInLaneBehavior)finalManeuver.PrimaryBehavior;
OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
{
ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
finalManeuver.PrimaryBehavior.Decorators = new List <BehaviorDecorator>(bds);
silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
}
finalManeuver.PrimaryBehavior = silb;
}
// return the final
return(finalManeuver);
}
#endregion
#region Stay in supra lane
else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
// get state
StayInSupraLaneState sisls = (StayInSupraLaneState)planningState;
// check reasoning
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sisls.Lane))
{
if (sisls.Lane.Initial.LaneOnLeft == null)
{
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
}
else if (sisls.Lane.Initial.LaneOnLeft.Way.Equals(sisls.Lane.Initial.Way))
{
this.leftLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
}
else
{
this.leftLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
}
if (sisls.Lane.Initial.LaneOnRight == null)
{
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
}
else if (sisls.Lane.Initial.LaneOnRight.Way.Equals(sisls.Lane.Initial.Way))
{
this.rightLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
}
else
{
this.rightLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
}
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sisls.Lane);
}
// populate navigation with road plan
taskReasoning.SetSupraRoadPlan(navigationalPlan, sisls.Lane);
// as penalties for lane changes already taken into account, can just look at
// best lane plan to figure out what to do
// TODO: NOTE THAT THIS BEST TASK SHOULD BE IN THE SUPRA LANE!! (DO WE NEED THIS)
TypeOfTasks bestTask = taskReasoning.Best;
// get the forward lane plan
Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore);
// get hte secondary
Maneuver?secondaryManeuver = forwardReasoning.AdvancedSecondary(sisls.Lane, vehicleState, navigationalPlan, blockages, new List <ArbiterWaypoint>(), bestTask); //forwardReasoning.SecondaryManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore, bestTask);
// final maneuver
Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;
// check if stay in lane
if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is SupraLaneBehavior)
{
SupraLaneBehavior silb = (SupraLaneBehavior)finalManeuver.PrimaryBehavior;
OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
{
ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
finalManeuver.PrimaryBehavior.Decorators = new List <BehaviorDecorator>(bds);
silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
}
finalManeuver.PrimaryBehavior = silb;
}
// return the final
return(finalManeuver);
// notify
/*if (secondaryManeuver.HasValue)
* ArbiterOutput.Output("Secondary Maneuver");
*
* // check for forward's secondary maneuver for desired behavior other than going straight
* if (secondaryManeuver.HasValue)
* {
* // return the secondary maneuver
* return secondaryManeuver.Value;
* }
* // otherwise our default behavior and posibly desired is going straight
* else
* {
* // return default forward maneuver
* return forwardManeuver;
* }*/
}
#endregion
#region Change Lanes State
// maneuver given we are changing lanes
else if (planningState is ChangeLanesState)
{
// get state
ChangeLanesState cls = (ChangeLanesState)planningState;
LaneChangeReasoning lcr = new LaneChangeReasoning();
Maneuver final = lcr.PlanLaneChange(cls, vehicleState, navigationalPlan, blockages, new List <ArbiterWaypoint>());
#warning need to filter through waypoints to ignore so don't get stuck by a stop line
//Maneuver final = new Maneuver(cls.Resume(vehicleState, vehicleSpeed), cls, cls.DefaultStateDecorators, vehicleState.Timestamp);
// return the final planned maneuver
return(final);
/*if (!cls.parameters..TargetIsOnComing)
* {
* // check reasoning
* if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(cls.TargetLane))
* {
* if (cls.TargetLane.LaneOnLeft.Way.Equals(cls.TargetLane.Way))
* this.leftLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnLeft);
* else
* this.leftLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnLeft);
*
* if (cls.TargetLane.LaneOnRight.Way.Equals(cls.TargetLane.Way))
* this.rightLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnRight);
* else
* this.rightLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnRight);
*
* this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, cls.TargetLane);
* }
*
*
* // get speed command
* double speed;
* double distance;
* this.forwardReasoning.ForwardMonitor.StoppingParams(new ArbiterWaypoint(cls.TargetUpperBound.pt, null), cls.TargetLane, vehicleState.Front, vehicleState.ENCovariance, out speed, out distance);
* SpeedCommand sc = new ScalarSpeedCommand(Math.Max(speed, 0.0));
* cls.distanceLeft = distance;
*
* // get behavior
* ChangeLaneBehavior clb = new ChangeLaneBehavior(cls.InitialLane.LaneId, cls.TargetLane.LaneId, cls.InitialLane.LaneOnLeft != null && cls.InitialLane.LaneOnLeft.Equals(cls.TargetLane),
* distance, sc, new List<int>(), cls.InitialLane.PartitionPath, cls.TargetLane.PartitionPath, cls.InitialLane.Width, cls.TargetLane.Width);
*
* // plan over the target lane
* //Maneuver targetManeuver = forwardReasoning.ForwardManeuver(cls.TargetLane, vehicleState, !cls.TargetIsOnComing, blockage, cls.InitialLaneState.IgnorableWaypoints);
*
* // plan over the initial lane
* //Maneuver initialManeuver = forwardReasoning.ForwardManeuver(cls.InitialLane, vehicleState, !cls.InitialIsOncoming, blockage, cls.InitialLaneState.IgnorableWaypoints);
*
* // generate the change lanes command
* //Maneuver final = laneChangeReasoning.PlanLaneChange(cls, initialManeuver, targetManeuver);
*
* }
* else
* {
* throw new Exception("Change lanes into oncoming not supported yet by road tactical");
* }*/
}
#endregion
#region Opposing Lanes State
// maneuver given we are in an opposing lane
else if (planningState is OpposingLanesState)
{
// get state
OpposingLanesState ols = (OpposingLanesState)planningState;
ArbiterWayId opposingWay = ols.OpposingWay;
ols.SetClosestGood(vehicleState);
ols.ResetLaneAgent = false;
// check reasoning
if (this.opposingReasoning == null || !this.opposingReasoning.Lane.Equals(ols.OpposingLane))
{
if (ols.OpposingLane.LaneOnRight == null)
{
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
}
else if (!ols.OpposingLane.LaneOnRight.Way.Equals(ols.OpposingLane.Way))
{
this.leftLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
}
else
{
this.leftLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
}
if (ols.OpposingLane.LaneOnLeft == null)
{
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
}
else if (!ols.OpposingLane.LaneOnLeft.Way.Equals(ols.OpposingLane.Way))
{
this.rightLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
}
else
{
this.rightLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
}
this.opposingReasoning = new OpposingReasoning(this.leftLateralReasoning, this.rightLateralReasoning, ols.OpposingLane);
}
// get the forward lane plan
Maneuver forwardManeuver = this.opposingReasoning.ForwardManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, navigationalPlan, blockages);
// get the secondary maneuver
Maneuver?secondaryManeuver = null;
if (ols.ClosestGoodLane != null)
{
secondaryManeuver = this.opposingReasoning.SecondaryManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, blockages, ols.EntryParameters);
}
// check for reasonings secondary maneuver for desired behavior other than going straight
if (secondaryManeuver != null)
{
// return the secondary maneuver
return(secondaryManeuver.Value);
}
// otherwise our default behavior and posibly desired is going straight
else
{
// return default forward maneuver
return(forwardManeuver);
}
}
#endregion
#region not imp
/*
#region Uturn
*
* // we are making a uturn
* else if (planningState is uTurnState)
* {
* // get the uturn state
* uTurnState uts = (uTurnState)planningState;
*
* // get the final lane we wish to be in
* ArbiterLane targetLane = uts.TargetLane;
*
* // get operational state
* Type operationalBehaviorType = CoreCommon.Communications.GetCurrentOperationalBehavior();
*
* // check if we have completed the uturn
* bool complete = operationalBehaviorType == typeof(StayInLaneBehavior);
*
* // default next behavior
* Behavior nextBehavior = new StayInLaneBehavior(targetLane.LaneId, new ScalarSpeedCommand(CoreCommon.OperationalStopSpeed), new List<int>());
* nextBehavior.Decorators = TurnDecorators.NoDecorators;
*
* // check if complete
* if (complete)
* {
* // stay in lane
* List<ArbiterLaneId> aprioriLanes = new List<ArbiterLaneId>();
* aprioriLanes.Add(targetLane.LaneId);
* return new Maneuver(nextBehavior, new StayInLaneState(targetLane), null, null, aprioriLanes, true);
* }
* // otherwise keep same
* else
* {
* // set abort behavior
* ((StayInLaneBehavior)nextBehavior).SpeedCommand = new ScalarSpeedCommand(0.0);
*
* // maneuver
* return new Maneuver(uts.DefaultBehavior, uts, nextBehavior, new StayInLaneState(targetLane));
* }
* }
*
#endregion*/
#endregion
#region Unknown
// unknown state
else
{
throw new Exception("Unknown Travel State type: planningState: " + planningState.ToString() + "\n with type: " + planningState.GetType().ToString());
}
#endregion
}
/// <summary>
/// Determines the state of the given vehicle.
/// The returned vehicle state is not necessarily valid.
/// </summary>
/// <param name="vehicleId"></param>
/// <returns>the vehicle state</returns>
internal VehicleState _GetVehicleState(ushort vehicleId)
{
VehicleState ret = VehicleStates[vehicleId];
return(ret);
}
/// <summary>Is called every frame.</summary>
/// <param name="data">The data.</param>
internal void Elapse(ElapseData data)
{
this.PluginInitializing = false;
if (data.ElapsedTime.Seconds > 0.0 & data.ElapsedTime.Seconds < 1.0)
{
// --- panel ---
for (int i = 0; i < this.Panel.Length; i++)
{
this.Panel[i] = 0;
}
// --- devices ---
this.State = data.Vehicle;
this.Handles = new ReadOnlyHandles(data.Handles);
bool blocking = false;
foreach (var device in this.Devices)
{
device.Elapse(data, ref blocking);
}
if (data.Handles.BrakeNotch != 0)
{
data.Handles.PowerNotch = 0;
}
// --- panel ---
this.Panel[255] = 1;
int seconds = (int)Math.Floor(data.TotalTime.Seconds);
this.Panel[10] = (seconds / 3600) % 24;
this.Panel[11] = (seconds / 60) % 60;
this.Panel[12] = seconds % 60;
this.Panel[269] = data.Handles.ConstSpeed ? 1 : 0;
if (data.Handles.Reverser != 0 & (this.Handles.PowerNotch > 0 & this.Handles.BrakeNotch == 0 | this.Handles.PowerNotch == 0 & this.Handles.BrakeNotch == 1 & this.Specs.HasHoldBrake))
{
this.Panel[100] = 1;
}
if (data.Handles.BrakeNotch >= this.Specs.AtsNotch & data.Handles.BrakeNotch <= this.Specs.BrakeNotches | data.Handles.Reverser != 0 & data.Handles.BrakeNotch == 1 & this.Specs.HasHoldBrake)
{
this.Panel[101] = 1;
}
for (int i = (int)VirtualKeys.S; i <= (int)VirtualKeys.C2; i++)
{
if (KeysPressed[i])
{
this.Panel[93 + i] = 1;
}
}
if (PanelIllumination)
{
this.Panel[161] = 1;
}
// --- accelerometer ---
this.AccelerometerTimer += data.ElapsedTime.Seconds;
if (this.AccelerometerTimer > AccelerometerMaximumTimer)
{
this.Acceleration = (data.Vehicle.Speed.MetersPerSecond - AccelerometerSpeed) / this.AccelerometerTimer;
this.AccelerometerSpeed = data.Vehicle.Speed.MetersPerSecond;
this.AccelerometerTimer = 0.0;
}
if (this.Acceleration < 0.0)
{
double value = -3.6 * this.Acceleration;
if (value >= 10.0)
{
this.Panel[74] = 9;
this.Panel[75] = 9;
}
else
{
this.Panel[74] = (int)Math.Floor(value) % 10;
this.Panel[75] = (int)Math.Floor(10.0 * value) % 10;
}
}
// --- sound ---
this.Sounds.Elapse(data);
// --- AI ---
this.Calling.Elapse(data);
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="currentArbiterState"></param>
/// <param name="internalCarMode"></param>
/// <param name="fullRoute"></param>
/// <param name="routeTime"></param>
/// <param name="actionPoint"></param>
/// <param name="currentGoal"></param>
/// <param name="goals"></param>
/// <param name="behavior"></param>
public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime,
RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue <RndfWaypointID> goals, Behavior behavior, VehicleState planningState)
{
this.CurrentArbiterState = currentArbiterState;
this.InternalCarMode = internalCarMode;
this.FullRoute = fullRoute;
this.RouteTime = routeTime;
this.ActionPoint = actionPoint;
this.CurrentGoal = currentGoal;
this.Goals = goals;
this.Behavior = behavior;
this.PlanningState = planningState;
}
public bool CustomStartPathFind(ushort vehicleID, ref Vehicle vehicleData, Vector3 startPos, Vector3 endPos, bool startBothWays, bool endBothWays)
{
#if DEBUG
//Log._Debug($"CustomTrainAI.CustomStartPathFind called for vehicle {vehicleID}");
#endif
/// NON-STOCK CODE START ///
ExtVehicleType vehicleType = VehicleStateManager.Instance._GetVehicleState(vehicleID).VehicleType;
if (vehicleType == ExtVehicleType.None)
{
#if DEBUG
Log.Warning($"CustomTrainAI.CustomStartPathFind: Vehicle {vehicleID} does not have a valid vehicle type!");
#endif
vehicleType = ExtVehicleType.RailVehicle;
}
else if (vehicleType == ExtVehicleType.CargoTrain)
{
vehicleType = ExtVehicleType.CargoVehicle;
}
#if DEBUG
/*if (vehicleType == ExtVehicleType.CargoVehicle) {
* bool reversed = (vehicleData.m_flags & Vehicle.Flags.Reversed) != 0;
* ushort frontVehicleId;
* if (reversed) {
* frontVehicleId = vehicleData.GetLastVehicle(vehicleId);
* } else {
* frontVehicleId = vehicleId;
* }
* Log._Debug($"CustomTrainAI.CustomStartPathFind. vehicleID={vehicleId}. reversed={reversed} frontVehicleId={frontVehicleId} type={this.GetType().ToString()} vehicleType={vehicleType} target={vehicleData.m_targetBuilding}");
* }*/
#endif
/// NON-STOCK CODE END ///
VehicleInfo info = this.m_info;
if ((vehicleData.m_flags & Vehicle.Flags.Spawned) == 0 && Vector3.Distance(startPos, endPos) < 100f)
{
startPos = endPos;
}
bool allowUnderground;
bool allowUnderground2;
if (info.m_vehicleType == VehicleInfo.VehicleType.Metro)
{
allowUnderground = true;
allowUnderground2 = true;
}
else
{
allowUnderground = ((vehicleData.m_flags & (Vehicle.Flags.Underground | Vehicle.Flags.Transition)) != 0);
allowUnderground2 = false;
}
PathUnit.Position startPosA;
PathUnit.Position startPosB;
float startSqrDistA;
float startSqrDistB;
PathUnit.Position endPosA;
PathUnit.Position endPosB;
float endSqrDistA;
float endSqrDistB;
if (CustomPathManager.FindPathPosition(startPos, ItemClass.Service.PublicTransport, NetInfo.LaneType.Vehicle, info.m_vehicleType, allowUnderground, false, 32f, out startPosA, out startPosB, out startSqrDistA, out startSqrDistB) &&
CustomPathManager.FindPathPosition(endPos, ItemClass.Service.PublicTransport, NetInfo.LaneType.Vehicle, info.m_vehicleType, allowUnderground2, false, 32f, out endPosA, out endPosB, out endSqrDistA, out endSqrDistB))
{
if (!startBothWays || startSqrDistB > startSqrDistA * 1.2f)
{
startPosB = default(PathUnit.Position);
}
if (!endBothWays || endSqrDistB > endSqrDistA * 1.2f)
{
endPosB = default(PathUnit.Position);
}
uint path;
if (CustomPathManager._instance.CreatePath((ExtVehicleType)vehicleType, vehicleID, ExtCitizenInstance.ExtPathType.None, out path, ref Singleton <SimulationManager> .instance.m_randomizer, Singleton <SimulationManager> .instance.m_currentBuildIndex, startPosA, startPosB, endPosA, endPosB, NetInfo.LaneType.Vehicle, info.m_vehicleType, 20000f, false, false, true, false))
{
#if USEPATHWAITCOUNTER
VehicleState state = VehicleStateManager.Instance._GetVehicleState(vehicleID);
state.PathWaitCounter = 0;
#endif
if (vehicleData.m_path != 0u)
{
Singleton <PathManager> .instance.ReleasePath(vehicleData.m_path);
}
vehicleData.m_path = path;
vehicleData.m_flags |= Vehicle.Flags.WaitingPath;
return(true);
}
}
return(false);
}
/// <summary>
/// Plans over the zone
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockages"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan,
VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles,
SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Zone Travelling State
if (planningState is ZoneTravelingState)
{
// check blockages
/*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage)
* {
* // create the blockage state
* EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
*
* // go to a blockage handling tactical
* return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
* }*/
// cast state
ZoneState zs = (ZoneState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// check zone path does not exist
if (zp.RecommendedPath.Count < 2)
{
// zone startup again
ZoneStartupState zss = new ZoneStartupState(zs.Zone, true);
return(new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// final path seg
LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL);
LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint);
LinePath lB = lp.ShiftLateral(TahoeParams.T);
LinePath rB = lp.ShiftLateral(-TahoeParams.T);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(),
sc, zp.RecommendedPath, lB, rB);
// maneuver
return(new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Parking State
else if (planningState is ParkingState)
{
// get state
ParkingState ps = (ParkingState)planningState;
// determine stay out areas to use
List <Polygon> stayOuts = new List <Polygon>();
foreach (Polygon p in ps.Zone.StayOutAreas)
{
if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position))
{
stayOuts.Add(p);
}
}
LinePath rB = ps.ParkingSpot.GetRightBound();
LinePath lB = ps.ParkingSpot.GetLeftBound();
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// create behavior
ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24),
ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0);
// maneuver
return(new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Pulling Out State
else if (planningState is PullingOutState)
{
// get state
PullingOutState pos = (PullingOutState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// final path seg
Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1];
Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0);
LinePath rp = new LinePath(new Coordinates[] { pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position,
zp.RecommendedPath[0], endBack });
LinePath lp = new LinePath(new Coordinates[] { zp.RecommendedPath[0], endBack });
LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0);
LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound));
// determine stay out areas to use
List <Polygon> stayOuts = new List <Polygon>();
foreach (Polygon p in pos.Zone.StayOutAreas)
{
if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position))
{
stayOuts.Add(p);
}
}
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(),
sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId,
rp, lB, rB);
// maneuver
return(new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Zone Startup State
else if (planningState is ZoneStartupState)
{
// state
ZoneStartupState zss = (ZoneStartupState)planningState;
// get the zone
ArbiterZone az = zss.Zone;
// navigational edge
INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
// check over all the parking spaces
foreach (ArbiterParkingSpot aps in az.ParkingSpots)
{
// inside both parts of spot
if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) ||
(vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position)))
{
// want to just park in it again
return(new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
Polygon forwardPolygon = vehicleState.ForwardPolygon;
Polygon rearPolygon = vehicleState.RearPolygon;
Navigator nav = CoreCommon.Navigation;
List <ZoneNavigationEdgeSort> forwardForward = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> reverseReverse = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> perpendicularPerpendicular = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> allEdges = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> allEdgesNoLimits = new List <ZoneNavigationEdgeSort>();
foreach (NavigableEdge ne in az.NavigableEdges)
{
if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
{
// get distance to edge
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
// get direction along segment
DirectionAlong da = vehicleState.DirectionAlongSegment(lp);
// check dist reasonable
if (distTmp > TahoeParams.VL)
{
// zone navigation edge sort item
ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp);
// add to lists
if (da == DirectionAlong.Forwards &&
(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)))
{
forwardForward.Add(znes);
}
/*else if (da == DirectionAlong.Perpendicular &&
* !(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) &&
* !(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)))
* perpendicularPerpendicular.Add(znes);
* else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))
* reverseReverse.Add(znes);*/
// add to all edges
allEdges.Add(znes);
}
}
}
// sort by distance
forwardForward.Sort();
reverseReverse.Sort();
perpendicularPerpendicular.Sort();
allEdges.Sort();
ZoneNavigationEdgeSort bestZnes = null;
for (int i = 0; i < allEdges.Count; i++)
{
// get line to initial
Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position);
Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position);
bool intersects = false;
Coordinates[] interPts;
foreach (Polygon sop in az.StayOutAreas)
{
if (!intersects &&
(sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts)))
{
intersects = true;
}
}
if (!intersects)
{
allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn);
allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
ZoneNavigationEdgeSort tmpZnes = allEdges[i];
if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) ||
(bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time))
{
bestZnes = tmpZnes;
}
}
if (i > allEdges.Count / 2 && bestZnes != null)
{
break;
}
}
if (bestZnes != null)
{
ArbiterOutput.Output("Found good edge to start in zone");
return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked");
List <ZoneNavigationEdgeSort> okZnes = new List <ZoneNavigationEdgeSort>();
foreach (NavigableEdge tmpOk in az.NavigableEdges)
{
// get line to initial
LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position });
double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath);
tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn);
tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
if (tmpZnes.zp.RecommendedPath.Count >= 2)
{
okZnes.Add(tmpZnes);
}
}
if (okZnes.Count == 0)
{
okZnes = allEdges;
}
else
{
okZnes.Sort();
}
// get random close edge
Random rand = new Random();
int chosen = rand.Next(Math.Min(5, okZnes.Count));
// get closest edge not part of a parking spot, get on it
NavigableEdge closest = okZnes[chosen].edge; //null;
//double distance = Double.MaxValue;
/*foreach (NavigableEdge ne in az.NavigableEdges)
* {
* if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
* {
* // get distance to edge
* LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
* double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
* if (closest == null || distTmp < distance)
* {
* closest = ne;
* distance = distTmp;
* }
* }
* }*/
return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
/// <summary>Called every frame to update the plugin.</summary>
public void UpdatePlugin()
{
if (Train.Cars == null || Train.Cars.Length == 0)
{
return;
}
/*
* Prepare the vehicle state.
* */
double location = this.Train.Cars[0].FrontAxle.Follower.TrackPosition - this.Train.Cars[0].FrontAxle.Position + 0.5 * this.Train.Cars[0].Length;
//If the list of stations has not been loaded, do so
if (!StationsLoaded)
{
currentRouteStations = new List <Station>();
int s = 0;
foreach (RouteStation selectedStation in TrainManagerBase.CurrentRoute.Stations)
{
double stopPosition = -1;
int stopIdx = TrainManagerBase.CurrentRoute.Stations[s].GetStopIndex(Train.NumberOfCars);
if (selectedStation.Stops.Length != 0)
{
stopPosition = selectedStation.Stops[stopIdx].TrackPosition;
}
Station i = new Station(selectedStation, stopPosition);
currentRouteStations.Add(i);
s++;
}
StationsLoaded = true;
}
//End of additions
double speed = this.Train.Cars[this.Train.DriverCar].Specs.PerceivedSpeed;
double bcPressure = this.Train.Cars[this.Train.DriverCar].CarBrake.brakeCylinder.CurrentPressure;
double mrPressure = this.Train.Cars[this.Train.DriverCar].CarBrake.mainReservoir.CurrentPressure;
double erPressure = this.Train.Cars[this.Train.DriverCar].CarBrake.equalizingReservoir.CurrentPressure;
double bpPressure = this.Train.Cars[this.Train.DriverCar].CarBrake.brakePipe.CurrentPressure;
double sapPressure = this.Train.Cars[this.Train.DriverCar].CarBrake.straightAirPipe.CurrentPressure;
VehicleState vehicle = new VehicleState(location, new Speed(speed), bcPressure, mrPressure, erPressure, bpPressure, sapPressure, this.Train.Cars[0].FrontAxle.Follower);
/*
* Prepare the preceding vehicle state.
* */
PrecedingVehicleState precedingVehicle;
try
{
AbstractTrain closestTrain = TrainManagerBase.currentHost.ClosestTrain(this.Train);
precedingVehicle = closestTrain != null ? new PrecedingVehicleState(closestTrain.RearCarTrackPosition(), closestTrain.RearCarTrackPosition() - location, new Speed(closestTrain.CurrentSpeed)) : new PrecedingVehicleState(Double.MaxValue, Double.MaxValue - location, new Speed(0.0));
}
catch
{
precedingVehicle = null;
}
/*
* Get the driver handles.
* */
OpenBveApi.Runtime.Handles handles = GetHandles();
/*
* Update the plugin.
* */
double totalTime = TrainManagerBase.currentHost.InGameTime;
double elapsedTime = TrainManagerBase.currentHost.InGameTime - LastTime;
ElapseData data = new ElapseData(vehicle, precedingVehicle, handles, this.Train.SafetySystems.DoorInterlockState, new Time(totalTime), new Time(elapsedTime), currentRouteStations, TrainManagerBase.Renderer.Camera.CurrentMode, Translations.CurrentLanguageCode, this.Train.Destination);
ElapseData inputDevicePluginData = data;
LastTime = TrainManagerBase.currentHost.InGameTime;
Elapse(ref data);
this.PluginMessage = data.DebugMessage;
this.Train.SafetySystems.DoorInterlockState = data.DoorInterlockState;
DisableTimeAcceleration = data.DisableTimeAcceleration;
for (int i = 0; i < InputDevicePlugin.AvailablePluginInfos.Count; i++)
{
if (InputDevicePlugin.AvailablePluginInfos[i].Status == InputDevicePlugin.PluginInfo.PluginStatus.Enable)
{
InputDevicePlugin.AvailablePlugins[i].SetElapseData(inputDevicePluginData);
}
}
/*
* Set the virtual handles.
* */
this.PluginValid = true;
SetHandles(data.Handles, true);
this.Train.Destination = data.Destination;
}
/// <summary>
/// Checks if vehicle is far enough away to leave
/// </summary>
public bool Clear(VehicleState ourState)
{
// if no current vehicle, the stuff is fine
if (currentVehicle == null)
{
return(true);
}
else
{
// check exit exists
if (this.exit != null)
{
// check not inside polygon
double distToExit;
if (this.exitPolygon.IsInside(this.currentVehicle.ClosestPointTo(this.exit.Position, ourState, out distToExit).Value))
{
if (!this.LargerExcessiveWaiting)
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " not clear, vehicle " + this.currentVehicle.ToString() + " in exit polygon" +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", is stopped: " + this.currentVehicle.IsStopped);
return(false);
}
else
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " not clear, vehicle " + this.currentVehicle.ToString() + " in exit polygon" +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", is stopped: " + this.currentVehicle.IsStopped + ", passed large excessive wait test so going");
return(true);
}
}
}
// get the point we are looking from
LinePath.PointOnPath referencePoint;
// reference from exit if exists
if (exit != null)
{
LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(exit.Position);
referencePoint = lane.LanePath().AdvancePoint(pop, 3.0);
}
// otherwise look from where we are closest
else
{
referencePoint = lane.LanePath().GetClosestPoint(ourState.Front);
}
// vehicle point
LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(currentVehicle.ClosestPosition);
// distance
double distance = lane.LanePath().DistanceBetween(vehiclePoint, referencePoint);
if (distance < 30 && (!this.currentVehicle.StateMonitor.Observed.speedValid || !this.currentVehicle.IsStopped))
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " not clear, moving vehicle " + this.currentVehicle.ToString() + " within 30m of intersection" +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", is stopped: " + this.currentVehicle.IsStopped);
return(false);
}
else if (distance > 0)
{
// vehicles speed
double vehicleSpeed = this.currentVehicle.StateMonitor.Observed.speedValid ? Math.Abs(currentVehicle.Speed) : lane.Way.Segment.SpeedLimits.MaximumSpeed;
if (!this.currentVehicle.StateMonitor.Observed.speedValid)
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + ", vehicle: " + this.currentVehicle.ToString() + " speed not valid, set to: " + lane.Way.Segment.SpeedLimits.MaximumSpeed.ToString("F2"));
}
// check time
double time = vehicleSpeed != 0 ? distance / vehicleSpeed : 777;
// clear if time greater than 7.0 seconds
if (time > 8.0)
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " clear, vehicle " + this.currentVehicle.ToString() +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", speed: " + this.currentVehicle.Speed.ToString("F2") +
", time to collision: " + time.ToString("F2") + " > 8");
return(true);
}
else
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " NOT clear, vehicle " + this.currentVehicle.ToString() +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", speed: " + this.currentVehicle.Speed.ToString("F2") +
", time to collision: " + time.ToString("F2") + " < 8");
return(false);
}
}
else
{
ArbiterOutput.Output("Exception: distance to vehicle negative in dlem is clear: " + distance.ToString("F3"));
return(false);
}
}
}
public bool CustomStartPathFind(ushort vehicleID, ref Vehicle vehicleData, Vector3 startPos, Vector3 endPos, bool startBothWays, bool endBothWays, bool undergroundTarget)
{
#if DEBUG
//Log._Debug($"CustomTaxiAI.CustomStartPathFind called for vehicle {vehicleID}");
#endif
#if PATHRECALC
VehicleState state = VehicleStateManager._GetVehicleState(vehicleID);
bool recalcRequested = state.PathRecalculationRequested;
state.PathRecalculationRequested = false;
#endif
CitizenManager instance = Singleton <CitizenManager> .instance;
ushort passengerInstanceId = CustomTaxiAI.GetPassengerInstance(vehicleID, ref vehicleData);
if (passengerInstanceId == 0 || (instance.m_instances.m_buffer[(int)passengerInstanceId].m_flags & CitizenInstance.Flags.Character) != CitizenInstance.Flags.None)
{
return(base.StartPathFind(vehicleID, ref vehicleData, startPos, endPos, startBothWays, endBothWays, undergroundTarget));
}
VehicleInfo info = this.m_info;
CitizenInfo info2 = instance.m_instances.m_buffer[(int)passengerInstanceId].Info;
NetInfo.LaneType laneTypes = NetInfo.LaneType.Vehicle | NetInfo.LaneType.Pedestrian | NetInfo.LaneType.TransportVehicle;
VehicleInfo.VehicleType vehicleType = this.m_info.m_vehicleType;
bool allowUnderground = (vehicleData.m_flags & Vehicle.Flags.Underground) != 0;
PathUnit.Position startPosA;
PathUnit.Position startPosB;
float startSqrDistA;
float startSqrDistB;
PathUnit.Position endPosA;
if (CustomPathManager.FindPathPosition(startPos, ItemClass.Service.Road, NetInfo.LaneType.Vehicle | NetInfo.LaneType.TransportVehicle, info.m_vehicleType, allowUnderground, false, 32f, out startPosA, out startPosB, out startSqrDistA, out startSqrDistB) &&
info2.m_citizenAI.FindPathPosition(passengerInstanceId, ref instance.m_instances.m_buffer[(int)passengerInstanceId], endPos, laneTypes, vehicleType, undergroundTarget, out endPosA))
{
if ((instance.m_instances.m_buffer[(int)passengerInstanceId].m_flags & CitizenInstance.Flags.CannotUseTransport) == CitizenInstance.Flags.None)
{
laneTypes |= NetInfo.LaneType.PublicTransport;
uint citizenId = instance.m_instances.m_buffer[passengerInstanceId].m_citizen;
if (citizenId != 0u && (instance.m_citizens.m_buffer[citizenId].m_flags & Citizen.Flags.Evacuating) != Citizen.Flags.None)
{
laneTypes |= NetInfo.LaneType.EvacuationTransport;
}
}
if (!startBothWays || startSqrDistA < 10f)
{
startPosB = default(PathUnit.Position);
}
PathUnit.Position endPosB = default(PathUnit.Position);
SimulationManager instance2 = Singleton <SimulationManager> .instance;
uint path;
if (CustomPathManager._instance.CreatePath(
#if PATHRECALC
recalcRequested,
#endif
ExtVehicleType.Taxi, vehicleID, 0, out path, ref instance2.m_randomizer, instance2.m_currentBuildIndex, startPosA, startPosB, endPosA, endPosB, laneTypes, vehicleType, 20000f))
{
#if USEPATHWAITCOUNTER
VehicleState state = VehicleStateManager.Instance._GetVehicleState(vehicleID);
state.PathWaitCounter = 0;
#endif
if (vehicleData.m_path != 0u)
{
Singleton <PathManager> .instance.ReleasePath(vehicleData.m_path);
}
vehicleData.m_path = path;
vehicleData.m_flags |= Vehicle.Flags.WaitingPath;
return(true);
}
}
return(false);
}
public IAction ReactTo(VehicleState state, int wayPoint) => actions.AllPossibleActions[random.Next(0, actions.AllPossibleActions.Count - 1)];
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Waiting At Intersection Exit
if (planningState is WaitingAtIntersectionExitState)
{
// state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// nullify turn reasoning
this.TurnReasoning = null;
#region Intersection Monitor Updates
// check correct intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) &&
(IntersectionTactical.IntersectionMonitor == null ||
!IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint)))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
waies.exitWaypoint,
CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
}
// update if exists
if (IntersectionTactical.IntersectionMonitor != null)
{
// update monitor
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
// print current
ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString());
}
#endregion
#region Desired Behavior
// get best option from previously saved
IConnectAreaWaypoints icaw = null;
if (waies.desired != null)
{
ArbiterInterconnect tmpInterconnect = waies.desired;
if (waies.desired.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric;
if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric))
{
icaw = init.NextPartition;
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = ip.BestOption;
waies.desired = icaw.ToInterconnect;
}
#endregion
#region Turn Feasibility Reasoning
// check uturn
if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn)
{
waies.turnTestState = TurnTestState.Completed;
}
// check already determined feasible
if (waies.turnTestState == TurnTestState.Unknown ||
waies.turnTestState == TurnTestState.Failed)
{
#region Determine Behavior to Accomplish Turn
// get default turn behavior
TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw);
// set saudi decorator
if (waies.saudi != SAUDILevel.None)
{
testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi));
}
// set to ignore all vehicles
testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
#endregion
#region Check Turn Feasible
// check if we have completed
CompletionReport turnCompletionReport;
bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport); //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true);
// if we have completed the test
if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic)
{
#region Can Complete
// check success
if (turnCompletionReport.Result == CompletionResult.Success)
{
// set completion state of the turn
waies.turnTestState = TurnTestState.Completed;
}
#endregion
#region No Saudi Level, Found Initial Blockage
// otherwise we cannot do the turn, check if saudi is still none
else if (waies.saudi == SAUDILevel.None)
{
// notify
ArbiterOutput.Output("Increased Saudi Level of Turn to L1");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Failed;
}
#endregion
#region Already at L1 Saudi
else if (waies.saudi == SAUDILevel.L1)
{
// notify
ArbiterOutput.Output("Turn with Saudi L1 Level failed");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString());
// set the interconnect as being blocked
CoreCommon.Navigation.AddInterconnectBlockage(waies.desired);
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
}
#endregion
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// notify
ArbiterOutput.Output("Had to fallout to using no turn bounds");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Completed;
waies.useTurnBounds = false;
}
#endregion
}
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Unknown;
waies.useTurnBounds = false;
}
#endregion
}
#endregion
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
#endregion
#region Entry Monitor Blocked
// checks the entry monitor vehicle for failure
if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null &&
IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed)
{
ArbiterOutput.Output("Entry area blocked");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired,
true);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95))
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final");
// set all the interconnects to the final as being blocked
if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry)
{
foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries)
{
CoreCommon.Navigation.AddInterconnectBlockage(toBlock);
}
}
// check if exists previous partition to block
if (waies.desired.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric;
if (finWaypoint.PreviousPartition != null)
{
CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false);
}
}
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
else
{
ArbiterOutput.Output("Entry area blocked, but no otehr valid route found");
}
}
#endregion
// check if can traverse
if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState))
{
#region If can traverse the intersection
// quick check not interconnect
if (!(icaw is ArbiterInterconnect))
{
icaw = icaw.ToInterconnect;
}
// get interconnect
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
// clear all old completion reports
CoreCommon.Communications.ClearCompletionReports();
// check if uturn
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
// go into turn
List <ArbiterLane> involvedLanes = new List <ArbiterLane>();
involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane);
involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane);
uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane,
IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes));
nextState.Interconnect = ai;
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
}
// otherwise need to wait
else
{
IState next = waies;
return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Stopping At Exit
else if (planningState is StoppingAtExitState)
{
// cast to exit stopping
StoppingAtExitState saes = (StoppingAtExitState)planningState;
saes.currentPosition = vehicleState.Front;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// if has an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
// update it
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
}
else
{
IntersectionTactical.IntersectionMonitor = null;
}
// otherwise update the stop parameters
saes.currentPosition = vehicleState.Front;
Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp));
}
#endregion
#region In uTurn
else if (planningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)planningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
{
// quick check
if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get the closest partition to the new lane
ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front);
// waypoints
ArbiterWaypoint partitionInitial = alpClose.Initial;
ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric;
// check initial past end
if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number)
{
// get waypoints inclusive
List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial);
bool found = false;
// loop through
foreach (ArbiterWaypoint aw in inclusive)
{
if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// set found
found = true;
}
}
}
// default check
else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
// check if the uturn is for a blockage
else if (uts.Interconnect == null)
{
// get final lane
ArbiterLane targetLane = uts.TargetLane;
// check has opposing
if (targetLane.Way.Segment.Lanes.Count > 1)
{
// check the final checkpoint is in our lane
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId))
{
// check that the final checkpoint is within the uturn polygon
if (uts.Polygon != null &&
uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position))
{
// remove the checkpoint
ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
}
}
// stay in target lane
IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true));
return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// otherwise continue uturn
else
{
// get polygon
Polygon p = uts.Polygon;
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon));
// check the type of uturn
if (!uts.singleLaneUturn)
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath();
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
else
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath().Clone();
endingPath = endingPath.ShiftLateral(-2.0); //uts.TargetLane.Width);
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound));
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
}
}
#endregion
#region In Turn
else if (planningState is TurnState)
{
// get state
TurnState ts = (TurnState)planningState;
// add bounds to observable
if (ts.LeftBound != null && ts.RightBound != null)
{
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound));
}
// create current turn reasoning
if (this.TurnReasoning == null)
{
this.TurnReasoning = new TurnReasoning(ts.Interconnect,
IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null);
}
// get primary maneuver
Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts);
// get secondary maneuver
Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan);
// return the manevuer
return(secondary.HasValue ? secondary.Value : primary);
}
#endregion
#region Itnersection Startup
else if (planningState is IntersectionStartupState)
{
// state and plan
IntersectionStartupState iss = (IntersectionStartupState)planningState;
IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan;
// initial path
LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front });
List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>();
// vehicle polygon forward of us
Polygon vehicleForward = vehicleState.ForwardPolygon;
// best waypoint
ITraversableWaypoint best = null;
double bestCost = Double.MaxValue;
// given feasible choose best, no feasible choose random
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
if (vehicleForward.IsInside(itw.Position))
{
feasibleEntries.Add(itw);
}
}
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
feasibleEntries.Add(itw);
}
}
}
// get best
foreach (ITraversableWaypoint itw in feasibleEntries)
{
if (isp.NodeTimeCosts.ContainsKey(itw))
{
KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]);
if (best == null || lookup.Value < bestCost)
{
best = lookup.Key;
bestCost = lookup.Value;
}
}
}
// get something going to this waypoint
ArbiterInterconnect interconnect = null;
if (best.IsEntry)
{
ArbiterInterconnect closest = null;
double closestDistance = double.MaxValue;
foreach (ArbiterInterconnect ai in best.Entries)
{
double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
if (closest == null || dist < closestDistance)
{
closest = ai;
closestDistance = dist;
}
}
interconnect = closest;
}
else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null)
{
interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect;
}
// get state
if (best is ArbiterWaypoint)
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString());
}
#endregion
}
public virtual void OnStateEnter(VehicleState previousState)
{
}
public virtual void OnStateExit(VehicleState nextState)
{
}
internal static float CalcMaxSpeed(ushort vehicleId, ref Vehicle vehicleData, PathUnit.Position position, Vector3 pos, float maxSpeed, bool isRecklessDriver)
{
var netManager = Singleton <NetManager> .instance;
NetInfo segmentInfo = netManager.m_segments.m_buffer[(int)position.m_segment].Info;
bool highwayRules = (segmentInfo.m_netAI is RoadBaseAI && ((RoadBaseAI)segmentInfo.m_netAI).m_highwayRules);
VehicleState state = VehicleStateManager.Instance.GetVehicleState(vehicleId);
if (!highwayRules)
{
if (netManager.m_treatWetAsSnow)
{
DistrictManager districtManager = Singleton <DistrictManager> .instance;
byte district = districtManager.GetDistrict(pos);
DistrictPolicies.CityPlanning cityPlanningPolicies = districtManager.m_districts.m_buffer[(int)district].m_cityPlanningPolicies;
if ((cityPlanningPolicies & DistrictPolicies.CityPlanning.StuddedTires) != DistrictPolicies.CityPlanning.None)
{
if (Options.strongerRoadConditionEffects)
{
if (maxSpeed > ICY_ROADS_STUDDED_MIN_SPEED)
{
maxSpeed = ICY_ROADS_STUDDED_MIN_SPEED + (float)(255 - netManager.m_segments.m_buffer[(int)position.m_segment].m_wetness) * 0.0039215686f * (maxSpeed - ICY_ROADS_STUDDED_MIN_SPEED);
}
}
else
{
maxSpeed *= 1f - (float)netManager.m_segments.m_buffer[(int)position.m_segment].m_wetness * 0.0005882353f; // vanilla: -15% .. ±0%
}
districtManager.m_districts.m_buffer[(int)district].m_cityPlanningPoliciesEffect |= DistrictPolicies.CityPlanning.StuddedTires;
}
else
{
if (Options.strongerRoadConditionEffects)
{
if (maxSpeed > ICY_ROADS_MIN_SPEED)
{
maxSpeed = ICY_ROADS_MIN_SPEED + (float)(255 - netManager.m_segments.m_buffer[(int)position.m_segment].m_wetness) * 0.0039215686f * (maxSpeed - ICY_ROADS_MIN_SPEED);
}
}
else
{
maxSpeed *= 1f - (float)netManager.m_segments.m_buffer[(int)position.m_segment].m_wetness * 0.00117647066f; // vanilla: -30% .. ±0%
}
}
}
else
{
if (Options.strongerRoadConditionEffects)
{
float minSpeed = Math.Min(maxSpeed * WET_ROADS_FACTOR, WET_ROADS_MAX_SPEED);
if (maxSpeed > minSpeed)
{
maxSpeed = minSpeed + (float)(255 - netManager.m_segments.m_buffer[(int)position.m_segment].m_wetness) * 0.0039215686f * (maxSpeed - minSpeed);
}
}
else
{
maxSpeed *= 1f - (float)netManager.m_segments.m_buffer[(int)position.m_segment].m_wetness * 0.0005882353f; // vanilla: -15% .. ±0%
}
}
if (Options.strongerRoadConditionEffects)
{
float minSpeed = Math.Min(maxSpeed * BROKEN_ROADS_FACTOR, BROKEN_ROADS_MAX_SPEED);
if (maxSpeed > minSpeed)
{
maxSpeed = minSpeed + (float)netManager.m_segments.m_buffer[(int)position.m_segment].m_condition * 0.0039215686f * (maxSpeed - minSpeed);
}
}
else
{
maxSpeed *= 1f + (float)netManager.m_segments.m_buffer[(int)position.m_segment].m_condition * 0.0005882353f; // vanilla: ±0% .. +15 %
}
}
if (Options.realisticSpeeds)
{
float vehicleRand = Math.Min(1f, (float)(vehicleId % 101) * 0.01f); // we choose 101 because it's a prime number
if (state != null && state.HeavyVehicle)
{
maxSpeed *= 0.9f + vehicleRand * 0.1f; // a little variance, 0.85 .. 1
}
else if (isRecklessDriver)
{
maxSpeed *= 1.2f + vehicleRand * 0.8f; // woohooo, 1.2 .. 2
}
else
{
maxSpeed *= 0.8f + vehicleRand * 0.5f; // a little variance, 0.8 .. 1.3
}
}
else
{
if (isRecklessDriver)
{
maxSpeed *= 1.4f;
}
}
maxSpeed = Math.Max(MIN_SPEED, maxSpeed); // at least 10 km/h
return(maxSpeed);
}
/// <summary>
/// Gets closest vehicle on lane before either the exit or where we are clsoest
/// </summary>
/// <param name="ourState"></param>
public void Update(VehicleState ourState)
{
#region Exit Polygon
// check the exit polygon
if (exit != null)
{
// create the polygon
Polygon p = this.exitPolygon;
// closest vehicle
VehicleAgent closest = null;
double closestDist = Double.MinValue;
// check all inside the polygon
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if stopped
if (va.IsStopped) // && va.StateMonitor.Observed.speedValid)
{
// get distance to exit
double distToExit;
Coordinates c = va.ClosestPointTo(this.exit.Position, ourState, out distToExit).Value;
// check inside polygon
if (p.IsInside(c))
{
// check dist
if (closest == null || distToExit < closestDist)
{
closest = va;
closestDist = distToExit;
}
}
}
}
// check closest not null
if (closest != null)
{
// check vehicle switch
if (currentVehicle != closest)
{
this.ResetTiming();
}
// check if waiting timer not running
if (!this.waitingTimer.IsRunning)
{
this.ResetTiming();
this.waitingTimer.Start();
}
// set the new vehicle
this.currentVehicle = closest;
// return to stop update
return;
}
else
{
// check if we were waiting for something in this area
if (this.globalMonitor.PreviouslyWaiting.Contains(this))
{
// remove the previous waiting
this.globalMonitor.PreviouslyWaiting.Remove(this);
}
}
}
#endregion
#region Lane Vehicles
// get the possible vehicles that could be in this stop
if (TacticalDirector.VehicleAreas.ContainsKey(this.lane))
{
// get vehicles in the the lane of the exit
List <VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[this.lane];
// get the point we are looking from
LinePath.PointOnPath referencePoint;
// reference from exit if exists
if (exit != null)
{
LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(exit.Position);
referencePoint = lane.LanePath().AdvancePoint(pop, 3.0);
}
// otherwise look from where we are closest
else
{
referencePoint = lane.LanePath().GetClosestPoint(ourState.Front);
}
// tmp
VehicleAgent tmp = null;
double tmpDist = Double.MaxValue;
// check over all possible vehicles in the lane
foreach (VehicleAgent possible in possibleVehicles)
{
// get vehicle point on lane
LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);
// check if the vehicle is behind the reference point
double vehicleDist = lane.LanePath().DistanceBetween(vehiclePoint, referencePoint);
// check for closest
if (vehicleDist > 0 && (tmp == null || vehicleDist < tmpDist))
{
tmp = possible;
tmpDist = vehicleDist;
}
}
// set the closest
this.currentVehicle = tmp;
}
else
{
// set the closest
this.currentVehicle = null;
}
#endregion
// reset the timing if got this far
this.ResetTiming();
}
public LocalVehicleController()
{
state = new VehicleState();
state.Position = Vector3.zero;
state.Rotation = Quaternion.identity;
}
/// <summary>
/// Check if we can go
/// </summary>
/// <param name="vs"></param>
private bool CanGo(VehicleState vs)
{
#region Moving Vehicles Inside Turn
// check if we can still go through this turn
if (TacticalDirector.VehicleAreas.ContainsKey(this.turn))
{
// get the subpath of the interconnect we care about
LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front);
LinePath aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint);
if (aiSubpath.PathLength > 4.0)
{
aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0);
// get vehicles
List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn];
// loop vehicles
foreach (VehicleAgent va in turnVehicles)
{
// check if inside turn
LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition);
if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint))
{
ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping");
return(false);
}
}
}
}
#endregion
// test if this turn is part of an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId))
{
// intersection
ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId];
// check if priority lanes exist for this interconnect
if (inter.PriorityLanes.ContainsKey(this.turn))
{
// get all the default priority lanes
List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn];
// get the subpath of the interconnect we care about
//LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front);
LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position })); //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint);
// check if path ended
if (aiSubpath.Count < 2)
{
return(true);
}
// determine all of the new priority lanes
List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>();
#region Determine new priority areas given position
// loop through old priorities
foreach (IntersectionInvolved ii in priorities)
{
// check ii lane
if (ii.Area is ArbiterLane)
{
#region Lane Intersects Turn Path w/ Point of No Return
// check if the waypoint is not the last waypoint in the lane
if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null)
{
// check where line intersects path
Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric);
// check for an intersection
if (intersect.HasValue)
{
// distance to intersection
double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0;
// determine if we can stop before or after the intersection
double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value);
// check dist to intersection > distance to stoppage
if (distanceToIntersection > distanceToStoppage)
{
// add updated priority
updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)),
ii.Area, ArbiterTurnDirection.Straight));
}
else
{
ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString());
}
}
}
#endregion
// we know there is an exit and it is the last waypoint of the segment, fuxk!
else
{
#region Turns Intersect
// get point to look at if exists
ArbiterInterconnect interconnect;
Coordinates? intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect);
// check for the intersect
if (intersect.HasValue)
{
ArbiterLane al = (ArbiterLane)ii.Area;
LinePath lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position));
lp.Add(interconnect.InterconnectPath.EndPoint.Location);
// get our time to the intersection point
//double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value));
// get our time to the intersection point
double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value);
double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0;
double extraTime = 1.5;
double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed;
double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime);
// get closest vehicle in that lane to the intersection
List <VehicleAgent> toLook = new List <VehicleAgent>();
if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area))
{
foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area])
{
double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position);
if (upstreamDist > 0 && tmpVa.PassedDelayedBirth)
{
toLook.Add(tmpVa);
}
}
}
if (TacticalDirector.VehicleAreas.ContainsKey(interconnect))
{
toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]);
}
// check length of stuff to look at
if (toLook.Count > 0)
{
foreach (VehicleAgent va in toLook)
{
// distance along path to location of intersect
double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location));
double speed = va.Speed == 0.0 ? 0.01 : va.Speed;
double vaTime = distToIntersect / Math.Abs(speed);
if (vaTime > 0 && vaTime < ourTime)
{
ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO");
return(false);
}
}
}
}
#endregion
}
}
}
#endregion
#region Updated Priority Intersection Code
// loop through updated priorities
bool updatedPrioritiesClear = true;
foreach (IntersectionInvolved ii in updatedPriorities)
{
// lane
ArbiterLane al = (ArbiterLane)ii.Area;
// get our time to the intersection point
double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value);
double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0;
double extraTime = 1.5;
double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed;
double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime);
// double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value));
// get closest vehicle in that lane to the intersection
if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area))
{
// get lane vehicles
List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area];
// determine for all
VehicleAgent closestLaneVa = null;
double closestDistanceVa = Double.MaxValue;
double closestTime = Double.MaxValue;
foreach (VehicleAgent testVa in vas)
{
// check upstream
double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position);
// get speed
double speed = testVa.Speed;
double time = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed;
// check distance > 0
if (distance > 0)
{
// check if closer or none other exists
if (closestLaneVa == null || time < closestTime)
{
closestLaneVa = testVa;
closestDistanceVa = distance;
closestTime = time;
}
}
}
// check if closest exists
if (closestLaneVa != null)
{
// set va
VehicleAgent va = closestLaneVa;
double distance = closestDistanceVa;
// check dist and birth time
if (distance > 0 && va.PassedDelayedBirth)
{
// check time
double speed = va.Speed == 0.0 ? 0.01 : va.Speed;
double time = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed;
// too close
if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) &&
distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) &&
CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20)
{
ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping");
//return false;
updatedPrioritiesClear = false;
return(false);
}
else if (time > 0 && time < ourTime)
{
ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2"));
//return false;
updatedPrioritiesClear = false;
return(false);
}
else
{
ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2"));
//return true;
}
}
}
else
{
ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles");
}
}
}
return(updatedPrioritiesClear);
#endregion
}
}
// fall through fine to go
ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream");
return(true);
}
/// <summary>Called every frame to update the plugin.</summary>
internal void UpdatePlugin()
{
/*
* Prepare the vehicle state.
* */
double location = this.Train.Cars[0].FrontAxle.Follower.TrackPosition - this.Train.Cars[0].FrontAxlePosition + 0.5 * this.Train.Cars[0].Length;
double speed = this.Train.Cars[this.Train.DriverCar].Specs.CurrentPerceivedSpeed;
double bcPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.BrakeCylinderCurrentPressure;
double mrPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.MainReservoirCurrentPressure;
double erPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.EqualizingReservoirCurrentPressure;
double bpPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.BrakePipeCurrentPressure;
double sapPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.StraightAirPipeCurrentPressure;
VehicleState vehicle = new VehicleState(location, new Speed(speed), bcPressure, mrPressure, erPressure, bpPressure, sapPressure);
/*
* Prepare the preceding vehicle state.
* */
double bestLocation = double.MaxValue;
double bestSpeed = 0.0;
for (int i = 0; i < TrainManager.Trains.Length; i++)
{
if (TrainManager.Trains[i] != this.Train & TrainManager.Trains[i].State == TrainManager.TrainState.Available)
{
int c = TrainManager.Trains[i].Cars.Length - 1;
double z = TrainManager.Trains[i].Cars[c].RearAxle.Follower.TrackPosition - TrainManager.Trains[i].Cars[c].RearAxlePosition - 0.5 * TrainManager.Trains[i].Cars[c].Length;
if (z >= location & z < bestLocation)
{
bestLocation = z;
bestSpeed = TrainManager.Trains[i].Specs.CurrentAverageSpeed;
}
}
}
PrecedingVehicleState precedingVehicle;
if (bestLocation != double.MaxValue)
{
precedingVehicle = new PrecedingVehicleState(bestLocation, bestLocation - location, new Speed(bestSpeed));
}
else
{
precedingVehicle = null;
}
/*
* Get the driver handles.
* */
Handles handles = GetHandles();
/*
* Update the plugin.
* */
double totalTime = Game.SecondsSinceMidnight;
double elapsedTime = Game.SecondsSinceMidnight - LastTime;
ElapseData data = new ElapseData(vehicle, precedingVehicle, handles, new Time(totalTime), new Time(elapsedTime));
LastTime = Game.SecondsSinceMidnight;
Elapse(data);
this.PluginMessage = data.DebugMessage;
/*
* Set the virtual handles.
* */
this.PluginValid = true;
SetHandles(data.Handles, true);
}
/// <summary>
/// Gets primary maneuver given our position and the turn we are traveling upon
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState)
{
#region Check are planning over the correct turn
if (CoreCommon.CorePlanningState is TurnState)
{
TurnState ts = (TurnState)CoreCommon.CorePlanningState;
if (this.turn == null || !this.turn.Equals(ts.Interconnect))
{
this.turn = ts.Interconnect;
this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null);
}
else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect))
{
this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null);
}
}
#endregion
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not at highest level already
if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds)
{
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic ||
(TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) &&
TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped))
{
// go to a blockage handling tactical
return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring");
}
}
else
{
ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report");
}
}
#endregion
#region Intersection Check
if (!this.CanGo(vehicleState))
{
if (turn.FinalGeneric is ArbiterWaypoint)
{
TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false);
return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL);
// hold brake
IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0));
TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId);
return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Final is Lane Waypoint
if (turn.FinalGeneric is ArbiterWaypoint)
{
// final point
ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric;
// plan down entry lane
RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>());
// point of interest downstream
DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;
// get path this represents
List <Coordinates> pathCoordinates = new List <Coordinates>();
pathCoordinates.Add(vehicleState.Position);
foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1]))
{
pathCoordinates.Add(aw.Position);
}
LinePath lp = new LinePath(pathCoordinates);
// list of all parameterizations
List <TravelingParameters> parameterizations = new List <TravelingParameters>();
// get lane navigation parameterization
TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp);
parameterizations.Add(navigationParameters);
// update forward tracker and get vehicle parameterizations if forward vehicle exists
this.forwardMonitor.Update(vehicleState, final, lp);
if (this.forwardMonitor.ShouldUseForwardTracker())
{
// get vehicle parameterization
TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final);
parameterizations.Add(vehicleParameters);
}
// sort and return funal
parameterizations.Sort();
// get the final behavior
TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior;
// get vehicles to ignore
tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore;
// add persistent information about saudi level
if (turnState.Saudi == SAUDILevel.L1)
{
tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray());
tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1));
}
// add persistent information about turn bounds
if (!turnState.UseTurnBounds)
{
tb.LeftBound = null;
tb.RightBound = null;
}
// return the behavior
return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp));
}
#endregion
#region Final is Zone Waypoint
else if (turn.FinalGeneric is ArbiterPerimeterWaypoint)
{
// get inteconnect path
Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center -
turn.FinalGeneric.Position;
entryVec = entryVec.Normalize(TahoeParams.VL / 2.0);
LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position });
// get distance from end
double d = ip.DistanceBetween(
ip.GetClosestPoint(vehicleState.Front),
ip.EndPoint);
// get speed command
SpeedCommand sc = null;
if (d < TahoeParams.VL)
{
sc = new StopAtDistSpeedCommand(d);
}
else
{
sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed));
}
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL);
// hold brake
IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc);
TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId);
// add persistent information about saudi level
if (turnState.Saudi == SAUDILevel.L1)
{
tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray());
tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1));
}
// add persistent information about turn bounds
if (!turnState.UseTurnBounds)
{
tb.LeftBound = null;
tb.RightBound = null;
}
// return maneuver
return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp));
}
#endregion
#region Unknown
else
{
throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString());
}
#endregion
}
public void SetState(VehicleState state)
{
transform.position = state.Position;
transform.rotation = state.Rotation;
}
/// <summary>
/// Plans if we need secondary maneuver if intersection breaks down and need to abort
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="intersectionPlan"></param>
/// <returns></returns>
public Maneuver?SecondaryManeuver(VehicleState vehicleState, IntersectionPlan intersectionPlan)
{
// don't say anything
return(null);
}
public Coordinates TransformCoordAbs(Coordinates c, VehicleState state)
{
c = c.Rotate(state.Heading.ArcTan);
c = c + state.Position;
return(c);
}
/// <summary>
/// Gets parameterization
/// </summary>
/// <param name="lane"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters GetParameters(double speed, double distance, ArbiterWaypoint final, VehicleState state, bool canUseDistance)
{
double distanceCutOff = CoreCommon.OperationalStopDistance;
SpeedCommand sc;
bool usingSpeed = true;
Maneuver m;
#region Generate Next State
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(final, out finalPath, out leftLL, out rightLL);
TurnState ts = new TurnState(this.turn, this.turn.TurnDirection, final.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(speed));
#endregion
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff && canUseDistance)
{
// default behavior
sc = new StopAtDistSpeedCommand(distance);
// stopping so not using speed param
usingSpeed = false;
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// default behavior
sc = new ScalarSpeedCommand(speed);
}
#endregion
#region Generate Maneuver
if (ts.Interconnect.InitialGeneric is ArbiterWaypoint &&
CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(((ArbiterWaypoint)ts.Interconnect.InitialGeneric).AreaSubtypeWaypointId))
{
Polygon p = CoreCommon.RoadNetwork.IntersectionLookup[((ArbiterWaypoint)ts.Interconnect.InitialGeneric).AreaSubtypeWaypointId].IntersectionPolygon;
// behavior
Behavior b = new TurnBehavior(ts.TargetLane.LaneId, ts.EndingPath, ts.LeftBound, ts.RightBound, sc, p, this.forwardMonitor.VehiclesToIgnore, ts.Interconnect.InterconnectId);
m = new Maneuver(b, ts, ts.DefaultStateDecorators, state.Timestamp);
}
else
{
// behavior
Behavior b = new TurnBehavior(ts.TargetLane.LaneId, ts.EndingPath, ts.LeftBound, ts.RightBound, sc, null, this.forwardMonitor.VehiclesToIgnore, ts.Interconnect.InterconnectId);
m = new Maneuver(b, ts, ts.DefaultStateDecorators, state.Timestamp);
}
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore;
#endregion
// return navigation params
return(tp);
}
/// <summary>
/// Updates this monitor
/// </summary>
public void Update(VehicleState ourState)
{
// make sure not our vehicle
if (!isOurs)
{
// get a list of vehicles possibly in the exit
List <VehicleAgent> stopVehicles = new List <VehicleAgent>();
// check all valid vehicles
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if any of the point inside the polygon
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
// get abs coord
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
// check if inside poly
if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
{
// add vehicle
stopVehicles.Add(va);
}
}
}
// check occluded
foreach (VehicleAgent va in TacticalDirector.OccludedVehicles.Values)
{
// check if any of the point inside the polygon
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
// get abs coord
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
// check if inside poly
if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
{
// add vehicle
ArbiterOutput.Output("Added occluded vehicle: " + va.ToString() + " to SEM: " + this.stoppedExit.ToString());
stopVehicles.Add(va);
}
}
}
// check if we already have a vehicle we are referencing
if (this.currentVehicle != null && !stopVehicles.Contains(this.currentVehicle))
{
// get the polygon of the current vehicle and inflate a tiny bit?
Polygon p = this.currentVehicle.GetAbsolutePolygon(ourState);
p = p.Inflate(1.0);
// Vehicle agent to switch to if exists
VehicleAgent newer = null;
double distance = Double.MaxValue;
// check for closest vehicle to exist in that polygon
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if vehicle is stopped
if (stopVehicles.Contains(va))
{
// get distance to stop
double stopLineDist;
va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out stopLineDist);
// check all vehicle points
foreach (Coordinates rel in va.StateMonitor.Observed.relativePoints)
{
Coordinates abs = va.TransformCoordAbs(rel, ourState);
double tmpDist = stopLineDist;
// check for the clsoest with points inside vehicle
if (p.IsInside(abs) && (newer == null || tmpDist < distance))
{
newer = va;
distance = tmpDist;
}
}
}
}
// check if we can switch vehicles
if (newer != null)
{
ArbiterOutput.Output("For StopLine: " + this.stoppedExit.ToString() + " switched vehicleId: " + this.currentVehicle.VehicleId.ToString() + " for vehicleId: " + newer.VehicleId.ToString());
this.currentVehicle = newer;
this.NotFoundPrevious = notFoundDefault;
}
else if (NotFoundPrevious == 0)
{
// set this as not having a vehicle
this.currentVehicle = null;
// set not found before
this.NotFoundPrevious = this.notFoundDefault;
// stop timers
this.timer.Stop();
this.timer.Reset();
}
else
{
// set not found before
this.NotFoundPrevious--;
ArbiterOutput.Output("current not found, not found previous: " + this.NotFoundPrevious.ToString() + ", at stopped exit: " + this.stoppedExit.Waypoint.ToString());
}
}
// update the current vehicle
else if (this.currentVehicle != null && stopVehicles.Contains(this.currentVehicle))
{
// udpate current
this.currentVehicle = stopVehicles[stopVehicles.IndexOf(this.currentVehicle)];
// set as found previous
this.NotFoundPrevious = this.notFoundDefault;
// check if need to update timer
if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
this.timer.Start();
}
// otherwise check if moving
else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
}
}
// otherwise if we have no vehicle and exist vehicles in stop area
else if (this.currentVehicle == null && stopVehicles.Count > 0)
{
// get closest vehicle to the stop
VehicleAgent toTrack = null;
double distance = Double.MaxValue;
// set as found previous
this.NotFoundPrevious = this.notFoundDefault;
// loop through
foreach (VehicleAgent va in stopVehicles)
{
// check if vehicle is stopped
if (va.IsStopped)
{
// distance of vehicle to stop line
double distanceToStop;
Coordinates closestToStop = va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out distanceToStop).Value;
// check if we don't have one or tmp closer than tracked
if (toTrack == null || distanceToStop < distance)
{
toTrack = va;
distance = distanceToStop;
}
}
}
// check if we have one to track
if (toTrack != null)
{
this.currentVehicle = toTrack;
}
}
}
}
/// <summary>Called every frame to update the plugin.</summary>
internal void UpdatePlugin()
{
/*
* Prepare the vehicle state.
* */
double location = this.Train.Cars[0].FrontAxle.Follower.TrackPosition - this.Train.Cars[0].FrontAxle.Position + 0.5 * this.Train.Cars[0].Length;
//Curve Radius, Cant and Pitch Added
double CurrentRadius = this.Train.Cars[0].FrontAxle.Follower.CurveRadius;
double CurrentCant = this.Train.Cars[0].FrontAxle.Follower.CurveCant;
double CurrentPitch = this.Train.Cars[0].FrontAxle.Follower.Pitch;
//If the list of stations has not been loaded, do so
if (!StationsLoaded)
{
currentRouteStations = new List <Station>();
foreach (Game.Station selectedStation in Game.Stations)
{
Station i = new Station
{
Name = selectedStation.Name,
ArrivalTime = selectedStation.ArrivalTime,
DepartureTime = selectedStation.DepartureTime,
StopTime = selectedStation.StopTime,
OpenLeftDoors = selectedStation.OpenLeftDoors,
OpenRightDoors = selectedStation.OpenRightDoors,
ForceStopSignal = selectedStation.ForceStopSignal,
DefaultTrackPosition = selectedStation.DefaultTrackPosition
};
currentRouteStations.Add(i);
}
StationsLoaded = true;
}
//End of additions
double speed = this.Train.Cars[this.Train.DriverCar].Specs.CurrentPerceivedSpeed;
double bcPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.BrakeCylinderCurrentPressure;
double mrPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.MainReservoirCurrentPressure;
double erPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.EqualizingReservoirCurrentPressure;
double bpPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.BrakePipeCurrentPressure;
double sapPressure = this.Train.Cars[this.Train.DriverCar].Specs.AirBrake.StraightAirPipeCurrentPressure;
VehicleState vehicle = new VehicleState(location, new Speed(speed), bcPressure, mrPressure, erPressure, bpPressure, sapPressure, CurrentRadius, CurrentCant, CurrentPitch);
/*
* Prepare the preceding vehicle state.
* */
double bestLocation = double.MaxValue;
double bestSpeed = 0.0;
for (int i = 0; i < TrainManager.Trains.Length; i++)
{
if (TrainManager.Trains[i] != this.Train & TrainManager.Trains[i].State == TrainManager.TrainState.Available)
{
int c = TrainManager.Trains[i].Cars.Length - 1;
double z = TrainManager.Trains[i].Cars[c].RearAxle.Follower.TrackPosition - TrainManager.Trains[i].Cars[c].RearAxle.Position - 0.5 * TrainManager.Trains[i].Cars[c].Length;
if (z >= location & z < bestLocation)
{
bestLocation = z;
bestSpeed = TrainManager.Trains[i].Specs.CurrentAverageSpeed;
}
}
}
var precedingVehicle = bestLocation != double.MaxValue ? new PrecedingVehicleState(bestLocation, bestLocation - location, new Speed(bestSpeed)) : null;
/*
* Get the driver handles.
* */
Handles handles = GetHandles();
/*
* Update the plugin.
* */
double totalTime = Game.SecondsSinceMidnight;
double elapsedTime = Game.SecondsSinceMidnight - LastTime;
/*
* Set the current camera view mode
* Could probably do away with the CurrentCameraViewMode and use a direct cast??
*
*/
CurrentCameraViewMode = (OpenBveApi.Runtime.CameraViewMode)World.CameraMode;
ElapseData data = new ElapseData(vehicle, precedingVehicle, handles, (DoorInterlockStates)this.Train.Specs.DoorInterlockState, new Time(totalTime), new Time(elapsedTime), currentRouteStations, CurrentCameraViewMode, Interface.CurrentLanguageCode);
LastTime = Game.SecondsSinceMidnight;
Elapse(data);
this.PluginMessage = data.DebugMessage;
this.Train.Specs.DoorInterlockState = (TrainManager.DoorInterlockStates)data.DoorInterlockState;
DisableTimeAcceleration = data.DisableTimeAcceleration;
/*
* Set the virtual handles.
* */
this.PluginValid = true;
SetHandles(data.Handles, true);
}
public void AsyncLoadingScript(GameObject myReturnValue)
{
#region 读取模型
transform.position += StartPointOffSet;
InstanceMesh = Instantiate(myReturnValue) as GameObject;
InstanceMesh.transform.SetParent(this.transform);
InstanceMesh.transform.localPosition = Vector3.zero;
InstanceMesh.transform.localEulerAngles = Vector3.zero;
#endregion
#if UNITY_EDITOR && UNITY_ANDROID
foreach (MeshRenderer meshRenderer in InstanceMesh.GetComponentsInChildren <MeshRenderer>())
{
meshRenderer.sharedMaterial.shader = Shader.Find("Standard");
}
#endif
#region 设置网络状态
if (_InstanceNetType != InstanceNetType.GarageTank && !isBot(_InstanceNetType) && _InstanceNetType != InstanceNetType.GameNetWorkOffline)
{
}
#endregion
//模块加载
dontDestroyManager = new VehicleDontDestroyManagerModule();
referenceManager = InstanceMesh.GetComponent <VehicleComponentsReferenceManager>();
#region 加载完毕游戏资源加载脚本
TankPrefabs = gameObject;
TurretTransform = InstanceMesh.transform.Find("TurretTransform").gameObject;
MainHitBox = InstanceMesh.transform.Find("MainHitBox").gameObject;
TankTransform = InstanceMesh.transform.Find("TankTransform").gameObject;
GunTransform = TurretTransform.transform.Find("GunTransform").gameObject;
FFPoint = GunTransform.transform.Find("FFPoint").gameObject;
EffectStart = GunTransform.transform.Find("EffectStart").gameObject;
TurretHitBox = TurretTransform.transform.Find("TurretHitBox").gameObject;
GameObject TankScript = null, TankCrossHair = null, MainCamera = null;
Rigidbody TankPhysic = null;
TurretController mt = null;
// SyncGroundVehicle _syncGroundVehicle = null;
PlayerTracksController PTC = null;
PlayerState PS = null;
PTC = InstanceMesh.AddComponent <PlayerTracksController> ();
#region 物理效果
TankPhysic = GetComponentInChildren <Rigidbody> ();
TankPhysic.mass = PTCParameter.Mass;
TankPhysic.drag = 0.1f; //PTCParameter.Drag;
TankPhysic.angularDrag = 2.5f; //PTCParameter.AirDrag;
TankPhysic.useGravity = false;
BoxCollider[] collisions = PTC.GetComponentsInChildren <BoxCollider> ();
foreach (Collider collision in collisions)
{
if (collision.isTrigger != true)
{
collision.material = StaticResourcesReferences.Instance.VehiclePhysicMaterial;
}
}
#endregion
if (_InstanceNetType != InstanceNetType.GarageTank)
{
if (isLocalPlayer(_InstanceNetType))
{
TankCrossHair = Instantiate((GameObject)Resources.Load("TankCrossHair"), Vector3.zero, Quaternion.identity) as GameObject;
MainCamera = Instantiate((GameObject)Resources.Load("MainCamera"), Vector3.zero, Quaternion.identity) as GameObject;
TankCrossHair.transform.parent = TankPrefabs.transform;
MainCamera.transform.parent = InstanceMesh.transform;
MainCamera.transform.name = "MainCamera";
}
TankScript = Instantiate((GameObject)Resources.Load("TankScript"), Vector3.zero, Quaternion.identity) as GameObject;
TankScript.transform.name = "TankScript";
TankScript.transform.parent = TankPrefabs.transform;
TankPhysic.useGravity = true;
#region PlayerState 脚本设置
if (!isBot(_InstanceNetType))
{
PS = InstanceMesh.AddComponent <PlayerState>();
PS.tankInitSystem = this;
foreach (HitBox HB in GetComponentsInChildren <HitBox>())
{
HB.SetTarget(PS);
}
PS.playerStateParameter = PSParameter;
PS.netType = _InstanceNetType;
PS.Turret = TurretTransform;
PS.TankScript = TankScript;
PS.DiedDestoryObjects = new Object[] { MainHitBox, TurretHitBox };
PS.TankName = VehicleName;
PS.Health = PSParameter.Health;
PS.engineType = PSParameter.engineType;
PS.IsMobile = isMobile;
}
#endregion
if (isBot(_InstanceNetType))
{
EnemyAiState enemyState = InstanceMesh.AddComponent <EnemyAiState>();
foreach (HitBox HB in GetComponentsInChildren <HitBox>())
{
HB.SetTarget(enemyState);
}
enemyState.TankScript = TankScript;
enemyState.Health = PSParameter.Health;
enemyState.TankName = VehicleName;
enemyState.MyTeam = ownerTeam;
}
#region TankScript 脚本设置
mt = TankScript.GetComponent <TurretController>();
if (isLocalPlayer(_InstanceNetType))
{
mt.target = MainCamera.transform.Find("CameraTarget");
}
mt.gun = GunTransform.transform;
mt.Turret = TurretTransform.transform;
mt.DownMaxDegree = MTParameter.DownMaxDegree;
mt.UpMaxDegree = MTParameter.UpMaxDegree;
mt.maxTurretAngle = MTParameter.maxTurretAngle;
mt.gunDegreesPerSecond = MTParameter.gunDegreesPerSecond;
mt.turretDegreesPerSecond = MTParameter.turretDegreesPerSecond;
mt.isMobile = isMobile;
#endregion
#region 主摄像机设置
if (isLocalPlayer(_InstanceNetType))
{
MainCamera.GetComponent <VehicleCamera>().target = referenceManager.MainCameraFollowTarget.transform;
MainCamera.GetComponent <VehicleCamera>().mainCameraFollowTarget = referenceManager.MainCameraFollowTarget.transform;
MainCamera.GetComponent <VehicleCamera>().mainCameraGunner = referenceManager.MachineGunFFPoint.transform;
referenceManager.MachineGunFFPoint.transform.position = referenceManager.FFPoint.transform.position;
MainCamera.GetComponent <VehicleCamera>().IsMobile = isMobile;
if (PSParameter.vehicleType == VehicleType.SPG)
{
MainCamera.GetComponent <VehicleCamera>().isVehicleSPG = true;
}
}
#endregion
#region 瞄准射线
if (isLocalPlayer(_InstanceNetType))
{
RayManager rayManager = TankScript.AddComponent <RayManager>();
rayManager.Init(FFPoint.transform);
}
#endregion
#region 瞄准镜设置
if (isLocalPlayer(_InstanceNetType))
{
TankCrossHair.GetComponent <CrossHair>().MainCamera = MainCamera.GetComponent <Camera>();
TankCrossHair.GetComponent <CrossHair>().tankCamera = MainCamera.GetComponent <VehicleCamera>();
TankCrossHair.GetComponent <CrossHair>().FFPoint = EffectStart.transform;
}
#endregion
#region 坦克发射器脚本设置
TFParameter.recoilTransform = referenceManager.FireForceFeedbackPoint.transform;
TankFire tf = TankScript.GetComponent <TankFire>();
tf.tankFireParameter = TFParameter;
tf.tankInitSystem = this;
tf.MachineGunFFPoint = referenceManager.MachineGunFFPoint.transform;
tf.FFPoint = referenceManager.FFPoint.transform;
tf.FireEffectPoint = referenceManager.EffectStart.transform;
tf.BulletCountList = BulletCountList;
tf.GunDym = GunTransform.transform.Find("GunDym").GetComponent <Animator>();
tf.MainBody = InstanceMesh.transform;
tf.netType = _InstanceNetType;
if (PSParameter.vehicleType == VehicleType.SPG)
{
tf.isAutoCaclulateGravity = true;
}
#endregion
#region 引擎音效控制
EngineSoundModule engineSoundModule = InstanceMesh.AddComponent <EngineSoundModule>();
engineSoundModule.tankInitSystem = this;
engineSoundModule.engineData = PTCParameter.vehicleEngineSoundData;
engineSoundModule.Init(referenceManager.EngineSound.transform);
#endregion
GameObject EngineSmoke = Instantiate <GameObject>(Resources.Load <GameObject>("EngineSmoke"));
EngineSmoke.transform.SetParent(referenceManager.EngineSmoke.transform);
EngineSmoke.transform.localPosition = Vector3.zero;
EngineSmoke.transform.localEulerAngles = Vector3.zero;
//GameObject grounddust = Instantiate(Resources.Load<GameObject>("TrackEffect"));
//grounddust.transform.SetParent(InstanceMesh..transform);
//grounddust.transform.localPosition = Vector3.zero;
//grounddust.transform.localEulerAngles = Vector3.zero;
VehicleState vehicleState = InstanceMesh.AddComponent <VehicleState>();
vehicleState.engineSoundModule = engineSoundModule;
vehicleState.tankTracksController = PTC;
}
#region 坦克控制器设置
PTC.tankInitSystem = this;
PTC.rightTrackUpperWheels = GetAllChilds(TankTransform.transform.Find("RightUpperWheel"));
PTC.leftTrackUpperWheels = GetAllChilds(TankTransform.transform.Find("LeftUpperWheel"));
PTC.rightTrackWheels = GetAllChilds(TankTransform.transform.Find("RightWheel"));
PTC.leftTrackWheels = GetAllChilds(TankTransform.transform.Find("LeftWheel"));
PTC.leftTrack = referenceManager.LeftTrack;
PTC.rightTrack = referenceManager.RightTrack;
//PTC.leftTrackBones = CreateTrack (true, TankTransform.transform, ref PTC);
//PTC.rightTrackBones = CreateTrack (false, TankTransform.transform, ref PTC);
PTC.tankTransform = TankTransform.transform;
PTC.maxAngularVelocity = PTCParameter.MaxAngularVelocity;
PTC.MaxSpeed = PTCParameter.MaxSpeed;
PTC.MinSpeed = PTCParameter.MinSpeed;
PTC.gravityCenter = PTCParameter.CenterOfGravity;
PTC.PushSpeed = PTCParameter.PushSpeed;
PTC.BackSpeed = PTCParameter.BackSpeed;
PTC.sidewaysFrictionAsymptoteFactor = PTCParameter.SideWaysFrictionAsymptoteFactor;
PTC.sidewaysFrictionExtremumFactor = PTCParameter.SideWaysFrictionExtremumFactor;
PTC.wheelCollider = PTCParameter.TankWheelCollider;
PTC.wheelsAndBonesAxisSettings = new PlayerTracksController.WheelsAxisSettings();
PTC.wheelsAndBonesAxisSettings.bonesPositionAxis = TankTracksController.Axis.Z;
PTC.wheelsAndBonesAxisSettings.wheelsPositionAxis = TankTracksController.Axis.Z;
PTC.wheelsAndBonesAxisSettings.wheelsRotationAxis = TankTracksController.Axis.X;
PTC.accelerationConfiguration = PTCParameter.VAconfigSetting;
PTC.rotationOnAccelerationConfiguration = PTCParameter.HAconfigSetting;
PTC.rotationOnStayConfiguration = PTCParameter.HAconfigSetting;
#endregion
#region 车库状态
if (_InstanceNetType == InstanceNetType.GarageTank)
{
PTC.enabled = false;
TankPhysic.isKinematic = true;
if (ShowHitBoxInspecter)
{
foreach (HitBox HB in GetComponentsInChildren <HitBox>())
{
HB.StartCoroutine(HB.ShowArmorInfo());
}
}
foreach (MeshCollider MC in transform.root.GetComponentsInChildren <MeshCollider>())
{
if (MC.GetComponent <HitBox> () == null)
{
MC.enabled = false;
}
}
//EventManager.StartListening ("TankInitSystem.InitTankShader", GarageInitTankShaderPreView);
return;
}
#endregion
if (ExtraTF && !isBot(_InstanceNetType))
{
for (int i = 0; i < multiTurrets.Length; i++)
{
GameObject TankScriptExtra = Instantiate((GameObject)Resources.Load("TankScript"), Vector3.zero, Quaternion.identity) as GameObject;
TankScriptExtra.transform.name = "TankScriptExtra";
TankScriptExtra.transform.parent = TankPrefabs.transform;
TurretController mouseTurretExtra = TankScriptExtra.GetComponent <TurretController> ();
if (isLocalPlayer(_InstanceNetType))
{
mouseTurretExtra.target = MainCamera.transform.Find("CameraTarget");
}
Transform ExtraTurret = InstanceMesh.transform.Find(multiTurrets [i].ObjectPath);
Transform ExtraGun = ExtraTurret.transform.Find("GunTransform");
mouseTurretExtra.gun = ExtraGun;
mouseTurretExtra.Turret = ExtraTurret;
mouseTurretExtra.DownMaxDegree = multiTurrets [i].MTParameter.DownMaxDegree;
mouseTurretExtra.UpMaxDegree = multiTurrets [i].MTParameter.UpMaxDegree;
mouseTurretExtra.maxTurretAngle = multiTurrets [i].MTParameter.maxTurretAngle;
mouseTurretExtra.gunDegreesPerSecond = multiTurrets [i].MTParameter.gunDegreesPerSecond;
mouseTurretExtra.turretDegreesPerSecond = multiTurrets [i].MTParameter.turretDegreesPerSecond;
mouseTurretExtra.isMobile = isMobile;
TankFire tfExtra = TankScriptExtra.GetComponent <TankFire> ();
Transform ExtraFFPoint = ExtraGun.Find("FFPoint");
Transform ExtraFireEffect = ExtraGun.Find("FireEffect");
tfExtra.FFPoint = ExtraFFPoint;
tfExtra.FireRecoilPoint = FFPoint.transform;
tfExtra.FireEffectPoint = ExtraFireEffect;
tfExtra.MainBody = InstanceMesh.transform;
//tfExtra.FireRecoil = multiTurrets [i].tankFireParameter.FireRecoil;
//tfExtra.ReloadTime = multiTurrets [i].tankFireParameter.ReloadTime;
//tfExtra.AmmoCount = multiTurrets [i].tankFireParameter.AmmoCount;
//tfExtra.HasMahineGun = multiTurrets [i].tankFireParameter.HasMachineGun;
//tfExtra.muzzleFire = multiTurrets [i].tankFireParameter.MuzzleFire;
//tfExtra.advanceFireClass = multiTurrets [i].tankFireParameter.advanceFireClass;
//tfExtra.fireState = multiTurrets [i].tankFireParameter.FireState;
//tfExtra.netType = InstanceNetType;
}
}
#endregion
InstanceMesh.AddComponent <Identity> ();
if (isLocalPlayer(_InstanceNetType))
{
GameEvent.InitPlayer(InstanceMesh.GetComponent <Identity>());
}
#region 初始化玩家网络
if (!GameDataManager.OfflineMode)
{
if (_InstanceNetType == InstanceNetType.GameNetworkOthers)
{
PTC.enableUserInput = false;
//PTC.AdvanceTrackSystem = false;
PTC.OnlyReceiveControlActions = true;
}
if (_InstanceNetType == InstanceNetType.GameNetworkMaster)
{
PTC.enableUserInput = false;
}
if (_InstanceNetType == InstanceNetType.GameNetworkClient)
{
PTC.enableUserInput = true;
PTC.OnlyReceiveControlActions = true;
}
if (isBot(_InstanceNetType))
{
PTC.enableUserInput = false;
if (_InstanceNetType == InstanceNetType.GameNetWorkBotClient)
{
PTC.OnlyReceiveControlActions = true;
}
else
{
PTC.OnlyReceiveControlActions = false;
}
}
//_syncGroundVehicle = InstanceMesh.AddComponent<SyncGroundVehicle> ();
//_syncGroundVehicle.tankInitSystem = this;
//_syncGroundVehicle.Init (PTC, mt, _InstanceNetType);
}
#region 设置离线标识
if (GameDataManager.OfflineMode)
{
if (_InstanceNetType == InstanceNetType.GameNetWorkOffline)
{
Identity MyIdentity = InstanceMesh.GetComponent <Identity> ();
MyIdentity.Init(ownerTeam, InstanceNetType.GameNetWorkOffline);
MyIdentity.SetOwnerInfo("LocalPlayer", VehicleName, 0);
}
}
#endregion
#endregion
if (!isBot(_InstanceNetType))
{
StartCoroutine(PS.Init());
}
if (GameEventManager.onNewVehicleSpawned != null)
{
GameEventManager.onNewVehicleSpawned(this);
}
if (onVehicleLoaded != null)
{
onVehicleLoaded();
}
}
private uint SpawnVehicle(VehiclePrototype prototype, DynamicTransform2 placement)
{
VehicleState initialState = new VehicleState(placement, prototype.ControlConfig.DefaultControl);
return(mMTSGame.AddVehicle(prototype, initialState));
}
public IPlan?FindOptimalPlanFor(VehicleState initialState)
{
var heuristic = createShortestPathHeuristic(initialState);
var open = new BinaryHeapOpenSet <DiscreteState, SearchNode>();
var closed = new ClosedSet <DiscreteState>();
open.Add(
new SearchNode(
discreteState: discretizer.Discretize(initialState, wayPoints.Count),
state: initialState,
actionFromPreviousState: null,
previousState: null,
costToCome: 0,
estimatedTotalCost: 0, // we don't need estimate for the initial node
targetWayPoint: 0));
while (!open.IsEmpty)
{
var node = open.DequeueMostPromissing();
if (node.TargetWayPoint == wayPoints.Count)
{
return(node.ReconstructPlan());
}
closed.Add(node.Key);
exploredStates.OnNext(node.State);
foreach (var action in actions.AllPossibleActions)
{
var predictedStates = motionModel.CalculateNextState(node.State, action, timeStep);
var elapsedTime = predictedStates.Last().relativeTime;
var nextVehicleState = predictedStates.Last().state;
var reachedWayPoint = false;
var collided = false;
foreach (var(simulationTime, state) in predictedStates)
{
if (collisionDetector.IsCollision(state))
{
elapsedTime = simulationTime;
nextVehicleState = state;
reachedWayPoint = false;
collided = true;
break;
}
if (wayPoints[node.TargetWayPoint].ReachedGoal(state.Position))
{
reachedWayPoint = true;
}
}
var nextTargetWayPoint = reachedWayPoint
? node.TargetWayPoint + 1
: node.TargetWayPoint;
var nextDiscreteState = discretizer.Discretize(nextVehicleState, nextTargetWayPoint);
if (closed.Contains(nextDiscreteState))
{
continue;
}
if (collided)
{
closed.Add(nextDiscreteState);
continue;
}
var costToCome = node.CostToCome + timeStep.TotalSeconds;
var reachedLastWayPoint = nextTargetWayPoint != wayPoints.Count;
var costToGo = reachedLastWayPoint
? heuristic.EstimateTimeToGoal(nextVehicleState, nextTargetWayPoint).TotalSeconds
: 0;
var discoveredNode = new SearchNode(
discreteState: nextDiscreteState,
state: nextVehicleState,
actionFromPreviousState: action,
previousState: node,
costToCome: costToCome,
estimatedTotalCost: costToCome + costToGo,
nextTargetWayPoint);
if (greedy && reachedLastWayPoint)
{
return(discoveredNode.ReconstructPlan());
}
if (!open.Contains(discoveredNode.Key))
{
open.Add(discoveredNode);
}
else if (discoveredNode.CostToCome < open.Get(discoveredNode.Key).CostToCome)
{
open.ReplaceExistingWithTheSameKey(discoveredNode);
}
}
}
return(null);
}
/// <summary>
/// Plan a lane change
/// </summary>
/// <param name="cls"></param>
/// <param name="initialManeuver"></param>
/// <param name="targetManeuver"></param>
/// <returns></returns>
public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan,
List <ITacticalBlockage> blockages, List <ArbiterWaypoint> ignorable)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// go to a blockage handling tactical
return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// lanes of the lane change
ArbiterLane initial = cls.Parameters.Initial;
ArbiterLane target = cls.Parameters.Target;
#region Initial Forwards
if (!cls.Parameters.InitialOncoming)
{
ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial);
#region Target Forwards
if (!cls.Parameters.TargetOncoming)
{
// target reasoning
ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);
#region Navigation
if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// parameters to follow
List <TravelingParameters> tps = new List <TravelingParameters>();
// vehicles to ignore
List <int> ignorableVehicles = new List <int>();
// params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3"));
if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
tps.Add(initialParams);
}
else
{
tps.Add(initialReasoning.ForwardMonitor.NavigationParameters);
}
ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);
// get params for the final lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List <ArbiterWaypoint>());
TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
tps.Add(targetParams);
ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);
try
{
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 &&
targetParams.Type == TravellingType.Vehicle &&
targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null &&
targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
catch (Exception) { }
ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3"));
// decorators
List <BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// check if need to modify distance to go
if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound;
double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0;
if (curDistToUpper > newVhcDistToUpper)
{
distanceToGo = newVhcDistToUpper;
}
}
// get final
tps.Sort();
// get the proper speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
if (sc.Speed < 8.84)
{
sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84));
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
}
#endregion
#region Failed Forwards
else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// parameters to follow
List <TravelingParameters> tps = new List <TravelingParameters>();
// vehicles to ignore
List <int> ignorableVehicles = new List <int>();
// params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
tps.Add(initialParams);
ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);
// get params for the final lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List <ArbiterWaypoint>());
TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
tps.Add(targetParams);
ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);
// decorators
List <BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
}
#endregion
#region Slow
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#endregion
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
#region Target Oncoming
else
{
OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target);
#region Failed Forward
if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// parameters to follow
List <TravelingParameters> tps = new List <TravelingParameters>();
// ignore the forward vehicle but keep params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
tps.Add(initialParams);
// get params for the final lane
targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages);
TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value;
tps.Add(targetParams);
// decorators
List <BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24));
// check final for stopped failed opposing
VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle;
if (forwardVa != null)
{
// dist between
double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);
// check stopped
bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;
// check distance
bool distOk = distToFV < 2.5 * TahoeParams.VL;
// check failed
bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;
// notify
ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());
// check all for failed
if (stopped && distOk && failed)
{
// check inside target
if (target.LanePolygon.IsInside(vehicleState.Front))
{
// blockage recovery
StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState);
StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List <int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false));
BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE,
new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None),
BlockageRecoverySTATUS.EXECUTING);
return(new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp));
}
// check which lane we are in
else
{
// return to forward lane
return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
}
#endregion
#region Other
else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#endregion
}
#endregion
}
#endregion
#region Initial Oncoming
else
{
OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial);
#region Target Forwards
if (!cls.Parameters.TargetOncoming)
{
ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);
if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#region Navigation
else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// parameters to follow
List <TravelingParameters> tps = new List <TravelingParameters>();
// distance to the upper bound of the change
double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get params for the initial lane
initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages);
// current params of the fqm
TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value;
if (initialParams.Type == TravellingType.Vehicle)
{
if (!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
tps.Add(initialParams);
}
else
{
tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL;
}
}
else
{
tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
}
// get params for forward lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
target.WaypointList[target.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
tps.Add(targetParams);
// ignoring vehicles add
List <int> ignoreVehicles = initialParams.VehiclesToIgnore;
ignoreVehicles.AddRange(targetParams.VehiclesToIgnore);
// decorators
List <BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = tps[0].SpeedCommand;
if (sc is StopAtDistSpeedCommand)
{
sc = new ScalarSpeedCommand(0.0);
}
// check final for stopped failed opposing
VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle;
if (forwardVa != null)
{
// dist between
double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);
// check stopped
bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;
// check distance
bool distOk = distToFV < 2.5 * TahoeParams.VL;
// check failed
bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;
// notify
ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());
// check all for failed
if (stopped && distOk && failed)
{
// check which lane we are in
if (initial.LanePolygon.IsInside(vehicleState.Front))
{
// return to opposing lane
return(new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
// lane state
return(new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false),
initial.NumberOfLanesRight(vehicleState.Front, false));
// standard maneuver
return(new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp));
}
#endregion
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
}
/// <summary>
/// Lightweight simulation step method.
/// This method is occasionally being called for different cars.
/// </summary>
/// <param name="vehicleId"></param>
/// <param name="vehicleData"></param>
/// <param name="physicsLodRefPos"></param>
public void CustomSimulationStep(ushort vehicleId, ref Vehicle vehicleData, Vector3 physicsLodRefPos)
{
PathManager pathMan = Singleton <PathManager> .instance;
#if DEBUG
/*if (!GlobalConfig.Instance.DebugSwitches[0]) {
* Log._Debug($"CustomCarAI.CustomSimulationStep({vehicleId}) called. flags: {vehicleData.m_flags} pfFlags: {pathMan.m_pathUnits.m_buffer[vehicleData.m_path].m_pathFindFlags}");
* }*/
#endif
// NON-STOCK CODE START
VehicleState state = null;
ExtCitizenInstance driverExtInstance = null;
bool prohibitPocketCars = Options.prohibitPocketCars;
if (prohibitPocketCars)
{
// check for valid driver and update return path state
state = VehicleStateManager.Instance._GetVehicleState(vehicleData.GetFirstVehicle(vehicleId));
if (state.VehicleType == ExtVehicleType.PassengerCar)
{
driverExtInstance = state.GetDriverExtInstance();
if (driverExtInstance == null)
{
prohibitPocketCars = false;
}
else
{
driverExtInstance.UpdateReturnPathState();
}
}
else
{
prohibitPocketCars = false;
}
}
// NON-STOCK CODE END
if ((vehicleData.m_flags & Vehicle.Flags.WaitingPath) != 0 &&
(!prohibitPocketCars || driverExtInstance.ReturnPathState != ExtCitizenInstance.ExtPathState.Calculating)) // NON-STOCK CODE: Parking AI: wait for the return path to be calculated
{
PathManager pathManager = Singleton <PathManager> .instance;
byte pathFindFlags = pathManager.m_pathUnits.m_buffer[vehicleData.m_path].m_pathFindFlags;
bool pathFindFailed = (pathFindFlags & PathUnit.FLAG_FAILED) != 0 || vehicleData.m_path == 0; // path == 0: non-stock code!
bool pathFindSucceeded = (pathFindFlags & PathUnit.FLAG_READY) != 0;
#if USEPATHWAITCOUNTER
if ((pathFindFlags & (PathUnit.FLAG_READY | PathUnit.FLAG_FAILED)) != 0)
{
VehicleState state = VehicleStateManager.Instance._GetVehicleState(vehicleId);
state.PathWaitCounter = 0; // NON-STOCK CODE
}
#endif
if (prohibitPocketCars)
{
if (driverExtInstance.ReturnPathState == ExtPathState.Failed)
{
// no walking path from parking position to target found. flag main path as 'failed'.
#if DEBUG
if (GlobalConfig.Instance.DebugSwitches[2])
{
Log._Debug($"CustomCarAI.CustomSimulationStep: Return path {driverExtInstance.ReturnPathId} FAILED. Forcing path-finding to fail.");
}
#endif
pathFindSucceeded = false;
pathFindFailed = true;
}
driverExtInstance.ReleaseReturnPath();
if (pathFindSucceeded)
{
CustomPassengerCarAI.OnPathFindSuccess(vehicleId, ref vehicleData, driverExtInstance);
}
else if (pathFindFailed)
{
CustomPassengerCarAI.OnPathFindFailure(driverExtInstance, vehicleId);
}
}
if (pathFindSucceeded)
{
vehicleData.m_pathPositionIndex = 255;
vehicleData.m_flags &= ~Vehicle.Flags.WaitingPath;
vehicleData.m_flags &= ~Vehicle.Flags.Arriving;
this.PathfindSuccess(vehicleId, ref vehicleData);
this.TrySpawn(vehicleId, ref vehicleData);
}
else if (pathFindFailed)
{
vehicleData.m_flags &= ~Vehicle.Flags.WaitingPath;
Singleton <PathManager> .instance.ReleasePath(vehicleData.m_path);
vehicleData.m_path = 0u;
this.PathfindFailure(vehicleId, ref vehicleData);
return;
}
#if USEPATHWAITCOUNTER
else
{
VehicleState state = VehicleStateManager.Instance._GetVehicleState(vehicleId);
state.PathWaitCounter = (ushort)Math.Min(ushort.MaxValue, (int)state.PathWaitCounter + 1); // NON-STOCK CODE
}
#endif
}
else
{
if ((vehicleData.m_flags & Vehicle.Flags.WaitingSpace) != 0)
{
this.TrySpawn(vehicleId, ref vehicleData);
}
}
/// NON-STOCK CODE START ///
VehicleStateManager vehStateManager = VehicleStateManager.Instance;
if (Options.prioritySignsEnabled || Options.timedLightsEnabled)
{
// update vehicle position for timed traffic lights and priority signs
try {
vehStateManager.UpdateVehiclePos(vehicleId, ref vehicleData);
} catch (Exception e) {
Log.Error("CarAI CustomSimulationStep Error: " + e.ToString());
}
}
if (!Options.isStockLaneChangerUsed())
{
// Advanced AI traffic measurement
try {
vehStateManager.LogTraffic(vehicleId, ref vehicleData, true);
} catch (Exception e) {
Log.Error("CarAI CustomSimulationStep Error: " + e.ToString());
}
}
/// NON-STOCK CODE END ///
Vector3 lastFramePosition = vehicleData.GetLastFramePosition();
int lodPhysics;
if (Vector3.SqrMagnitude(physicsLodRefPos - lastFramePosition) >= 1210000f)
{
lodPhysics = 2;
}
else if (Vector3.SqrMagnitude(Singleton <SimulationManager> .instance.m_simulationView.m_position - lastFramePosition) >= 250000f)
{
lodPhysics = 1;
}
else
{
lodPhysics = 0;
}
this.SimulationStep(vehicleId, ref vehicleData, vehicleId, ref vehicleData, lodPhysics);
if (vehicleData.m_leadingVehicle == 0 && vehicleData.m_trailingVehicle != 0)
{
VehicleManager vehManager = Singleton <VehicleManager> .instance;
ushort num = vehicleData.m_trailingVehicle;
int num2 = 0;
while (num != 0)
{
ushort trailingVehicle = vehManager.m_vehicles.m_buffer[(int)num].m_trailingVehicle;
VehicleInfo info = vehManager.m_vehicles.m_buffer[(int)num].Info;
info.m_vehicleAI.SimulationStep(num, ref vehManager.m_vehicles.m_buffer[(int)num], vehicleId, ref vehicleData, lodPhysics);
num = trailingVehicle;
if (++num2 > 16384)
{
CODebugBase <LogChannel> .Error(LogChannel.Core, "Invalid list detected!\n" + Environment.StackTrace);
break;
}
}
}
#if PATHRECALC
ushort recalcSegmentId = 0;
#endif
int privateServiceIndex = ItemClass.GetPrivateServiceIndex(this.m_info.m_class.m_service);
int maxBlockCounter = (privateServiceIndex == -1) ? 150 : 100;
if ((vehicleData.m_flags & (Vehicle.Flags.Spawned | Vehicle.Flags.WaitingPath | Vehicle.Flags.WaitingSpace)) == 0 && vehicleData.m_cargoParent == 0)
{
Singleton <VehicleManager> .instance.ReleaseVehicle(vehicleId);
}
else if ((int)vehicleData.m_blockCounter >= maxBlockCounter && Options.enableDespawning)
{
Singleton <VehicleManager> .instance.ReleaseVehicle(vehicleId);
}
#if PATHRECALC
else if (vehicleData.m_leadingVehicle == 0 && CustomVehicleAI.ShouldRecalculatePath(vehicleId, ref vehicleData, maxBlockCounter, out recalcSegmentId))
{
CustomVehicleAI.MarkPathRecalculation(vehicleId, recalcSegmentId);
InvalidPath(vehicleId, ref vehicleData, vehicleId, ref vehicleData);
}
#endif
}
public abstract void HandleIncomingState(VehicleState state);
public override void HandleIncomingState(VehicleState state)
{
this.state = state;
Target.SetState(state);
}
private void HandleStateUpdate(NetIncomingMessage msg)
{
for (int i = 0; i < vehcileNetworkStates.Count; ++i)
{
vehcileNetworkStates[i].Read(msg);
VehicleState state = new VehicleState();
state.FromNetworkState(vehcileNetworkStates[i]);
vehicles.SetVehicleState(i, state);
}
}
public override void HandleIncomingState(VehicleState state)
{
// TODO: detect desync state changes etc
}
public class CustomTrainAI : TrainAI { // TODO inherit from VehicleAI (in order to keep the correct references to `base`)
public void TrafficManagerSimulationStep(ushort vehicleId, ref Vehicle vehicleData, Vector3 physicsLodRefPos)
{
if ((vehicleData.m_flags & Vehicle.Flags.WaitingPath) != 0)
{
byte pathFindFlags = Singleton <PathManager> .instance.m_pathUnits.m_buffer[(int)((UIntPtr)vehicleData.m_path)].m_pathFindFlags;
#if USEPATHWAITCOUNTER
if ((pathFindFlags & (PathUnit.FLAG_READY | PathUnit.FLAG_FAILED)) != 0)
{
VehicleState state = VehicleStateManager.Instance._GetVehicleState(vehicleId);
state.PathWaitCounter = 0; // NON-STOCK CODE
}
#endif
if ((pathFindFlags & PathUnit.FLAG_READY) != 0)
{
try {
this.PathFindReady(vehicleId, ref vehicleData);
} catch (Exception e) {
Log.Warning($"TrainAI.PathFindReady({vehicleId}) for vehicle {vehicleData.Info?.m_class?.name} threw an exception: {e.ToString()}");
vehicleData.m_flags &= ~Vehicle.Flags.WaitingPath;
Singleton <PathManager> .instance.ReleasePath(vehicleData.m_path);
vehicleData.m_path = 0u;
vehicleData.Unspawn(vehicleId);
return;
}
}
else if ((pathFindFlags & PathUnit.FLAG_FAILED) != 0 || vehicleData.m_path == 0)
{
vehicleData.m_flags &= ~Vehicle.Flags.WaitingPath;
Singleton <PathManager> .instance.ReleasePath(vehicleData.m_path);
vehicleData.m_path = 0u;
vehicleData.Unspawn(vehicleId);
return;
}
#if USEPATHWAITCOUNTER
else
{
VehicleState state = VehicleStateManager.Instance._GetVehicleState(vehicleId);
state.PathWaitCounter = (ushort)Math.Min(ushort.MaxValue, (int)state.PathWaitCounter + 1); // NON-STOCK CODE
}
#endif
}
else
{
if ((vehicleData.m_flags & Vehicle.Flags.WaitingSpace) != 0)
{
this.TrySpawn(vehicleId, ref vehicleData);
}
}
bool reversed = (vehicleData.m_flags & Vehicle.Flags.Reversed) != 0;
ushort frontVehicleId;
if (reversed)
{
frontVehicleId = vehicleData.GetLastVehicle(vehicleId);
}
else
{
frontVehicleId = vehicleId;
}
/// NON-STOCK CODE START ///
VehicleStateManager vehStateManager = VehicleStateManager.Instance;
if (Options.prioritySignsEnabled || Options.timedLightsEnabled)
{
try {
vehStateManager.UpdateVehiclePos(frontVehicleId, ref Singleton <VehicleManager> .instance.m_vehicles.m_buffer[frontVehicleId]);
} catch (Exception e) {
Log.Error("TrainAI TrafficManagerSimulationStep (2) Error: " + e.ToString());
}
}
if (!Options.isStockLaneChangerUsed())
{
try {
//Log._Debug($"HandleVehicle for trams. vehicleId={vehicleId} frontVehicleId={frontVehicleId}");
vehStateManager.LogTraffic(frontVehicleId, ref Singleton <VehicleManager> .instance.m_vehicles.m_buffer[frontVehicleId], true);
} catch (Exception e) {
Log.Error("TrainAI TrafficManagerSimulationStep (1) Error: " + e.ToString());
}
}
/// NON-STOCK CODE END ///
VehicleManager instance = Singleton <VehicleManager> .instance;
VehicleInfo info = instance.m_vehicles.m_buffer[(int)frontVehicleId].Info;
info.m_vehicleAI.SimulationStep(frontVehicleId, ref instance.m_vehicles.m_buffer[(int)frontVehicleId], vehicleId, ref vehicleData, 0);
if ((vehicleData.m_flags & (Vehicle.Flags.Created | Vehicle.Flags.Deleted)) != Vehicle.Flags.Created)
{
return;
}
bool flag2 = (vehicleData.m_flags & Vehicle.Flags.Reversed) != 0;
if (flag2 != reversed)
{
reversed = flag2;
if (reversed)
{
frontVehicleId = vehicleData.GetLastVehicle(vehicleId);
}
else
{
frontVehicleId = vehicleId;
}
info = instance.m_vehicles.m_buffer[(int)frontVehicleId].Info;
info.m_vehicleAI.SimulationStep(frontVehicleId, ref instance.m_vehicles.m_buffer[(int)frontVehicleId], vehicleId, ref vehicleData, 0);
if ((vehicleData.m_flags & (Vehicle.Flags.Created | Vehicle.Flags.Deleted)) != Vehicle.Flags.Created)
{
return;
}
flag2 = ((vehicleData.m_flags & Vehicle.Flags.Reversed) != 0);
if (flag2 != reversed)
{
Singleton <VehicleManager> .instance.ReleaseVehicle(vehicleId);
return;
}
}
if (reversed)
{
frontVehicleId = instance.m_vehicles.m_buffer[(int)frontVehicleId].m_leadingVehicle;
int num2 = 0;
while (frontVehicleId != 0)
{
info = instance.m_vehicles.m_buffer[(int)frontVehicleId].Info;
info.m_vehicleAI.SimulationStep(frontVehicleId, ref instance.m_vehicles.m_buffer[(int)frontVehicleId], vehicleId, ref vehicleData, 0);
if ((vehicleData.m_flags & (Vehicle.Flags.Created | Vehicle.Flags.Deleted)) != Vehicle.Flags.Created)
{
return;
}
frontVehicleId = instance.m_vehicles.m_buffer[(int)frontVehicleId].m_leadingVehicle;
if (++num2 > 16384)
{
CODebugBase <LogChannel> .Error(LogChannel.Core, "Invalid list detected!\n" + Environment.StackTrace);
break;
}
}
}
else
{
frontVehicleId = instance.m_vehicles.m_buffer[(int)frontVehicleId].m_trailingVehicle;
int num3 = 0;
while (frontVehicleId != 0)
{
info = instance.m_vehicles.m_buffer[(int)frontVehicleId].Info;
info.m_vehicleAI.SimulationStep(frontVehicleId, ref instance.m_vehicles.m_buffer[(int)frontVehicleId], vehicleId, ref vehicleData, 0);
if ((vehicleData.m_flags & (Vehicle.Flags.Created | Vehicle.Flags.Deleted)) != Vehicle.Flags.Created)
{
return;
}
frontVehicleId = instance.m_vehicles.m_buffer[(int)frontVehicleId].m_trailingVehicle;
if (++num3 > 16384)
{
CODebugBase <LogChannel> .Error(LogChannel.Core, "Invalid list detected!\n" + Environment.StackTrace);
break;
}
}
}
if ((vehicleData.m_flags & (Vehicle.Flags.Spawned | Vehicle.Flags.WaitingPath | Vehicle.Flags.WaitingSpace | Vehicle.Flags.WaitingCargo)) == 0 || (vehicleData.m_blockCounter == 255 /*&& Options.enableDespawning*/))
{
Singleton <VehicleManager> .instance.ReleaseVehicle(vehicleId);
}
}
