making codec draft more compliant with IETF submission rules
[opus.git] / libcelt / psy.c
index e89f41b..e2c8f44 100644 (file)
@@ -37,6 +37,8 @@
 #include <math.h>
 #include "os_support.h"
 #include "arch.h"
+#include "stack_alloc.h"
+#include "mathops.h"
 
 /* The Vorbis freq<->Bark mapping */
 #define toBARK(n)   (13.1f*atan(.00074f*(n))+2.24f*atan((n)*(n)*1.85e-8f)+1e-4f*(n))
@@ -78,7 +80,7 @@ void psydecay_clear(struct PsyDecay *decay)
 }
 #endif
 
-static void spreading_func(const struct PsyDecay *d, celt_word32_t *psd, celt_mask_t *mask, int len)
+static void spreading_func(const struct PsyDecay *d, celt_word32_t * restrict psd, int len)
 {
    int i;
    celt_word32_t mem;
@@ -87,22 +89,20 @@ static void spreading_func(const struct PsyDecay *d, celt_word32_t *psd, celt_ma
    mem=psd[0];
    for (i=0;i<len;i++)
    {
-      mask[i] = MULT16_32_Q15(Q15ONE-d->decayR[i],psd[i]) + MULT16_32_Q15(d->decayR[i],mem);
-      if (mask[i]<1)
-         mask[i]=1;
-      mem = mask[i];
+      /* psd = (1-decay)*psd + decay*mem */
+      psd[i] = EPSILON + psd[i] + MULT16_32_Q15(d->decayR[i],mem-psd[i]);
+      mem = psd[i];
    }
    /* Compute left slope (-25 dB/Bark) */
-   mem=mask[len-1];
+   mem=psd[len-1];
    for (i=len-1;i>=0;i--)
    {
       /* Left side has around twice the slope as the right side, so we just
          square the coef instead of storing two sets of decay coefs */
       celt_word16_t decayL = MULT16_16_Q15(d->decayR[i], d->decayR[i]);
-      mask[i] = MULT16_32_Q15(Q15ONE-decayL,mask[i]) + MULT16_32_Q15(decayL,mem);
-      if (mask[i]<1)
-         mask[i]=1;
-      mem = mask[i];
+      /* psd = (1-decay)*psd + decay*mem */
+      psd[i] = EPSILON + psd[i] + MULT16_32_Q15(decayL,mem-psd[i]);
+      mem = psd[i];
    }
    /*for (i=0;i<len;i++) printf ("%f ", mask[i]); printf ("\n");*/
 #if 0 /* Prints signal and mask energy per critical band */
@@ -131,7 +131,7 @@ static void spreading_func(const struct PsyDecay *d, celt_word32_t *psd, celt_ma
 }
 
 /* Compute a marking threshold from the spectrum X. */
-void compute_masking(const struct PsyDecay *decay, celt_word16_t *X, celt_mask_t *mask, int len)
+void compute_masking(const struct PsyDecay *decay, celt_word16_t *X, celt_mask_t * restrict mask, int len)
 {
    int i;
    int N;
@@ -141,26 +141,71 @@ void compute_masking(const struct PsyDecay *decay, celt_word16_t *X, celt_mask_t
       mask[i] = ADD32(MULT16_16(X[i*2], X[i*2]), MULT16_16(X[i*2+1], X[i*2+1]));
    /* TODO: Do tone masking */
    /* Noise masking */
-   spreading_func(decay, mask, mask, N);
+   spreading_func(decay, mask, N);
 }
 
-#if 0 /* Not needed for now, but will be useful in the future */
-void compute_mdct_masking(const struct PsyDecay *decay, celt_word32_t *X, celt_mask_t *mask, int len)
+#ifdef EXP_PSY /* Not needed for now, but will be useful in the future */
+void compute_mdct_masking(const struct PsyDecay *decay, celt_word32_t *X, celt_word16_t *tonality, celt_word16_t *long_window, celt_mask_t *mask, int len)
 {
    int i;
-   VARDECL(float *psd);
+   VARDECL(floatpsd);
    SAVE_STACK;
    ALLOC(psd, len, float);
    for (i=0;i<len;i++)
-      mask[i] = X[i]*X[i];
+      psd[i] = X[i]*X[i]*tonality[i];
    for (i=1;i<len-1;i++)
-      psd[i] = .5*mask[i] + .25*(mask[i-1]+mask[i+1]);
+      mask[i] = .5*psd[i] + .25*(psd[i-1]+psd[i+1]);
    /*psd[0] = .5*mask[0]+.25*(mask[1]+mask[2]);*/
-   psd[0] = .5*mask[0]+.5*mask[1];
-   psd[len-1] = .5*(mask[len-1]+mask[len-2]);
+   mask[0] = .5*psd[0]+.5*psd[1];
+   mask[len-1] = .5*(psd[len-1]+psd[len-2]);
    /* TODO: Do tone masking */
    /* Noise masking */
-   spreading_func(decay, psd, mask, len);
+   spreading_func(decay, mask, len);
    RESTORE_STACK;  
 }
+
+void compute_tonality(const CELTMode *m, celt_word16_t * restrict X, celt_word16_t * mem, int len, celt_word16_t *tonality, int mdct_size)
+{
+   int i;
+   celt_word16_t norm_1;
+   celt_word16_t *mem2;
+   int N = len>>2;
+
+   mem2 = mem+2*N;
+   X[0] = 0;
+   X[1] = 0;
+   tonality[0] = 1;
+   for (i=1;i<N;i++)
+   {
+      celt_word16_t re, im, re2, im2;
+      re = X[2*i];
+      im = X[2*i+1];
+      /* Normalise spectrum */
+      norm_1 = celt_rsqrt(.01+MAC16_16(MULT16_16(re,re), im,im));
+      re = MULT16_16(re, norm_1);
+      im = MULT16_16(im, norm_1);
+      /* Phase derivative */
+      re2 = re*mem[2*i] + im*mem[2*i+1];
+      im2 = im*mem[2*i] - re*mem[2*i+1];
+      mem[2*i] = re;
+      mem[2*i+1] = im;
+      /* Phase second derivative */
+      re = re2*mem2[2*i] + im2*mem2[2*i+1];
+      im = im2*mem2[2*i] - re2*mem2[2*i+1];
+      mem2[2*i] = re2;
+      mem2[2*i+1] = im2;
+      /*printf ("%f ", re);*/
+      X[2*i] = re;
+      X[2*i+1] = im;
+   }
+   /*printf ("\n");*/
+   for (i=0;i<mdct_size;i++)
+   {
+      tonality[i] = 1.0-X[2*i]*X[2*i]*X[2*i];
+      if (tonality[i]>1)
+         tonality[i] = 1;
+      if (tonality[i]<.02)
+         tonality[i]=.02;
+   }
+}
 #endif