Пример #1
0
 Vect2 AziEquiProjection::project2(const LatLonAlt& lla) const {
   Vect2 p =  sphere_to_plane(ref, spherical2xyz(lla.lat(),lla.lon()));
   if (p.norm() <= 0.0) {
 	  return Vect2::ZERO;
   } else {
       return p.Scal(GreatCircle::distance(lla, llaRef)/p.norm());
   }
 }
Пример #2
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);
}
Пример #3
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);
}
Пример #4
0
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnByDist2D(const LatLonAlt& so, const LatLonAlt& center, int dir, double d, double gsAtd) {
    double R = GreatCircle::distance(so, center);
	double alpha = dir*d/R;
	double vFinalTrk = GreatCircle::initial_course(center,so);
	double nTrk = vFinalTrk + alpha;
	LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, R);
	sn = sn.mkAlt(0.0);
	double final_course = GreatCircle::final_course(center,sn);
	double finalTrk = final_course + dir*M_PI/2;
    Velocity vn = Velocity::mkTrkGsVs(finalTrk,gsAtd,0.0);
	//double finalTrk = vo.trk()+alpha;
	//double finalTrk = final_course + Util.sign(d)*Math.PI/2;
	//Velocity vn = vo.mkTrk(finalTrk);          // TODO:  THIS IS WRONG -- cannot assume gs is constant!!!!
	return std::pair<LatLonAlt,Velocity>(sn,vn);
}
Пример #5
0
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;
}
Пример #6
0
scene::LatLonAlt
scene::ECEFToLLATransform::transform(const Vector3& ecef) const
{
   LatLonAlt lla;

   Vector3 myecef = ecef;

   //do conversion here; store result in lla struct

   double initLat = getInitialLatitude(myecef);
   double tempLat = computeLatitude(myecef, initLat);
   double threshold = 0.0;

   int idx = 0;
   do
   {
      if (idx++ > 4) break;

      lla.setLatRadians(tempLat);
      //recompute reducedLatitude
      initLat = computeReducedLatitude(tempLat);
      //recompute latitude
      tempLat = computeLatitude(myecef, initLat);

      threshold = lla.getLatRadians() - tempLat;
   }
   while (std::abs(threshold) > .00000000000000000001);

   lla.setLatRadians(tempLat);
   lla.setLonRadians(computeLongitude(myecef));
   lla.setAlt(computeAltitude(myecef, lla.getLatRadians()));

   return lla;
}
Пример #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
double KinematicsLatLon::closestTimeOnTurn(const LatLonAlt& s0, const Velocity& v0, double omega, const LatLonAlt& x, double endTime) {
	LatLonAlt cent = center(s0,v0,omega);
	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, Util::sign(omega));
	double t = std::abs(delta/omega);
	if (endTime > 0 && (t < 0 || t > endTime)) {
		double maxTime = 2*M_PI/std::abs(omega);
		if (t > (maxTime + endTime) / 2) {
			return 0.0;
		} else {
			return endTime;
		}
	}
	return t;

}
Пример #9
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);
	}
Пример #10
0
void SceneGeometry::initialize()
{
    // Compute slant plane vectors
    mXs = mPa - mPo;
    mXs.normalize();


    mZs = math::linear::cross(mXs, mVa);
    mZs.normalize();

    // Figure out if we are pointing up or down
    mSideOfTrack = (mZs.dot(mPo) < 0) ? -1 : 1;
    mZs *= mSideOfTrack;

    mYs = math::linear::cross(mZs, mXs);

    
    // Transform mRefPosition to LLA; compute 'up'
    LatLonAlt lla = Utilities::ecefToLatLon(mPo);

    double sinLat = sin(lla.getLatRadians());
    double cosLat = cos(lla.getLatRadians());
    double sinLon = sin(lla.getLonRadians());
    double cosLon = cos(lla.getLonRadians());

    // mZg is also up
    mZg[0] = cosLat * cosLon;
    mZg[1] = cosLat * sinLon;
    mZg[2] = sinLat;

    mNorth[0] = -sinLat * cosLon;
    mNorth[1] = -sinLat * sinLon;
    mNorth[2] = cosLat;

    // Calculate ground range
    mRg = mXs - mZg * (mXs.dot(mZg));
    mRg *= -1;

    // Calculate ground track
    mVg = mVa - mZg * (mVa.dot(mZg));
}
Пример #11
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);
 }
Пример #12
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);		
 }
Пример #13
0
double scene::LLAToECEFTransform::computeRadius(const LatLonAlt& lla)
{
    double f = model->calculateFlattening();
    
    double flatLat = computeLatitude(lla.getLatRadians());
    
    double denominator = (1.0 / pow((1.0 - f), 2)) - 1.0;
    denominator *= pow(sin(flatLat), 2);
    denominator += 1.0;
    
    double flatRadius = model->getEquatorialRadius();
    flatRadius = pow(flatRadius, 2);
    flatRadius /= denominator;
    flatRadius = sqrt(flatRadius);
    
    return flatRadius;
}
Пример #14
0
 AziEquiProjection::AziEquiProjection(const LatLonAlt& lla) {
     projAlt = lla.alt();
     ref = spherical2xyz(lla.lat(),lla.lon());
     llaRef = lla;
 }
Пример #15
0
 Vect3 AziEquiProjection::project(const LatLonAlt& lla) const {
    return Vect3(project2(lla),lla.alt() - projAlt);
 }      
Пример #16
0
scene::Vector3 scene::LLAToECEFTransform::transform(const LatLonAlt& lla)
{
    Vector3 ecef;
    
    LatLonAlt mylla = lla;
    
    if (std::abs(mylla.getLatRadians()) > M_PI/2
	|| std::abs(mylla.getLonRadians()) > M_PI)
    {
	//invalid lla coordinate
	std::ostringstream str;
	str <<  "Invalid lla coordinate: ";
	str << "lat=";
	str << lla.getLatRadians();
	str << ", lon=";
	str << lla.getLonRadians();
	str << ", alt=";
	str << lla.getAlt();
	
	throw except::InvalidFormatException(str.str());
    }
    
    //do conversion here; store result in ecef struct
    
    double r = computeRadius(mylla);
    double flatLat = computeLatitude(mylla.getLatRadians());
    
    double coslat = cos(mylla.getLatRadians());
    double coslon = cos(mylla.getLonRadians());
    double cosflatlat = cos(flatLat);
    double sinlat = sin(mylla.getLatRadians());
    double sinlon = sin(mylla.getLonRadians());
    double sinflatlat = sin(flatLat);
    
    
    ecef[0] = (r * cosflatlat * coslon) + (mylla.getAlt() * coslat * coslon);
    ecef[1] = (r * cosflatlat * sinlon) + (mylla.getAlt() * coslat * sinlon);
    ecef[2] = (r * sinflatlat) + (mylla.getAlt() * sinlat);
    
    return ecef;
}