1 /*
2 * audiocapture.c
3 * capture audio data using portaudio library api
4 * analyse data to determine frequency of a note close to a target note.
5 * Hacked from patest_record.c
6 * Record input into an array.
7 * Save array to a file.
8 * Playback recorded data.
9 *
10 * Author: Phil Burk http://www.softsynth.com
11 *
12 * This program uses the PortAudio Portable Audio Library.
13 * For more information see: http://www.portaudio.com
14 * Copyright (c) 1999-2000 Ross Bencina and Phil Burk
15 *
16 * Permission is hereby granted, free of charge, to any person obtaining
17 * a copy of this software and associated documentation files
18 * (the "Software"), to deal in the Software without restriction,
19 * including without limitation the rights to use, copy, modify, merge,
20 * publish, distribute, sublicense, and/or sell copies of the Software,
21 * and to permit persons to whom the Software is furnished to do so,
22 * subject to the following conditions:
23 *
24 * The above copyright notice and this permission notice shall be
25 * included in all copies or substantial portions of the Software.
26 *
27 * Any person wishing to distribute modifications to the Software is
28 * requested to send the modifications to the original developer so that
29 * they can be incorporated into the canonical version.
30 *
31 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
32 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
33 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
34 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR
35 * ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
36 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
37 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
38 *
39 */
40 #ifdef _HAVE_PORTAUDIO_
41 #include <stdio.h>
42 #include <stdlib.h>
43 #include <math.h>
44 #include <portaudio.h>
45 #include <glib.h>
46 #include "audio/audiocapture.h"
47 #include "audio/audio.h"
48 #ifndef paNonInterleaved
49 #undef PA_VERSION_19
50 #else
51 #define PA_VERSION_19
52 #endif
53
54 // #define SAMPLE_RATE (17932) // Test failure to open with this value.
55 #define SAMPLE_RATE DENEMO_SAMPLE_RATE
56 #define NUM_SECONDS (10)
57
58
59 #define PA_SAMPLE_TYPE paFloat32 /*paUInt8 */
60 typedef DENEMO_SAMPLE_TYPE SAMPLE;
61
62
63 static AubioCallback *aubio_routine;
64 typedef struct
65 {
66 int frameIndex; /* Index into sample array. */
67 int maxFrameIndex;
68 int samplesPerFrame;
69 SAMPLE *recordedSamples;
70 } paTestData;
71
72 typedef struct
73 {
74 int frameIndex; /* Index into sample array. */
75 int maxFrameIndex;
76 double pitch;
77 double volume;
78 int channel;
79 SAMPLE *recordedSamples;
80 } OutData;
81
82
83 static paTestData data;
84 static OutData out_data;
85 static int tuning = 0; /* copy data for instrument tuning routines */
86
87
88
89
90
91 #define TABLE_SIZE (20000)
92 #ifndef PA_VERSION_19
93 static PortAudioStream *out_stream = NULL;
94 #define StreamActive Pa_StreamActive
95 #else
96 static PaStream *out_stream = NULL;
97 PaStreamParameters inputParameters, outputParameters;
98 #define StreamActive Pa_IsStreamActive
99 #endif
100
101 static int
init_audio_out(void)102 init_audio_out (void)
103 {
104 PaError err;
105 int i;
106 err = Pa_Initialize ();
107 if (err != paNoError)
108 {
109 g_warning ("Error initializing portaudio library");
110 return 0;
111 }
112 out_data.frameIndex = 0;
113 out_data.recordedSamples = (SAMPLE *) malloc (TABLE_SIZE * sizeof (SAMPLE));
114 if (out_data.recordedSamples == NULL)
115 {
116 printf ("Could not allocate record array.\n");
117 return 0;
118 }
119 for (i = 0; i < TABLE_SIZE; i++)
120 out_data.recordedSamples[i] = sin (2.0 * M_PI * i / (double) TABLE_SIZE);
121 play_pitch (440.0, 0.0, 0.0, 0); /* to start the stream */
122 return 1;
123 }
124
125 /* This routine will be called by the PortAudio engine when audio is needed.
126 ** It may be called at interrupt level on some machines so don't do anything
127 ** that could mess up the system like calling malloc() or free().
128 */
129 static int
playCallback(void * inputBuffer,void * outputBuffer,unsigned long framesPerBuffer,PaTimestamp outTime,void * userData)130 playCallback (void *inputBuffer, void *outputBuffer, unsigned long framesPerBuffer,
131 #ifndef PA_VERSION_19
132 PaTimestamp outTime,
133 #else
134 PaStreamCallbackTimeInfo * outTime, PaStreamCallbackFlags status,
135 #endif
136 void *userData)
137 {
138
139 SAMPLE *wptr = (SAMPLE *) outputBuffer;
140 unsigned int i;
141
142 (void) inputBuffer; /* Prevent unused variable warnings. */
143 (void) outTime;
144 for (i = 0; i < framesPerBuffer; i++)
145 {
146 if (out_data.frameIndex >= out_data.maxFrameIndex)
147 wptr[i] = 0;
148 else
149 {
150 wptr[i] = out_data.volume * out_data.recordedSamples[out_data.frameIndex % TABLE_SIZE];
151 if (out_data.channel && wptr[i] < 0)
152 wptr[i] = -wptr[i]; //distort sound for other channels
153
154 out_data.frameIndex += TABLE_SIZE * out_data.pitch / SAMPLE_RATE;
155 }
156 }
157 return 0;
158 // return out_data.frameIndex >= out_data.maxFrameIndex;
159 }
160
161 void
play_pitch(double pitch,double duration,double volume,int channel)162 play_pitch (double pitch, double duration, double volume, int channel)
163 {
164 //g_debug("playing");
165 if (out_data.recordedSamples == NULL && !init_audio_out ())
166 {
167 fprintf (stderr, "Could not initialize audio out\n");
168 return;
169 } //else
170 //g_debug("already initialized");
171 if (out_stream && StreamActive (out_stream))
172 {
173 out_data.maxFrameIndex = duration * TABLE_SIZE * pitch /*SAMPLE_RATE */ ;
174 out_data.pitch = pitch;
175 out_data.volume = volume;
176 out_data.channel = channel;
177 out_data.frameIndex = 0;
178 return;
179 }
180 //g_debug("starting stream ...");
181
182 #ifdef PA_VERSION_19
183 outputParameters.device = Pa_GetDefaultOutputDevice (); /* default output device */
184 if (outputParameters.device == paNoDevice)
185 {
186 g_critical ("Error: No default output device.");
187 return;
188 }
189 outputParameters.channelCount = 1; /* mono output */
190 outputParameters.sampleFormat = PA_SAMPLE_TYPE;
191 outputParameters.suggestedLatency = Pa_GetDeviceInfo (outputParameters.device)->defaultLowInputLatency;
192 outputParameters.hostApiSpecificStreamInfo = NULL;
193 #endif
194
195
196 out_data.maxFrameIndex = duration * SAMPLE_RATE;
197 out_data.pitch = pitch;
198 PaError err;
199 out_data.frameIndex = 0;
200 out_stream = NULL;
201 err = Pa_OpenStream (&out_stream,
202 #ifndef PA_VERSION_19
203 paNoDevice, 0, /* NO input */
204 PA_SAMPLE_TYPE, NULL, Pa_GetDefaultOutputDeviceID (), 1, /* mono output */
205 PA_SAMPLE_TYPE, NULL, SAMPLE_RATE, 1024, /* frames per buffer */
206 0, /* number of buffers, if zero then use default minimum */
207 paClipOff, /* we won't output out of range samples so don't bother clipping them */
208 playCallback, &out_data);
209 #else
210 NULL, /* input parameters */
211 &outputParameters, SAMPLE_RATE, 1024, /* frames per buffer */
212 paClipOff, /* we won't output out of range samples so don't bother clipping them */
213 (PaStreamCallback *) playCallback, &out_data);
214 #endif
215 if (err != paNoError)
216 {
217 g_critical ("Error opening stream");
218 return;
219 }
220 if (out_stream)
221 err = Pa_StartStream (out_stream);
222 if (err != paNoError)
223 {
224 g_critical ("Error starting stream");
225 out_stream = NULL;
226 return;
227 }
228 }
229
230 void
stop_audio(void)231 stop_audio (void)
232 {
233 if (out_stream != NULL)
234 Pa_CloseStream (out_stream);
235 out_stream = NULL;
236 }
237
238 /* This routine will be called by the PortAudio engine when audio is needed.
239 ** It may be called at interrupt level on some machines so don't do anything
240 ** that could mess up the system like calling malloc() or free().
241 */
242
243
244 static int
recordCallback(const void * inputBuffer,const void * outputBuffer,unsigned long framesPerBuffer,PaTimestamp outTime,void * userData)245 recordCallback (const void *inputBuffer, const void *outputBuffer, unsigned long framesPerBuffer,
246 #ifndef PA_VERSION_19
247 PaTimestamp outTime,
248 #else
249 PaStreamCallbackTimeInfo * outTime, PaStreamCallbackFlags status,
250 #endif
251 void *userData)
252 {
253
254 SAMPLE *rptr = (SAMPLE *) inputBuffer;
255
256 /* copy data to data.recordedSamples */
257 if (tuning)
258 {
259 float *wptr = &data.recordedSamples[data.frameIndex * data.samplesPerFrame];
260 long framesToCalc;
261 long i;
262 unsigned long framesLeft = data.maxFrameIndex - data.frameIndex;
263
264 (void) outputBuffer; /* Prevent unused variable warnings. */
265 (void) outTime;
266
267 if (framesLeft < framesPerBuffer)
268 {
269 framesToCalc = framesLeft;
270 }
271 else
272 {
273 framesToCalc = framesPerBuffer;
274 }
275 if (inputBuffer == NULL)
276 {
277 for (i = 0; i < framesToCalc; i++)
278 {
279 *wptr++ = 0; /* left */
280 }
281 }
282 else
283 {
284 for (i = 0; i < framesToCalc; i++)
285 {
286 *wptr++ = *rptr++; /* left */
287 }
288 }
289 data.frameIndex += framesToCalc;
290 }
291 rptr = (SAMPLE *) inputBuffer;
292 return aubio_routine (&rptr, NULL, framesPerBuffer);
293 }
294
295
296 int
collect_data_for_tuning(int ok)297 collect_data_for_tuning (int ok)
298 {
299 tuning = ok;
300 return 1;
301 }
302
303 /******************************************************************
304 with param FN non null, start audio capture with FN as callback
305 else shutdown audio capture.
306 return 0 for success
307 */
308
309 int
pa_main(AubioCallback * fn)310 pa_main (AubioCallback * fn)
311 {
312 #ifndef PA_VERSION_19
313 static PortAudioStream *stream;
314 #else
315 static PaStream *stream;
316 PaStreamParameters inputParameters;
317 #endif
318 PaError err = -1;
319 static int last_tried;
320 int totalFrames;
321 int numSamples;
322 int numBytes;
323
324 if ((fn == NULL) && (stream))
325 {
326 err = Pa_StopStream (stream);
327 if (err != paNoError)
328 goto error;
329 err = Pa_CloseStream (stream);
330 if (err != paNoError)
331 goto error;
332 Pa_Terminate ();
333 return 0;
334 }
335 if (fn == NULL)
336 return -1;
337 aubio_routine = fn;
338 err = Pa_Initialize ();
339 if (err != paNoError)
340 goto error;
341 data.maxFrameIndex = totalFrames = NUM_SECONDS * SAMPLE_RATE; /* Record for a few seconds. */
342 data.frameIndex = 0;
343 data.samplesPerFrame = 1; /*mono */
344 numSamples = totalFrames * data.samplesPerFrame;
345
346 numBytes = numSamples * sizeof (float);
347 data.recordedSamples = (float *) malloc (numBytes); //FIXME multiple inits, memory leak...
348
349 #ifdef PA_VERSION_19
350 inputParameters.device = Pa_GetDefaultInputDevice (); /* default input device */
351 if (inputParameters.device == paNoDevice)
352 {
353
354 //g_debug("Number of devices %d now trying = %d\n", Pa_GetDeviceCount(), last_tried);
355 inputParameters.device = last_tried++; // guess
356 }
357 inputParameters.channelCount = 1; /* mono input */
358 inputParameters.sampleFormat = PA_SAMPLE_TYPE;
359 const PaDeviceInfo *info = Pa_GetDeviceInfo (inputParameters.device);
360 if (info)
361 inputParameters.suggestedLatency = info->defaultLowInputLatency;
362 else
363 goto error;
364 inputParameters.hostApiSpecificStreamInfo = NULL;
365 #endif
366
367
368
369 /* Record some audio. -------------------------------------------- */
370 err = Pa_OpenStream (&stream,
371 #ifndef PA_VERSION_19
372 Pa_GetDefaultInputDeviceID (), 1, /* mono input */
373 PA_SAMPLE_TYPE, NULL, paNoDevice, 0, PA_SAMPLE_TYPE, NULL, SAMPLE_RATE, 1024, /* frames per buffer */
374 0, /* number of buffers, if zero then use default minimum */
375 paClipOff, /* we won't output out of range samples so don't bother clipping them */
376 recordCallback, NULL);
377 #else
378 &inputParameters, NULL, /* output parameters */
379 SAMPLE_RATE, 1024, /* frames per buffer */
380 paClipOff, /* we won't output out of range samples so don't bother clipping them */
381 (PaStreamCallback *) recordCallback, NULL);
382 #endif
383 if (err != paNoError)
384 goto error;
385
386 err = Pa_StartStream (stream);
387 if (err != paNoError)
388 goto error;
389 last_tried--;
390 printf ("Now recording!!\n");
391 fflush (stdout);
392 return 0;
393
394
395 error:
396 Pa_Terminate ();
397 fprintf (stderr, "An error occurred while using the portaudio stream\n");
398 fprintf (stderr, "Error number: %d\n", err);
399 fprintf (stderr, "Error message: %s\n", Pa_GetErrorText (err));
400 return -1;
401 }
402
403
404
405
406
407 /********************** code from accordeur ****************************/
408
409 static int Autocorrelation (float mData[], // In
410 int mDataLen, // In
411 // int mWindowSize, In
412 float *processed[] // Out
413 //, int* mProcessedSize Out
414 );
415
416 static float bestPeak2 (float *mProcessed, // IN
417 int mProcessedSize, // IN
418 float mRate // IN
419 );
420
421 static float Freq2Pitch (float freq);
422
423
424
425 #define OCTAVE 4
426 static unsigned long WindowSize = 16384 >> OCTAVE;
427
428
429 int Pitch = 69 /* 69 */ ;
430 /* 69 = 5 octaves *12 + 9th step = A, so A 440 */
431 #define MIN_DB (-96)
432 #define BACKGROUND_DB (-54) // This seemed to work okay
433 #define INF_DB (MIN_DB - 1)
434
435 static float
level2db(SAMPLE level)436 level2db (SAMPLE level)
437 {
438 const SAMPLE maxLevel = 1;
439 // wxASSERT((0 < level) && (level <= maxLevel));
440 return 20 * log10 (level / maxLevel);
441 }
442
443 float
PitchToFreq(float pitch)444 PitchToFreq (float pitch)
445 {
446 return 440.0 * pow (2, (pitch - 69.0) / 12.0);
447 }
448
449
450
451
452
453 /* void setTargetPitch(int pitch) { */
454 void
setTuningTarget(double note)455 setTuningTarget (double note)
456 {
457 int pitch = (int) (Freq2Pitch (note) + 0.5);
458 Pitch = pitch;
459 WindowSize = 16384 >> ((Pitch - 12) / 12);
460 if (WindowSize < 1024)
461 WindowSize = 1024;
462 }
463
464 static double PARTDERNIER = 0.25;
465 void
set_frequency_smoothing(double m)466 set_frequency_smoothing (double m)
467 {
468 PARTDERNIER = m;
469 fprintf (stderr, "smoothing %f\n", PARTDERNIER);
470 }
471
472 double
determine_frequency(void)473 determine_frequency (void)
474 {
475 static float *autocorr = 0;
476 static float *autocorr2;
477 /* initialize if needed */
478 if (autocorr == NULL)
479 {
480 autocorr = calloc (sizeof (float), 16384);
481 autocorr2 = calloc (sizeof (float), 16384);
482 }
483
484 // Analyze the audio
485 SAMPLE avg_abs, *average_abs;
486
487 int numSamples = data.frameIndex * data.samplesPerFrame;
488 /* Measure average absolute amplitude. */
489 SAMPLE val;
490 average_abs = &avg_abs;
491 *average_abs = 0;
492 int i;
493 for (i = 0; i < numSamples; i++)
494 {
495 val = data.recordedSamples[i];
496 if (val < 0)
497 val = -val; /* ABS */
498 *average_abs += val;
499 }
500 *average_abs /= numSamples;
501 int gotSound = (numSamples > 0) && (*average_abs > 0) && Autocorrelation (data.recordedSamples, numSamples, &autocorr);
502 /* Reset the frame index to 0, so we keep going with only new sound */
503 data.frameIndex = 0;
504 /* smooth the autocorrelation */
505 /*#define PARTDERNIER 0.25 */
506 for (i = 0; i < WindowSize; i++)
507 {
508 autocorr2[i] = (autocorr[i]) * PARTDERNIER + autocorr2[i] * (1 - PARTDERNIER);
509 }
510 for (; i < 16384; i++)
511 {
512 autocorr2[i] = autocorr2[i] * (1 - PARTDERNIER);
513 }
514
515 float db = level2db (avg_abs);
516 gotSound = gotSound && (db > BACKGROUND_DB);
517
518 if (gotSound)
519 {
520 #define INTERVAL (1) /*(2) */
521 double m_sample_rate = 44100.0;
522 int lower = lround (m_sample_rate / PitchToFreq (Pitch - INTERVAL)); /* lowest allowed value of Pitch is 12 */
523 int upper = lround (m_sample_rate / PitchToFreq (Pitch + INTERVAL));
524 double bestpeak_x = upper + bestPeak2 (&autocorr2[upper], lower - upper, m_sample_rate);
525 double psd;
526 for (psd = 0.0, i = upper; i < lower; i++)
527 psd += autocorr[i];
528 psd *= (Pitch * Pitch * Pitch * Pitch / (400000000.0));
529 if (psd < 1.0)
530 return -1.0;
531 double bestpeak_freq2 = m_sample_rate / bestpeak_x;
532 int pitch = -1;
533
534 if (db > BACKGROUND_DB + 6)
535 {
536 pitch = (int) (Freq2Pitch (bestpeak_freq2) + 0.5); // note found this time
537 } // loud enough
538 else
539 return -4.0;
540 if (pitch > 0)
541 {
542 return bestpeak_freq2;
543 }
544 return -3.0;
545 } // if gotPitch
546 return -2.0;
547 }
548
549
550
551
552 /**********************************************************************
553
554 from FFT.cpp Dominic Mazzoni September 2000
555 **********************************************************************/
556 #define M_PI 3.14159265358979323846 /* pi */
557 #define false 0
558 #define true 1
559 #define bool int
560 static int **gFFTBitTable = NULL;
561 static const int MaxFastBits = 16;
562
563 static int
IsPowerOfTwo(int x)564 IsPowerOfTwo (int x)
565 {
566 if (x < 2)
567 return false;
568
569 if (x & (x - 1)) /* Thanks to 'byang' for this cute trick! */
570 return false;
571
572 return true;
573 }
574
575 static int
NumberOfBitsNeeded(int PowerOfTwo)576 NumberOfBitsNeeded (int PowerOfTwo)
577 {
578 int i;
579
580 if (PowerOfTwo < 2)
581 {
582 fprintf (stderr, "Error: FFT called with size %d\n", PowerOfTwo);
583 exit (1);
584 }
585
586 for (i = 0;; i++)
587 if (PowerOfTwo & (1 << i))
588 return i;
589 }
590
591 static int
ReverseBits(int index,int NumBits)592 ReverseBits (int index, int NumBits)
593 {
594 int i, rev;
595
596 for (i = rev = 0; i < NumBits; i++)
597 {
598 rev = (rev << 1) | (index & 1);
599 index >>= 1;
600 }
601
602 return rev;
603 }
604
605 static void
InitFFT()606 InitFFT ()
607 {
608 gFFTBitTable = malloc (sizeof (int *) * MaxFastBits);
609
610 int len = 2;
611 int b;
612 for (b = 1; b <= MaxFastBits; b++)
613 {
614
615 gFFTBitTable[b - 1] = malloc (sizeof (int) * len);
616 int i;
617 for (i = 0; i < len; i++)
618 gFFTBitTable[b - 1][i] = ReverseBits (i, b);
619
620 len <<= 1;
621 }
622 }
623
624 static inline int
FastReverseBits(int i,int NumBits)625 FastReverseBits (int i, int NumBits)
626 {
627 if (NumBits <= MaxFastBits)
628 return gFFTBitTable[NumBits - 1][i];
629 else
630 return ReverseBits (i, NumBits);
631 }
632
633 /*
634 * Complex Fast Fourier Transform
635 */
636
637 static void
FFT(int NumSamples,bool InverseTransform,float * RealIn,float * ImagIn,float * RealOut,float * ImagOut)638 FFT (int NumSamples, bool InverseTransform, float *RealIn, float *ImagIn, float *RealOut, float *ImagOut)
639 {
640 int NumBits; /* Number of bits needed to store indices */
641 int i, j, k, n;
642 int BlockSize, BlockEnd;
643
644 double angle_numerator = 2.0 * M_PI;
645 float tr, ti; /* temp real, temp imaginary */
646
647 if (!IsPowerOfTwo (NumSamples))
648 {
649 fprintf (stderr, "%d is not a power of two\n", NumSamples);
650 exit (1);
651 }
652
653 if (!gFFTBitTable)
654 InitFFT ();
655
656 if (InverseTransform)
657 angle_numerator = -angle_numerator;
658
659 NumBits = NumberOfBitsNeeded (NumSamples);
660
661 /*
662 ** Do simultaneous data copy and bit-reversal ordering into outputs...
663 */
664
665 for (i = 0; i < NumSamples; i++)
666 {
667 j = FastReverseBits (i, NumBits);
668 RealOut[j] = RealIn[i];
669 ImagOut[j] = (ImagIn == NULL) ? 0.0 : ImagIn[i];
670 }
671
672 /*
673 ** Do the FFT itself...
674 */
675
676 BlockEnd = 1;
677 for (BlockSize = 2; BlockSize <= NumSamples; BlockSize <<= 1)
678 {
679
680 double delta_angle = angle_numerator / (double) BlockSize;
681
682 float sm2 = sin (-2 * delta_angle);
683 float sm1 = sin (-delta_angle);
684 float cm2 = cos (-2 * delta_angle);
685 float cm1 = cos (-delta_angle);
686 float w = 2 * cm1;
687 float ar0, ar1, ar2, ai0, ai1, ai2;
688
689 for (i = 0; i < NumSamples; i += BlockSize)
690 {
691 ar2 = cm2;
692 ar1 = cm1;
693
694 ai2 = sm2;
695 ai1 = sm1;
696
697 for (j = i, n = 0; n < BlockEnd; j++, n++)
698 {
699 ar0 = w * ar1 - ar2;
700 ar2 = ar1;
701 ar1 = ar0;
702
703 ai0 = w * ai1 - ai2;
704 ai2 = ai1;
705 ai1 = ai0;
706
707 k = j + BlockEnd;
708 tr = ar0 * RealOut[k] - ai0 * ImagOut[k];
709 ti = ar0 * ImagOut[k] + ai0 * RealOut[k];
710
711 RealOut[k] = RealOut[j] - tr;
712 ImagOut[k] = ImagOut[j] - ti;
713
714 RealOut[j] += tr;
715 ImagOut[j] += ti;
716 }
717 }
718
719 BlockEnd = BlockSize;
720 }
721
722 /*
723 ** Need to normalize if inverse transform...
724 */
725
726 if (InverseTransform)
727 {
728 float denom = (float) NumSamples;
729
730 for (i = 0; i < NumSamples; i++)
731 {
732 RealOut[i] /= denom;
733 ImagOut[i] /= denom;
734 }
735 }
736 }
737
738 /*
739 * Windowing Functions
740 */
741
742 static void
WindowFunc(int whichFunction,int NumSamples,float * in)743 WindowFunc (int whichFunction, int NumSamples, float *in)
744 {
745 int i;
746
747 if (whichFunction == 1)
748 {
749 // Bartlett (triangular) window
750 for (i = 0; i < NumSamples / 2; i++)
751 {
752 in[i] *= (i / (float) (NumSamples / 2));
753 in[i + (NumSamples / 2)] *= (1.0 - (i / (float) (NumSamples / 2)));
754 }
755 }
756
757 if (whichFunction == 2)
758 {
759 // Hamming
760 for (i = 0; i < NumSamples; i++)
761 in[i] *= 0.54 - 0.46 * cos (2 * M_PI * i / (NumSamples - 1));
762 }
763
764 if (whichFunction == 3)
765 {
766 // Hanning
767 for (i = 0; i < NumSamples; i++)
768 in[i] *= 0.50 - 0.50 * cos (2 * M_PI * i / (NumSamples - 1));
769 }
770 }
771
772
773 // This is Audacity's FreqWindow::Recalc(), but shaved down to
774 // 1) be enhanced auto-correlation only
775 // 2) take parameters, and return values.
776 static bool
Autocorrelation(float mData[],int mDataLen,float * processed[])777 Autocorrelation (float mData[], // In
778 int mDataLen, // In
779 float *processed[] // Out
780 )
781 {
782 if (mDataLen < WindowSize)
783 {
784 // Not enough data to get even one window
785 return false;
786 }
787
788 // float *mProcessed = NULL;
789 float *mProcessed = *processed;
790
791 // mProcessed = new float[WindowSize];
792
793 int i;
794 for (i = 0; i < WindowSize; i++)
795 mProcessed[i] = 0.0;
796 int half = WindowSize / 2;
797
798 float *in = malloc (sizeof (float) * WindowSize);
799 float *out = malloc (sizeof (float) * WindowSize);
800 float *out2 = malloc (sizeof (float) * WindowSize);
801
802 int start = 0;
803 int windows = 0;
804 while (start + WindowSize <= mDataLen)
805 {
806 // Copy stuff into in
807 for (i = 0; i < WindowSize; i++)
808 in[i] = mData[start + i];
809
810 // Window the data to lose crazy artifacts
811 // due to finite-length window
812 WindowFunc (2 /* Hamming */ , WindowSize, in);
813
814 // Enhanced AC
815
816 // Take FFT
817 FFT (WindowSize, false, in, NULL, out, out2);
818
819 // Compute power
820 for (i = 0; i < WindowSize; i++)
821 in[i] = (out[i] * out[i]) + (out2[i] * out2[i]);
822
823 // Tolonen and Karjalainen recommend taking the cube root
824 // of the power, instead of the square root
825
826 for (i = 0; i < WindowSize; i++)
827 in[i] = pow (in[i], 1.0 / 3.0);
828
829 // Take FFT
830 FFT (WindowSize, false, in, NULL, out, out2);
831
832 // Take real part of result
833 for (i = 0; i < half; i++)
834 mProcessed[i] += out[i];
835
836 start += half;
837 windows++;
838 }
839
840 // Enhanced Autocorrelation
841 for (i = 0; i < half; i++)
842 mProcessed[i] = mProcessed[i] / windows;
843
844 // Peak Pruning as described by Tolonen and Karjalainen, 2000
845
846 // Clip at zero, copy to temp array
847 for (i = 0; i < half; i++)
848 {
849 if (mProcessed[i] < 0.0)
850 mProcessed[i] = 0.0;
851 out[i] = mProcessed[i];
852 }
853
854 // Subtract a time-doubled signal (linearly interp.) from the original
855 // (clipped) signal
856 for (i = 0; i < half; i++)
857 if ((i % 2) == 0)
858 mProcessed[i] -= out[i / 2];
859 else
860 mProcessed[i] -= ((out[i / 2] + out[i / 2 + 1]) / 2);
861
862 // Clip at zero again
863 for (i = 0; i < half; i++)
864 if (mProcessed[i] < 0.0)
865 mProcessed[i] = 0.0;
866
867 /* *mProcessedSize = half; */
868
869 free (in);
870 free (out);
871 free (out2);
872
873 // *processed = mProcessed;
874
875 return true;
876 }
877
878 static float
Freq2Pitch(float freq)879 Freq2Pitch (float freq)
880 {
881 return (69.0 + 12.0 * (log (freq / 440.0) / log (2.0)));
882 }
883
884 float
CubicMaximize(float y0,float y1,float y2,float y3,float * maxyVal)885 CubicMaximize (float y0, float y1, float y2, float y3, float *maxyVal)
886 {
887 // Find coefficients of cubic
888 float a, b, c, d;
889
890 a = y0 / -6.0 + y1 / 2.0 - y2 / 2.0 + y3 / 6.0;
891 b = y0 - 5.0 * y1 / 2.0 + 2.0 * y2 - y3 / 2.0;
892 c = -11.0 * y0 / 6.0 + 3.0 * y1 - 3.0 * y2 / 2.0 + y3 / 3.0;
893 d = y0;
894
895 // Take derivative
896 float da, db, dc;
897
898 da = 3 * a;
899 db = 2 * b;
900 dc = c;
901
902 // Find zeroes of derivative using quadratic equation
903 float discriminant = db * db - 4 * da * dc;
904 if (discriminant < 0.0)
905 return -1.0; // error
906
907 float x1 = (-db + sqrt (discriminant)) / (2 * da);
908 float x2 = (-db - sqrt (discriminant)) / (2 * da);
909
910 // The one which corresponds to a local _maximum_ in the
911 // cubic is the one we want - the one with a negative
912 // second derivative
913 float dda = 2 * da;
914 float ddb = db;
915
916 #define CUBIC(x,a,b,c,d) (a*x*x*x + b*x*x + c*x + d)
917
918 if (dda * x1 + ddb < 0)
919 {
920 *maxyVal = CUBIC (x1, a, b, c, d);
921 return x1;
922 }
923 else
924 {
925 *maxyVal = CUBIC (x2, a, b, c, d);
926 return x2;
927 }
928
929 #undef CUBIC
930 }
931
932
933
934
935 static float
Parabole(float * y,int nb,float * maxyVal)936 Parabole (float *y, int nb, float *maxyVal)
937 {
938 int i;
939 float mx4 = 0, mx3 = 0, mx2 = 0, mx = 0;
940 float mx2y = 0, mxy = 0, my = 0;
941 float a, b, c;
942 float highX;
943 for (i = 0; i < nb; i++)
944 {
945 mx += i;
946 mx2 += i * i;
947 mx3 += i * i * i;
948 mx4 += i * i * i * i;
949 mxy += i * y[i];
950 mx2y += i * i * y[i];
951 my += y[i];
952 }
953 mx /= nb;
954 mx2 /= nb;
955 mx3 /= nb;
956 mx4 /= nb;
957 my /= nb;
958 mxy /= nb;
959 mx2y /= nb;
960 a = ((mx2y - mx2 * my) * (mx2 - mx * mx) - (mxy - mx * my) * (mx3 - mx2 * mx)) / ((mx4 - mx2 * mx2) * (mx2 - mx * mx) - (mx3 - mx * mx2) * (mx3 - mx * mx2));
961 b = (mxy - mx * my - a * (mx3 - mx * mx2)) / (mx2 - mx * mx);
962 c = my - a * mx2 - b * mx;
963 highX = (-b / (2 * a));
964 *maxyVal = a * highX * highX + b * highX + c;
965 return (-b / (2 * a));
966 }
967
968 static float
bestPeak2(float * mProcessed,int mProcessedSize,float mRate)969 bestPeak2 (float *mProcessed, // IN
970 int mProcessedSize, // IN
971 float mRate // IN
972 )
973 {
974 float highestpeak_y = 0;
975 int iMaxX = 0;
976 int bin;
977
978 bool up = (mProcessed[1] > mProcessed[0]);
979 for (bin = 2; bin < mProcessedSize; bin++)
980 {
981 bool nowUp = mProcessed[bin] > mProcessed[bin - 1];
982 if (!nowUp && up)
983 {
984 if (mProcessed[bin - 1] > highestpeak_y)
985 {
986 highestpeak_y = mProcessed[bin - 1];
987 iMaxX = bin - 1;
988 }
989 }
990 up = nowUp;
991 }
992 // cherche le pic par recherche de la parabole la plus proche.
993 int leftbin = iMaxX - 1;
994 while (leftbin > 1 && leftbin > (iMaxX - 20) && mProcessed[leftbin - 1] > mProcessed[iMaxX] / 2)
995 leftbin--;
996 int nb = (iMaxX - leftbin) * 2 + 1;
997 float thispeak_y;
998 float max = leftbin + Parabole (&mProcessed[leftbin], nb, &thispeak_y);
999 return max;
1000 }
1001 #endif
1002