RdCost.cpp

/* The copyright in this software is being made available under the BSD
 * License, included below. This software may be subject to other third party
 * and contributor rights, including patent rights, and no such rights are
 * granted under this license.
 *
 * Copyright (c) 2010-2025, ITU/ISO/IEC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *  * Neither the name of the ITU/ISO/IEC nor the names of its contributors may
 *    be used to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 */

/** \file     RdCost.cpp
    \brief    RD cost computation class
*/

#define DONT_UNDEF_SIZE_AWARE_PER_EL_OP

#include "RdCost.h"

#include "Rom.h"
#include "UnitPartitioner.h"

#include <limits>

//! \ingroup CommonLib
//! \{

EnumArray<DistFunc, DFunc> RdCost::m_distortionFunc;

RdCost::RdCost()
{
  init();
}

RdCost::~RdCost()
{
}

#if WCG_EXT
EnumArray<DistFuncWtd, DFuncWtd> RdCost::m_distortionFuncWtd;
double RdCost::calcRdCost( uint64_t fracBits, Distortion distortion, bool useUnadjustedLambda )
#else
double RdCost::calcRdCost( uint64_t fracBits, Distortion distortion )
#endif
{
  if (m_costMode == COST_LOSSLESS_CODING && 0 != distortion && m_isLosslessRDCost)
  {
    return MAX_DOUBLE;
  }
#if WCG_EXT
  return (useUnadjustedLambda ? m_distScaleUnadjusted : m_distScale) * double(distortion) + double(fracBits);
#else
  return m_distScale * double(distortion) + double(fracBits);
#endif
}

void RdCost::setLambda( double dLambda, const BitDepths &bitDepths )
{
  m_dLambda             = dLambda;
  m_distScale           = double(1 << SCALE_BITS) / m_dLambda;
  m_dLambdaMotionSAD    = sqrt(m_dLambda);
}

void RdCost::lambdaAdjustColorTrans(bool forward, ComponentID componentID, bool applyChromaScale, int* resScaleInv)
{
  if (m_resetStore)
  {
    for (uint8_t component = 0; component < MAX_NUM_COMPONENT; component++)
    {
      ComponentID compID = (ComponentID)component;
      int       delta_QP = DELTA_QP_ACT[compID];
      double lamdbaAdjustRate = pow(2.0, delta_QP / 3.0);

      m_lambdaStore[0][component] = m_dLambda;
      m_distScaleStore[0][component] = m_distScale;

      m_lambdaStore[1][component] = m_dLambda * lamdbaAdjustRate;
      m_distScaleStore[1][component] = double(1 << SCALE_BITS) / m_lambdaStore[1][component];
    }
    m_resetStore = false;
  }

  if (forward)
  {
    CHECK(m_pairCheck == 1, "lambda has been already adjusted");
    m_pairCheck = 1;
  }
  else
  {
    CHECK(m_pairCheck == 0, "lambda has not been adjusted");
    m_pairCheck = 0;
  }

  m_dLambda = m_lambdaStore[m_pairCheck][componentID];
  m_distScale = m_distScaleStore[m_pairCheck][componentID];
  if (applyChromaScale)
  {
    CHECK(m_pairCheck == 0 || componentID == COMPONENT_Y, "wrong lambda adjustment for CS");
    double cResScale = (double)(1 << CSCALE_FP_PREC) / (double)(*resScaleInv);
    m_dLambda = m_dLambda / (cResScale*cResScale);
    m_distScale      = double(1 << SCALE_BITS) / m_dLambda;
  }
  if (m_pairCheck == 0)
  {
    CHECK(m_distScale != m_distScaleUnadjusted, "lambda should be adjusted to the original value");
  }
}

