public VehicleDistance GetDrivedOutVehicleFrontAbsPositin(MyLinkedList<IVehicle> VehicleDrivedOut, VehicleDistance FrontVehicle)
{
if (VehicleDrivedOut != null)
{
foreach (IVehicle v in VehicleDrivedOut)
{
double x = Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.X - _state.positionAbs.X) + Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.Y - _state.positionAbs.Y);
double y = -Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.X - _state.positionAbs.X) + Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.Y - _state.positionAbs.Y);
double x_front = Constants.lookaheadDistance;
double y_front = 0.0;
if (FrontVehicle != null)
{
x_front = Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (FrontVehicle.vehicle.state.positionAbs.X - _state.positionAbs.X) + Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (FrontVehicle.vehicle.state.positionAbs.Y - _state.positionAbs.Y);
y_front = -Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (FrontVehicle.vehicle.state.positionAbs.X - _state.positionAbs.X) + Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (FrontVehicle.vehicle.state.positionAbs.Y - _state.positionAbs.Y);
}
if (x < x_front && x > 0 && Math.Abs(y) < 50.0)
{
return new VehicleDistance(v, x);
}
}
}
return FrontVehicle;
}
public VehicleDistance GetDrivedOutVehicleBehindAbsPosition(MyLinkedList<IVehicle> VehicleDrivedOut, VehicleDistance BehindVehicle)
{
if (VehicleDrivedOut != null)
{
foreach (IVehicle v in VehicleDrivedOut)
{
double x = Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.X - _state.positionAbs.X) + Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.Y - _state.positionAbs.Y);
double y = -Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.X - _state.positionAbs.X) + Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (v.state.positionAbs.Y - _state.positionAbs.Y);
double x_rear = -300.0;
double y_rear = -300.0;
if (BehindVehicle != null)
{
x_rear = Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (BehindVehicle.vehicle.state.positionAbs.X - _state.positionAbs.X) + Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (BehindVehicle.vehicle.state.positionAbs.Y - _state.positionAbs.Y);
y_rear = -Math.Sin(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (BehindVehicle.vehicle.state.positionAbs.X - _state.positionAbs.X) + Math.Cos(Math.Atan(_state.orientation.Y / _state.orientation.X)) * (BehindVehicle.vehicle.state.positionAbs.Y - _state.positionAbs.Y);
}
if (x > x_rear && x < 0 && ((Math.Abs(y) < 50.0 && Math.Abs(y) > 10.0)))
{
return new VehicleDistance(v, x);
}
}
}
return BehindVehicle;
}
/// <summary>
/// Returns the VehicleDistance with the smaller distance to the tail of the vehicle.
/// </summary>
/// <param name="lhs">Left object to compare</param>
/// <param name="rhs">Right object to compare</param>
/// <returns>The one with the smaller distance to the tail of the vehicle. The other if one is null.</returns>
public static VehicleDistance MinTail(VehicleDistance lhs, VehicleDistance rhs)
{
if (lhs == null)
{
return(rhs);
}
if (rhs == null)
{
return(lhs);
}
return((lhs.distance - lhs.vehicle.length < rhs.distance - rhs.vehicle.length) ? lhs : rhs);
}
/// <summary>
/// Returns the VehicleDistance with the greater distance.
/// </summary>
/// <param name="lhs">Left object to compare</param>
/// <param name="rhs">Right object to compare</param>
/// <returns>The one with the greater distance. The other if one is null.</returns>
public static VehicleDistance Max(VehicleDistance lhs, VehicleDistance rhs)
{
if (lhs == null)
{
return(rhs);
}
if (rhs == null)
{
return(lhs);
}
return((lhs.distance > rhs.distance) ? lhs : rhs);
}
/// <summary>
/// Returns the VehicleDistance with the greater distance.
/// </summary>
/// <param name="lhs">Left object to compare</param>
/// <param name="rhs">Right object to compare</param>
/// <returns>The one with the greater distance. The other if one is null.</returns>
public static VehicleDistance Max(VehicleDistance lhs, VehicleDistance rhs)
{
if (lhs == null)
return rhs;
if (rhs == null)
return lhs;
return (lhs.distance > rhs.distance) ? lhs : rhs;
}
/// <summary>
/// Returns the VehicleDistance with the smaller distance to the tail of the vehicle.
/// </summary>
/// <param name="lhs">Left object to compare</param>
/// <param name="rhs">Right object to compare</param>
/// <returns>The one with the smaller distance to the tail of the vehicle. The other if one is null.</returns>
public static VehicleDistance MinTail(VehicleDistance lhs, VehicleDistance rhs)
{
if (lhs == null)
return rhs;
if (rhs == null)
return lhs;
return (lhs.distance - lhs.vehicle.length < rhs.distance - rhs.vehicle.length) ? lhs : rhs;
}
