double *padfTerms)

{

padfTerms[0] = 1.0;

padfTerms[1] = dfLong;

padfTerms[2] = dfLat;

padfTerms[3] = dfHeight;

padfTerms[4] = dfLong * dfLat;

padfTerms[5] = dfLong * dfHeight;

padfTerms[6] = dfLat * dfHeight;

padfTerms[7] = dfLong * dfLong;

padfTerms[8] = dfLat * dfLat;

padfTerms[9] = dfHeight * dfHeight;

padfTerms[10] = dfLong * dfLat * dfHeight;

padfTerms[11] = dfLong * dfLong * dfLong;

padfTerms[12] = dfLong * dfLat * dfLat;

padfTerms[13] = dfLong * dfHeight * dfHeight;

padfTerms[14] = dfLong * dfLong * dfLat;

padfTerms[15] = dfLat * dfLat * dfLat;

padfTerms[16] = dfLat * dfHeight * dfHeight;

padfTerms[17] = dfLong * dfLong * dfHeight;

padfTerms[18] = dfLat * dfLat * dfHeight;

padfTerms[19] = dfHeight * dfHeight * dfHeight;

double &dfSum1, double &dfSum2, double &dfSum3,

double &dfSum4)

{

XMMReg2Double sum1 = XMMReg2Double::Zero();

XMMReg2Double sum2 = XMMReg2Double::Zero();

XMMReg2Double sum3 = XMMReg2Double::Zero();

XMMReg2Double sum4 = XMMReg2Double::Zero();

for (int i = 0; i < 20; i += 2)

{

const XMMReg2Double terms =

XMMReg2Double::Load2ValAligned(padfTerms + i);

// LINE_NUM_COEFF.

const XMMReg2Double coefs1 =

XMMReg2Double::Load2ValAligned(padfCoefs + i);

// LINE_DEN_COEFF.

const XMMReg2Double coefs2 =

XMMReg2Double::Load2ValAligned(padfCoefs + i + 20);

// SAMP_NUM_COEFF.

const XMMReg2Double coefs3 =

XMMReg2Double::Load2ValAligned(padfCoefs + i + 40);

// SAMP_DEN_COEFF.

const XMMReg2Double coefs4 =

XMMReg2Double::Load2ValAligned(padfCoefs + i + 60);

sum1 += terms * coefs1;

sum2 += terms * coefs2;

sum3 += terms * coefs3;

sum4 += terms * coefs4;

}

dfSum1 = sum1.GetHorizSum();

dfSum2 = sum2.GetHorizSum();

dfSum3 = sum3.GetHorizSum();

dfSum4 = sum4.GetHorizSum();

{

double dfSum1 = 0.0;

double dfSum2 = 0.0;

for (int i = 0; i < 20; i += 2)

{

dfSum1 += padfTerms[i] * padfCoefs[i];

dfSum2 += padfTerms[i + 1] * padfCoefs[i + 1];

}

return dfSum1 + dfSum2;

double dfLong, double dfLat, double dfHeight,

double *pdfPixel, double *pdfLine)

{

double adfTermsWithMargin[20 + 1] = {};

// Make padfTerms aligned on 16-byte boundary for SSE2 aligned loads.

double *padfTerms =

adfTermsWithMargin +

(reinterpret_cast<GUIntptr_t>(adfTermsWithMargin) % 16) / 8;

// Avoid dateline issues.

double diffLong = dfLong - psRPCTransformInfo->sRPC.dfLONG_OFF;

if (diffLong < -270)

{

diffLong += 360;

}

else if (diffLong > 270)

{

diffLong -= 360;

}

const double dfNormalizedLong =

diffLong / psRPCTransformInfo->sRPC.dfLONG_SCALE;

const double dfNormalizedLat =

(dfLat - psRPCTransformInfo->sRPC.dfLAT_OFF) /

psRPCTransformInfo->sRPC.dfLAT_SCALE;

const double dfNormalizedHeight =

(dfHeight - psRPCTransformInfo->sRPC.dfHEIGHT_OFF) /

psRPCTransformInfo->sRPC.dfHEIGHT_SCALE;

// The absolute values of the 3 above normalized values are supposed to be

// below 1. Warn (as debug message) if it is not the case. We allow for some

// margin above 1 (1.5, somewhat arbitrary chosen) before warning.

static int nCountWarningsAboutAboveOneNormalizedValues = 0;

if (nCountWarningsAboutAboveOneNormalizedValues < MAX_ABS_VALUE_WARNINGS)

{

bool bWarned = false;

if (fabs(dfNormalizedLong) > 1.5)

{

bWarned = true;

CPLDebug(

"RPC",

"Normalized %s for (lon,lat,height)=(%f,%f,%f) is %f, "

"i.e. with an absolute value of > 1, which may cause numeric "

"stability problems",

"longitude", dfLong, dfLat, dfHeight, dfNormalizedLong);

}

if (fabs(dfNormalizedLat) > 1.5)

{

bWarned = true;

CPLDebug(

"RPC",

"Normalized %s for (lon,lat,height)=(%f,%f,%f) is %f, "

"ie with an absolute value of > 1, which may cause numeric "

"stability problems",

"latitude", dfLong, dfLat, dfHeight, dfNormalizedLat);

}

if (fabs(dfNormalizedHeight) > 1.5)

{

bWarned = true;

CPLDebug(

"RPC",

"Normalized %s for (lon,lat,height)=(%f,%f,%f) is %f, "

"i.e. with an absolute value of > 1, which may cause numeric "

"stability problems",

"height", dfLong, dfLat, dfHeight, dfNormalizedHeight);

}

if (bWarned)

{

// Limit the number of warnings.

nCountWarningsAboutAboveOneNormalizedValues++;

if (nCountWarningsAboutAboveOneNormalizedValues ==

MAX_ABS_VALUE_WARNINGS)

{

CPLDebug("RPC", "No more such debug warnings will be emitted");

}

}

}

RPCComputeTerms(dfNormalizedLong, dfNormalizedLat, dfNormalizedHeight,

padfTerms);

#ifdef USE_SSE2_OPTIM

double dfSampNum = 0.0;

double dfSampDen = 0.0;

double dfLineNum = 0.0;

double dfLineDen = 0.0;

RPCEvaluate4(padfTerms, psRPCTransformInfo->padfCoeffs, dfLineNum,

dfLineDen, dfSampNum, dfSampDen);

const double dfResultX = dfSampNum / dfSampDen;

const double dfResultY = dfLineNum / dfLineDen;

#else

const double dfResultX =

RPCEvaluate(padfTerms, psRPCTransformInfo->sRPC.adfSAMP_NUM_COEFF) /

RPCEvaluate(padfTerms, psRPCTransformInfo->sRPC.adfSAMP_DEN_COEFF);

const double dfResultY =

RPCEvaluate(padfTerms, psRPCTransformInfo->sRPC.adfLINE_NUM_COEFF) /

RPCEvaluate(padfTerms, psRPCTransformInfo->sRPC.adfLINE_DEN_COEFF);

#endif

// RPCs are using the center of upper left pixel = 0,0 convention

// convert to top left corner = 0,0 convention used in GDAL.

*pdfPixel = dfResultX * psRPCTransformInfo->sRPC.dfSAMP_SCALE +

psRPCTransformInfo->sRPC.dfSAMP_OFF + 0.5;

*pdfLine = dfResultY * psRPCTransformInfo->sRPC.dfLINE_SCALE +

psRPCTransformInfo->sRPC.dfLINE_OFF + 0.5;

const double dfXIn, const double dfYIn,

double *pdfHeight,

double *pdfDEMPixel = nullptr,

double *pdfDEMLine = nullptr)

{

double dfVDatumShift = 0.0;

double dfDEMH = 0.0;

if (psTransform->poDS)

{

double dfX = 0.0;

double dfY = 0.0;

double dfXTemp = dfXIn;

double dfYTemp = dfYIn;

// Check if dem is not in WGS84 and transform points padfX[i], padfY[i].

if (psTransform->poCT)

{

double dfZ = 0.0;

if (!psTransform->poCT->Transform(1, &dfXTemp, &dfYTemp, &dfZ))

{

return false;

}

// We must take the opposite since poCT transforms from

// WGS84 to geoid. And we are going to do the reverse:

// take an elevation over the geoid and transforms it to WGS84.

if (psTransform->bApplyDEMVDatumShift)

dfVDatumShift = -dfZ;

}

bool bRetried = false;

retry:

GDALApplyGeoTransform(psTransform->adfDEMReverseGeoTransform, dfXTemp,

dfYTemp, &dfX, &dfY);

if (pdfDEMPixel)

*pdfDEMPixel = dfX;

if (pdfDEMLine)

*pdfDEMLine = dfY;

if (!GDALRPCGetDEMHeight(psTransform, dfX, dfY, &dfDEMH))

{

// Try to handle the case where the DEM is in LL WGS84 and spans

// over [-180,180], (or very close to it ), presumably with much

// hole in the middle if using VRT, and the longitude goes beyond

// that interval.

if (!bRetried && psTransform->poCT == nullptr &&

(dfXIn >= 180.0 || dfXIn <= -180.0))

{

const int nRasterXSize = psTransform->poDS->GetRasterXSize();

const double dfMinDEMLong = psTransform->adfDEMGeoTransform[0];

const double dfMaxDEMLong =

psTransform->adfDEMGeoTransform[0] +

nRasterXSize * psTransform->adfDEMGeoTransform[1];

if (fabs(dfMinDEMLong - -180) < 0.1 &&

fabs(dfMaxDEMLong - 180) < 0.1)

{

if (dfXIn >= 180)

{

dfXTemp = dfXIn - 360;

dfYTemp = dfYIn;

}

else

{

dfXTemp = dfXIn + 360;

dfYTemp = dfYIn;

}

bRetried = true;

goto retry;

}

}

if (psTransform->bHasDEMMissingValue)

dfDEMH = psTransform->dfDEMMissingValue;

else

{

return false;

}

}

#ifdef DEBUG_VERBOSE_EXTRACT_DEM

CPLDebug("RPC_DEM", "X=%f, Y=%f -> Z=%f", dfX, dfY, dfDEMH);

#endif

}

*pdfHeight = dfVDatumShift + (psTransform->dfHeightOffset +

dfDEMH * psTransform->dfHeightScale);

return true;