// Initialize Function Pointer by [distFunc]
void RdCost::init()
{
  m_distortionFunc[DFunc::SSE]    = RdCost::xGetSSE;
  m_distortionFunc[DFunc::SSE2]   = RdCost::xGetSSE;
  m_distortionFunc[DFunc::SSE4]   = RdCost::xGetSSE4;
  m_distortionFunc[DFunc::SSE8]   = RdCost::xGetSSE8;
  m_distortionFunc[DFunc::SSE16]  = RdCost::xGetSSE16;
  m_distortionFunc[DFunc::SSE32]  = RdCost::xGetSSE32;
  m_distortionFunc[DFunc::SSE64]  = RdCost::xGetSSE64;
  m_distortionFunc[DFunc::SSE16N] = RdCost::xGetSSE16N;

  m_distortionFunc[DFunc::SAD]    = RdCost::xGetSAD;
  m_distortionFunc[DFunc::SAD2]   = RdCost::xGetSAD;
  m_distortionFunc[DFunc::SAD4]   = RdCost::xGetSAD4;
  m_distortionFunc[DFunc::SAD8]   = RdCost::xGetSAD8;
  m_distortionFunc[DFunc::SAD16]  = RdCost::xGetSAD16;
  m_distortionFunc[DFunc::SAD32]  = RdCost::xGetSAD32;
  m_distortionFunc[DFunc::SAD64]  = RdCost::xGetSAD64;
  m_distortionFunc[DFunc::SAD16N] = RdCost::xGetSAD16N;

  m_distortionFunc[DFunc::SAD12] = RdCost::xGetSAD12;
  m_distortionFunc[DFunc::SAD24] = RdCost::xGetSAD24;
  m_distortionFunc[DFunc::SAD48] = RdCost::xGetSAD48;

  m_distortionFunc[DFunc::HAD]    = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD2]   = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD4]   = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD8]   = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD16]  = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD32]  = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD64]  = RdCost::xGetHADs;
  m_distortionFunc[DFunc::HAD16N] = RdCost::xGetHADs;

  m_distortionFunc[DFunc::MRSAD]    = RdCost::xGetMRSAD;
  m_distortionFunc[DFunc::MRSAD2]   = RdCost::xGetMRSAD;
  m_distortionFunc[DFunc::MRSAD4]   = RdCost::xGetMRSAD4;
  m_distortionFunc[DFunc::MRSAD8]   = RdCost::xGetMRSAD8;
  m_distortionFunc[DFunc::MRSAD16]  = RdCost::xGetMRSAD16;
  m_distortionFunc[DFunc::MRSAD32]  = RdCost::xGetMRSAD32;
  m_distortionFunc[DFunc::MRSAD64]  = RdCost::xGetMRSAD64;
  m_distortionFunc[DFunc::MRSAD16N] = RdCost::xGetMRSAD16N;

  m_distortionFunc[DFunc::MRSAD12] = RdCost::xGetMRSAD12;
  m_distortionFunc[DFunc::MRSAD24] = RdCost::xGetMRSAD24;
  m_distortionFunc[DFunc::MRSAD48] = RdCost::xGetMRSAD48;

  m_distortionFunc[DFunc::MRHAD]    = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD2]   = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD4]   = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD8]   = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD16]  = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD32]  = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD64]  = RdCost::xGetMRHADs;
  m_distortionFunc[DFunc::MRHAD16N] = RdCost::xGetMRHADs;

  m_distortionFunc[DFunc::SAD_FULL_NBIT]    = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT2]   = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT4]   = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT8]   = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT16]  = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT32]  = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT64]  = RdCost::xGetSAD_full;
  m_distortionFunc[DFunc::SAD_FULL_NBIT16N] = RdCost::xGetSAD_full;

#if WCG_EXT
  m_distortionFuncWtd[DFuncWtd::SSE_WTD]    = &RdCost::xGetSSE_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE2_WTD]   = &RdCost::xGetSSE2_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE4_WTD]   = &RdCost::xGetSSE4_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE8_WTD]   = &RdCost::xGetSSE8_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE16_WTD]  = &RdCost::xGetSSE16_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE32_WTD]  = &RdCost::xGetSSE32_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE64_WTD]  = &RdCost::xGetSSE64_WTD;
  m_distortionFuncWtd[DFuncWtd::SSE16N_WTD] = &RdCost::xGetSSE16N_WTD;
