std::pair<Position,Velocity> ProjectedKinematics::gsAccel(const Position& so, const Velocity& vo, double t, double a) {
Vect3 s3 = so.point();
if (so.isLatLon()) {
s3 = Projection::createProjection(so.lla().zeroAlt()).project(so);
}
Vect3 pres = Kinematics::gsAccelPos(s3,vo,t,a);
Velocity vres = Velocity::mkTrkGsVs(vo.trk(),vo.gs()+a*t,vo.vs());
if (so.isLatLon()) {
return Projection::createProjection(so.lla().zeroAlt()).inverse(pres,vres,true);
} else {
return std::pair<Position,Velocity>(Position(pres), vres);
}
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::vsAccelUntil(const LatLonAlt& so, const Velocity& vo, double t, double goalVs, double vsAccel_d) {
if (vsAccel_d < 0 ) {
fpln("Kinematics::vsAccelUntil: user supplied negative vsAccel!!");
vsAccel_d = -vsAccel_d; // make sure user supplies positive value
}
double accelTime = Kinematics::vsAccelTime(vo,goalVs, vsAccel_d);
int sgn = 1;
if (goalVs < vo.vs()) sgn = -1;
//LatLonAlt ns = LatLonAlt.ZERO;
if (t <= accelTime)
return vsAccel(so, vo, t, sgn*vsAccel_d);
else {
LatLonAlt posEnd = vsAccel(so,vo,accelTime,sgn*vsAccel_d).first;
Velocity nvo = Velocity::mkVxyz(vo.x,vo.y, goalVs);
return linear(posEnd,nvo,t-accelTime);
}
}
// f should be between 0 and 1 to interpolate
Velocity VectFuns::interpolateVelocity(const Velocity& v1, const Velocity& v2, double f) {
double newtrk = v1.trk() + f*(v2.trk() - v1.trk());
double newgs = v1.gs() + f*(v2.gs() - v1.gs());
double newvs = v1.vs() + f*(v2.vs() - v1.vs());
return Velocity::mkTrkGsVs(newtrk,newgs,newvs);
}