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