#endif

  m_distortionFunc[DFunc::SAD_INTERMEDIATE_BITDEPTH] = RdCost::xGetSAD;

  m_distortionFunc[DFunc::SAD_WITH_MASK] = RdCost::xGetSADwMask;

#if ENABLE_SIMD_OPT_DIST
#ifdef TARGET_SIMD_X86
  initRdCostX86();
#endif
#endif

  m_costMode                   = COST_STANDARD_LOSSY;

  m_motionLambda               = 0;
  m_iCostScale                 = 0;
  m_resetStore = true;
  m_pairCheck    = 0;
}

void RdCost::setDistParam(DistParam &rcDP, const CPelBuf &org, const Pel *piRefY, ptrdiff_t iRefStride, int bitDepth,
                          ComponentID compID, int subShiftMode, int step, bool useHadamard)
{
  rcDP.bitDepth   = bitDepth;
  rcDP.compID     = compID;

  // set Original & Curr Pointer / Stride
  rcDP.org        = org;

  rcDP.cur.buf    = piRefY;
  rcDP.cur.stride = iRefStride;

  // set Block Width / Height
  rcDP.cur.width    = org.width;
  rcDP.cur.height   = org.height;
  rcDP.step         = step;
  rcDP.maximumDistortionForEarlyExit = std::numeric_limits<Distortion>::max();

  if( !useHadamard )
  {
    const DFunc baseIdx = rcDP.useMR ? DFunc::MRSAD : DFunc::SAD;

    rcDP.distFunc = m_distortionFunc[baseIdx + sizeOffset<true>(org.width)];
  }
  else
  {
    const DFunc baseIdx = rcDP.useMR ? DFunc::MRHAD : DFunc::HAD;

    rcDP.distFunc = m_distortionFunc[baseIdx + sizeOffset<false>(org.width)];
  }

  // initialize
  rcDP.subShift  = 0;

  if( subShiftMode == 1 )
  {
    if( rcDP.org.height > 32 && ( rcDP.org.height & 15 ) == 0 )
    {
      rcDP.subShift = 4;
    }
    else if( rcDP.org.height > 16 && ( rcDP.org.height & 7 ) == 0 )
    {
      rcDP.subShift = 3;
    }
    else if( rcDP.org.height > 8 && ( rcDP.org.height & 3 ) == 0 )
    {
      rcDP.subShift = 2;
    }
    else if( ( rcDP.org.height & 1 ) == 0 )
    {
      rcDP.subShift = 1;
    }
  }
  else if( subShiftMode == 2 )
  {
    if( rcDP.org.height > 8 && rcDP.org.width <= 64 )
    {
      rcDP.subShift = 1;
    }
  }
  else if( subShiftMode == 3 )
  {
    if (rcDP.org.height > 8 )
    {
      rcDP.subShift = 1;
    }
  }
}

void RdCost::setDistParam( DistParam &rcDP, const CPelBuf &org, const CPelBuf &cur, int bitDepth, ComponentID compID, bool useHadamard )
{
  rcDP.org          = org;
  rcDP.cur          = cur;
  rcDP.step         = 1;
  rcDP.subShift     = 0;
  rcDP.bitDepth     = bitDepth;
  rcDP.compID       = compID;

  if( !useHadamard )
  {
    const DFunc baseIdx = rcDP.useMR ? DFunc::MRSAD : DFunc::SAD;

    rcDP.distFunc = m_distortionFunc[baseIdx + sizeOffset<true>(org.width)];
  }
  else
  {
    const DFunc baseIdx = rcDP.useMR ? DFunc::MRHAD : DFunc::HAD;

    rcDP.distFunc = m_distortionFunc[baseIdx + sizeOffset<false>(org.width)];
  }

  rcDP.maximumDistortionForEarlyExit = std::numeric_limits<Distortion>::max();
}

