// trajdir: false is left bool KinematicIntegerBands::red_band_exist(Detection3D* conflict_det, Detection3D* recovery_det, double tstep, double B, double T, double B2, double T2, bool trajdir, int max, const TrafficState& ownship, const std::vector<TrafficState>& traffic, const TrafficState& repac, int epsh, int epsv) const { bool usehcrit = repac.isValid() && epsh != 0; bool usevcrit = repac.isValid() && epsv != 0; return (usehcrit && first_nonrepulsive_step(tstep,trajdir,max,ownship,repac,epsh) >= 0) || (usevcrit && first_nonvert_repul_step(tstep,trajdir,max,ownship,repac,epsv) >= 0) || any_conflict_step(conflict_det,tstep,B,T,trajdir,max,ownship,traffic) || (recovery_det != NULL && any_conflict_step(recovery_det,tstep,B2,T2,trajdir,max,ownship,traffic)); }
bool KinematicVsBands::all_red(Detection3D* conflict_det, Detection3D* recovery_det, const TrafficState& repac, double B, double T, const OwnshipState& ownship, const std::vector<TrafficState>& traffic) const { double vso = ownship.getVelocity().vs(); int maxdown = (int)std::max(std::ceil((vso-min)/step),0.0)+1; int maxup = (int)std::max(std::ceil((max-vso)/step),0.0)+1; double tstep = step/vertical_accel; int epsv = 0; if (repac.isValid()) { epsv = KinematicBandsCore::epsilonV(ownship,repac); } return KinematicIntegerBands::all_int_red(conflict_det,recovery_det,tstep,B,T,0,B,maxdown,maxup,ownship,traffic,repac,0,epsv,0); }
void KinematicVsBands::none_bands(IntervalSet& noneset, Detection3D* conflict_det, Detection3D* recovery_det, const TrafficState& repac, double B, double T, const OwnshipState& ownship, const std::vector<TrafficState>& traffic) const { double vso = ownship.getVelocity().vs(); int maxdown = (int)std::max(std::ceil((vso-min)/step),0.0)+1; int maxup = (int)std::max(std::ceil((max-vso)/step),0.0)+1; double tstep = step/vertical_accel; std::vector<Integerval> vsint = std::vector<Integerval>(); int epsv = 0; if (repac.isValid()) { epsv = KinematicBandsCore::epsilonV(ownship,repac); } KinematicIntegerBands::kinematic_bands_combine(vsint,conflict_det,recovery_det,tstep,B,T,0,B,maxdown,maxup,ownship,traffic,repac,0,epsv); KinematicIntegerBands::toIntervalSet(noneset,vsint,step,vso,min,max); }
TrafficState DCPAUrgencyStrategy::mostUrgentAircraft(Detection3D* detector, const TrafficState& ownship, const std::vector<TrafficState>& traffic, double T) { TrafficState repac = TrafficState::INVALID; if (!ownship.isValid() || traffic.empty()) { return repac; } double mindcpa = 0; double mintcpa = 0; double D = ACCoRDConfig::NMAC_D; double H = ACCoRDConfig::NMAC_H; Vect3 so = ownship.get_s(); Velocity vo = ownship.get_v(); for (TrafficState::nat ac = 0; ac < traffic.size(); ++ac) { Vect3 si = traffic[ac].get_s(); Velocity vi = traffic[ac].get_v(); Vect3 s = so.Sub(si); Velocity v = vo.Sub(vi); ConflictData det = detector->conflictDetection(so,vo,si,vi,0,T); if (det.conflict()) { double tcpa = CD3D::tccpa(s,vo,vi,D,H); double dcpa = v.ScalAdd(tcpa,s).cyl_norm(D,H); // If aircraft have almost same tcpa, select the one with smallest dcpa // Otherwise, select aircraft with smallest tcpa bool tcpa_strategy = Util::almost_equals(tcpa,mintcpa,PRECISION5) ? dcpa < mindcpa : tcpa < mintcpa; // If aircraft have almost same dcpa, select the one with smallest tcpa // Otherwise, select aircraft with smallest dcpa bool dcpa_strategy = Util::almost_equals(dcpa,mindcpa,PRECISION5) ? tcpa < mintcpa : dcpa < mindcpa; // If aircraft are both in a min recovery trajectory, follows tcpa strategy. Otherwise follows dcpa strategy if (!repac.isValid() || // There are no candidates (dcpa <= 1 ? mindcpa > 1 || tcpa_strategy : dcpa_strategy)) { repac = traffic[ac]; mindcpa = dcpa; mintcpa = tcpa; } } } return repac; }
int KinematicIntegerBands::bands_search_index(Detection3D* conflict_det, Detection3D* recovery_det, double tstep, double B, double T, double B2, double T2, bool trajdir, int max, const TrafficState& ownship, const std::vector<TrafficState>& traffic, const TrafficState& repac, int epsh, int epsv) const { bool usehcrit = repac.isValid() && epsh != 0; bool usevcrit = repac.isValid() && epsv != 0; int FirstLos = first_los_search_index(conflict_det,recovery_det,tstep,B,T,B2,T2,trajdir,max,ownship,traffic); int FirstNonHRep = !usehcrit || FirstLos == 0 ? FirstLos : first_nonrepulsive_step(tstep,trajdir,FirstLos-1,ownship,repac,epsh); int FirstProbHcrit = FirstNonHRep < 0 ? max+1 : FirstNonHRep; int FirstProbHL = std::min(FirstLos,FirstProbHcrit); int FirstNonVRep = !usevcrit || FirstProbHL == 0 ? FirstProbHL : first_nonvert_repul_step(tstep,trajdir,FirstProbHL-1,ownship,repac,epsv); int FirstProbVcrit = FirstNonVRep < 0 ? max+1 : FirstNonVRep; return std::min(FirstProbHL,FirstProbVcrit); }
// trajdir: false is left int KinematicIntegerBands::first_green(Detection3D* conflict_det, Detection3D* recovery_det, double tstep, double B, double T, double B2, double T2, bool trajdir, int max, const TrafficState& ownship, const std::vector<TrafficState>& traffic, const TrafficState& repac, int epsh, int epsv) const { bool usehcrit = repac.isValid() && epsh != 0; bool usevcrit = repac.isValid() && epsv != 0; for (int k=0; k <= max; ++k) { double tsk = tstep*k; if ((tsk >= B && tsk <= T && any_los_aircraft(conflict_det,trajdir,tsk,ownship,traffic)) || (recovery_det != NULL && tsk >= B2 && tsk <= T2 && any_los_aircraft(recovery_det,trajdir,tsk,ownship,traffic)) || (usehcrit && !repulsive_at(tstep,trajdir,k,ownship,repac,epsh)) || (usevcrit && !vert_repul_at(tstep,trajdir,k,ownship,repac,epsv))) { return -1; } else if (!any_conflict_aircraft(conflict_det,B,T,trajdir,tsk,ownship,traffic) && !(recovery_det != NULL && any_conflict_aircraft(recovery_det,B2,T2,trajdir,tsk,ownship,traffic))) return k; } return -1; }
int KinematicBandsCore::epsilonV(const TrafficState& ownship, const TrafficState& ac) { if (ownship.isValid() && ac.isValid()) { Vect3 s = ownship.get_s().Sub(ac.get_s()); return CriteriaCore::verticalCoordinationLoS(s,ownship.get_v(),ac.get_v(), ownship.getId(), ac.getId()); } else { return 0; } }
int KinematicBandsCore::epsilonH(const TrafficState& ownship, const TrafficState& ac) { if (ownship.isValid() && ac.isValid()) { Vect2 s = ownship.get_s().Sub(ac.get_s()).vect2(); Vect2 v = ownship.get_v().Sub(ac.get_v()).vect2(); return CriteriaCore::horizontalCoordination(s,v); } else { return 0; } }
bool KinematicIntegerBands::no_instantaneous_conflict(Detection3D* conflict_det, Detection3D* recovery_det, double B, double T, double B2, double T2, bool trajdir, const TrafficState& ownship, const std::vector<TrafficState>& traffic, const TrafficState& repac, int epsh, int epsv) { bool usehcrit = repac.isValid() && epsh != 0; bool usevcrit = repac.isValid() && epsv != 0; std::pair<Vect3,Velocity> nsovo = trajectory(ownship,0,trajdir); Vect3 so = ownship.get_s(); Vect3 vo = ownship.get_v(); Vect3 si = repac.get_s(); Vect3 vi = repac.get_v(); Vect3 nvo = nsovo.second; Vect3 s = so.Sub(si); return (!usehcrit || CriteriaCore::horizontal_new_repulsive_criterion(s,vo,vi,nvo,epsh)) && (!usevcrit || CriteriaCore::vertical_new_repulsive_criterion(s,vo,vi,nvo,epsv)) && no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic); }