sound3d_filter.cpp

audio-video-codecs.rar语音编解码器

/*//////////////////////////////////////////////////////////////////////////////
//
//                  INTEL CORPORATION PROPRIETARY INFORMATION
//     This software is supplied under the terms of a license agreement or
//     nondisclosure agreement with Intel Corporation and may not be copied
//     or disclosed except in accordance with the terms of that agreement.
//          Copyright(c) 2005-2006 Intel Corporation. All Rights Reserved.
//
*/

#include <math.h>
#include "umc_sound3d_filter.h"

namespace UMC {

  Ipp32s  LMSInvertFilter(Ipp32f *Denumerator, Ipp32s lenDen, Ipp32s Delay,
                          Ipp32f *InvFilter, Ipp32s LenInvFilter)
  {
    Ipp32s  dataLen = 1000 * lenDen;
    Ipp32f  mu = 0.3f;
    Ipp32f *w = ippsMalloc_32f(dataLen);
    Ipp32f *xx = ippsMalloc_32f(dataLen);
    Ipp32f *ref = ippsMalloc_32f(dataLen);
    Ipp32f *y = ippsMalloc_32f(dataLen);
    IppsFIRState_32f *StateDenum;
    IppsFIRLMSState_32f *StateLMS;

    ippsZero_32f(ref, Delay);
    ippsZero_32f(InvFilter, LenInvFilter);

// GenWhiteNoice(w, dataLen);
    for (Ipp32s i = 0; i < dataLen; i++) {
      w[i] = (Ipp32f)rand() / (Ipp32f)RAND_MAX;
    } ippsFIRInitAlloc_32f(&StateDenum, Denumerator, lenDen, 0);

    ippsFIRLMSInitAlloc_32f(&StateLMS, InvFilter, LenInvFilter, 0, 0);

    ippsCopy_32f(w, ref + Delay, dataLen - Delay);

    ippsFIR_32f(w, y, dataLen, StateDenum);

    ippsFIRLMS_32f(y, ref, xx, dataLen, mu / dataLen, StateLMS);
    ippsFIRLMSGetTaps_32f(StateLMS, InvFilter);

    ippsFIRLMSFree_32f(StateLMS);
    ippsFIRFree_32f(StateDenum);
    ippsFree(ref);
    ippsFree(xx);
    ippsFree(w);
    ippsFree(y);

    return 1;
  }

  Ipp32s  FFTInvertFilter(Ipp32f *Denumerator, Ipp32s lenDen, Ipp32s Delay,
                          Ipp32f *InvFilter, Ipp32s LenInvFilter)
  {
    Ipp32s  FS;
    Ipp32s  I;
    Ipp32s  N = lenDen;

    Ipp32fc *X1;
    Ipp32fc *X2;
    Ipp32fc *X3;
    Ipp32fc *x1;
    Ipp32fc *x2;
    Ipp32f *x3;
    Ipp32fc *x_tmp;
    Ipp32f *y_tmp;
    Ipp32f *y2_tmp;
    Ipp32f *y_zerro;
    Ipp32s  size;
    Ipp32s  order;
    IppsFFTSpec_C_32fc *pCFFTSpec;
    Ipp8u  *pCBuffer;
    Ipp32f  a;

// Ipp32f F = 48000.f;
    Ipp32f  max_level_in = 0.f;
    Ipp32s  delay = Delay;
    Ipp32f  EffortFactor;

    for (FS = 1; FS <= lenDen; FS <<= 1);
    FS <<= 1;

    X1 = ippsMalloc_32fc(FS + 2);
    X2 = ippsMalloc_32fc(FS + 2);
    X3 = ippsMalloc_32fc(2 * FS + 2);
    x1 = ippsMalloc_32fc(FS + 2);
    x2 = ippsMalloc_32fc(FS + 2);
    x3 = ippsMalloc_32f(2 * FS + 2);
    x_tmp = ippsMalloc_32fc(FS + 2);
    y_tmp = ippsMalloc_32f(2 * FS + 2);
    y2_tmp = ippsMalloc_32f(2 * FS + 2);
    y_zerro = ippsMalloc_32f(2 * FS + 2);
    order = (Ipp32s)(log(0.0 + FS) / 0.69314718055994530942f);     /* "*"/ln(2) */

    ippsZero_32f(y_zerro, 2 * FS + 2);

    ippsFFTInitAlloc_C_32fc(&pCFFTSpec, order, IPP_FFT_DIV_INV_BY_N,
                            ippAlgHintNone);
    ippsFFTGetBufSize_C_32fc(pCFFTSpec, &size);
    pCBuffer = ippsMalloc_8u(size);

    ippsRealToCplx_32f(Denumerator, y_zerro, x1, N);
    ippsZero_32fc(x1 + N, FS - N);

    ippsFFTFwd_CToC_32fc(x1, X1, pCFFTSpec, pCBuffer);

/*
 * get level max
 */
    ippsMagnitude_32fc(X1, y_tmp, FS);
    ippsMax_32f(y_tmp, FS, &max_level_in);

/*
 * next
 */
    ippsConj_32fc(X1, X2, FS);
    ippsMul_32fc(X1, X2, X3, FS);

    for (I = 0; I < FS; I++) {
/*
 * if( (I*F/FS <80) || (I*F/FS >16000) ) EffortFactor = 0.001f; else
 * EffortFactor = 1;
 */

      EffortFactor = 10.f;

      a = X3[I].re + EffortFactor;
      X2[I].re /= a;
      X2[I].im /= a;
    }

/*
 * amplitude limit
 */
    ippsThreshold_32fc_I(X2, FS, max_level_in, ippCmpGreater);

    ippsFFTInv_CToC_32fc(X2, x1, pCFFTSpec, pCBuffer);
    ippsCplxToReal_32fc(x1, y_tmp, x3, N);

/*
 * window and delay
 */
    ippsWinBlackmanStd_32f(y_tmp + delay, InvFilter, LenInvFilter);

    ippsFFTFree_C_32fc(pCFFTSpec);
    ippsFree((void *)pCBuffer);

    ippsFree((void *)X1);
    ippsFree((void *)X2);
    ippsFree((void *)X3);

    ippsFree((void *)x1);
    ippsFree((void *)x2);
    ippsFree((void *)x3);

    ippsFree((void *)x_tmp);
    ippsFree((void *)y_tmp);
    ippsFree((void *)y2_tmp);
    ippsFree((void *)y_zerro);

    return UMC_OK;
  }