void RdCost::setDistParam(DistParam &rcDP, const Pel *pOrg, const Pel *piRefY, ptrdiff_t iOrgStride,
                          ptrdiff_t iRefStride, int bitDepth, ComponentID compID, int width, int height,
                          int subShiftMode, int step, bool useHadamard, bool bioApplied)
{
  rcDP.bitDepth   = bitDepth;
  rcDP.compID     = compID;

  rcDP.org.buf    = pOrg;
  rcDP.org.stride = iOrgStride;
  rcDP.org.width  = width;
  rcDP.org.height = height;

  rcDP.cur.buf    = piRefY;
  rcDP.cur.stride = iRefStride;
  rcDP.cur.width  = width;
  rcDP.cur.height = height;
  rcDP.subShift = subShiftMode;
  rcDP.step       = step;
  rcDP.maximumDistortionForEarlyExit = std::numeric_limits<Distortion>::max();
  CHECK(useHadamard || rcDP.useMR, "only used in xDmvrCost with these default parameters (so far...)");
  if ( bioApplied )
  {
    rcDP.distFunc = m_distortionFunc[DFunc::SAD_INTERMEDIATE_BITDEPTH];
    return;
  }

  rcDP.distFunc = m_distortionFunc[DFunc::SAD + sizeOffset<true>(width)];
}

#if WCG_EXT
Distortion RdCost::getDistPart(const CPelBuf &org, const CPelBuf &cur, int bitDepth, const ComponentID compID,
                               DFuncWtd distFuncWtd, const CPelBuf &orgLuma) const
{
  DistParam cDtParam;

  cDtParam.org        = org;
  cDtParam.cur        = cur;
  cDtParam.step       = 1;
  cDtParam.bitDepth   = bitDepth;
  cDtParam.compID     = compID;

  if (isChroma(compID))
  {
    cDtParam.orgLuma  = orgLuma;
  }
  else
  {
    cDtParam.orgLuma  = org;
  }

  Distortion dist;
  if (isChroma(compID) && (m_signalType == RESHAPE_SIGNAL_SDR || m_signalType == RESHAPE_SIGNAL_HLG))
  {
       cDtParam.distFunc = m_distortionFunc[DFunc::SSE + sizeOffset<false>(org.width)];
       int64_t weight = m_chromaWeight;
       dist = (weight * cDtParam.distFunc(cDtParam ) + (1 << MSE_WEIGHT_FRAC_BITS >> 1)) >> (MSE_WEIGHT_FRAC_BITS);
  }
  else
  {
    cDtParam.cShiftX = getComponentScaleX(compID,  m_cf);
    cDtParam.cShiftY = getComponentScaleY(compID,  m_cf);
    cDtParam.distFuncWtd = m_distortionFuncWtd[distFuncWtd + sizeOffset<false>(org.width)];
    dist = cDtParam.distFuncWtd(this, cDtParam);
  }
  if (isChroma(compID))
  {
    dist = (Distortion)(m_distortionWeight[MAP_CHROMA(compID)] * dist);
  }
  return dist;
}
#endif
Distortion RdCost::getDistPart(const CPelBuf &org, const CPelBuf &cur, int bitDepth, const ComponentID compID,
                                   DFunc distFunc) const
{
  DistParam cDtParam;

  cDtParam.org        = org;
  cDtParam.cur        = cur;
  cDtParam.step       = 1;
  cDtParam.bitDepth   = bitDepth;
  cDtParam.compID     = compID;

  cDtParam.distFunc = m_distortionFunc[distFunc + sizeOffset<false>(org.width)];

  if (isChroma(compID))
  {
    return ((Distortion) (m_distortionWeight[ MAP_CHROMA(compID) ] * cDtParam.distFunc( cDtParam )));
  }
  else
  {
    return cDtParam.distFunc( cDtParam );
  }
}

// ====================================================================================================================
// Distortion functions
// ====================================================================================================================

// --------------------------------------------------------------------------------------------------------------------
// SAD
// --------------------------------------------------------------------------------------------------------------------

