Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
// 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;
}
Ejemplo n.º 3
0
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);
  }
}