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; }