merge new MTDM code from Fons' latest release

iodelay.c

@@ -28,23 +28,28 @@ struct Freq
 {
     int   p;
     int   f;
-    float a;
     float xa;
     float ya;
-    float xf;
-    float yf;
+    float x1;
+    float y1;
+    float x2;
+    float y2;
 };
 
+
 struct MTDM
 {
     double  _del;
     double  _err;
+    float   _wlp;
     int     _cnt;
     int     _inv;
-    struct Freq    _freq [5];
+
+    struct Freq _freq [13];
 };
 
-struct MTDM * mtdm_new (void)
+
+struct MTDM * mtdm_new (double fsamp)
 {
     int   i;
     struct Freq  *F;
@@ -57,23 +62,25 @@ struct MTDM * mtdm_new (void)
     retval->_cnt = 0;
     retval->_inv = 0;
 
-    retval->_freq [0].f = 4096;
-    retval->_freq [1].f =  512;
-    retval->_freq [2].f = 1088;
-    retval->_freq [3].f = 1544;
-    retval->_freq [4].f = 2049;
-
-    retval->_freq [0].a = 0.2f;
-    retval->_freq [1].a = 0.1f;
-    retval->_freq [2].a = 0.1f;
-    retval->_freq [3].a = 0.1f;
-    retval->_freq [4].a = 0.1f;
-
-    for (i = 0, F = retval->_freq; i < 5; i++, F++)
-    {
+    retval->_freq [0].f  = 4096;
+    retval->_freq [1].f  = 2048;
+    retval->_freq [2].f  = 3072;
+    retval->_freq [3].f  = 2560;
+    retval->_freq [4].f  = 2304;
+    retval->_freq [5].f  = 2176; 
+    retval->_freq [6].f  = 1088;
+    retval->_freq [7].f  = 1312;
+    retval->_freq [8].f  = 1552;
+    retval->_freq [9].f  = 1800;
+    retval->_freq [10].f = 3332;
+    retval->_freq [11].f = 3586;
+    retval->_freq [12].f = 3841;
+    retval->_wlp = 200.0f / fsamp;
+    for (i = 0, F = retval->_freq; i < 13; i++, F++) {
 	F->p = 128;
 	F->xa = F->ya = 0.0f;
-	F->xf = F->yf = 0.0f;
+	F->x1 = F->y1 = 0.0f;
+	F->x2 = F->y2 = 0.0f;
     }
 
     return retval;
@@ -89,23 +96,25 @@ int mtdm_process (struct MTDM *self, size_t len, float *ip, float *op)
     {
         vop = 0.0f;
 	vip = *ip++;
-	for (i = 0, F = self->_freq; i < 5; i++, F++)
+	for (i = 0, F = self->_freq; i < 13; i++, F++)
 	{
 	    a = 2 * (float) M_PI * (F->p & 65535) / 65536.0; 
 	    F->p += F->f;
 	    c =  cosf (a); 
 	    s = -sinf (a); 
-	    vop += F->a * s;
+	    vop += (i ? 0.01f : 0.20f) * s;
 	    F->xa += s * vip;
 	    F->ya += c * vip;
 	} 
 	*op++ = vop;
-	if (++(self->_cnt) == 16)
+	if (++self->_cnt == 16)
 	{
-	    for (i = 0, F = self->_freq; i < 5; i++, F++)
+	    for (i = 0, F = self->_freq; i < 13; i++, F++)
 	    {
-		F->xf += 1e-3f * (F->xa - F->xf + 1e-20);
-		F->yf += 1e-3f * (F->ya - F->yf + 1e-20);
+		F->x1 += self->_wlp * (F->xa - F->x1 + 1e-20);
+		F->y1 += self->_wlp * (F->ya - F->y1 + 1e-20);
+		F->x2 += self->_wlp * (F->x1 - F->x2 + 1e-20);
+		F->y2 += self->_wlp * (F->y1 - F->y2 + 1e-20);
 		F->xa = F->ya = 0.0f;
 	    }
             self->_cnt = 0;
@@ -119,28 +128,28 @@ int mtdm_resolve (struct MTDM *self)
 {
     int     i, k, m;
     double  d, e, f0, p;
-    struct Freq    *F = self->_freq;
+    struct Freq *F = self->_freq;
 
-    if (hypot (F->xf, F->yf) < 0.01) return -1;
-    d = atan2 (F->yf, F->xf) / (2 * M_PI);
-    if (self->_inv) d += 0.5f;
-    if (d > 0.5f) d -= 1.0f;
+    if (hypot (F->x2, F->y2) < 0.001) return -1;
+    d = atan2 (F->y2, F->x2) / (2 * M_PI);
+    if (self->_inv) d += 0.5;
+    if (d > 0.5) d -= 1.0;
     f0 = self->_freq [0].f;
     m = 1;
     self->_err = 0.0;
-    for (i = 0; i < 4; i++)
+    for (i = 0; i < 12; i++)
     {
 	F++;
-	p = atan2 (F->yf, F->xf) / (2 * M_PI) - d * F->f / f0;
-        if (self->_inv) p += 0.5f;
+	p = atan2 (F->y2, F->x2) / (2 * M_PI) - d * F->f / f0;
+        if (self->_inv) p += 0.5;
 	p -= floor (p);
-	p *= 8;
+	p *= 2;
 	k = (int)(floor (p + 0.5));
 	e = fabs (p - k);
         if (e > self->_err) self->_err = e;
         if (e > 0.4) return 1; 
-	d += m * (k & 7);
-	m *= 8;
+	d += m * (k & 1);
+	m *= 2;
     }  
     self->_del = 16 * d;
 
@@ -201,8 +210,6 @@ int main (int ac, char *av [])
     float          t;
     jack_status_t  s;
 
-    mtdm = mtdm_new();
-
     jack_handle = jack_client_open ("jack_delay", JackNoStartServer, &s);
     if (jack_handle == 0)
     {
@@ -210,6 +217,8 @@ int main (int ac, char *av [])
         exit (1);
     }
 
+    mtdm = mtdm_new(jack_get_sample_rate(jack_handle));
+
     jack_set_process_callback (jack_handle, jack_callback, 0);
 
     if (jack_set_latency_callback)