taudiofilterheadphone.cpp.svn-base

ffshow源码

/*
 * Copyright (c) 2004-2006 Milan Cutka
 * based on VideoLAN headphone virtual spatialization channel mixer module by Boris Dor鑣
 * HRTF by ylai
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "stdafx.h"
#include "TaudioFilterHeadphone.h"
#include "TmixerSettings.h"
#include "firfilter.h"
#include "TfirSettings.h"

TaudioFilterHeadphone::TaudioFilterHeadphone(IffdshowBase *Ideci,Tfilters *Iparent):TaudioFilter(Ideci,Iparent)
{
 oldfmt.freq=0;olddim=-1;
 p_sys=NULL;
 inited=false;
}
void TaudioFilterHeadphone::done(void)
{
 if (p_sys)
  {
   if (p_sys->p_overflow_buffer) free(p_sys->p_overflow_buffer);
   if (p_sys->p_atomic_operations) free(p_sys->p_atomic_operations);
   free(p_sys);p_sys=NULL;
  }
}

/*****************************************************************************
 * Init: initialize internal data structures
 * and computes the needed atomic operations
 *****************************************************************************/
/* x and z represent the coordinates of the virtual speaker
 *  relatively to the center of the listener's head, measured in meters :
 *
 *  left              right
 *Z
 *-
 *a          head
 *x
 *i
 *s
 *  rear left    rear right
 *
 *          x-axis
 *  */
void TaudioFilterHeadphone::aout_filter_sys_t::ComputeChannelOperations (
        unsigned int i_rate , unsigned int i_next_atomic_operation
        , int i_source_channel_offset , double d_x , double d_z
        , double d_channel_amplitude_factor )
{
    double d_c = 340; /*sound celerity (unit: m/s)*/

    /* Left ear */
    p_atomic_operations[i_next_atomic_operation]
        .i_source_channel_offset = i_source_channel_offset;
    p_atomic_operations[i_next_atomic_operation]
        .i_dest_channel_offset = 0;/* left */
    p_atomic_operations[i_next_atomic_operation]
        .i_delay = (int)( sqrt( (-0.1-d_x)*(-0.1-d_x) + (0-d_z)*(0-d_z) )
                          / d_c * i_rate );
    if ( d_x < 0 )
    {
        p_atomic_operations[i_next_atomic_operation]
            .d_amplitude_factor = d_channel_amplitude_factor * 1.1 / 2;
    }
    else if ( d_x > 0 )
    {
        p_atomic_operations[i_next_atomic_operation]
            .d_amplitude_factor = d_channel_amplitude_factor * 0.9 / 2;
    }
    else
    {
        p_atomic_operations[i_next_atomic_operation]
            .d_amplitude_factor = d_channel_amplitude_factor / 2;
    }

    /* Right ear */
    p_atomic_operations[i_next_atomic_operation + 1]
        .i_source_channel_offset = i_source_channel_offset;
    p_atomic_operations[i_next_atomic_operation + 1]
        .i_dest_channel_offset = 1;/* right */
    p_atomic_operations[i_next_atomic_operation + 1]
        .i_delay = (int)( sqrt( (0.1-d_x)*(0.1-d_x) + (0-d_z)*(0-d_z) )
                          / d_c * i_rate );
    if ( d_x < 0 )
    {
        p_atomic_operations[i_next_atomic_operation + 1]
            .d_amplitude_factor = d_channel_amplitude_factor * 0.9 / 2;
    }
    else if ( d_x > 0 )
    {
        p_atomic_operations[i_next_atomic_operation + 1]
            .d_amplitude_factor = d_channel_amplitude_factor * 1.1 / 2;
    }
    else
    {
        p_atomic_operations[i_next_atomic_operation + 1]
            .d_amplitude_factor = d_channel_amplitude_factor / 2;
    }
}

int TaudioFilterHeadphone::aout_filter_sys_t::Init(const TsampleFormat &fmt,const TmixerSettings *cfg)
{
    double d_x = cfg->headphone_dim;//config_GetInt ( p_filter , "headphone-dim" );
    double d_z = d_x;
    double d_z_rear = -d_x/3;
    unsigned int i_next_atomic_operation;
    int i_source_channel_offset;

    /* Number of elementary operations */
    i_nb_atomic_operations = fmt.nchannels * 2;
    p_atomic_operations = (atomic_operation_t*)malloc ( sizeof(atomic_operation_t)
            * i_nb_atomic_operations );
    /* For each virtual speaker, computes elementary wave propagation time
     * to each ear */
    i_next_atomic_operation = 0;
    i_source_channel_offset = 0;
    for (unsigned int ch=0;ch<fmt.nchannels;ch++)
     if ( fmt.speakers[ch] == SPEAKER_FRONT_LEFT)
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , -d_x , d_z , 2.0 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] ==  SPEAKER_FRONT_RIGHT )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , d_x , d_z , 2.0 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] == SPEAKER_BACK_LEFT )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , -d_x , d_z_rear , 1.5 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] == SPEAKER_BACK_RIGHT )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , d_x , d_z_rear , 1.5 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] ==  SPEAKER_BACK_CENTER )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , 0 , -d_z , 1.5 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] ==  SPEAKER_FRONT_CENTER )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , 0 , d_z , 1.5 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] == SPEAKER_LOW_FREQUENCY )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , 0 , d_z_rear , 5.0 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] ==  SPEAKER_SIDE_LEFT )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , -d_x , 0 , 1.5 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }
     else if ( fmt.speakers[ch] == SPEAKER_SIDE_RIGHT )
      {
          ComputeChannelOperations ( fmt.freq
                  , i_next_atomic_operation , i_source_channel_offset
                  , d_x , 0 , 1.5 / fmt.nchannels );
          i_next_atomic_operation += 2;
          i_source_channel_offset++;
      }

    /* Initialize the overflow buffer
     * we need it because the process induce a delay in the samples */
    i_overflow_buffer_size = 0;
    for (unsigned int i= 0 ; i < i_nb_atomic_operations ; i++ )
    {
        if ( i_overflow_buffer_size
                < p_atomic_operations[i].i_delay * fmt.nchannels
                * sizeof (float) )
        {
            i_overflow_buffer_size
                = p_atomic_operations[i].i_delay * fmt.nchannels
                * sizeof (float);
        }
    }
    p_overflow_buffer = (byte_t*)malloc ( i_overflow_buffer_size );
    memset ( p_overflow_buffer , 0 , i_overflow_buffer_size );

    /* end */
    return 0;
}

