Program: 175223
Project: 175223
Buggy source:
/** 
 * In Matlab forwardgeolocation
 */
@Override public double[] geoFromPixel(final double p,final double l) throws GeoLocationException {
  try {
    double[] results=new double[2];
    double lon=0;
    double lat=0;
    double t0=0;
    int idxStartT0=0;
    if (meta.getType().equalsIgnoreCase("String_Node_Str") && meta.getProductType().equalsIgnoreCase("String_Node_Str") && (meta.getMode().equalsIgnoreCase("String_Node_Str") || meta.getMode().equalsIgnoreCase("String_Node_Str"))) {
      double timeRef=meta.getOrbitStatePosVelox().get(0).timeStampInitSeconds - orbitInterpolation.getSecondsDiffFromRefTime()[0];
    }
 else {
      double t01=(orbitInterpolation.getZeroDopplerTimeFirstRef()) * (meta.getNlines() - 1 - l);
      double t02=orbitInterpolation.getZeroDopplerTimeLastRef() * l;
      t0=(t01 + t02) / (meta.getNlines() - 1);
      for (idxStartT0=0; idxStartT0 < orbitInterpolation.getTimeStampInterp().size(); idxStartT0++) {
        if (orbitInterpolation.getTimeStampInterp().get(idxStartT0) >= t0) {
          break;
        }
      }
      if (idxStartT0 == statepVecInterp.size())       idxStartT0--;
    }
    final double[] pT0=statepVecInterp.get(idxStartT0);
    final double[] vT0=statevVecInterp.get(idxStartT0);
    double distance=0;
    if (meta.isPixelTimeOrderingAscending()) {
      distance=p * meta.getSamplePixelSpacing() - meta.getGroundRangeOrigin();
    }
 else {
      distance=(meta.getNumberOfSamplesPerLine() - 1 - p) * meta.getSamplePixelSpacing() - meta.getGroundRangeOrigin();
    }
    if (coordConv != null) {
      double zeroDopplerTime=t0;
      double timeRef=meta.getOrbitStatePosVelox().get(0).timeStampInitSeconds - orbitInterpolation.getSecondsDiffFromRefTime()[0];
      double[] groundToSlantRangePolyTimesSeconds=new double[coordConv.length];
      int index=0;
      for (      S1Metadata.CoordinateConversion cc : coordConv) {
        groundToSlantRangePolyTimesSeconds[index]=cc.groundToSlantRangePolyTimesSeconds - timeRef;
        index++;
      }
      int idx=0;
      if (zeroDopplerTime < groundToSlantRangePolyTimesSeconds[0]) {
        idx=1;
      }
 else       if (zeroDopplerTime > groundToSlantRangePolyTimesSeconds[groundToSlantRangePolyTimesSeconds.length - 1]) {
        idx=groundToSlantRangePolyTimesSeconds.length - 2;
      }
 else {
        for (idx=0; idx < groundToSlantRangePolyTimesSeconds.length; idx++) {
          if (groundToSlantRangePolyTimesSeconds[idx] < zeroDopplerTime)           break;
        }
      }
      double factor1=(groundToSlantRangePolyTimesSeconds[idx + 1] - zeroDopplerTime) / (groundToSlantRangePolyTimesSeconds[idx + 1] - groundToSlantRangePolyTimesSeconds[idx]);
      double factor2=(zeroDopplerTime - groundToSlantRangePolyTimesSeconds[idx]) / (groundToSlantRangePolyTimesSeconds[idx + 1] - groundToSlantRangePolyTimesSeconds[idx]);
      double[] groundToSlantRangeCoefficientsInterp=ArrayUtils.clone(coordConv[idx].groundToSlantRangeCoefficients);
      double[] groundToSlantRangeCoefficientsInterp2=coordConv[idx + 1].groundToSlantRangeCoefficients;
      for (int idCoeff=0; idCoeff < groundToSlantRangeCoefficientsInterp.length; idCoeff++) {
        groundToSlantRangeCoefficientsInterp[idCoeff]=factor1 * groundToSlantRangeCoefficientsInterp[idCoeff] + factor2 * groundToSlantRangeCoefficientsInterp2[idCoeff];
      }
      double[] slantToGroundRangeCoefficientsInterp=ArrayUtils.clone(coordConv[idx].slantToGroundRangeCoefficients);
      double[] slantToGroundRangeCoefficientsInterp2=coordConv[idx + 1].slantToGroundRangeCoefficients;
      for (int idCoeff=0; idCoeff < slantToGroundRangeCoefficientsInterp.length; idCoeff++) {
        slantToGroundRangeCoefficientsInterp[idCoeff]=factor1 * slantToGroundRangeCoefficientsInterp[idCoeff] + factor2 * slantToGroundRangeCoefficientsInterp2[idCoeff];
      }
      int nCoeffs=groundToSlantRangeCoefficientsInterp.length;
      int[] vExps=new int[nCoeffs];
      for (int i=0; i < nCoeffs; i++) {
        vExps[i]=i;
      }
      double tmpD=distance - meta.getGroundRangeOrigin();
      double[] pows=MathUtil.powValue2Coeffs(tmpD,vExps);
      double sRdist=MathUtil.vectorProd1XN(groundToSlantRangeCoefficientsInterp,pows);
      logger.debug("String_Node_Str" + sRdist);
      double normPt0=MathUtil.norm(pT0);
      double vRadial=MathUtil.vectorProd1XN(vT0,pT0) / normPt0;
      double vTangential=FastMath.sqrt(FastMath.pow(MathUtil.norm(vT0),2) - FastMath.pow(vRadial,2));
      double tanPsi=vRadial / vTangential;
      double[] zsUnit=MathUtil.divVectByVal(pT0,-normPt0);
      double[] vTmp=MathUtil.crossProd3x3(zsUnit,vT0);
      double[] ysUnit=MathUtil.divVectByVal(vTmp,MathUtil.norm(vTmp));
      double[] xsUnit=MathUtil.crossProd3x3(ysUnit,zsUnit);
      double pH=0;
      double rEarth=normPt0 / FastMath.sqrt((pT0[0] * pT0[0] + pT0[1] * pT0[1]) / GeoUtils.semiMajorAxis2 + (pT0[2] * pT0[2]) / GeoUtils.semiMinorAxis2);
      for (int iidx=0; iidx < 2; iidx++) {
        rEarth=rEarth + pH;
        double rEarthOld=rEarth;
        double rEarthChange=100000;
        double[] q=new double[3];
        for (; rEarthChange > 0.1; ) {
          double Rz=(normPt0 * normPt0 + sRdist * sRdist - rEarth * rEarth) / (2 * normPt0);
          double Rx=Rz * tanPsi;
          double Ry=FastMath.sqrt(sRdist * sRdist - Rz * Rz - Rx * Rx);
          if (!meta.getAntennaPointing().equalsIgnoreCase("String_Node_Str")) {
            Ry=-Ry;
          }
          double[][] sRvect=new double[][]{{Rx},{Ry},{Rz - normPt0}};
          double[][] qbig=new double[][]{xsUnit,ysUnit,zsUnit};
          qbig=MathUtil.transpose(qbig);
          double[][] qmat=MathUtil.multiplyMatrix(qbig,sRvect);
          q[0]=qmat[0][0];
          q[1]=qmat[1][0];
          q[2]=qmat[2][0];
          rEarth=MathUtil.norm(q) / FastMath.sqrt((q[0] * q[0] + q[1] * q[1]) / GeoUtils.semiMajorAxis2 + (q[2] * q[2] / GeoUtils.semiMinorAxis2)) + pH;
          rEarthChange=FastMath.abs(rEarth - rEarthOld);
          rEarthOld=rEarth;
        }
        lon=FastMath.atan2(q[1],q[0]) * 180 / FastMath.PI;
        double lattmp=FastMath.asin(q[2] / FastMath.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));
        lat=FastMath.atan(FastMath.tan(lattmp) * FastMath.pow((GeoUtils.semiMajorAxis + pH),2) / FastMath.pow((GeoUtils.semiMinorAxis + pH),2)) * 180 / FastMath.PI;
        pH=GeoUtils.getGeoidH(lon,lat);
      }
    }
    logger.debug("String_Node_Str" + lat + "String_Node_Str"+ lon);
    results[0]=lon;
    results[1]=lat;
    return results;
  }
 catch (  MathException me) {
    throw new GeoLocationException(GeoLocationException.MSG_CONV_FROM_PIXEL_TO_GEO + "String_Node_Str" + me.getMessage());
  }
}

