1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
|
/* Copyright (c) 1998 The Regents of the University of California. The
contents of this file are free for any use, but BOTH THE AUTHOR AND UCSD
DISCLAIM ALL WARRANTIES related to it. Although not written in Java, this
still should not be used to control any machinery containing a sharp blade or
combustible materiel, or as part of any life support system or weapon. */
#include "m_pd.h"
#include <math.h>
static t_class *pique_class;
typedef struct _pique
{
t_object x_obj;
} t_pique;
static void *pique_new(void)
{
t_pique *x = (t_pique *)pd_new(pique_class);
return (x);
}
/* we aren't measuring phase yet */
static void pique_doit(int npts, t_float *fpreal, t_float *fpimag,
int npeak, t_float *fpfreq, t_float *fpamp)
{
float srate = sys_getsr(); /* not sure how to get this correctly */
float oneovern = 1.0/ (float)npts;
float fperbin = srate * oneovern;
float pow1, pow2 = 0, pow3 = 0, pow4 = 0, pow5 = 0;
float re1, re2 = 0, re3 = *fpreal;
float im1, im2 = 0, im3 = 0, powthresh;
int count, peakcount = 0, n2 = (npts >> 1);
float *fp1, *fp2;
for (count = n2, fp1 = fpreal, fp2 = fpimag, powthresh = 0;
count--; fp1++, fp2++)
powthresh += (*fp1) * (*fp1) + (*fp2) * (*fp2) ;
powthresh *= 0.00001;
for (count = 1; count < n2; count++)
{
float windreal, windimag, pi = 3.14159;
float detune, pidetune, ampcorrect, freqout, ampout;
float rpeak, rpeaknext, rpeakprev;
float ipeak, ipeaknext, ipeakprev;
fpreal++;
fpimag++;
re1 = re2;
re2 = re3;
re3 = *fpreal;
im1 = im2;
im2 = im3;
im3 = *fpimag;
if (count < 2) continue;
pow1 = pow2;
pow2 = pow3;
pow3 = pow4;
pow4 = pow5;
/* get Hanning-windowed spectrum by convolution */
windreal = re2 - 0.5 * (re1 + re3);
windimag = im2 - 0.5 * (im1 + im3);
pow5 = windreal * windreal + windimag * windimag;
/* if (count < 30) post("power %f", pow5); */
if (count < 5) continue;
/* check for a peak. The actual bin is count-3. */
if (pow3 <= pow2 || pow3 <= pow4 || pow3 <= pow1 || pow3 <= pow5
|| pow3 < powthresh)
continue;
/* go back for the raw FFT values around the peak. */
rpeak = fpreal[-3];
rpeaknext = fpreal[-2];
rpeakprev = fpreal[-4];
ipeak = fpimag[-3];
ipeaknext = fpimag[-2];
ipeakprev = fpimag[-4];
detune = ((rpeakprev - rpeaknext) *
(2.0 * rpeak - rpeakprev - rpeaknext) +
(ipeakprev - ipeaknext) *
(2.0 * ipeak - ipeakprev - ipeaknext)) /
(4.0 * pow3);
/* if (count < 30) post("detune %f", detune); */
if (detune > 0.7 || detune < -0.7) continue;
/* the frequency is the sum of the bin frequency and detuning */
freqout = fperbin * ((float)(count-3) + detune);
*fpfreq++ = freqout;
pidetune = pi * detune;
if (pidetune < 0.01 && pidetune > -0.01) ampcorrect = 1;
else ampcorrect = 1.0 / (sin(pidetune)/pidetune + 0.5 *
(sin(pidetune + pi)/(pidetune+pi) +
sin(pidetune-pi)/(pidetune-pi)));
/* amplitude is peak height, corrected for Hanning window shape.
Multiply by 2 to get real-sinusoid peak amplitude */
ampout = 2. * oneovern * sqrt(pow3) * ampcorrect;
*fpamp++ = ampout;
/* post("peak %f %f", freqout, ampout); */
if (++peakcount == npeak) break;
}
while (peakcount < npeak)
{
*fpfreq++ = 0;
*fpamp++ = 0;
peakcount++;
}
}
static void pique_list(t_pique *x, t_symbol *s, int argc, t_atom *argv)
{
int npts = atom_getintarg(0, argc, argv);
t_symbol *symreal = atom_getsymbolarg(1, argc, argv);
t_symbol *symimag = atom_getsymbolarg(2, argc, argv);
int npeak = atom_getintarg(3, argc, argv);
t_symbol *symfreq = atom_getsymbolarg(4, argc, argv);
t_symbol *symamp = atom_getsymbolarg(5, argc, argv);
t_garray *a, *afreq, *aamp;
int n;
t_float *fpreal, *fpimag, *fpfreq, *fpamp, *fpphase;
if (npts < 8 || npeak < 1) error("pique: bad npoints or npeak");
else if (!(a = (t_garray *)pd_findbyclass(symreal, garray_class)) ||
!garray_getfloatarray(a, &n, &fpreal) ||
n < npts)
error("%s: missing or bad array", symreal->s_name);
else if (!(a = (t_garray *)pd_findbyclass(symimag, garray_class)) ||
!garray_getfloatarray(a, &n, &fpimag) ||
n < npts)
error("%s: missing or bad array", symimag->s_name);
else if (!(afreq = (t_garray *)pd_findbyclass(symfreq, garray_class)) ||
!garray_getfloatarray(afreq, &n, &fpfreq) ||
n < npeak)
error("%s: missing or bad array", symfreq->s_name);
else if (!(aamp = (t_garray *)pd_findbyclass(symamp, garray_class)) ||
!garray_getfloatarray(aamp, &n, &fpamp) ||
n < npeak)
error("%s: missing or bad array", symamp->s_name);
else
{
pique_doit(npts, fpreal, fpimag, npeak, fpfreq, fpamp);
garray_redraw(afreq);
garray_redraw(aamp);
}
}
void pique_setup(void)
{
pique_class = class_new(gensym("pique"), pique_new, 0,
sizeof(t_pique), 0, 0);
class_addlist(pique_class, pique_list);
}
|