  Sound3dFilter::Sound3dFilter()
  {
    s3d_params = new Sound3dFilterParams;
/*
 * default
 */
    s3d_params->Delay = 20;
    s3d_params->LenFilter = 200;
    s3d_params->LenInvFilter = 200;
    s3d_params->pos_speakers = 30;
  }

  Status  Sound3dFilter::Init(BaseCodecParams * params)
  {
    Ipp32f *h_sum;
    Ipp32f *h_sub;
    Ipp32f *inv_h_sum;
    Ipp32f *inv_h_sub;
    BaseCodecParams info;

    ippsCopy_8u((Ipp8u *)params, (Ipp8u *)s3d_params,
                sizeof(Sound3dFilterParams));

    out_data_32f = ippsMalloc_32f(2 * s3d_params->MaxSamples);
    out_data_16s = ippsMalloc_16s(2 * s3d_params->MaxSamples);

    s3d_params->user_hrtf->GetInfo(&info);
    s3d_params->LenFilter = info.m_SuggestedInputSize;

    hrir_left = ippsMalloc_32f(s3d_params->LenFilter);
    hrir_right = ippsMalloc_32f(s3d_params->LenFilter);

    s3d_params->user_hrtf->GetFilterFromDataBase(hrir_left, hrir_right,
                                                 (Ipp32f)(s3d_params->Azimuth),
                                                 (Ipp32f)(s3d_params->Elevation));

    ippsFIRInitAlloc_32f(&state_left, hrir_left, s3d_params->LenFilter, NULL);
    ippsFIRInitAlloc_32f(&state_right, hrir_right, s3d_params->LenFilter, NULL);

/*
 * cross-talk cancellation
 */
    h_sum = ippsMalloc_32f(s3d_params->LenFilter);
    h_sub = ippsMalloc_32f(s3d_params->LenFilter);
    inv_h_sum = ippsMalloc_32f(s3d_params->LenInvFilter);
    inv_h_sub = ippsMalloc_32f(s3d_params->LenInvFilter);

    buff_in1 = ippsMalloc_32f(s3d_params->MaxSamples);
    buff_in2 = ippsMalloc_32f(s3d_params->MaxSamples);
    buff_out1 = ippsMalloc_32f(s3d_params->MaxSamples);
    buff_out2 = ippsMalloc_32f(s3d_params->MaxSamples);

    s3d_params->user_hrtf->GetFilterFromDataBase(hrir_left, hrir_right,
                                                 (Ipp32f)(s3d_params->Azimuth),
                                                 (Ipp32f)(s3d_params->Elevation));
    ippsAdd_32f(hrir_left, hrir_right, h_sum, s3d_params->LenFilter);
    ippsSub_32f(hrir_right, hrir_left, h_sub, s3d_params->LenFilter);
/*
 * scale
 */
    ScaleSignal_Linf(h_sum, s3d_params->LenFilter, 1.f);
    ScaleSignal_Linf(h_sub, s3d_params->LenFilter, 1.f);
    if (s3d_params->InvertFilter == NULL) {
      s3d_params->InvertFilter = FFTInvertFilter;
    }

    s3d_params->InvertFilter(h_sum, s3d_params->LenFilter, s3d_params->Delay,
                             inv_h_sum, s3d_params->LenInvFilter);
    s3d_params->InvertFilter(h_sub, s3d_params->LenFilter, s3d_params->Delay,
                             inv_h_sub, s3d_params->LenInvFilter);

    ScaleSignal_Linf(inv_h_sum, s3d_params->LenInvFilter, 1.f);
    ScaleSignal_Linf(inv_h_sub, s3d_params->LenInvFilter, 1.f);

    ippsFIRInitAlloc_32f(&state_inv_filter_sum, inv_h_sum,
                         s3d_params->LenInvFilter, 0);
    ippsFIRInitAlloc_32f(&state_inv_filter_sub, inv_h_sub,
                         s3d_params->LenInvFilter, 0);

/*
 * free memory
 */
    ippsFree(h_sum);
    ippsFree(h_sub);
    ippsFree(inv_h_sum);
    ippsFree(inv_h_sub);

    return UMC_OK;
  }

