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;
}
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;
}
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;
}
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));
}