00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include <glib.h>
00015 #include <math.h>
00016 #include <string.h>
00017
00018 #include <libaudcore/audstrings.h>
00019 #include <libaudcore/hook.h>
00020
00021 #include "equalizer.h"
00022 #include "misc.h"
00023
00024 #define EQ_BANDS AUD_EQUALIZER_NBANDS
00025 #define MAX_CHANNELS 10
00026
00027
00028
00029 #define Q 1.2247449
00030
00031
00032
00033
00034
00035 static const gfloat CF[EQ_BANDS] = {31.25, 62.5, 125, 250, 500, 1000, 2000,
00036 4000, 8000, 16000};
00037
00038 static GStaticMutex mutex = G_STATIC_MUTEX_INIT;
00039 static gboolean active;
00040 static gint channels, rate;
00041 static gfloat a[EQ_BANDS][2];
00042 static gfloat b[EQ_BANDS][2];
00043 static gfloat wqv[MAX_CHANNELS][EQ_BANDS][2];
00044 static gfloat gv[MAX_CHANNELS][EQ_BANDS];
00045 static gint K;
00046
00047
00048 static void bp2 (gfloat * a, gfloat * b, gfloat fc, gfloat q)
00049 {
00050 gfloat th = 2 * M_PI * fc;
00051 gfloat C = (1 - tanf (th * q / 2)) / (1 + tanf (th * q / 2));
00052
00053 a[0] = (1 + C) * cosf (th);
00054 a[1] = -C;
00055 b[0] = (1 - C) / 2;
00056 b[1] = -1.005;
00057 }
00058
00059 void eq_set_format (gint new_channels, gint new_rate)
00060 {
00061 gint k;
00062
00063 g_static_mutex_lock (& mutex);
00064
00065 channels = new_channels;
00066 rate = new_rate;
00067
00068
00069 K = EQ_BANDS;
00070
00071 while (CF[K - 1] > (gfloat) rate / 2.2)
00072 K --;
00073
00074
00075 for (k = 0; k < K; k ++)
00076 bp2 (a[k], b[k], CF[k] / (gfloat) rate, Q);
00077
00078
00079 memset (wqv[0][0], 0, sizeof wqv);
00080
00081 g_static_mutex_unlock (& mutex);
00082 }
00083
00084 static void eq_set_bands_real (gdouble preamp, gdouble * values)
00085 {
00086 gfloat adj[EQ_BANDS];
00087 for (gint i = 0; i < EQ_BANDS; i ++)
00088 adj[i] = preamp + values[i];
00089
00090 for (gint c = 0; c < MAX_CHANNELS; c ++)
00091 for (gint i = 0; i < EQ_BANDS; i ++)
00092 gv[c][i] = pow (10, adj[i] / 20) - 1;
00093 }
00094
00095 void eq_filter (gfloat * data, gint samples)
00096 {
00097 gint channel;
00098
00099 g_static_mutex_lock (& mutex);
00100
00101 if (! active)
00102 {
00103 g_static_mutex_unlock (& mutex);
00104 return;
00105 }
00106
00107 for (channel = 0; channel < channels; channel ++)
00108 {
00109 gfloat * g = gv[channel];
00110 gfloat * end = data + samples;
00111 gfloat * f;
00112
00113 for (f = data + channel; f < end; f += channels)
00114 {
00115 gint k;
00116 gfloat yt = * f;
00117
00118 for (k = 0; k < K; k ++)
00119 {
00120
00121 gfloat * wq = wqv[channel][k];
00122
00123 gfloat w = yt * b[k][0] + wq[0] * a[k][0] + wq[1] * a[k][1];
00124
00125
00126 yt += (w + wq[1] * b[k][1]) * g[k];
00127
00128
00129 wq[1] = wq[0];
00130 wq[0] = w;
00131 }
00132
00133
00134 * f = yt;
00135 }
00136 }
00137
00138 g_static_mutex_unlock (& mutex);
00139 }
00140
00141 static void eq_update (void * data, void * user)
00142 {
00143 g_static_mutex_lock (& mutex);
00144
00145 active = get_bool (NULL, "equalizer_active");
00146
00147 gdouble values[EQ_BANDS];
00148 eq_get_bands (values);
00149 eq_set_bands_real (get_double (NULL, "equalizer_preamp"), values);
00150
00151 g_static_mutex_unlock (& mutex);
00152 }
00153
00154 void eq_init (void)
00155 {
00156 eq_update (NULL, NULL);
00157 hook_associate ("set equalizer_active", eq_update, NULL);
00158 hook_associate ("set equalizer_preamp", eq_update, NULL);
00159 hook_associate ("set equalizer_bands", eq_update, NULL);
00160 }
00161
00162 void eq_set_bands (const gdouble * values)
00163 {
00164 gchar * string = double_array_to_string (values, EQ_BANDS);
00165 g_return_if_fail (string);
00166 set_string (NULL, "equalizer_bands", string);
00167 g_free (string);
00168 }
00169
00170 void eq_get_bands (gdouble * values)
00171 {
00172 memset (values, 0, sizeof (gdouble) * EQ_BANDS);
00173 gchar * string = get_string (NULL, "equalizer_bands");
00174 string_to_double_array (string, values, EQ_BANDS);
00175 g_free (string);
00176 }
00177
00178 void eq_set_band (gint band, gdouble value)
00179 {
00180 g_return_if_fail (band >= 0 && band < EQ_BANDS);
00181 gdouble values[EQ_BANDS];
00182 eq_get_bands (values);
00183 values[band] = value;
00184 eq_set_bands (values);
00185 }
00186
00187 gdouble eq_get_band (gint band)
00188 {
00189 g_return_val_if_fail (band >= 0 && band < EQ_BANDS, 0);
00190 gdouble values[EQ_BANDS];
00191 eq_get_bands (values);
00192 return values[band];
00193 }