  Status  Sound3dFilter::Close()
  {
    ippsFree(out_data_32f);
    ippsFree(out_data_16s);
    ippsFree(hrir_right);
    ippsFree(hrir_left);
    ippsFree(buff_in1);
    ippsFree(buff_in2);
    ippsFree(buff_out1);
    ippsFree(buff_out2);

    ippsFIRFree_32f(state_left);
    ippsFIRFree_32f(state_right);

    ippsFIRFree_32f(state_inv_filter_sum);
    ippsFIRFree_32f(state_inv_filter_sub);

// s3d_params->user_hrtf = NULL;

    return UMC_OK;
  }

  Status  Sound3dFilter::SetParams(BaseCodecParams * params)
  {
    Ipp32f *h_sum;
    Ipp32f *h_sub;
    Ipp32f *inv_h_sum;
    Ipp32f *inv_h_sub;

    if (((Sound3dFilterParams *) params)->type_set_param == CHANGE_ALL)
      ippsCopy_8u((Ipp8u *)params, (Ipp8u *)s3d_params,
                  sizeof(Sound3dFilterParams));

/*
 * HRTF
 */
    if ((((Sound3dFilterParams *) params)->type_set_param == CHANGE_ALL) ||
        (((Sound3dFilterParams *) params)->type_set_param ==
         CHANGE_ONLY_HRTF)) {
      s3d_params->user_hrtf->GetFilterFromDataBase(hrir_left, hrir_right,
                                                   (Ipp32f)(s3d_params->Azimuth),
                                                   (Ipp32f)(s3d_params->Elevation));
      ippsFIRSetTaps_32f(hrir_left, state_left);
      ippsFIRSetTaps_32f(hrir_right, state_right);
    }

/*
 * CrossTalkCancellation
 */
    if ((((Sound3dFilterParams *) params)->type_set_param == CHANGE_ALL) ||
        (((Sound3dFilterParams *) params)->type_set_param == CHANGE_ONLY_CTC)) {
      h_sum = ippsMalloc_32f(s3d_params->LenFilter);
      h_sub = ippsMalloc_32f(s3d_params->LenFilter);
      inv_h_sum = ippsMalloc_32f(s3d_params->LenInvFilter);
      inv_h_sub = ippsMalloc_32f(s3d_params->LenInvFilter);

      s3d_params->user_hrtf->GetFilterFromDataBase(hrir_left, hrir_right,
                                                   (Ipp32f)(s3d_params->Azimuth),
                                                   (Ipp32f)(s3d_params->Elevation));
      ippsAdd_32f(hrir_left, hrir_right, h_sum, s3d_params->LenFilter);
      ippsSub_32f(hrir_right, hrir_left, h_sub, s3d_params->LenFilter);
      ScaleSignal_Linf(h_sum, s3d_params->LenFilter, 1.f);
      ScaleSignal_Linf(h_sub, s3d_params->LenFilter, 1.f);
/*
 * invert filter
 */
      s3d_params->InvertFilter(h_sum, s3d_params->LenFilter, s3d_params->Delay,
                               inv_h_sum, s3d_params->LenInvFilter);
      s3d_params->InvertFilter(h_sub, s3d_params->LenFilter, s3d_params->Delay,
                               inv_h_sub, s3d_params->LenInvFilter);

      ScaleSignal_Linf(inv_h_sum, s3d_params->LenInvFilter, 1.f);
      ScaleSignal_Linf(inv_h_sub, s3d_params->LenInvFilter, 1.f);

      ippsFIRSetTaps_32f(inv_h_sum, state_inv_filter_sum);
      ippsFIRSetTaps_32f(inv_h_sub, state_inv_filter_sub);

/*
 * free memory
 */
      ippsFree(h_sum);
      ippsFree(h_sub);
      ippsFree(inv_h_sum);
      ippsFree(inv_h_sub);
    }

    if ((((Sound3dFilterParams *) params)->type_set_param == ONLY_CTC_OFF) ||
        (((Sound3dFilterParams *) params)->type_set_param == ONLY_CTC_ON))
      s3d_params->flag_ctc =
        ((Sound3dFilterParams *) params)->type_set_param - 10;

    return UMC_OK;
  }

