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

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;
	}
//.........这里部分代码省略.........

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