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());
}
}
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);
}
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<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);
}
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;
}
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;
}
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);
}
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;
}
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);
}
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));
}
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);
}
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);
}
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;
}
AziEquiProjection::AziEquiProjection(const LatLonAlt& lla) {
projAlt = lla.alt();
ref = spherical2xyz(lla.lat(),lla.lon());
llaRef = lla;
}
Vect3 AziEquiProjection::project(const LatLonAlt& lla) const {
return Vect3(project2(lla),lla.alt() - projAlt);
}
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;
}