  Status  Sound3dFilter::GetInfo(BaseCodecParams * info)
  {
    ippsCopy_8u((Ipp8u *)s3d_params, (Ipp8u *)info,
                sizeof(Sound3dFilterParams));
    return UMC_OK;
  }

  Status  Sound3dFilter::Reset()
  {
    return UMC_OK;
  }

  Sound3dFilter::~Sound3dFilter()
  {
    Close();
    delete  s3d_params;
  }

  Status  Sound3dFilter::GetFrame(MediaData * in_data, MediaData * out_data)
  {
    Ipp32u size = in_data->GetDataSize();
    Ipp32f *ptr_indata = (Ipp32f *)in_data->GetDataPointer();
    Ipp32f *tmp[2];

//      Ipp32s          i;
    Ipp32s  MaxSamples = s3d_params->MaxSamples;

/* HRTF */
    size /= sizeof(Ipp32f);
    ippsFIR_32f(ptr_indata, out_data_32f, size, state_left);
    ippsFIR_32f(ptr_indata, out_data_32f + MaxSamples, size, state_right);

    if (s3d_params->flag_ctc) {
      FilterSpeakers(out_data_32f, out_data_32f + MaxSamples, size,
                     out_data_32f, out_data_32f + MaxSamples);
    }

    tmp[0] = out_data_32f;
    tmp[1] = out_data_32f + MaxSamples;
    ippsJoin_32f16s_D2L((const Ipp32f **)(tmp), 2, size, out_data_16s);
/*
 * ippsConvert_32f16s_Sfs(out_data_32f, out_data_16s, size, ippRndNear, 0);
 * ippsConvert_32f16s_Sfs(out_data_32f+MaxSamples, out_data_16s+size, size,
 * ippRndNear, 0);
 */

    out_data->SetBufferPointer((Ipp8u *)out_data_16s,
                               2 * size * sizeof(Ipp16s));
    out_data->SetDataSize(2 * size * sizeof(Ipp16s));

    return UMC_OK;
  }

//#ifdef USED_IN_CLASS

  Status  Sound3dFilter::FilterSpeakers(Ipp32f *ch1, Ipp32f *ch2, Ipp32s Len,
                                        Ipp32f *out_ch1, Ipp32f *out_ch2)
  {
    Ipp32f  max1;
    Ipp32f  max2;
    const Ipp32f mul = (Ipp32f)(1.f / IPP_SQRT2);
    Ipp32f  mul1;
    Ipp32f  mul2;

/*
 * equivalent RMS IN signal and OUT
 */
    ippsNorm_L2_32f(ch1, Len, &mul1);
    ippsNorm_L2_32f(ch2, Len, &mul2);

    ippsAdd_32f(ch1, ch2, buff_in1, Len);
    ippsSub_32f(ch2, ch1, buff_in2, Len);
    ippsMulC_32f_I(mul, buff_in1, Len);
    ippsMulC_32f_I(mul, buff_in2, Len);

    ippsFIR_32f(buff_in1, buff_out1, Len, state_inv_filter_sum);
    ippsFIR_32f(buff_in2, buff_out2, Len, state_inv_filter_sub);

    ippsAdd_32f(buff_out1, buff_out2, out_ch1, Len);
    ippsSub_32f(buff_out2, buff_out1, out_ch2, Len);

    ippsMulC_32f_I(mul, out_ch1, Len);
    ippsMulC_32f_I(mul, out_ch2, Len);

/*
 * scale RMS in && RMS out
 */
    ippsNorm_L2_32f(out_ch1, Len, &max1);
    ippsMulC_32f_I(mul1 / max1, out_ch1, Len);
    ippsNorm_L2_32f(out_ch2, Len, &max2);
    ippsMulC_32f_I(mul2 / max2, out_ch2, Len);

    return UMC_OK;
  }

  Status  Sound3dFilter::ScaleSignal_Linf(Ipp32f *pSrc, Ipp32s N, Ipp32f MAX)
  {
    Ipp32f  max;

    ippsNorm_Inf_32f(pSrc, N, &max);
    ippsMulC_32f_I(MAX / max, pSrc, N);

    return UMC_OK;
  }
}       // namespace UMC