//*****************************************************************************
// METHOD: ossimRpcModel::lineSampleToWorld()
//
// Overrides base class implementation. Performs DEM intersection.
//*****************************************************************************
void ossimRpcModel::lineSampleToWorld(const ossimDpt& imagePoint,
ossimGpt& worldPoint) const
{
//---
// Under debate... (drb 20130610)
// this seems to be more accurate for the round trip
//---
#if 0
if(!imagePoint.hasNans())
{
lineSampleHeightToWorld(imagePoint,
worldPoint.height(),
worldPoint);
}
else
{
worldPoint.makeNan();
}
#else
if(!imagePoint.hasNans())
{
ossimEcefRay ray;
imagingRay(imagePoint, ray);
ossimElevManager::instance()->intersectRay(ray, worldPoint);
}
else
{
worldPoint.makeNan();
}
#endif
}
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);
}
void ossimRpcProjection::lineSampleToWorld(const ossimDpt& imagePoint,
ossimGpt& worldPoint) const
{
if(!imagePoint.hasNans())
{
lineSampleHeightToWorld(imagePoint,
worldPoint.height(),
worldPoint);
}
else
{
worldPoint.makeNan();
}
}
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;
}
//*****************************************************************************
// 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;
}
