/** * Put in conflict_acs_ the list of aircraft predicted to be in conflict for the given alert level. * Requires: 1 <= alert_level <= parameters.alertor.mostSevereAlertLevel() */ void KinematicBandsCore::conflict_aircraft(int alert_level) { double tin = PINFINITY; double tout = NINFINITY; bool conflict_band = BandsRegion::isConflictBand(parameters.alertor.getLevel(alert_level).getRegion()); Detection3D* detector = parameters.alertor.getLevel(alert_level).getDetectorRef(); double alerting_time = Util::min(parameters.getLookaheadTime(), parameters.alertor.getLevel(alert_level).getAlertingTime()); for (TrafficState::nat i = 0; i < traffic.size(); ++i) { TrafficState ac = traffic[i]; ConflictData det = detector->conflictDetection(ownship.get_s(),ownship.get_v(),ac.get_s(),ac.get_v(), 0,parameters.getLookaheadTime()); bool lowc = detector->violation(ownship.get_s(),ownship.get_v(),ac.get_s(),ac.get_v()); if (lowc || det.conflict()) { if (conflict_band && (lowc || det.getTimeIn() < alerting_time)) { conflict_acs_[alert_level-1].push_back(ac); } tin = Util::min(tin,det.getTimeIn()); tout = Util::max(tout,det.getTimeOut()); } } tiov_.push_back(Interval(tin,tout)); }
bool KinematicIntegerBands::cd_future_traj(Detection3D* det, double B, double T, bool trajdir, double t, const TrafficState& ownship, const TrafficState& ac) const { if (t > T || B > T) return false; std::pair<Vect3,Velocity> sovot = trajectory(ownship,t,trajdir); Vect3 sot = sovot.first; Velocity vot = sovot.second; Vect3 si = ac.get_s(); Velocity vi = ac.get_v(); Vect3 sit = vi.ScalAdd(t,si); if (B > t) { return conflict(det, sot, vot, sit, vi, B-t, T-t); } return conflict(det, sot, vot, sit, vi, 0, T-t); }
bool KinematicIntegerBands::any_los_aircraft(Detection3D* det, bool trajdir, double tsk, const TrafficState& ownship, const std::vector<TrafficState>& traffic) const { for (TrafficState::nat i=0; i < traffic.size(); ++i) { TrafficState ac = traffic[i]; std::pair<Vect3,Velocity> sovot = trajectory(ownship,tsk,trajdir); Vect3 sot = sovot.first; Velocity vot = sovot.second; Vect3 si = ac.get_s(); Velocity vi = ac.get_v(); Vect3 sit = vi.ScalAdd(tsk,si); if (det->violation(sot, vot, sit, vi)) return true; } return false; }
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 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::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship, const TrafficState& repac, int epsv) const { // repac is not NULL at this point and k >= 0 if (k==0) { return true; } std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir); Vect3 so = sovo.first; Vect3 vo = sovo.second; Vect3 si = repac.get_s(); Vect3 vi = repac.get_v(); bool rep = true; if (k==1) { rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv); } if (rep) { std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir); Vect3 sot = sovot.first; Vect3 vot = sovot.second; Vect3 sit = vi.ScalAdd(k*tstep,si); Vect3 st = sot.Sub(sit); Vect3 vop = linvel(ownship,tstep,trajdir,k-1); Vect3 vok = linvel(ownship,tstep,trajdir,k); return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) && CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) && CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv); } return false; }
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); }