std::pair<LatLonAlt,Velocity> KinematicsLatLon::vsAccel(const LatLonAlt& so, const Velocity& vo, double t, double a) {
double dist = vo.gs()*t;
double currentTrk = vo.trk();
LatLonAlt sn = GreatCircle::linear_initial(so, currentTrk, dist);
double nsz = so.alt() + vo.z*t + 0.5*a*t*t;
sn = sn.mkAlt(nsz);
Velocity vn = vo.mkVs(vo.z + a*t);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
Triple<Position,Velocity,double> ProjectedKinematics::vsLevelOutFinal(const Position& so, const Velocity& vo, double climbRate, double targetAlt, double a) {
if (climbRate == 0) {
return Triple<Position,Velocity,double>(so.mkZ(targetAlt),vo.mkVs(0),0.0);
} else {
std::pair<Vect3, Velocity> sv = std::pair<Vect3, Velocity>(so.point(),vo);
if (so.isLatLon()) {
sv = Projection::createProjection(so.lla().zeroAlt()).project(so, vo);
}
StateVector vat = Kinematics::vsLevelOutFinal(sv, climbRate, targetAlt, a);
if (vat.t() < 0) return Triple<Position,Velocity,double>(Position::INVALID(), Velocity::INVALIDV(), vat.t());
if (so.isLatLon()) {
std::pair<Position,Velocity>p = Projection::createProjection(so.lla().zeroAlt()).inverse(vat.s(),vat.v(),true);
return Triple<Position,Velocity,double>(p.first, p.second, vat.t());
} else {
return Triple<Position,Velocity,double>(Position(vat.s()), vat.v(), vat.t());
}
}
}