C++ TrafficState::get_s方法代码示例

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

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

bool KinematicIntegerBands::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship,
    const TrafficState& repac, int epsv) const {
  // repac is not NULL at this point and k >= 0
  if (k==0) {
    return true;
  }
  std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir);
  Vect3 so = sovo.first;
  Vect3 vo = sovo.second;
  Vect3 si = repac.get_s();
  Vect3 vi = repac.get_v();
  bool rep = true;
  if (k==1) {
    rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv);
  }
  if (rep) {
    std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir);
    Vect3 sot = sovot.first;
    Vect3 vot = sovot.second;
    Vect3 sit = vi.ScalAdd(k*tstep,si);
    Vect3 st = sot.Sub(sit);
    Vect3 vop = linvel(ownship,tstep,trajdir,k-1);
    Vect3 vok = linvel(ownship,tstep,trajdir,k);
    return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) &&
        CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) &&
        CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv);
  }
  return false;
}

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

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

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