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);
}
/**
* Given two points on a turn and the velocity (direction) at the first point, determine the direction for the shortest turn going through the second point,
* returning true if that relative direction is to the right
*/
bool ProjectedKinematics::clockwise(Position s1, Velocity v1, Position s2) {
double trk1 = v1.trk();
double trk2;
if (s1.isLatLon()) {
trk2 = GreatCircle::velocity_initial(s1.lla(), s2.lla(), 1).trk();
} else {
trk2 = s2.point().Sub(s1.point()).vect2().track();
}
return Util::clockwise(trk1, trk2);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::gsAccel(const LatLonAlt& so, const Velocity& vo, double t, double a) {
double dist = vo.gs()*t + 0.5*a*t*t;
double currentTrk = vo.trk();
LatLonAlt sn = GreatCircle::linear_initial(so, currentTrk, dist);
sn = sn.mkAlt(so.alt() + vo.z*t);
double vnGs = vo.gs() + a*t;
Velocity vn = vo.mkGs(vnGs);
//fpln(" $$$$$ gsAccel: sn = "+sn+" vn = "+vn);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
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);
}
}
double KinematicsLatLon::closestDistOnTurn(const LatLonAlt& s0, const Velocity& v0, double R, int dir, const LatLonAlt& x, double maxDist) {
LatLonAlt cent = center(s0, v0.trk(), R, dir);
if (x.mkAlt(0).almostEquals(cent.mkAlt(0))) return -1.0;
double ang1 = GreatCircle::initial_course(cent,s0);
double ang2 = GreatCircle::initial_course(cent,x);
double delta = Util::turnDelta(ang1, ang2, dir);
double t = GreatCircle::small_circle_arc_length(R, delta);
if (maxDist > 0 && (t < 0 || t > maxDist)) {
double maxD = 2*M_PI*R;
if (t > (maxD + maxDist) / 2) {
return 0.0;
} else {
return maxDist;
}
}
return t;
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnOmegaAlt(const LatLonAlt& so, const Velocity& vo, double t, double omega) {
double currentTrk = vo.trk();
double perpTrk;
if (omega > 0.0) {
perpTrk = currentTrk+M_PI/2;
} else {
perpTrk = currentTrk-M_PI/2;
}
double radius = turnRadiusByRate(vo.gs(), omega);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
//f.pln("center="+center);
LatLonAlt sn = GreatCircle::small_circle_rotation(so,center,omega*t).mkAlt(so.alt()+vo.z*t);
double finalPerpTrk = GreatCircle::initial_course(sn,center);
double nTrk = finalPerpTrk - M_PI/2 * Util::sign(omega);
Velocity vn = vo.mkTrk(nTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnOmega(const LatLonAlt& so, const Velocity& vo, double t, double omega) {
double currentTrk = vo.trk();
double perpTrk;
if (omega > 0) perpTrk = currentTrk+M_PI/2;
else perpTrk = currentTrk-M_PI/2;
double radius = Kinematics::turnRadiusByRate(vo.gs(), omega);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
double alpha = omega*t;
double vFinalTrk = GreatCircle::initial_course(center,so);
double nTrk = vFinalTrk + alpha;
LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, radius);
sn = sn.mkAlt(so.alt() + vo.z*t);
//double finalTrk = currentTrk+theta;
double final_course = GreatCircle::final_course(center,sn); // TODO: THIS IS PROBABLY BETTER
double finalTrk = final_course + Util::sign(omega)*M_PI/2;
Velocity vn = vo.mkTrk(finalTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnByDist(const LatLonAlt& so, const Velocity& vo, double signedRadius, double d) {
double currentTrk = vo.trk();
double perpTrk;
if (signedRadius > 0) perpTrk = currentTrk+M_PI/2;
else perpTrk = currentTrk-M_PI/2;
double radius = std::abs(signedRadius);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
double alpha = d/signedRadius;
double vFinalTrk = GreatCircle::initial_course(center,so);
double nTrk = vFinalTrk + alpha;
LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, radius);
double t = d/vo.gs();
sn = sn.mkAlt(so.alt() + vo.z*t);
double final_course = GreatCircle::final_course(center,sn);
//double finalTrk = currentTrk+alpha;
double finalTrk = final_course + Util::sign(d)*M_PI/2;
Velocity vn = vo.mkTrk(finalTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnRadius(const LatLonAlt& so, const Velocity& vo, double t, double signedRadius) {
double currentTrk = vo.trk();
double dir = Util::sign(signedRadius);
double perpTrk = currentTrk+dir*M_PI/2;
double radius = std::abs(signedRadius);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
double pathDist = vo.gs()*t;
double theta = pathDist/radius;
//TODO: theta = pathDist/radius, assumes radius is a chord radius, when it is actually a great circle radius.
// for small distances the difference is not that big, but still...
//Note: The other problem is that this assumes a constant speed and constant ground speed through the turn.
// this may or may not be true.
double vFinalTrk = GreatCircle::initial_course(center,so);
double nTrk = vFinalTrk + dir*theta;
LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, radius);
sn = sn.mkAlt(so.alt() + vo.z*t);
double final_course = GreatCircle::final_course(center,sn);
double finalTrk = final_course + dir*M_PI/2;
Velocity vn = vo.mkTrk(finalTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
LatLonAlt KinematicsLatLon::center(const LatLonAlt& s0, const Velocity& v0, double omega) {
double v = v0.gs();
double R = v/omega;
return center(s0, v0.trk(), R, Util::sign(omega));
}
// 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);
}