508 lines
12 KiB
C
508 lines
12 KiB
C
/* Copyright (C) 2005 Jean-Marc Valin, CSIRO, Christopher Montgomery
|
|
File: vorbis_psy.c
|
|
|
|
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 Xiph.org Foundation 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 FOUNDATION 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.
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#ifdef VORBIS_PSYCHO
|
|
|
|
#include "arch.h"
|
|
#include "smallft.h"
|
|
#include "lpc.h"
|
|
#include "vorbis_psy.h"
|
|
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
/* psychoacoustic setup ********************************************/
|
|
|
|
static VorbisPsyInfo example_tuning = {
|
|
|
|
.5,.5,
|
|
3,3,25,
|
|
|
|
/*63 125 250 500 1k 2k 4k 8k 16k*/
|
|
// vorbis mode 4 style
|
|
//{-32,-32,-32,-32,-28,-24,-22,-20,-20, -20, -20, -8, -6, -6, -6, -6, -6},
|
|
{ -4, -6, -6, -6, -6, -6, -6, -6, -8, -8,-10,-10, -8, -6, -4, -4, -2},
|
|
|
|
{
|
|
0, 1, 2, 3, 4, 5, 5, 5, /* 7dB */
|
|
6, 6, 6, 5, 4, 4, 4, 4, /* 15dB */
|
|
4, 4, 5, 5, 5, 6, 6, 6, /* 23dB */
|
|
7, 7, 7, 8, 8, 8, 9, 10, /* 31dB */
|
|
11,12,13,14,15,16,17, 18, /* 39dB */
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
/* there was no great place to put this.... */
|
|
#include <stdio.h>
|
|
static void _analysis_output(char *base,int i,float *v,int n,int bark,int dB){
|
|
int j;
|
|
FILE *of;
|
|
char buffer[80];
|
|
|
|
sprintf(buffer,"%s_%d.m",base,i);
|
|
of=fopen(buffer,"w");
|
|
|
|
if(!of)perror("failed to open data dump file");
|
|
|
|
for(j=0;j<n;j++){
|
|
if(bark){
|
|
float b=toBARK((4000.f*j/n)+.25);
|
|
fprintf(of,"%f ",b);
|
|
}else
|
|
fprintf(of,"%f ",(double)j);
|
|
|
|
if(dB){
|
|
float val;
|
|
if(v[j]==0.)
|
|
val=-140.;
|
|
else
|
|
val=todB(v[j]);
|
|
fprintf(of,"%f\n",val);
|
|
}else{
|
|
fprintf(of,"%f\n",v[j]);
|
|
}
|
|
}
|
|
fclose(of);
|
|
}
|
|
|
|
static void bark_noise_hybridmp(int n,const long *b,
|
|
const float *f,
|
|
float *noise,
|
|
const float offset,
|
|
const int fixed){
|
|
|
|
float *N=alloca(n*sizeof(*N));
|
|
float *X=alloca(n*sizeof(*N));
|
|
float *XX=alloca(n*sizeof(*N));
|
|
float *Y=alloca(n*sizeof(*N));
|
|
float *XY=alloca(n*sizeof(*N));
|
|
|
|
float tN, tX, tXX, tY, tXY;
|
|
int i;
|
|
|
|
int lo, hi;
|
|
float R, A, B, D;
|
|
float w, x, y;
|
|
|
|
tN = tX = tXX = tY = tXY = 0.f;
|
|
|
|
y = f[0] + offset;
|
|
if (y < 1.f) y = 1.f;
|
|
|
|
w = y * y * .5;
|
|
|
|
tN += w;
|
|
tX += w;
|
|
tY += w * y;
|
|
|
|
N[0] = tN;
|
|
X[0] = tX;
|
|
XX[0] = tXX;
|
|
Y[0] = tY;
|
|
XY[0] = tXY;
|
|
|
|
for (i = 1, x = 1.f; i < n; i++, x += 1.f) {
|
|
|
|
y = f[i] + offset;
|
|
if (y < 1.f) y = 1.f;
|
|
|
|
w = y * y;
|
|
|
|
tN += w;
|
|
tX += w * x;
|
|
tXX += w * x * x;
|
|
tY += w * y;
|
|
tXY += w * x * y;
|
|
|
|
N[i] = tN;
|
|
X[i] = tX;
|
|
XX[i] = tXX;
|
|
Y[i] = tY;
|
|
XY[i] = tXY;
|
|
}
|
|
|
|
for (i = 0, x = 0.f;; i++, x += 1.f) {
|
|
|
|
lo = b[i] >> 16;
|
|
if( lo>=0 ) break;
|
|
hi = b[i] & 0xffff;
|
|
|
|
tN = N[hi] + N[-lo];
|
|
tX = X[hi] - X[-lo];
|
|
tXX = XX[hi] + XX[-lo];
|
|
tY = Y[hi] + Y[-lo];
|
|
tXY = XY[hi] - XY[-lo];
|
|
|
|
A = tY * tXX - tX * tXY;
|
|
B = tN * tXY - tX * tY;
|
|
D = tN * tXX - tX * tX;
|
|
R = (A + x * B) / D;
|
|
if (R < 0.f)
|
|
R = 0.f;
|
|
|
|
noise[i] = R - offset;
|
|
}
|
|
|
|
for ( ;; i++, x += 1.f) {
|
|
|
|
lo = b[i] >> 16;
|
|
hi = b[i] & 0xffff;
|
|
if(hi>=n)break;
|
|
|
|
tN = N[hi] - N[lo];
|
|
tX = X[hi] - X[lo];
|
|
tXX = XX[hi] - XX[lo];
|
|
tY = Y[hi] - Y[lo];
|
|
tXY = XY[hi] - XY[lo];
|
|
|
|
A = tY * tXX - tX * tXY;
|
|
B = tN * tXY - tX * tY;
|
|
D = tN * tXX - tX * tX;
|
|
R = (A + x * B) / D;
|
|
if (R < 0.f) R = 0.f;
|
|
|
|
noise[i] = R - offset;
|
|
}
|
|
for ( ; i < n; i++, x += 1.f) {
|
|
|
|
R = (A + x * B) / D;
|
|
if (R < 0.f) R = 0.f;
|
|
|
|
noise[i] = R - offset;
|
|
}
|
|
|
|
if (fixed <= 0) return;
|
|
|
|
for (i = 0, x = 0.f;; i++, x += 1.f) {
|
|
hi = i + fixed / 2;
|
|
lo = hi - fixed;
|
|
if(lo>=0)break;
|
|
|
|
tN = N[hi] + N[-lo];
|
|
tX = X[hi] - X[-lo];
|
|
tXX = XX[hi] + XX[-lo];
|
|
tY = Y[hi] + Y[-lo];
|
|
tXY = XY[hi] - XY[-lo];
|
|
|
|
|
|
A = tY * tXX - tX * tXY;
|
|
B = tN * tXY - tX * tY;
|
|
D = tN * tXX - tX * tX;
|
|
R = (A + x * B) / D;
|
|
|
|
if (R - offset < noise[i]) noise[i] = R - offset;
|
|
}
|
|
for ( ;; i++, x += 1.f) {
|
|
|
|
hi = i + fixed / 2;
|
|
lo = hi - fixed;
|
|
if(hi>=n)break;
|
|
|
|
tN = N[hi] - N[lo];
|
|
tX = X[hi] - X[lo];
|
|
tXX = XX[hi] - XX[lo];
|
|
tY = Y[hi] - Y[lo];
|
|
tXY = XY[hi] - XY[lo];
|
|
|
|
A = tY * tXX - tX * tXY;
|
|
B = tN * tXY - tX * tY;
|
|
D = tN * tXX - tX * tX;
|
|
R = (A + x * B) / D;
|
|
|
|
if (R - offset < noise[i]) noise[i] = R - offset;
|
|
}
|
|
for ( ; i < n; i++, x += 1.f) {
|
|
R = (A + x * B) / D;
|
|
if (R - offset < noise[i]) noise[i] = R - offset;
|
|
}
|
|
}
|
|
|
|
static void _vp_noisemask(VorbisPsy *p,
|
|
float *logfreq,
|
|
float *logmask){
|
|
|
|
int i,n=p->n/2;
|
|
float *work=alloca(n*sizeof(*work));
|
|
|
|
bark_noise_hybridmp(n,p->bark,logfreq,logmask,
|
|
140.,-1);
|
|
|
|
for(i=0;i<n;i++)work[i]=logfreq[i]-logmask[i];
|
|
|
|
bark_noise_hybridmp(n,p->bark,work,logmask,0.,
|
|
p->vi->noisewindowfixed);
|
|
|
|
for(i=0;i<n;i++)work[i]=logfreq[i]-work[i];
|
|
|
|
{
|
|
static int seq=0;
|
|
|
|
float work2[n];
|
|
for(i=0;i<n;i++){
|
|
work2[i]=logmask[i]+work[i];
|
|
}
|
|
|
|
//_analysis_output("logfreq",seq,logfreq,n,0,0);
|
|
//_analysis_output("median",seq,work,n,0,0);
|
|
//_analysis_output("envelope",seq,work2,n,0,0);
|
|
seq++;
|
|
}
|
|
|
|
for(i=0;i<n;i++){
|
|
int dB=logmask[i]+.5;
|
|
if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
|
|
if(dB<0)dB=0;
|
|
logmask[i]= work[i]+p->vi->noisecompand[dB]+p->noiseoffset[i];
|
|
}
|
|
|
|
}
|
|
|
|
VorbisPsy *vorbis_psy_init(int rate, int n)
|
|
{
|
|
long i,j,lo=-99,hi=1;
|
|
VorbisPsy *p = speex_alloc(sizeof(VorbisPsy));
|
|
memset(p,0,sizeof(*p));
|
|
|
|
p->n = n;
|
|
spx_drft_init(&p->lookup, n);
|
|
p->bark = speex_alloc(n*sizeof(*p->bark));
|
|
p->rate=rate;
|
|
p->vi = &example_tuning;
|
|
|
|
/* BH4 window */
|
|
p->window = speex_alloc(sizeof(*p->window)*n);
|
|
float a0 = .35875f;
|
|
float a1 = .48829f;
|
|
float a2 = .14128f;
|
|
float a3 = .01168f;
|
|
for(i=0;i<n;i++)
|
|
p->window[i] = //a0 - a1*cos(2.*M_PI/n*(i+.5)) + a2*cos(4.*M_PI/n*(i+.5)) - a3*cos(6.*M_PI/n*(i+.5));
|
|
sin((i+.5)/n * M_PI)*sin((i+.5)/n * M_PI);
|
|
/* bark scale lookups */
|
|
for(i=0;i<n;i++){
|
|
float bark=toBARK(rate/(2*n)*i);
|
|
|
|
for(;lo+p->vi->noisewindowlomin<i &&
|
|
toBARK(rate/(2*n)*lo)<(bark-p->vi->noisewindowlo);lo++);
|
|
|
|
for(;hi<=n && (hi<i+p->vi->noisewindowhimin ||
|
|
toBARK(rate/(2*n)*hi)<(bark+p->vi->noisewindowhi));hi++);
|
|
|
|
p->bark[i]=((lo-1)<<16)+(hi-1);
|
|
|
|
}
|
|
|
|
/* set up rolling noise median */
|
|
p->noiseoffset=speex_alloc(n*sizeof(*p->noiseoffset));
|
|
|
|
for(i=0;i<n;i++){
|
|
float halfoc=toOC((i+.5)*rate/(2.*n))*2.;
|
|
int inthalfoc;
|
|
float del;
|
|
|
|
if(halfoc<0)halfoc=0;
|
|
if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1;
|
|
inthalfoc=(int)halfoc;
|
|
del=halfoc-inthalfoc;
|
|
|
|
p->noiseoffset[i]=
|
|
p->vi->noiseoff[inthalfoc]*(1.-del) +
|
|
p->vi->noiseoff[inthalfoc+1]*del;
|
|
|
|
}
|
|
#if 0
|
|
_analysis_output_always("noiseoff0",ls,p->noiseoffset,n,1,0,0);
|
|
#endif
|
|
|
|
return p;
|
|
}
|
|
|
|
void vorbis_psy_destroy(VorbisPsy *p)
|
|
{
|
|
if(p){
|
|
spx_drft_clear(&p->lookup);
|
|
if(p->bark)
|
|
speex_free(p->bark);
|
|
if(p->noiseoffset)
|
|
speex_free(p->noiseoffset);
|
|
if(p->window)
|
|
speex_free(p->window);
|
|
memset(p,0,sizeof(*p));
|
|
speex_free(p);
|
|
}
|
|
}
|
|
|
|
void compute_curve(VorbisPsy *psy, float *audio, float *curve)
|
|
{
|
|
int i;
|
|
float work[psy->n];
|
|
|
|
float scale=4.f/psy->n;
|
|
float scale_dB;
|
|
|
|
scale_dB=todB(scale);
|
|
|
|
/* window the PCM data; use a BH4 window, not vorbis */
|
|
for(i=0;i<psy->n;i++)
|
|
work[i]=audio[i] * psy->window[i];
|
|
|
|
{
|
|
static int seq=0;
|
|
|
|
//_analysis_output("win",seq,work,psy->n,0,0);
|
|
|
|
seq++;
|
|
}
|
|
|
|
/* FFT yields more accurate tonal estimation (not phase sensitive) */
|
|
spx_drft_forward(&psy->lookup,work);
|
|
|
|
/* magnitudes */
|
|
work[0]=scale_dB+todB(work[0]);
|
|
for(i=1;i<psy->n-1;i+=2){
|
|
float temp = work[i]*work[i] + work[i+1]*work[i+1];
|
|
work[(i+1)>>1] = scale_dB+.5f * todB(temp);
|
|
}
|
|
|
|
/* derive a noise curve */
|
|
_vp_noisemask(psy,work,curve);
|
|
#define SIDEL 12
|
|
for (i=0;i<SIDEL;i++)
|
|
{
|
|
curve[i]=curve[SIDEL];
|
|
}
|
|
#define SIDEH 12
|
|
for (i=0;i<SIDEH;i++)
|
|
{
|
|
curve[(psy->n>>1)-i-1]=curve[(psy->n>>1)-SIDEH];
|
|
}
|
|
for(i=0;i<((psy->n)>>1);i++)
|
|
curve[i] = fromdB(1.2*curve[i]+.2*i);
|
|
//curve[i] = fromdB(0.8*curve[i]+.35*i);
|
|
//curve[i] = fromdB(0.9*curve[i])*pow(1.0*i+45,1.3);
|
|
}
|
|
|
|
/* Transform a masking curve (power spectrum) into a pole-zero filter */
|
|
void curve_to_lpc(VorbisPsy *psy, float *curve, float *awk1, float *awk2, int ord)
|
|
{
|
|
int i;
|
|
float ac[psy->n];
|
|
float tmp;
|
|
int len = psy->n >> 1;
|
|
for (i=0;i<2*len;i++)
|
|
ac[i] = 0;
|
|
for (i=1;i<len;i++)
|
|
ac[2*i-1] = curve[i];
|
|
ac[0] = curve[0];
|
|
ac[2*len-1] = curve[len-1];
|
|
|
|
spx_drft_backward(&psy->lookup, ac);
|
|
_spx_lpc(awk1, ac, ord);
|
|
tmp = 1.;
|
|
for (i=0;i<ord;i++)
|
|
{
|
|
tmp *= .99;
|
|
awk1[i] *= tmp;
|
|
}
|
|
#if 0
|
|
for (i=0;i<ord;i++)
|
|
awk2[i] = 0;
|
|
#else
|
|
/* Use the second (awk2) filter to correct the first one */
|
|
for (i=0;i<2*len;i++)
|
|
ac[i] = 0;
|
|
for (i=0;i<ord;i++)
|
|
ac[i+1] = awk1[i];
|
|
ac[0] = 1;
|
|
spx_drft_forward(&psy->lookup, ac);
|
|
/* Compute (power) response of awk1 (all zero) */
|
|
ac[0] *= ac[0];
|
|
for (i=1;i<len;i++)
|
|
ac[i] = ac[2*i-1]*ac[2*i-1] + ac[2*i]*ac[2*i];
|
|
ac[len] = ac[2*len-1]*ac[2*len-1];
|
|
/* Compute correction required */
|
|
for (i=0;i<len;i++)
|
|
curve[i] = 1. / (1e-6f+curve[i]*ac[i]);
|
|
|
|
for (i=0;i<2*len;i++)
|
|
ac[i] = 0;
|
|
for (i=1;i<len;i++)
|
|
ac[2*i-1] = curve[i];
|
|
ac[0] = curve[0];
|
|
ac[2*len-1] = curve[len-1];
|
|
|
|
spx_drft_backward(&psy->lookup, ac);
|
|
_spx_lpc(awk2, ac, ord);
|
|
tmp = 1;
|
|
for (i=0;i<ord;i++)
|
|
{
|
|
tmp *= .99;
|
|
awk2[i] *= tmp;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#if 0
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
#define ORDER 10
|
|
#define CURVE_SIZE 24
|
|
|
|
int main()
|
|
{
|
|
int i;
|
|
float curve[CURVE_SIZE];
|
|
float awk1[ORDER], awk2[ORDER];
|
|
for (i=0;i<CURVE_SIZE;i++)
|
|
scanf("%f ", &curve[i]);
|
|
for (i=0;i<CURVE_SIZE;i++)
|
|
curve[i] = pow(10.f, .1*curve[i]);
|
|
curve_to_lpc(curve, CURVE_SIZE, awk1, awk2, ORDER);
|
|
for (i=0;i<ORDER;i++)
|
|
printf("%f ", awk1[i]);
|
|
printf ("\n");
|
|
for (i=0;i<ORDER;i++)
|
|
printf("%f ", awk2[i]);
|
|
printf ("\n");
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
#endif
|