bool KinematicIntegerBands::conflict(Detection3D* det, const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi,
double B, double T) {
if (Util::almost_equals(B,T)) {
Vect3 sot = vo.ScalAdd(B,so);
Vect3 sit = vi.ScalAdd(B,si);
return det->violation(sot,vo,sit,vi);
}
return det->conflict(so,vo,si,vi,B,T);
}
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;
}