new MTDM implementation from Fons' latest jack_delay implementation

git-svn-id: svn://localhost/ardour2/branches/3.0@13394 d708f5d6-7413-0410-9779-e7cbd77b26cf

1 files changed, 92 insertions, 101 deletions

diff --git a/libs/ardour/mtdm.cc b/libs/ardour/mtdm.cc
index 4c27b3b600..940f0b8c25 100644
--- a/libs/ardour/mtdm.cc
+++ b/libs/ardour/mtdm.cc
@@ -1,5 +1,5 @@
 /*
-    Copyright (C) 2003-2008 Fons Adriaensen <fons@kokkinizita.net>
+    Copyright (C) 2003-2012 Fons Adriaensen <fons@kokkinizita.net>
 
     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
@@ -20,115 +20,106 @@
 
 #include "ardour/mtdm.h"
 
-MTDM::MTDM ()
-	: _cnt (0)
-	, _inv (0)
+MTDM::MTDM (int fsamp) 
+  : _cnt (0)
+  , _inv (0)
 {
-	int   i;
-	Freq  *F;
-
-	_freq [0].f = 4096;
-	_freq [1].f =  512;
-	_freq [2].f = 1088;
-	_freq [3].f = 1544;
-	_freq [4].f = 2049;
-
-	_freq [0].a = 0.2f;
-	_freq [1].a = 0.1f;
-	_freq [2].a = 0.1f;
-	_freq [3].a = 0.1f;
-	_freq [4].a = 0.1f;
-
-	for (i = 0, F = _freq; i < 5; i++, F++) {
-		F->p = 128;
-		F->xa = F->ya = 0.0f;
-		F->xf = F->yf = 0.0f;
-	}
+    int   i;
+    Freq  *F;
+
+    _freq [0].f  = 4096;
+    _freq [1].f  = 2048;
+    _freq [2].f  = 3072;
+    _freq [3].f  = 2560;
+    _freq [4].f  = 2304;
+    _freq [5].f  = 2176; 
+    _freq [6].f  = 1088;
+    _freq [7].f  = 1312;
+    _freq [8].f  = 1552;
+    _freq [9].f  = 1800;
+    _freq [10].f = 3332;
+    _freq [11].f = 3586;
+    _freq [12].f = 3841;
+    _wlp = 200.0f / fsamp;
+    for (i = 0, F = _freq; i < 13; i++, F++)
+    {
+	F->p = 128;
+	F->xa = F->ya = 0.0f;
+	F->x1 = F->y1 = 0.0f;
+	F->x2 = F->y2 = 0.0f;
+    }
 }
 
-int
-MTDM::process (size_t len, float *ip, float *op)
-{
-	int    i;
-	float  vip, vop, a, c, s;
-	Freq   *F;
 
-	while (len--)
+int MTDM::process (size_t len, float *ip, float *op)
+{
+    int    i;
+    float  vip, vop, a, c, s;
+    Freq   *F;
+
+    while (len--)
+    {
+        vop = 0.0f;
+	vip = *ip++;
+	for (i = 0, F = _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 += (i ? 0.01f : 0.20f) * s;
+	    F->xa += s * vip;
+	    F->ya += c * vip;
+	} 
+	*op++ = vop;
+	if (++_cnt == 16)
 	{
-		vop = 0.0f;
-		vip = *ip++;
-		for (i = 0, F = _freq; i < 5; 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;
-			F->xa += s * vip;
-			F->ya += c * vip;
-		}
-		*op++ = vop;
-		if (++_cnt == 16)
-		{
-			for (i = 0, F = _freq; i < 5; i++, F++)
-			{
-				F->xf += 1e-3f * (F->xa - F->xf + 1e-20);
-				F->yf += 1e-3f * (F->ya - F->yf + 1e-20);
-				F->xa = F->ya = 0.0f;
-			}
-			_cnt = 0;
-		}
+	    for (i = 0, F = _freq; i < 13; i++, F++)
+	    {
+		F->x1 += _wlp * (F->xa - F->x1 + 1e-20);
+		F->y1 += _wlp * (F->ya - F->y1 + 1e-20);
+		F->x2 += _wlp * (F->x1 - F->x2 + 1e-20);
+		F->y2 += _wlp * (F->y1 - F->y2 + 1e-20);
+		F->xa = F->ya = 0.0f;
+	    }
+            _cnt = 0;
 	}
+    }
 
-	return 0;
+    return 0;
 }
 
-int
-MTDM::resolve ()
-{
-	int     i, k, m;
-	double  d, e, f0, p;
-	Freq    *F = _freq;
-
-	if (hypot (F->xf, F->yf) < 0.01) {
-		return -1;
-	}
-
-	d = atan2 (F->yf, F->xf) / (2 * M_PI);
-
-	if (_inv) {
-		d += 0.5f;
-	}
-
-	if (d > 0.5f) {
-		d -= 1.0f;
-	}
 
-	f0 = _freq [0].f;
-	m = 1;
-	_err = 0.0;
-
-	for (i = 0; i < 4; i++) {
-		F++;
-		p = atan2 (F->yf, F->xf) / (2 * M_PI) - d * F->f / f0;
-		if (_inv) {
-			p += 0.5f;
-		}
-		p -= floor (p);
-		p *= 8;
-		k = (int)(floor (p + 0.5));
-		e = fabs (p - k);
-		if (e > _err) {
-			_err = e;
-		}
-		if (e > 0.4) {
-			return 1;
-		}
-		d += m * (k & 7);
-		m *= 8;
-	}
+int MTDM::resolve (void)
+{
+    int     i, k, m;
+    double  d, e, f0, p;
+    Freq    *F = _freq;
+
+    if (hypot (F->x2, F->y2) < 0.001) return -1;
+    d = atan2 (F->y2, F->x2) / (2 * M_PI);
+    if (_inv) d += 0.5;
+    if (d > 0.5) d -= 1.0;
+    f0 = _freq [0].f;
+    m = 1;
+    _err = 0.0;
+    for (i = 0; i < 12; i++)
+    {
+	F++;
+	p = atan2 (F->y2, F->x2) / (2 * M_PI) - d * F->f / f0;
+        if (_inv) p += 0.5;
+	p -= floor (p);
+	p *= 2;
+	k = (int)(floor (p + 0.5));
+	e = fabs (p - k);
+        if (e > _err) _err = e;
+        if (e > 0.4) return 1; 
+	d += m * (k & 1);
+	m *= 2;
+    }  
+    _del = 16 * d;
+
+    return 0;
+}
 
-	_del = 16 * d;
 
-	return 0;
-}