int KinematicIntegerBands::first_los_step(Detection3D* det, double tstep,bool trajdir,
int min, int max, const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
for (int k=min; k<=max; ++k) {
if (any_los_aircraft(det,trajdir,k*tstep,ownship,traffic)) {
return k;
}
}
return -1;
}
// 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;
}
bool KinematicAltBands::conflict_free_traj_step(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
bool trajdir = true;
if (instantaneous_bands()) {
return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
} else {
double tstep = time_step(ownship);
double target_alt = min_val(ownship)+j_step_*get_step();
Tuple5<double,double,double,double,double> tsqj = Kinematics::vsLevelOutTimes(ownship.altitude(),ownship.verticalSpeed(),
vertical_rate_,target_alt,vertical_accel_,-vertical_accel_,true);
double tsqj1 = tsqj.first+0;
double tsqj2 = tsqj.second+0;
double tsqj3 = tsqj.third+tstep;
for (int i=0; i<=std::floor(tsqj1/tstep);++i) {
double tsi = i*tstep;
if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
(recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
return false;
}
}
if ((tsqj2>=B &&
any_conflict_aircraft(conflict_det,B,std::min(T,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic)) ||
(recovery_det != NULL && tsqj2>=B2 &&
any_conflict_aircraft(recovery_det,B2,std::min(T2,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic))) {
return false;
}
for (int i=(int)std::ceil(tsqj2/tstep); i<=std::floor(tsqj3/tstep);++i) {
double tsi = i*tstep;
if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
(recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
return false;
}
}
return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,std::max(tsqj3,0.0),ownship,traffic);
}
}