bool TaudioFilterHeadphone::getOutputFmt(TsampleFormat &fmt,const TfilterSettingsAudio *cfg)
{
 if (super::getOutputFmt(fmt,cfg))
  {
   fmt.setChannels(2);
   return true;
  }
 else
  return false;  
}

HRESULT TaudioFilterHeadphone::process(TfilterQueue::iterator it,TsampleFormat &fmt,void *samples0,size_t numsamples,const TfilterSettingsAudio *cfg0)
{
 const TmixerSettings *cfg=(const TmixerSettings*)cfg0;

 if (!p_sys || oldfmt!=fmt || olddim!=cfg->headphone_dim)
  {
   oldfmt=fmt;olddim=cfg->headphone_dim;
   done();
   /* Allocate the memory needed to store the module's structure */
   p_sys=(aout_filter_sys_t*)malloc(sizeof(aout_filter_sys_t));
   p_sys->i_overflow_buffer_size=0;
   p_sys->p_overflow_buffer=NULL;
   p_sys->i_nb_atomic_operations=0;
   p_sys->p_atomic_operations=NULL;
   inited=p_sys->Init(fmt,cfg)>=0;
  }
 
 if (inited)
  {
   float *p_in=(float*)init(cfg,fmt,samples0,numsamples);
   unsigned int i_input_nb=fmt.nchannels;
   fmt.setChannels(2);
   float *p_out=(float*)alloc_buffer(fmt,numsamples,buf);
   unsigned int i_output_nb=fmt.nchannels;
 
   /* Slide the overflow buffer */
   byte_t *p_overflow = p_sys->p_overflow_buffer;
   size_t i_overflow_size = p_sys->i_overflow_buffer_size;
   size_t i_out_size=numsamples*fmt.blockAlign();

   memset ( p_out , 0 , i_out_size );
   if ( i_out_size > i_overflow_size )
    memcpy ( p_out , p_overflow , i_overflow_size );
   else
    memcpy ( p_out , p_overflow , i_out_size );
 
   byte_t *p_slide = p_sys->p_overflow_buffer;
   while ( p_slide < p_overflow + i_overflow_size )
    {
     if ( p_slide + i_out_size < p_overflow + i_overflow_size )
      {
       memset ( p_slide , 0 , i_out_size );
       if ( p_slide + 2 * i_out_size < p_overflow + i_overflow_size )
        memcpy ( p_slide , p_slide + i_out_size , i_out_size );
       else
        memcpy ( p_slide , p_slide + i_out_size , p_overflow + i_overflow_size - ( p_slide + i_out_size ) );
      }
     else
      {
       memset ( p_slide , 0 , p_overflow + i_overflow_size - p_slide );
      }
     p_slide += i_out_size;
    }

   /* apply the atomic operations */
   for ( unsigned int i = 0 ; i < p_sys->i_nb_atomic_operations ; i++ )
    {
     /* shorter variable names */
     int i_source_channel_offset = p_sys->p_atomic_operations[i].i_source_channel_offset;
     int i_dest_channel_offset = p_sys->p_atomic_operations[i].i_dest_channel_offset;
     unsigned int i_delay = p_sys->p_atomic_operations[i].i_delay;
     double d_amplitude_factor = p_sys->p_atomic_operations[i].d_amplitude_factor;

     if ( numsamples > i_delay )
      {
       unsigned int j;
       /* current buffer coefficients */
       for ( j = 0 ; j < numsamples - i_delay ; j++ )
        {
         p_out[ (i_delay+j)*i_output_nb + i_dest_channel_offset ]
             += float(p_in[ j * i_input_nb + i_source_channel_offset ]
                * d_amplitude_factor);
        }

       /* overflow buffer coefficients */
       for ( j = 0 ; j < i_delay ; j++ )
        {
         ((float*)p_overflow)[ j*i_output_nb + i_dest_channel_offset ]
             += float(p_in[ (numsamples - i_delay + j)
                * i_input_nb + i_source_channel_offset ]
                * d_amplitude_factor);
        }
      }