Distortion RdCost::xGetSAD_full( const DistParam& rcDtParam )
{
  CHECK( rcDtParam.applyWeight, "Cannot apply weight when using full-bit SAD!" );
  const Pel* piOrg = rcDtParam.org.buf;
  const Pel* piCur = rcDtParam.cur.buf;
  const int  height    = rcDtParam.org.height;
  const int  width     = rcDtParam.org.width;
  const int  subShift  = rcDtParam.subShift;
  const int  subStep   = (1 << subShift);
  const ptrdiff_t strideCur = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

#define SAD_OP(ADDR) sum += abs(piOrg[ADDR] - piCur[ADDR]);
#define SAD_INC                                                                                                        \
  piOrg += strideOrg;                                                                                                  \
  piCur += strideCur;

  SIZE_AWARE_PER_EL_OP( SAD_OP, SAD_INC )

#undef SAD_OP
#undef SAD_INC

  sum <<= subShift;
  return sum;
}

Distortion RdCost::xGetSAD( const DistParam& rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg           = rcDtParam.org.buf;
  const Pel* piCur           = rcDtParam.cur.buf;
  const int      cols            = rcDtParam.org.width;
  int            rows            = rcDtParam.org.height;
  const int      subShift        = rcDtParam.subShift;
  const int      subStep         = (1 << subShift);
  const ptrdiff_t strideCur       = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg       = rcDtParam.org.stride * subStep;
  const uint32_t distortionShift = DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth);

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    for (int n = 0; n < cols; n++)
    {
      sum += abs(piOrg[n] - piCur[n]);
    }
    if (rcDtParam.maximumDistortionForEarlyExit < (sum >> distortionShift))
    {
      return (sum >> distortionShift);
    }
    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> distortionShift);
}

