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