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