Distortion RdCost::xGetSAD4( const DistParam& rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg   = rcDtParam.org.buf;
  const Pel* piCur   = rcDtParam.cur.buf;
  int        rows      = rcDtParam.org.height;
  const int  subShift  = rcDtParam.subShift;
  const int  subStep   = (1 << subShift);
  const ptrdiff_t strideCur = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD8( const DistParam& rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  const int  subShift   = rcDtParam.subShift;
  const int  subStep    = (1 << subShift);
  const ptrdiff_t strideCur  = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD16( const DistParam& rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  const int  subShift   = rcDtParam.subShift;
  const int  subStep    = (1 << subShift);
  const ptrdiff_t strideCur  = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);
    sum += abs(piOrg[8] - piCur[8]);
    sum += abs(piOrg[9] - piCur[9]);
    sum += abs(piOrg[10] - piCur[10]);
    sum += abs(piOrg[11] - piCur[11]);
    sum += abs(piOrg[12] - piCur[12]);
    sum += abs(piOrg[13] - piCur[13]);
    sum += abs(piOrg[14] - piCur[14]);
    sum += abs(piOrg[15] - piCur[15]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD12( const DistParam& rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  const int  subShift   = rcDtParam.subShift;
  const int  subStep    = (1 << subShift);
  const ptrdiff_t strideCur  = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);
    sum += abs(piOrg[8] - piCur[8]);
    sum += abs(piOrg[9] - piCur[9]);
    sum += abs(piOrg[10] - piCur[10]);
    sum += abs(piOrg[11] - piCur[11]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD16N( const DistParam &rcDtParam )
{
  const Pel* piOrg  = rcDtParam.org.buf;
  const Pel* piCur  = rcDtParam.cur.buf;
  int        rows      = rcDtParam.org.height;
  const int  cols      = rcDtParam.org.width;
  const int  subShift  = rcDtParam.subShift;
  const int  subStep   = (1 << subShift);
  const ptrdiff_t strideCur = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    for (int n = 0; n < cols; n += 16)
    {
      sum += abs(piOrg[n + 0] - piCur[n + 0]);
      sum += abs(piOrg[n + 1] - piCur[n + 1]);
      sum += abs(piOrg[n + 2] - piCur[n + 2]);
      sum += abs(piOrg[n + 3] - piCur[n + 3]);
      sum += abs(piOrg[n + 4] - piCur[n + 4]);
      sum += abs(piOrg[n + 5] - piCur[n + 5]);
      sum += abs(piOrg[n + 6] - piCur[n + 6]);
      sum += abs(piOrg[n + 7] - piCur[n + 7]);
      sum += abs(piOrg[n + 8] - piCur[n + 8]);
      sum += abs(piOrg[n + 9] - piCur[n + 9]);
      sum += abs(piOrg[n + 10] - piCur[n + 10]);
      sum += abs(piOrg[n + 11] - piCur[n + 11]);
      sum += abs(piOrg[n + 12] - piCur[n + 12]);
      sum += abs(piOrg[n + 13] - piCur[n + 13]);
      sum += abs(piOrg[n + 14] - piCur[n + 14]);
      sum += abs(piOrg[n + 15] - piCur[n + 15]);
    }
    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD32( const DistParam &rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  int        subShift   = rcDtParam.subShift;
  int        subStep    = (1 << subShift);
  ptrdiff_t  strideCur  = rcDtParam.cur.stride * subStep;
  ptrdiff_t  strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);
    sum += abs(piOrg[8] - piCur[8]);
    sum += abs(piOrg[9] - piCur[9]);
    sum += abs(piOrg[10] - piCur[10]);
    sum += abs(piOrg[11] - piCur[11]);
    sum += abs(piOrg[12] - piCur[12]);
    sum += abs(piOrg[13] - piCur[13]);
    sum += abs(piOrg[14] - piCur[14]);
    sum += abs(piOrg[15] - piCur[15]);
    sum += abs(piOrg[16] - piCur[16]);
    sum += abs(piOrg[17] - piCur[17]);
    sum += abs(piOrg[18] - piCur[18]);
    sum += abs(piOrg[19] - piCur[19]);
    sum += abs(piOrg[20] - piCur[20]);
    sum += abs(piOrg[21] - piCur[21]);
    sum += abs(piOrg[22] - piCur[22]);
    sum += abs(piOrg[23] - piCur[23]);
    sum += abs(piOrg[24] - piCur[24]);
    sum += abs(piOrg[25] - piCur[25]);
    sum += abs(piOrg[26] - piCur[26]);
    sum += abs(piOrg[27] - piCur[27]);
    sum += abs(piOrg[28] - piCur[28]);
    sum += abs(piOrg[29] - piCur[29]);
    sum += abs(piOrg[30] - piCur[30]);
    sum += abs(piOrg[31] - piCur[31]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD24( const DistParam &rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  int        subShift   = rcDtParam.subShift;
  int        subStep    = (1 << subShift);
  ptrdiff_t  strideCur  = rcDtParam.cur.stride * subStep;
  ptrdiff_t  strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);
    sum += abs(piOrg[8] - piCur[8]);
    sum += abs(piOrg[9] - piCur[9]);
    sum += abs(piOrg[10] - piCur[10]);
    sum += abs(piOrg[11] - piCur[11]);
    sum += abs(piOrg[12] - piCur[12]);
    sum += abs(piOrg[13] - piCur[13]);
    sum += abs(piOrg[14] - piCur[14]);
    sum += abs(piOrg[15] - piCur[15]);
    sum += abs(piOrg[16] - piCur[16]);
    sum += abs(piOrg[17] - piCur[17]);
    sum += abs(piOrg[18] - piCur[18]);
    sum += abs(piOrg[19] - piCur[19]);
    sum += abs(piOrg[20] - piCur[20]);
    sum += abs(piOrg[21] - piCur[21]);
    sum += abs(piOrg[22] - piCur[22]);
    sum += abs(piOrg[23] - piCur[23]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD64( const DistParam &rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  int        subShift   = rcDtParam.subShift;
  int        subStep    = (1 << subShift);
  ptrdiff_t  strideCur  = rcDtParam.cur.stride * subStep;
  ptrdiff_t  strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);
    sum += abs(piOrg[8] - piCur[8]);
    sum += abs(piOrg[9] - piCur[9]);
    sum += abs(piOrg[10] - piCur[10]);
    sum += abs(piOrg[11] - piCur[11]);
    sum += abs(piOrg[12] - piCur[12]);
    sum += abs(piOrg[13] - piCur[13]);
    sum += abs(piOrg[14] - piCur[14]);
    sum += abs(piOrg[15] - piCur[15]);
    sum += abs(piOrg[16] - piCur[16]);
    sum += abs(piOrg[17] - piCur[17]);
    sum += abs(piOrg[18] - piCur[18]);
    sum += abs(piOrg[19] - piCur[19]);
    sum += abs(piOrg[20] - piCur[20]);
    sum += abs(piOrg[21] - piCur[21]);
    sum += abs(piOrg[22] - piCur[22]);
    sum += abs(piOrg[23] - piCur[23]);
    sum += abs(piOrg[24] - piCur[24]);
    sum += abs(piOrg[25] - piCur[25]);
    sum += abs(piOrg[26] - piCur[26]);
    sum += abs(piOrg[27] - piCur[27]);
    sum += abs(piOrg[28] - piCur[28]);
    sum += abs(piOrg[29] - piCur[29]);
    sum += abs(piOrg[30] - piCur[30]);
    sum += abs(piOrg[31] - piCur[31]);
    sum += abs(piOrg[32] - piCur[32]);
    sum += abs(piOrg[33] - piCur[33]);
    sum += abs(piOrg[34] - piCur[34]);
    sum += abs(piOrg[35] - piCur[35]);
    sum += abs(piOrg[36] - piCur[36]);
    sum += abs(piOrg[37] - piCur[37]);
    sum += abs(piOrg[38] - piCur[38]);
    sum += abs(piOrg[39] - piCur[39]);
    sum += abs(piOrg[40] - piCur[40]);
    sum += abs(piOrg[41] - piCur[41]);
    sum += abs(piOrg[42] - piCur[42]);
    sum += abs(piOrg[43] - piCur[43]);
    sum += abs(piOrg[44] - piCur[44]);
    sum += abs(piOrg[45] - piCur[45]);
    sum += abs(piOrg[46] - piCur[46]);
    sum += abs(piOrg[47] - piCur[47]);
    sum += abs(piOrg[48] - piCur[48]);
    sum += abs(piOrg[49] - piCur[49]);
    sum += abs(piOrg[50] - piCur[50]);
    sum += abs(piOrg[51] - piCur[51]);
    sum += abs(piOrg[52] - piCur[52]);
    sum += abs(piOrg[53] - piCur[53]);
    sum += abs(piOrg[54] - piCur[54]);
    sum += abs(piOrg[55] - piCur[55]);
    sum += abs(piOrg[56] - piCur[56]);
    sum += abs(piOrg[57] - piCur[57]);
    sum += abs(piOrg[58] - piCur[58]);
    sum += abs(piOrg[59] - piCur[59]);
    sum += abs(piOrg[60] - piCur[60]);
    sum += abs(piOrg[61] - piCur[61]);
    sum += abs(piOrg[62] - piCur[62]);
    sum += abs(piOrg[63] - piCur[63]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}

Distortion RdCost::xGetSAD48( const DistParam &rcDtParam )
{
  if ( rcDtParam.applyWeight )
  {
    return RdCostWeightPrediction::xGetSADw( rcDtParam );
  }

  const Pel* piOrg      = rcDtParam.org.buf;
  const Pel* piCur      = rcDtParam.cur.buf;
  int        rows       = rcDtParam.org.height;
  int        subShift   = rcDtParam.subShift;
  int        subStep    = (1 << subShift);
  ptrdiff_t  strideCur  = rcDtParam.cur.stride * subStep;
  ptrdiff_t  strideOrg  = rcDtParam.org.stride * subStep;

  Distortion sum = 0;

  for (; rows != 0; rows -= subStep)
  {
    sum += abs(piOrg[0] - piCur[0]);
    sum += abs(piOrg[1] - piCur[1]);
    sum += abs(piOrg[2] - piCur[2]);
    sum += abs(piOrg[3] - piCur[3]);
    sum += abs(piOrg[4] - piCur[4]);
    sum += abs(piOrg[5] - piCur[5]);
    sum += abs(piOrg[6] - piCur[6]);
    sum += abs(piOrg[7] - piCur[7]);
    sum += abs(piOrg[8] - piCur[8]);
    sum += abs(piOrg[9] - piCur[9]);
    sum += abs(piOrg[10] - piCur[10]);
    sum += abs(piOrg[11] - piCur[11]);
    sum += abs(piOrg[12] - piCur[12]);
    sum += abs(piOrg[13] - piCur[13]);
    sum += abs(piOrg[14] - piCur[14]);
    sum += abs(piOrg[15] - piCur[15]);
    sum += abs(piOrg[16] - piCur[16]);
    sum += abs(piOrg[17] - piCur[17]);
    sum += abs(piOrg[18] - piCur[18]);
    sum += abs(piOrg[19] - piCur[19]);
    sum += abs(piOrg[20] - piCur[20]);
    sum += abs(piOrg[21] - piCur[21]);
    sum += abs(piOrg[22] - piCur[22]);
    sum += abs(piOrg[23] - piCur[23]);
    sum += abs(piOrg[24] - piCur[24]);
    sum += abs(piOrg[25] - piCur[25]);
    sum += abs(piOrg[26] - piCur[26]);
    sum += abs(piOrg[27] - piCur[27]);
    sum += abs(piOrg[28] - piCur[28]);
    sum += abs(piOrg[29] - piCur[29]);
    sum += abs(piOrg[30] - piCur[30]);
    sum += abs(piOrg[31] - piCur[31]);
    sum += abs(piOrg[32] - piCur[32]);
    sum += abs(piOrg[33] - piCur[33]);
    sum += abs(piOrg[34] - piCur[34]);
    sum += abs(piOrg[35] - piCur[35]);
    sum += abs(piOrg[36] - piCur[36]);
    sum += abs(piOrg[37] - piCur[37]);
    sum += abs(piOrg[38] - piCur[38]);
    sum += abs(piOrg[39] - piCur[39]);
    sum += abs(piOrg[40] - piCur[40]);
    sum += abs(piOrg[41] - piCur[41]);
    sum += abs(piOrg[42] - piCur[42]);
    sum += abs(piOrg[43] - piCur[43]);
    sum += abs(piOrg[44] - piCur[44]);
    sum += abs(piOrg[45] - piCur[45]);
    sum += abs(piOrg[46] - piCur[46]);
    sum += abs(piOrg[47] - piCur[47]);

    piOrg += strideOrg;
    piCur += strideCur;
  }

  sum <<= subShift;
  return (sum >> DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth));
}




// --------------------------------------------------------------------------------------------------------------------
// MRSAD
// --------------------------------------------------------------------------------------------------------------------

Distortion RdCost::xGetMRSAD( const DistParam& rcDtParam )
{
  const Pel* piOrg           = rcDtParam.org.buf;
  const Pel* piCur           = rcDtParam.cur.buf;
  const int      cols            = rcDtParam.org.width;
  int            rows            = rcDtParam.org.height;
  const int      subShift        = rcDtParam.subShift;
  const int      subStep         = (1 << subShift);
  const ptrdiff_t strideCur       = rcDtParam.cur.stride * subStep;
  const ptrdiff_t strideOrg       = rcDtParam.org.stride * subStep;
  const uint32_t distortionShift = DISTORTION_PRECISION_ADJUSTMENT(rcDtParam.bitDepth);

  int32_t deltaSum = 0;
  for (int r = rows; r != 0; r -= subStep, piOrg += strideOrg, piCur += strideCur)
  {
    for (int n = 0; n < cols; n++)
    {
      deltaSum += ( piOrg[n] - piCur[n] );
    }
  }

  const Pel offset  = Pel(deltaSum / (cols * (rows >> subShift)));
  piOrg             = rcDtParam.org.buf;
  piCur             = rcDtParam.cur.buf;
  Distortion sum    = 0;
  for (; rows != 0; rows -= subStep)
  {
    for (int n = 0; n < cols; n++)
    {
      sum += abs(piOrg[n] - piCur[n] - offset);
    }
    if (rcDtParam.maximumDistortionForEarlyExit < (sum >> distortionShift))
    {
      return (sum >> distortionShift);