void intervalOfConflict(Daidalus& daa) {
bool comma = false;
std::string s="";
switch (format) {
case STANDARD:
for (int ac=1; ac <= daa.lastTrafficIndex(); ++ac) {
ConflictData conf = daa.detection(ac);
if (conf.conflict()) {
(*out) << "Predicted Loss of Well-Clear With " << daa.getAircraftState(ac).getId() <<
" in " << Units::str("s",conf.getTimeIn()) << " - "+Units::str("s",conf.getTimeOut()) << std::endl;
}
}
break;
case PVS:
s += "(: ";
for (int ac=1; ac <= daa.lastTrafficIndex(); ++ac) {
if (comma) {
s += ", ";
} else {
comma = true;
}
ConflictData conf = daa.detection(ac);
s += "("+FmPrecision(conf.getTimeIn(),precision)+","+FmPrecision(conf.getTimeOut(),precision)+")";
}
s += " :)";
(*out) << "%%% Time Interval of Violation:\n" << s << std::endl;
break;
default:
break;
}
}
/**
* 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));
}
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;
}
