示例#1
0
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);
}
示例#2
0
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turn(LatLonAlt s0, Velocity v0, double t, double R, bool turnRight) {
	if (Util::almost_equals(R,0))
		return std::pair<LatLonAlt,Velocity>(s0,v0);
	int dir = -1;
	if (turnRight) dir = 1;
	double omega = dir*v0.gs()/R;
	return turnOmega(s0,v0,t,omega);
}
示例#3
0
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);
}
示例#4
0
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);
	}
}
示例#5
0
std::pair<LatLonAlt,Velocity> KinematicsLatLon::gsAccelUntil(const LatLonAlt& so, const Velocity& vo, double t, double goalGS, double gsAccel_d) {
	if (gsAccel_d < 0 ) {
		fpln("Kinematics::gsAccelUntil: user supplied negative gsAccel!!");
		gsAccel_d = -gsAccel_d;                              // make sure user supplies positive value
	}
	double accelTime = Kinematics::gsAccelTime(vo,goalGS,gsAccel_d);
	int sgn = 1;
	if (goalGS < vo.gs()) sgn = -1;
	double a = sgn*gsAccel_d;
	std::pair<LatLonAlt, Velocity> nsv = gsAccel(so, vo, accelTime, a);
	if (t<=accelTime) return gsAccel(so, vo, t, a);
	else  return gsAccel(nsv.first, nsv.second, t-accelTime, 0);
}
示例#6
0
 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);		
 }
示例#7
0
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);
}
示例#8
0
  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);
	}
示例#9
0
 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);
 }
示例#10
0
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));
}
示例#11
0
// 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);
}