int KinematicBandsCore::epsilonH(const OwnshipState& ownship, const TrafficState& ac) { Position pi = ac.getPosition(); Velocity vi = ac.getVelocity(); Vect2 s = ownship.get_s().Sub(ownship.pos_to_s(pi)).vect2(); Vect2 v = ownship.get_v().Sub(ownship.vel_to_v(pi,vi)).vect2(); return CriteriaCore::horizontalCoordination(s,v); }
int KinematicBandsCore::epsilonV(const OwnshipState& ownship, const TrafficState& ac) { Position pi = ac.getPosition(); Velocity vi = ac.getVelocity(); Vect3 si = ownship.pos_to_s(pi); Vect3 s = ownship.get_s().Sub(si); return CriteriaCore::verticalCoordinationLoS(s,ownship.get_v(),ownship.vel_to_v(pi,vi), ownship.getId(), ac.getId()); }
int KinematicAltBands::first_band_alt_generic(Detection3D* conflict_det, Detection3D* recovery_det, double B, double T, double B2, double T2, const TrafficState& ownship, const std::vector<TrafficState>& traffic, bool dir, bool green) { int upper = (int)(dir ? std::floor((max_val(ownship)-min_val(ownship))/get_step())+1 : std::floor((ownship.altitude()-min_val(ownship))/get_step())); int lower = dir ? (int)(std::ceil(ownship.altitude()-min_val(ownship))/get_step()) : 0; if (ownship.altitude() < min_val(ownship) || ownship.altitude() > max_val(ownship)) { return -1; } else { return first_nat(lower,upper,dir,conflict_det,recovery_det,B,T,B2,T2,ownship,traffic,green); } }
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; }
std::pair<Vect3, Velocity> KinematicTrkBands::trajectory(const TrafficState& ownship, double time, bool dir) const { std::pair<Position,Velocity> posvel; if (instantaneous_bands()) { double trk = ownship.getVelocity().trk()+(dir?1:-1)*j_step_*get_step(); posvel = std::pair<Position,Velocity>(ownship.getPosition(),ownship.getVelocity().mkTrk(trk)); } else { double gso = ownship.groundSpeed(); double bank = turn_rate_ == 0 ? bank_angle_ : std::abs(Kinematics::bankAngle(gso,turn_rate_)); double R = Kinematics::turnRadius(ownship.get_v().gs(), bank); posvel = ProjectedKinematics::turn(ownship.getPosition(),ownship.getVelocity(),time,R,dir); } return std::pair<Vect3, Velocity>(ownship.pos_to_s(posvel.first),ownship.vel_to_v(posvel.first,posvel.second)); }
// trajdir: false is left bool KinematicIntegerBands::red_band_exist(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; return (usehcrit && first_nonrepulsive_step(tstep,trajdir,max,ownship,repac,epsh) >= 0) || (usevcrit && first_nonvert_repul_step(tstep,trajdir,max,ownship,repac,epsv) >= 0) || any_conflict_step(conflict_det,tstep,B,T,trajdir,max,ownship,traffic) || (recovery_det != NULL && any_conflict_step(recovery_det,tstep,B2,T2,trajdir,max,ownship,traffic)); }
bool KinematicVsBands::all_red(Detection3D* conflict_det, Detection3D* recovery_det, const TrafficState& repac, double B, double T, const OwnshipState& ownship, const std::vector<TrafficState>& traffic) const { double vso = ownship.getVelocity().vs(); int maxdown = (int)std::max(std::ceil((vso-min)/step),0.0)+1; int maxup = (int)std::max(std::ceil((max-vso)/step),0.0)+1; double tstep = step/vertical_accel; int epsv = 0; if (repac.isValid()) { epsv = KinematicBandsCore::epsilonV(ownship,repac); } return KinematicIntegerBands::all_int_red(conflict_det,recovery_det,tstep,B,T,0,B,maxdown,maxup,ownship,traffic,repac,0,epsv,0); }
/** * 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)); }
void KinematicVsBands::none_bands(IntervalSet& noneset, Detection3D* conflict_det, Detection3D* recovery_det, const TrafficState& repac, double B, double T, const OwnshipState& ownship, const std::vector<TrafficState>& traffic) const { double vso = ownship.getVelocity().vs(); int maxdown = (int)std::max(std::ceil((vso-min)/step),0.0)+1; int maxup = (int)std::max(std::ceil((max-vso)/step),0.0)+1; double tstep = step/vertical_accel; std::vector<Integerval> vsint = std::vector<Integerval>(); int epsv = 0; if (repac.isValid()) { epsv = KinematicBandsCore::epsilonV(ownship,repac); } KinematicIntegerBands::kinematic_bands_combine(vsint,conflict_det,recovery_det,tstep,B,T,0,B,maxdown,maxup,ownship,traffic,repac,0,epsv); KinematicIntegerBands::toIntervalSet(noneset,vsint,step,vso,min,max); }
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; }
int KinematicIntegerBands::bands_search_index(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; int FirstLos = first_los_search_index(conflict_det,recovery_det,tstep,B,T,B2,T2,trajdir,max,ownship,traffic); int FirstNonHRep = !usehcrit || FirstLos == 0 ? FirstLos : first_nonrepulsive_step(tstep,trajdir,FirstLos-1,ownship,repac,epsh); int FirstProbHcrit = FirstNonHRep < 0 ? max+1 : FirstNonHRep; int FirstProbHL = std::min(FirstLos,FirstProbHcrit); int FirstNonVRep = !usevcrit || FirstProbHL == 0 ? FirstProbHL : first_nonvert_repul_step(tstep,trajdir,FirstProbHL-1,ownship,repac,epsv); int FirstProbVcrit = FirstNonVRep < 0 ? max+1 : FirstNonVRep; return std::min(FirstProbHL,FirstProbVcrit); }
// 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); } }
double KinematicAltBands::own_val(const TrafficState& ownship) const { return ownship.altitude(); }
void printBands(Daidalus& daa, KinematicMultiBands& bands) { bool nowind = daa.getWindField().isZero(); TrafficState own = daa.getOwnshipState(); std::string trkstr = nowind ? "Track" : "Heading"; std::string gsstr = nowind ? "Ground Speed" : "Airspeed"; std::cout << std::endl; for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) { std::cout << "Conflict Aircraft for Alert Level " << Fmi(alert_level) << ": " << TrafficState::listToString(bands.conflictAircraft(alert_level)) << std::endl; } std::cout << std::endl; // Track/Heading double trk_deg = own.track("deg"); std::cout << "Ownship " << trkstr << ": "+Fm2(trk_deg) << " [deg]" << std::endl; std::cout << "Region of Current " << trkstr+": " << BandsRegion::to_string(bands.regionOfTrack(trk_deg,"deg")) << std::endl; std::cout << trkstr << " Bands [deg,deg]" << std::endl; for (int i=0; i < bands.trackLength(); ++i) { Interval ii = bands.track(i,"deg"); std::cout << " " << BandsRegion::to_string(bands.trackRegion(i)) << ":\t" << ii.toString(2) << std::endl; } for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) { std::cout << "Peripheral " << trkstr << " Aircraft for Alert Level " << Fmi(alert_level) << ": " << TrafficState::listToString(bands.peripheralTrackAircraft(alert_level)) << std::endl; } std::cout << trkstr << " Resolution (right): " << num2str(bands.trackResolution(true,"deg"),"deg") << std::endl; std::cout << trkstr << " Resolution (left): " << num2str(bands.trackResolution(false,"deg"),"deg") << std::endl; std::cout << "Preferred "+trkstr+" Direction: "; if (bands.preferredTrackDirection()) { std::cout << "right" << std::endl; } else { std::cout << "left" << std::endl; } std::cout << "Time to " << trkstr << " Recovery: " << num2str(bands.timeToTrackRecovery(),"s") << std::endl; // Ground Speed/Air Speed double gs_knot = own.groundSpeed("knot"); std::cout << "Ownship " << gsstr << ": "+Fm2(gs_knot) << " [knot]" << std::endl; std::cout << "Region of Current " << gsstr+": " << BandsRegion::to_string(bands.regionOfGroundSpeed(gs_knot,"knot")) << std::endl; std::cout << gsstr << " Bands [knot,knot]:" << std::endl; for (int i=0; i < bands.groundSpeedLength(); ++i) { Interval ii = bands.groundSpeed(i,"knot"); std::cout << " " << BandsRegion::to_string(bands.groundSpeedRegion(i)) << ":\t" << ii.toString(2) << std::endl; } for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) { std::cout << "Peripheral " << gsstr << " Aircraft for Alert Level " << Fmi(alert_level) << ": " << TrafficState::listToString(bands.peripheralGroundSpeedAircraft(alert_level)) << std::endl; } std::cout << gsstr << " Resolution (up): " << num2str(bands.groundSpeedResolution(true,"knot"),"knot") << std::endl; std::cout << gsstr << " Resolution (down): " << num2str(bands.groundSpeedResolution(false,"knot"),"knot") << std::endl; std::cout << "Preferred "+gsstr+" Direction: "; if (bands.preferredGroundSpeedDirection()) { std::cout << "up" << std::endl; } else { std::cout << "down" << std::endl; } std::cout << "Time to " << gsstr << " Recovery: " << num2str(bands.timeToGroundSpeedRecovery(),"s") << std::endl; // Vertical Speed double vs_fpm = own.verticalSpeed("fpm"); std::cout << "Ownship Vertical Speed: "+Fm2(vs_fpm) << " [fpm]" << std::endl; std::cout << "Region of Current Vertical Speed: " << BandsRegion::to_string(bands.regionOfVerticalSpeed(vs_fpm,"fpm")) << std::endl; std::cout << "Vertical Speed Bands [fpm,fpm]:" << std::endl; for (int i=0; i < bands.verticalSpeedLength(); ++i) { Interval ii = bands.verticalSpeed(i,"fpm"); std::cout << " " << BandsRegion::to_string(bands.verticalSpeedRegion(i)) << ":\t" << ii.toString(2) << std::endl; } for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) { std::cout << "Peripheral Vertical Speed Aircraft for Alert Level " << Fmi(alert_level) << ": " << TrafficState::listToString(bands.peripheralVerticalSpeedAircraft(alert_level)) << std::endl; } std::cout << "Vertical Speed Resolution (up): " << num2str(bands.verticalSpeedResolution(true,"fpm"),"fpm") << std::endl; std::cout << "Vertical Speed Resolution (down): " << num2str(bands.verticalSpeedResolution(false,"fpm"),"fpm") << std::endl; std::cout << "Preferred Vertical Speed Direction: "; if (bands.preferredVerticalSpeedDirection()) { std::cout << "up" << std::endl; } else { std::cout << "down" << std::endl; } std::cout << "Time to Vertical Speed Recovery: " << num2str(bands.timeToVerticalSpeedRecovery(),"s") << std::endl; // Altitude double alt_ft = own.altitude("ft"); std::cout << "Ownship Altitude: "+Fm2(alt_ft) << " [ft]" << std::endl; std::cout << "Region of Current Altitude: " << BandsRegion::to_string(bands.regionOfAltitude(alt_ft,"ft")) << std::endl; std::cout << "Altitude Bands [ft,ft]:" << std::endl; for (int i=0; i < bands.altitudeLength(); ++i) { Interval ii = bands.altitude(i,"ft"); std::cout << " " << BandsRegion::to_string(bands.altitudeRegion(i)) << ":\t" << ii.toString(2) << std::endl; } for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) { std::cout << "Peripheral Altitude Aircraft for Alert Level " << Fmi(alert_level) << ": " << TrafficState::listToString(bands.peripheralAltitudeAircraft(alert_level)) << std::endl; } std::cout << "Altitude Resolution (up): " << num2str(bands.altitudeResolution(true,"ft"),"ft") << std::endl; std::cout << "Altitude Resolution (down): " << num2str(bands.altitudeResolution(false,"ft"),"ft") << std::endl; std::cout << "Preferred Altitude Direction: "; if (bands.preferredAltitudeDirection()) { std::cout << "up" << std::endl; } else { std::cout << "down" << std::endl; } std::cout << "Time to Altitude Recovery: " << num2str(bands.timeToAltitudeRecovery(),"s") << std::endl; std::cout << std::endl; // Last times to maneuver for (int ac_idx=1; ac_idx <= daa.lastTrafficIndex(); ++ac_idx) { TrafficState ac = daa.getAircraftState(ac_idx); std::cout << "Last Times to Maneuver with Respect to " << ac.getId() << ":" << std::endl; std::cout << " "+trkstr+" Maneuver: "+num2str(bands.lastTimeToTrackManeuver(ac),"s") << std::endl; std::cout << " "+gsstr+" Maneuver: "+num2str(bands.lastTimeToGroundSpeedManeuver(ac),"s") << std::endl; std::cout <<" Vertical Speed Maneuver: "+num2str(bands.lastTimeToVerticalSpeedManeuver(ac),"s") << std::endl; std::cout <<" Altitude Maneuver: "+num2str(bands.lastTimeToAltitudeManeuver(ac),"s") << std::endl; } std::cout << std::endl; }
std::pair<Vect3, Velocity> KinematicAltBands::trajectory(const TrafficState& ownship, double time, bool dir) const { double target_alt = min_val(ownship)+j_step_*get_step(); std::pair<Position,Velocity> posvel; if (instantaneous_bands()) { posvel = std::pair<Position,Velocity>(ownship.getPosition().mkZ(target_alt),ownship.getVelocity().mkVs(0)); } else { double tsqj = ProjectedKinematics::vsLevelOutTime(ownship.getPosition(),ownship.getVelocity(),vertical_rate_, target_alt,vertical_accel_)+time_step(ownship); if (time <= tsqj) { posvel = ProjectedKinematics::vsLevelOut(ownship.getPosition(), ownship.getVelocity(), time, vertical_rate_, target_alt, vertical_accel_); } else { Position npo = ownship.getPosition().linear(ownship.getVelocity(),time); posvel = std::pair<Position,Velocity>(npo.mkZ(target_alt),ownship.getVelocity().mkVs(0)); } } return std::pair<Vect3,Velocity>(ownship.pos_to_s(posvel.first),ownship.vel_to_v(posvel.first,posvel.second)); }
double KinematicTrkBands::own_val(const TrafficState& ownship) const { return ownship.track(); }
double KinematicTrkBands::time_step(const TrafficState& ownship) const { double gso = ownship.groundSpeed(); double omega = turn_rate_ == 0 ? Kinematics::turnRate(gso,bank_angle_) : turn_rate_; return get_step()/omega; }
Velocity KinematicBandsCore::traffic_v(const TrafficState& ac) const { return vel_to_v(ac.getPosition(),ac.getVelocity()); }
Vect3 KinematicBandsCore::traffic_s(const TrafficState& ac) const { return pos_to_s(ac.getPosition()); }
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; }
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::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); }
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; } }