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