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