Fixed version (ground truth):
/** 
 * @see In Matlab forwardgeolocation
 */
@Override public double[] geoFromPixel(final double p,final double l) throws GeoLocationException {
  try {
    double[] results=new double[2];
    double lon=0;
    double lat=0;
    double t0=0;
    int idxStartT0=0;
    if (meta.getType().equalsIgnoreCase("String_Node_Str") && meta.getProductType().equalsIgnoreCase("String_Node_Str") && (meta.getMode().equalsIgnoreCase("String_Node_Str") || meta.getMode().equalsIgnoreCase("String_Node_Str"))) {
      double timeRef=meta.getOrbitStatePosVelox().get(0).timeStampInitSeconds - orbitInterpolation.getSecondsDiffFromRefTime()[0];
    }
 else {
      double t01=(orbitInterpolation.getZeroDopplerTimeFirstRef()) * (meta.getNlines() - 1 - l);
      double t02=orbitInterpolation.getZeroDopplerTimeLastRef() * l;
      t0=(t01 + t02) / (meta.getNlines() - 1);
      for (idxStartT0=0; idxStartT0 < orbitInterpolation.getTimeStampInterp().size(); idxStartT0++) {
        if (orbitInterpolation.getTimeStampInterp().get(idxStartT0) >= t0) {
          break;
        }
      }
      if (idxStartT0 == statepVecInterp.size())       idxStartT0--;
    }
    final double[] pT0=statepVecInterp.get(idxStartT0);
    final double[] vT0=statevVecInterp.get(idxStartT0);
    double distance=0;
    if (meta.isPixelTimeOrderingAscending()) {
      distance=p * meta.getSamplePixelSpacing() - meta.getGroundRangeOrigin();
    }
 else {
      distance=(meta.getNumberOfSamplesPerLine() - 1 - p) * meta.getSamplePixelSpacing() - meta.getGroundRangeOrigin();
    }
    if (coordConv != null) {
      double zeroDopplerTime=t0;
      double timeRef=meta.getOrbitStatePosVelox().get(0).timeStampInitSeconds - orbitInterpolation.getSecondsDiffFromRefTime()[0];
      double[] groundToSlantRangePolyTimesSeconds=new double[coordConv.length];
      int index=0;
      for (      S1Metadata.CoordinateConversion cc : coordConv) {
        groundToSlantRangePolyTimesSeconds[index]=cc.groundToSlantRangePolyTimesSeconds - timeRef;
        index++;
      }
      int idx=0;
      if (zeroDopplerTime < groundToSlantRangePolyTimesSeconds[0]) {
        idx=1;
      }
 else       if (zeroDopplerTime > groundToSlantRangePolyTimesSeconds[groundToSlantRangePolyTimesSeconds.length - 1]) {
        idx=groundToSlantRangePolyTimesSeconds.length - 2;
      }
 else {
        for (idx=0; idx < groundToSlantRangePolyTimesSeconds.length; idx++) {
          if (groundToSlantRangePolyTimesSeconds[idx] < zeroDopplerTime)           break;
        }
      }
      double factor1=(groundToSlantRangePolyTimesSeconds[idx + 1] - zeroDopplerTime) / (groundToSlantRangePolyTimesSeconds[idx + 1] - groundToSlantRangePolyTimesSeconds[idx]);
      double factor2=(zeroDopplerTime - groundToSlantRangePolyTimesSeconds[idx]) / (groundToSlantRangePolyTimesSeconds[idx + 1] - groundToSlantRangePolyTimesSeconds[idx]);
      double[] groundToSlantRangeCoefficientsInterp=ArrayUtils.clone(coordConv[idx].groundToSlantRangeCoefficients);
      double[] groundToSlantRangeCoefficientsInterp2=coordConv[idx + 1].groundToSlantRangeCoefficients;
      for (int idCoeff=0; idCoeff < groundToSlantRangeCoefficientsInterp.length; idCoeff++) {
        groundToSlantRangeCoefficientsInterp[idCoeff]=factor1 * groundToSlantRangeCoefficientsInterp[idCoeff] + factor2 * groundToSlantRangeCoefficientsInterp2[idCoeff];
      }
      double[] slantToGroundRangeCoefficientsInterp=ArrayUtils.clone(coordConv[idx].slantToGroundRangeCoefficients);
      double[] slantToGroundRangeCoefficientsInterp2=coordConv[idx + 1].slantToGroundRangeCoefficients;
      for (int idCoeff=0; idCoeff < slantToGroundRangeCoefficientsInterp.length; idCoeff++) {
        slantToGroundRangeCoefficientsInterp[idCoeff]=factor1 * slantToGroundRangeCoefficientsInterp[idCoeff] + factor2 * slantToGroundRangeCoefficientsInterp2[idCoeff];
      }
      int nCoeffs=groundToSlantRangeCoefficientsInterp.length;
      int[] vExps=new int[nCoeffs];
      for (int i=0; i < nCoeffs; i++) {
        vExps[i]=i;
      }
      double tmpD=distance - meta.getGroundRangeOrigin();
      double[] pows=MathUtil.powValue2Coeffs(tmpD,vExps);
      double sRdist=MathUtil.vectorProd1XN(groundToSlantRangeCoefficientsInterp,pows);
      logger.debug("String_Node_Str" + sRdist);
      double normPt0=MathUtil.norm(pT0);
      double vRadial=MathUtil.vectorProd1XN(vT0,pT0) / normPt0;
      double vTangential=FastMath.sqrt(FastMath.pow(MathUtil.norm(vT0),2) - FastMath.pow(vRadial,2));
      double tanPsi=vRadial / vTangential;
      double[] zsUnit=MathUtil.divVectByVal(pT0,-normPt0);
      double[] vTmp=MathUtil.crossProd3x3(zsUnit,vT0);
      double[] ysUnit=MathUtil.divVectByVal(vTmp,MathUtil.norm(vTmp));
      double[] xsUnit=MathUtil.crossProd3x3(ysUnit,zsUnit);
      double pH=0;
      double rEarth=normPt0 / FastMath.sqrt((pT0[0] * pT0[0] + pT0[1] * pT0[1]) / GeoUtils.semiMajorAxis2 + (pT0[2] * pT0[2]) / GeoUtils.semiMinorAxis2);
      for (int iidx=0; iidx < 2; iidx++) {
        rEarth=rEarth + pH;
        double rEarthOld=rEarth;
        double rEarthChange=100000;
        double[] q=new double[3];
        for (; rEarthChange > 0.1; ) {
          double Rz=(normPt0 * normPt0 + sRdist * sRdist - rEarth * rEarth) / (2 * normPt0);
          double Rx=Rz * tanPsi;
          double Ry=FastMath.sqrt(sRdist * sRdist - Rz * Rz - Rx * Rx);
          if (!meta.getAntennaPointing().equalsIgnoreCase("String_Node_Str")) {
            Ry=-Ry;
          }
          double[][] sRvect=new double[][]{{Rx},{Ry},{Rz - normPt0}};
          double[][] qbig=new double[][]{xsUnit,ysUnit,zsUnit};
          qbig=MathUtil.transpose(qbig);
          double[][] qmat=MathUtil.multiplyMatrix(qbig,sRvect);
          q[0]=qmat[0][0];
          q[1]=qmat[1][0];
          q[2]=qmat[2][0];
          rEarth=MathUtil.norm(q) / FastMath.sqrt((q[0] * q[0] + q[1] * q[1]) / GeoUtils.semiMajorAxis2 + (q[2] * q[2] / GeoUtils.semiMinorAxis2)) + pH;
          rEarthChange=FastMath.abs(rEarth - rEarthOld);
          rEarthOld=rEarth;
        }
        lon=FastMath.atan2(q[1],q[0]) * 180 / FastMath.PI;
        double lattmp=FastMath.asin(q[2] / FastMath.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));
        lat=FastMath.atan(FastMath.tan(lattmp) * FastMath.pow((GeoUtils.semiMajorAxis + pH),2) / FastMath.pow((GeoUtils.semiMinorAxis + pH),2)) * 180 / FastMath.PI;
        pH=GeoUtils.getGeoidH(lon,lat);
      }
    }
    logger.debug("String_Node_Str" + lat + "String_Node_Str"+ lon);
    results[0]=lon;
    results[1]=lat;
    return results;
  }
 catch (  MathException me) {
    throw new GeoLocationException(GeoLocationException.MSG_CONV_FROM_PIXEL_TO_GEO + "String_Node_Str" + me.getMessage());
  }
}