ossimGpt ossimThreeParamDatum::shiftToWgs84(const ossimGpt &aPt)const
{
if(ossim::almostEqual(param1(), 0.0)&&
ossim::almostEqual(param2(), 0.0)&&
ossim::almostEqual(param3(), 0.0))
{
return ossimGpt(aPt.latd(),
aPt.lond(),
aPt.latd(),
ossimGpt().datum());
}
ossimEcefPoint p1 = aPt;
ossimEcefPoint p2;
if(withinMolodenskyRange(aPt.latd()))
{
ossimWgs84Datum wgs84;
double latin, lonin, hgtin;
double latout, lonout, hgtout;
double da = wgs84.ellipsoid()->getA() - ellipsoid()->getA();
double df = wgs84.ellipsoid()->getFlattening() - ellipsoid()->getFlattening();
latin = aPt.latr();
lonin = aPt.lonr();
hgtin = aPt.height();
if(aPt.isHgtNan())
{
hgtin = 0.0;
}
molodenskyShift(ellipsoid()->getA(), da, ellipsoid()->getFlattening(), df, param1(), param2(), param3(),
latin, lonin, hgtin,
latout, lonout, hgtout);
ossimGpt g;
g.latr(latout);
g.lonr(lonout);
g.height(hgtout);
g.datum(this);
return g;
}
else
{
p2 = ossimEcefPoint(p1.x() + theParam1,
p1.y() + theParam2,
p1.z() + theParam3);
}
return ossimGpt(p2); // defaults to WGS84
}
ossimGpt ossimNadconNarDatum::shift(const ossimGpt &aPt)const
{
const ossimDatum* datum = aPt.datum();
ossimString code = datum->code();
ossimString subCode(code.begin(),
code.begin() + 3);
if(subCode == "NAR")
{
return aPt;
}
else
{
if(subCode == "NAS")
{
checkGrid(aPt);
if(!theLatGrid.getFileOkFlag()||
!theLonGrid.getFileOkFlag())
{
return ossimThreeParamDatum::shift(aPt);
}
double shiftLat = theLatGrid.getShiftAtLatLon(aPt.latd(), aPt.lond());
double shiftLon = theLonGrid.getShiftAtLatLon(aPt.latd(), aPt.lond());
if( (ossim::isnan(shiftLat)) || (ossim::isnan(shiftLon)) )
{
return ossimThreeParamDatum::shift(aPt);
}
else
{
// Note the shifts are stored in the file
// as seconds.
//
// convert the seconds into decimal degrees.
//
shiftLat /= 3600.0;
shiftLon /= 3600.0;
return ossimGpt(aPt.latd() + shiftLat,
aPt.lond() - shiftLon,
aPt.height(),
this);
}
}
else
{
return ossimThreeParamDatum::shift(aPt);
}
}
return ossimThreeParamDatum::shift(aPt);
}
ossim_int32 ossimUtmProjection::computeZone(const ossimGpt& ground)
{
ossim_int32 result = 0;
double longitude = ground.lonr();
double lat_Degrees = (ossim_int32)( (ground.latd()) + 0.00000005);
double long_Degrees = (ossim_int32)( (ground.lond()) + 0.00000005);
if (longitude < M_PI)
result = (ossim_int32)( (31 + ((180 * longitude) / (6 * M_PI)) ) + 0.00000005);
else
result = (ossim_int32)( (((180 * longitude) / (6 * M_PI)) - 29) + 0.00000005);
if (result > 60)
result = 1;
/* UTM special cases */
if ((lat_Degrees > 55) && (lat_Degrees < 64) && (long_Degrees > -1)
&& (long_Degrees < 3))
result = 31;
if ((lat_Degrees > 55) && (lat_Degrees < 64) && (long_Degrees > 2)
&& (long_Degrees < 12))
result = 32;
if ((lat_Degrees > 71) && (long_Degrees > -1) && (long_Degrees < 9))
result = 31;
if ((lat_Degrees > 71) && (long_Degrees > 8) && (long_Degrees < 21))
result = 33;
if ((lat_Degrees > 71) && (long_Degrees > 20) && (long_Degrees < 33))
result = 35;
if ((lat_Degrees > 71) && (long_Degrees > 32) && (long_Degrees < 42))
result = 37;
return result;
}
void ossimUtmProjection::setOrigin(const ossimGpt& origin)
{
setZone(origin);
// NOTE: We will not set the hemisphere if the origin latitude is 0.0.
if (origin.latd() != 0.0)
{
setHemisphere(origin);
}
ossimMapProjection::setOrigin(origin);
}
ossimGpt ossimThreeParamDatum::shift(const ossimGpt &aPt)const
{
const ossimDatum *aDatum = aPt.datum();
if( code() == aDatum->code())
{
return ossimGpt(aPt.latd(), aPt.lond(), aPt.height(), this);
}
if(aDatum)
{
return shiftFromWgs84(aDatum->shiftToWgs84(aPt));
}
return aPt;
}
void ossimDtedElevationDatabase::createRelativePath(ossimFilename& file, const ossimGpt& gpt)const
{
ossimFilename lon, lat;
int ilon = static_cast<int>(floor(gpt.lond()));
if (ilon < 0)
{
lon = "w";
}
else
{
lon = "e";
}
ilon = abs(ilon);
std::ostringstream s1;
s1 << std::setfill('0') << std::setw(3)<< ilon;
lon += s1.str().c_str();//ossimString::toString(ilon);
int ilat = static_cast<int>(floor(gpt.latd()));
if (ilat < 0)
{
lat += "s";
}
else
{
lat += "n";
}
ilat = abs(ilat);
std::ostringstream s2;
s2<< std::setfill('0') << std::setw(2)<< ilat;
lat += s2.str().c_str();
file = lon.dirCat(lat+m_extension);
}
void ossimSrtmElevationDatabase::createRelativePath(ossimFilename& file, const ossimGpt& gpt)const
{
int ilat = static_cast<int>(floor(gpt.latd()));
if (ilat < 0)
{
file = "S";
}
else
{
file = "N";
}
ilat = abs(ilat);
std::ostringstream os1;
os1 << std::setfill('0') << std::setw(2) <<ilat;
file += os1.str().c_str();
int ilon = static_cast<int>(floor(gpt.lond()));
if (ilon < 0)
{
file += "W";
}
else
{
file += "E";
}
ilon = abs(ilon);
std::ostringstream os2;
os2 << std::setfill('0') << std::setw(3) << ilon;
file += os2.str().c_str();
file.setExtension("hgt");
}
//*****************************************************************************
// METHOD: ossimSarModel::getForwardDeriv()
//
// Compute partials of samp/line WRT to ground.
//
//*****************************************************************************
ossimDpt ossimRpcModel::getForwardDeriv(int derivMode,
const ossimGpt& pos,
double h)
{
// If derivMode (parmIdx) >= 0 call base class version
// for "adjustable parameters"
if (derivMode >= 0)
{
return ossimSensorModel::getForwardDeriv(derivMode, pos, h);
}
// Use alternative derivMode definitions
else
{
ossimDpt returnData;
//******************************************
// OBS_INIT mode
// [1]
// [2]
// Note: In this mode, pos is used to pass
// in the (s,l) observations.
//******************************************
if (derivMode==OBS_INIT)
{
// Image coordinates
ossimDpt obs;
obs.samp = pos.latd();
obs.line = pos.lond();
theObs = obs;
}
//******************************************
// EVALUATE mode
// [1] evaluate & save partials, residuals
// [2] return residuals
//******************************************
else if (derivMode==EVALUATE)
{
//***
// Normalize the lat, lon, hgt:
//***
double nlat = (pos.lat - theLatOffset) / theLatScale;
double nlon = (pos.lon - theLonOffset) / theLonScale;
double nhgt;
if( ossim::isnan(pos.hgt) )
{
nhgt = (theHgtScale - theHgtOffset) / theHgtScale;
}
else
{
nhgt = (pos.hgt - theHgtOffset) / theHgtScale;
}
//***
// Compute the normalized line (Un) and sample (Vn):
//***
double Pu = polynomial(nlat, nlon, nhgt, theLineNumCoef);
double Qu = polynomial(nlat, nlon, nhgt, theLineDenCoef);
double Pv = polynomial(nlat, nlon, nhgt, theSampNumCoef);
double Qv = polynomial(nlat, nlon, nhgt, theSampDenCoef);
double Un = Pu / Qu;
double Vn = Pv / Qv;
//***
// Compute the actual line (U) and sample (V):
//***
double U = Un*theLineScale + theLineOffset;
double V = Vn*theSampScale + theSampOffset;
//***
// Compute the partials of each polynomial wrt lat, lon, hgt
//***
double dPu_dLat, dQu_dLat, dPv_dLat, dQv_dLat;
double dPu_dLon, dQu_dLon, dPv_dLon, dQv_dLon;
double dPu_dHgt, dQu_dHgt, dPv_dHgt, dQv_dHgt;
dPu_dLat = dPoly_dLat(nlat, nlon, nhgt, theLineNumCoef);
dQu_dLat = dPoly_dLat(nlat, nlon, nhgt, theLineDenCoef);
dPv_dLat = dPoly_dLat(nlat, nlon, nhgt, theSampNumCoef);
dQv_dLat = dPoly_dLat(nlat, nlon, nhgt, theSampDenCoef);
dPu_dLon = dPoly_dLon(nlat, nlon, nhgt, theLineNumCoef);
dQu_dLon = dPoly_dLon(nlat, nlon, nhgt, theLineDenCoef);
dPv_dLon = dPoly_dLon(nlat, nlon, nhgt, theSampNumCoef);
dQv_dLon = dPoly_dLon(nlat, nlon, nhgt, theSampDenCoef);
dPu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineNumCoef);
dQu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineDenCoef);
dPv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampNumCoef);
dQv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampDenCoef);
//***
// Compute partials of quotients U and V wrt lat, lon, hgt
//***
double dU_dLat, dU_dLon, dU_dHgt, dV_dLat, dV_dLon, dV_dHgt;
dU_dLat = (Qu*dPu_dLat - Pu*dQu_dLat)/(Qu*Qu);
dU_dLon = (Qu*dPu_dLon - Pu*dQu_dLon)/(Qu*Qu);
dU_dHgt = (Qu*dPu_dHgt - Pu*dQu_dHgt)/(Qu*Qu);
dV_dLat = (Qv*dPv_dLat - Pv*dQv_dLat)/(Qv*Qv);
dV_dLon = (Qv*dPv_dLon - Pv*dQv_dLon)/(Qv*Qv);
dV_dHgt = (Qv*dPv_dHgt - Pv*dQv_dHgt)/(Qv*Qv);
//***
// Apply necessary scale factors
//***
dU_dLat *= theLineScale/theLatScale;
dU_dLon *= theLineScale/theLonScale;
dU_dHgt *= theLineScale/theHgtScale;
dV_dLat *= theSampScale/theLatScale;
dV_dLon *= theSampScale/theLonScale;
dV_dHgt *= theSampScale/theHgtScale;
dU_dLat *= DEG_PER_RAD;
dU_dLon *= DEG_PER_RAD;
dV_dLat *= DEG_PER_RAD;
dV_dLon *= DEG_PER_RAD;
// Save the partials referenced to ECF
ossimEcefPoint location(pos);
NEWMAT::Matrix jMat(3,3);
pos.datum()->ellipsoid()->jacobianWrtEcef(location, jMat);
// Line
theParWRTx.u = dU_dLat*jMat(1,1)+dU_dLon*jMat(2,1)+dU_dHgt*jMat(3,1);
theParWRTy.u = dU_dLat*jMat(1,2)+dU_dLon*jMat(2,2)+dU_dHgt*jMat(3,2);
theParWRTz.u = dU_dLat*jMat(1,3)+dU_dLon*jMat(2,3)+dU_dHgt*jMat(3,3);
// Samp
theParWRTx.v = dV_dLat*jMat(1,1)+dV_dLon*jMat(2,1)+dV_dHgt*jMat(3,1);
theParWRTy.v = dV_dLat*jMat(1,2)+dV_dLon*jMat(2,2)+dV_dHgt*jMat(3,2);
theParWRTz.v = dV_dLat*jMat(1,3)+dV_dLon*jMat(2,3)+dV_dHgt*jMat(3,3);
// Residuals
ossimDpt resid(theObs.samp-V, theObs.line-U);
returnData = resid;
}
//******************************************
// P_WRT_X, P_WRT_Y, P_WRT_Z modes
// [1] 3 separate calls required
// [2] return 3 sets of partials
//******************************************
else if (derivMode==P_WRT_X)
{
returnData = theParWRTx;
}
else if (derivMode==P_WRT_Y)
{
returnData = theParWRTy;
}
else
{
returnData = theParWRTz;
}
return returnData;
}
}
ossimDpt ossimRpcModel::getForwardDeriv(int derivMode,
const ossimGpt& pos,
double h)
{
if (derivMode >= 0)
{
return ossimSensorModel::getForwardDeriv(derivMode, pos, h);
}
else
{
ossimDpt returnData;
if (derivMode==OBS_INIT)
{
ossimDpt obs;
obs.samp = pos.latd();
obs.line = pos.lond();
theObs = obs;
}
else if (derivMode==EVALUATE)
{
double nlat = (pos.lat - theLatOffset) / theLatScale;
double nlon = (pos.lon - theLonOffset) / theLonScale;
double nhgt;
if( ossim::isnan(pos.hgt) )
{
nhgt = (theHgtScale - theHgtOffset) / theHgtScale;
}
else
{
nhgt = (pos.hgt - theHgtOffset) / theHgtScale;
}
double Pu = polynomial(nlat, nlon, nhgt, theLineNumCoef);
double Qu = polynomial(nlat, nlon, nhgt, theLineDenCoef);
double Pv = polynomial(nlat, nlon, nhgt, theSampNumCoef);
double Qv = polynomial(nlat, nlon, nhgt, theSampDenCoef);
double Un = Pu / Qu;
double Vn = Pv / Qv;
double U = Un*theLineScale + theLineOffset;
double V = Vn*theSampScale + theSampOffset;
double dPu_dLat, dQu_dLat, dPv_dLat, dQv_dLat;
double dPu_dLon, dQu_dLon, dPv_dLon, dQv_dLon;
double dPu_dHgt, dQu_dHgt, dPv_dHgt, dQv_dHgt;
dPu_dLat = dPoly_dLat(nlat, nlon, nhgt, theLineNumCoef);
dQu_dLat = dPoly_dLat(nlat, nlon, nhgt, theLineDenCoef);
dPv_dLat = dPoly_dLat(nlat, nlon, nhgt, theSampNumCoef);
dQv_dLat = dPoly_dLat(nlat, nlon, nhgt, theSampDenCoef);
dPu_dLon = dPoly_dLon(nlat, nlon, nhgt, theLineNumCoef);
dQu_dLon = dPoly_dLon(nlat, nlon, nhgt, theLineDenCoef);
dPv_dLon = dPoly_dLon(nlat, nlon, nhgt, theSampNumCoef);
dQv_dLon = dPoly_dLon(nlat, nlon, nhgt, theSampDenCoef);
dPu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineNumCoef);
dQu_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theLineDenCoef);
dPv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampNumCoef);
dQv_dHgt = dPoly_dHgt(nlat, nlon, nhgt, theSampDenCoef);
double dU_dLat, dU_dLon, dU_dHgt, dV_dLat, dV_dLon, dV_dHgt;
dU_dLat = (Qu*dPu_dLat - Pu*dQu_dLat)/(Qu*Qu);
dU_dLon = (Qu*dPu_dLon - Pu*dQu_dLon)/(Qu*Qu);
dU_dHgt = (Qu*dPu_dHgt - Pu*dQu_dHgt)/(Qu*Qu);
dV_dLat = (Qv*dPv_dLat - Pv*dQv_dLat)/(Qv*Qv);
dV_dLon = (Qv*dPv_dLon - Pv*dQv_dLon)/(Qv*Qv);
dV_dHgt = (Qv*dPv_dHgt - Pv*dQv_dHgt)/(Qv*Qv);
dU_dLat *= theLineScale/theLatScale;
dU_dLon *= theLineScale/theLonScale;
dU_dHgt *= theLineScale/theHgtScale;
dV_dLat *= theSampScale/theLatScale;
dV_dLon *= theSampScale/theLonScale;
dV_dHgt *= theSampScale/theHgtScale;
dU_dLat *= DEG_PER_RAD;
dU_dLon *= DEG_PER_RAD;
dV_dLat *= DEG_PER_RAD;
dV_dLon *= DEG_PER_RAD;
ossimEcefPoint location(pos);
NEWMAT::Matrix jMat(3,3);
pos.datum()->ellipsoid()->jacobianWrtEcef(location, jMat);
theParWRTx.u = dU_dLat*jMat(1,1)+dU_dLon*jMat(2,1)+dU_dHgt*jMat(3,1);
theParWRTy.u = dU_dLat*jMat(1,2)+dU_dLon*jMat(2,2)+dU_dHgt*jMat(3,2);
theParWRTz.u = dU_dLat*jMat(1,3)+dU_dLon*jMat(2,3)+dU_dHgt*jMat(3,3);
theParWRTx.v = dV_dLat*jMat(1,1)+dV_dLon*jMat(2,1)+dV_dHgt*jMat(3,1);
theParWRTy.v = dV_dLat*jMat(1,2)+dV_dLon*jMat(2,2)+dV_dHgt*jMat(3,2);
theParWRTz.v = dV_dLat*jMat(1,3)+dV_dLon*jMat(2,3)+dV_dHgt*jMat(3,3);
ossimDpt resid(theObs.samp-V, theObs.line-U);
returnData = resid;
}
else if (derivMode==P_WRT_X)
{
returnData = theParWRTx;
}
else if (derivMode==P_WRT_Y)
{
returnData = theParWRTy;
}
else
{
returnData = theParWRTz;
}
return returnData;
}
}
std::string oms::WktUtility::toWktGeometryGivenCenterRadius(const ossimGpt& center,
double radius,
ossimUnitType radialUnit,
unsigned int numberOfSamples,
int directionalSign)const
{
std::string result = "";
if(ossim::almostEqual(radius, 0.0))
{
result = "POINT(" + ossimString::toString(center.lond()).string() + " " + ossimString::toString(center.latd()).string() + ")";
}
else
{
double headingStepSizeInDegrees = (360.0/static_cast<double>(numberOfSamples))*
directionalSign;
ossim_uint32 step = 0;
double degreesDistance = ossimUnitConversionTool(radius, radialUnit).getValue(OSSIM_DEGREES);
ossimDpt centerGpt(center);
ossimDpt newPoint;
ossimDpt firstPoint;
if(numberOfSamples > 0)
{
result = "POLYGON((";
}
for(step = 0; step < numberOfSamples; ++step)
{
double x = ossim::degreesToRadians(headingStepSizeInDegrees*step);// - 90.0));
double dy = sin(x);
double dx = cos(x);
newPoint.lat = dy*degreesDistance;
newPoint.lon = dx*degreesDistance;
newPoint = centerGpt + newPoint;
if(step == 0)
{
firstPoint = newPoint;
}
result += (ossimString::toString(ossim::clamp(newPoint.lon, -180.0, 180.0)).string()+" "
+ossimString::toString(ossim::clamp(newPoint.lat, -90.0, 90.0)).string()+",");
}
if(numberOfSamples > 0)
{
result += (ossimString::toString(firstPoint.lon ).string() + " " +
ossimString::toString(firstPoint.lat ).string() );
result += "))";
}
}
return result;
}
//*****************************************************************************
// METHOD: ossimHgtRef::getLocalTerrainNormal()
//
// Get get local terrain normal unit vector.
//
//*****************************************************************************
ossimColumnVector3d ossimHgtRef::getLocalTerrainNormal(const ossimGpt& pg) const
{
ossimColumnVector3d tNorm;
const ossim_float64 delta = 100.0;
switch (theCurrentHeightRefType)
{
case AT_HGT:
// Use ellipsoid tangent plane mormal
tNorm = tNorm.zAligned();
break;
case AT_DEM:
{
// Use local 3X3 grid around point to get mean slope
NEWMAT::Matrix h(3,3);
ossimDpt mpd;
mpd = pg.metersPerDegree();
ossim_float64 dLon = delta/mpd.x;
ossim_float64 dLat = delta/mpd.y;
for (ossim_int32 lon=-1; lon<=1; ++lon)
{
ossim_float64 clon = pg.lond()+lon*dLon;
for (ossim_int32 lat=-1; lat<=1; ++lat)
{
ossim_float64 clat = pg.latd()+lat*dLat;
ossimGpt p(clat, clon, pg.height());
h(2-lat,lon+2) =
ossimElevManager::instance()->getHeightAboveEllipsoid(p);
}
}
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<<"DEBUG: getLocalTerrainNormal... 3X3 grid"<<endl;
for (ossim_int32 lat=-1; lat<=1; ++lat)
{
for (ossim_int32 lon=-1; lon<=1; ++lon)
ossimNotify(ossimNotifyLevel_DEBUG)<<" "<<h(lat+2,lon+2);
ossimNotify(ossimNotifyLevel_DEBUG)<<endl;
}
}
ossim_float64 dz_dlon =
((h(1,3)+2*h(2,3)+h(3,3))-(h(1,1)+2*h(2,1)+h(3,1)))/(8*delta);
ossim_float64 dz_dlat =
((h(1,1)+2*h(1,2)+h(1,3))-(h(3,1)+2*h(3,2)+h(3,3)))/(8*delta);
tNorm[0] = -dz_dlon;
tNorm[1] = -dz_dlat;
tNorm[2] = 1.0 - sqrt(dz_dlon*dz_dlon+dz_dlat*dz_dlat);
}
// If error condition, return z-aligned vector to allow continuation
if (ossim::isnan(tNorm[0]) ||
ossim::isnan(tNorm[1]) ||
ossim::isnan(tNorm[2]))
{
tNorm = tNorm.zAligned();
if(traceDebug())
{
ossimNotify(ossimNotifyLevel_WARN)
<< "WARNING: ossimHgtRef::getLocalTerrainNormal(): "
<< "\n error... terrain normal set to vertical..."
<< std::endl;
}
}
break;
default:
tNorm = tNorm.zAligned();
break;
}
return tNorm;
}
void ossimUtmProjection::setHemisphere(const ossimGpt& ground)
{
char hemisphere = ground.latd()<0.0?'S':'N';
setHemisphere(hemisphere);
}
