• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /* Sonic library
2    Copyright 2010
3    Bill Cox
4    This file is part of the Sonic Library.
5 
6    This file is licensed under the Apache 2.0 license.
7 */
8 
9 #include <stdio.h>
10 #include <stdlib.h>
11 #include <string.h>
12 #include <stdarg.h>
13 #ifdef SONIC_USE_SIN
14 #include <math.h>
15 #ifndef M_PI
16 #define M_PI 3.14159265358979323846
17 #endif
18 #endif
19 #include "sonic.h"
20 
21 struct sonicStreamStruct {
22     short *inputBuffer;
23     short *outputBuffer;
24     short *pitchBuffer;
25     short *downSampleBuffer;
26     float speed;
27     float volume;
28     float pitch;
29     float rate;
30     int oldRatePosition;
31     int newRatePosition;
32     int useChordPitch;
33     int quality;
34     int numChannels;
35     int inputBufferSize;
36     int pitchBufferSize;
37     int outputBufferSize;
38     int numInputSamples;
39     int numOutputSamples;
40     int numPitchSamples;
41     int minPeriod;
42     int maxPeriod;
43     int maxRequired;
44     int remainingInputToCopy;
45     int sampleRate;
46     int prevPeriod;
47     int prevMinDiff;
48 };
49 
50 /* Just used for debugging */
51 /*
52 void sonicMSG(char *format, ...)
53 {
54     char buffer[4096];
55     va_list ap;
56     FILE *file;
57 
58     va_start(ap, format);
59     vsprintf((char *)buffer, (char *)format, ap);
60     va_end(ap);
61     file=fopen("/tmp/sonic.log", "a");
62     fprintf(file, "%s", buffer);
63     fclose(file);
64 }
65 */
66 
67 /* Scale the samples by the factor. */
scaleSamples(short * samples,int numSamples,float volume)68 static void scaleSamples(
69     short *samples,
70     int numSamples,
71     float volume)
72 {
73     int fixedPointVolume = volume*4096.0f;
74     int value;
75 
76     while(numSamples--) {
77 	value = (*samples*fixedPointVolume) >> 12;
78 	if(value > 32767) {
79 	    value = 32767;
80 	} else if(value < -32767) {
81 	    value = -32767;
82 	}
83 	*samples++ = value;
84     }
85 }
86 
87 /* Get the speed of the stream. */
sonicGetSpeed(sonicStream stream)88 float sonicGetSpeed(
89     sonicStream stream)
90 {
91     return stream->speed;
92 }
93 
94 /* Set the speed of the stream. */
sonicSetSpeed(sonicStream stream,float speed)95 void sonicSetSpeed(
96     sonicStream stream,
97     float speed)
98 {
99     stream->speed = speed;
100 }
101 
102 /* Get the pitch of the stream. */
sonicGetPitch(sonicStream stream)103 float sonicGetPitch(
104     sonicStream stream)
105 {
106     return stream->pitch;
107 }
108 
109 /* Set the pitch of the stream. */
sonicSetPitch(sonicStream stream,float pitch)110 void sonicSetPitch(
111     sonicStream stream,
112     float pitch)
113 {
114     stream->pitch = pitch;
115 }
116 
117 /* Get the rate of the stream. */
sonicGetRate(sonicStream stream)118 float sonicGetRate(
119     sonicStream stream)
120 {
121     return stream->rate;
122 }
123 
124 /* Set the playback rate of the stream. This scales pitch and speed at the same time. */
sonicSetRate(sonicStream stream,float rate)125 void sonicSetRate(
126     sonicStream stream,
127     float rate)
128 {
129     stream->rate = rate;
130 
131     stream->oldRatePosition = 0;
132     stream->newRatePosition = 0;
133 }
134 
135 /* Get the vocal chord pitch setting. */
sonicGetChordPitch(sonicStream stream)136 int sonicGetChordPitch(
137     sonicStream stream)
138 {
139     return stream->useChordPitch;
140 }
141 
142 /* Set the vocal chord mode for pitch computation.  Default is off. */
sonicSetChordPitch(sonicStream stream,int useChordPitch)143 void sonicSetChordPitch(
144     sonicStream stream,
145     int useChordPitch)
146 {
147     stream->useChordPitch = useChordPitch;
148 }
149 
150 /* Get the quality setting. */
sonicGetQuality(sonicStream stream)151 int sonicGetQuality(
152     sonicStream stream)
153 {
154     return stream->quality;
155 }
156 
157 /* Set the "quality".  Default 0 is virtually as good as 1, but very much faster. */
sonicSetQuality(sonicStream stream,int quality)158 void sonicSetQuality(
159     sonicStream stream,
160     int quality)
161 {
162     stream->quality = quality;
163 }
164 
165 /* Get the scaling factor of the stream. */
sonicGetVolume(sonicStream stream)166 float sonicGetVolume(
167     sonicStream stream)
168 {
169     return stream->volume;
170 }
171 
172 /* Set the scaling factor of the stream. */
sonicSetVolume(sonicStream stream,float volume)173 void sonicSetVolume(
174     sonicStream stream,
175     float volume)
176 {
177     stream->volume = volume;
178 }
179 
180 /* Free stream buffers. */
freeStreamBuffers(sonicStream stream)181 static void freeStreamBuffers(
182     sonicStream stream)
183 {
184     if(stream->inputBuffer != NULL) {
185 	free(stream->inputBuffer);
186     }
187     if(stream->outputBuffer != NULL) {
188 	free(stream->outputBuffer);
189     }
190     if(stream->pitchBuffer != NULL) {
191 	free(stream->pitchBuffer);
192     }
193     if(stream->downSampleBuffer != NULL) {
194 	free(stream->downSampleBuffer);
195     }
196 }
197 
198 /* Destroy the sonic stream. */
sonicDestroyStream(sonicStream stream)199 void sonicDestroyStream(
200     sonicStream stream)
201 {
202     freeStreamBuffers(stream);
203     free(stream);
204 }
205 
206 /* Allocate stream buffers. */
allocateStreamBuffers(sonicStream stream,int sampleRate,int numChannels)207 static int allocateStreamBuffers(
208     sonicStream stream,
209     int sampleRate,
210     int numChannels)
211 {
212     int minPeriod = sampleRate/SONIC_MAX_PITCH;
213     int maxPeriod = sampleRate/SONIC_MIN_PITCH;
214     int maxRequired = 2*maxPeriod;
215 
216     stream->inputBufferSize = maxRequired;
217     stream->inputBuffer = (short *)calloc(maxRequired, sizeof(short)*numChannels);
218     if(stream->inputBuffer == NULL) {
219 	sonicDestroyStream(stream);
220 	return 0;
221     }
222     stream->outputBufferSize = maxRequired;
223     stream->outputBuffer = (short *)calloc(maxRequired, sizeof(short)*numChannels);
224     if(stream->outputBuffer == NULL) {
225 	sonicDestroyStream(stream);
226 	return 0;
227     }
228     stream->pitchBufferSize = maxRequired;
229     stream->pitchBuffer = (short *)calloc(maxRequired, sizeof(short)*numChannels);
230     if(stream->pitchBuffer == NULL) {
231 	sonicDestroyStream(stream);
232 	return 0;
233     }
234     stream->downSampleBuffer = (short *)calloc(maxRequired, sizeof(short));
235     if(stream->downSampleBuffer == NULL) {
236 	sonicDestroyStream(stream);
237 	return 0;
238     }
239     stream->sampleRate = sampleRate;
240     stream->numChannels = numChannels;
241     stream->oldRatePosition = 0;
242     stream->newRatePosition = 0;
243     stream->minPeriod = minPeriod;
244     stream->maxPeriod = maxPeriod;
245     stream->maxRequired = maxRequired;
246     stream->prevPeriod = 0;
247     return 1;
248 }
249 
250 /* Create a sonic stream.  Return NULL only if we are out of memory and cannot
251    allocate the stream. */
sonicCreateStream(int sampleRate,int numChannels)252 sonicStream sonicCreateStream(
253     int sampleRate,
254     int numChannels)
255 {
256     sonicStream stream = (sonicStream)calloc(1, sizeof(struct sonicStreamStruct));
257 
258     if(stream == NULL) {
259 	return NULL;
260     }
261     if(!allocateStreamBuffers(stream, sampleRate, numChannels)) {
262         return NULL;
263     }
264     stream->speed = 1.0f;
265     stream->pitch = 1.0f;
266     stream->volume = 1.0f;
267     stream->rate = 1.0f;
268     stream->oldRatePosition = 0;
269     stream->newRatePosition = 0;
270     stream->useChordPitch = 0;
271     stream->quality = 0;
272     return stream;
273 }
274 
275 /* Get the sample rate of the stream. */
sonicGetSampleRate(sonicStream stream)276 int sonicGetSampleRate(
277     sonicStream stream)
278 {
279     return stream->sampleRate;
280 }
281 
282 /* Set the sample rate of the stream.  This will cause samples buffered in the stream to
283    be lost. */
sonicSetSampleRate(sonicStream stream,int sampleRate)284 void sonicSetSampleRate(
285     sonicStream stream,
286     int sampleRate)
287 {
288     freeStreamBuffers(stream);
289     allocateStreamBuffers(stream, sampleRate, stream->numChannels);
290 }
291 
292 /* Get the number of channels. */
sonicGetNumChannels(sonicStream stream)293 int sonicGetNumChannels(
294     sonicStream stream)
295 {
296     return stream->numChannels;
297 }
298 
299 /* Set the num channels of the stream.  This will cause samples buffered in the stream to
300    be lost. */
sonicSetNumChannels(sonicStream stream,int numChannels)301 void sonicSetNumChannels(
302     sonicStream stream,
303     int numChannels)
304 {
305     freeStreamBuffers(stream);
306     allocateStreamBuffers(stream, stream->sampleRate, numChannels);
307 }
308 
309 /* Enlarge the output buffer if needed. */
enlargeOutputBufferIfNeeded(sonicStream stream,int numSamples)310 static int enlargeOutputBufferIfNeeded(
311     sonicStream stream,
312     int numSamples)
313 {
314     if(stream->numOutputSamples + numSamples > stream->outputBufferSize) {
315 	stream->outputBufferSize += (stream->outputBufferSize >> 1) + numSamples;
316 	stream->outputBuffer = (short *)realloc(stream->outputBuffer,
317 	    stream->outputBufferSize*sizeof(short)*stream->numChannels);
318 	if(stream->outputBuffer == NULL) {
319 	    return 0;
320 	}
321     }
322     return 1;
323 }
324 
325 /* Enlarge the input buffer if needed. */
enlargeInputBufferIfNeeded(sonicStream stream,int numSamples)326 static int enlargeInputBufferIfNeeded(
327     sonicStream stream,
328     int numSamples)
329 {
330     if(stream->numInputSamples + numSamples > stream->inputBufferSize) {
331 	stream->inputBufferSize += (stream->inputBufferSize >> 1) + numSamples;
332 	stream->inputBuffer = (short *)realloc(stream->inputBuffer,
333 	    stream->inputBufferSize*sizeof(short)*stream->numChannels);
334 	if(stream->inputBuffer == NULL) {
335 	    return 0;
336 	}
337     }
338     return 1;
339 }
340 
341 /* Add the input samples to the input buffer. */
addFloatSamplesToInputBuffer(sonicStream stream,float * samples,int numSamples)342 static int addFloatSamplesToInputBuffer(
343     sonicStream stream,
344     float *samples,
345     int numSamples)
346 {
347     short *buffer;
348     int count = numSamples*stream->numChannels;
349 
350     if(numSamples == 0) {
351 	return 1;
352     }
353     if(!enlargeInputBufferIfNeeded(stream, numSamples)) {
354 	return 0;
355     }
356     buffer = stream->inputBuffer + stream->numInputSamples*stream->numChannels;
357     while(count--) {
358         *buffer++ = (*samples++)*32767.0f;
359     }
360     stream->numInputSamples += numSamples;
361     return 1;
362 }
363 
364 /* Add the input samples to the input buffer. */
addShortSamplesToInputBuffer(sonicStream stream,short * samples,int numSamples)365 static int addShortSamplesToInputBuffer(
366     sonicStream stream,
367     short *samples,
368     int numSamples)
369 {
370     if(numSamples == 0) {
371 	return 1;
372     }
373     if(!enlargeInputBufferIfNeeded(stream, numSamples)) {
374 	return 0;
375     }
376     memcpy(stream->inputBuffer + stream->numInputSamples*stream->numChannels, samples,
377         numSamples*sizeof(short)*stream->numChannels);
378     stream->numInputSamples += numSamples;
379     return 1;
380 }
381 
382 /* Add the input samples to the input buffer. */
addUnsignedCharSamplesToInputBuffer(sonicStream stream,unsigned char * samples,int numSamples)383 static int addUnsignedCharSamplesToInputBuffer(
384     sonicStream stream,
385     unsigned char *samples,
386     int numSamples)
387 {
388     short *buffer;
389     int count = numSamples*stream->numChannels;
390 
391     if(numSamples == 0) {
392 	return 1;
393     }
394     if(!enlargeInputBufferIfNeeded(stream, numSamples)) {
395 	return 0;
396     }
397     buffer = stream->inputBuffer + stream->numInputSamples*stream->numChannels;
398     while(count--) {
399         *buffer++ = (*samples++ - 128) << 8;
400     }
401     stream->numInputSamples += numSamples;
402     return 1;
403 }
404 
405 /* Remove input samples that we have already processed. */
removeInputSamples(sonicStream stream,int position)406 static void removeInputSamples(
407     sonicStream stream,
408     int position)
409 {
410     int remainingSamples = stream->numInputSamples - position;
411 
412     if(remainingSamples > 0) {
413 	memmove(stream->inputBuffer, stream->inputBuffer + position*stream->numChannels,
414 	    remainingSamples*sizeof(short)*stream->numChannels);
415     }
416     stream->numInputSamples = remainingSamples;
417 }
418 
419 /* Just copy from the array to the output buffer */
copyToOutput(sonicStream stream,short * samples,int numSamples)420 static int copyToOutput(
421     sonicStream stream,
422     short *samples,
423     int numSamples)
424 {
425     if(!enlargeOutputBufferIfNeeded(stream, numSamples)) {
426 	return 0;
427     }
428     memcpy(stream->outputBuffer + stream->numOutputSamples*stream->numChannels,
429 	samples, numSamples*sizeof(short)*stream->numChannels);
430     stream->numOutputSamples += numSamples;
431     return 1;
432 }
433 
434 /* Just copy from the input buffer to the output buffer.  Return 0 if we fail to
435    resize the output buffer.  Otherwise, return numSamples */
copyInputToOutput(sonicStream stream,int position)436 static int copyInputToOutput(
437     sonicStream stream,
438     int position)
439 {
440     int numSamples = stream->remainingInputToCopy;
441 
442     if(numSamples > stream->maxRequired) {
443 	numSamples = stream->maxRequired;
444     }
445     if(!copyToOutput(stream, stream->inputBuffer + position*stream->numChannels,
446 	    numSamples)) {
447 	return 0;
448     }
449     stream->remainingInputToCopy -= numSamples;
450     return numSamples;
451 }
452 
453 /* Read data out of the stream.  Sometimes no data will be available, and zero
454    is returned, which is not an error condition. */
sonicReadFloatFromStream(sonicStream stream,float * samples,int maxSamples)455 int sonicReadFloatFromStream(
456     sonicStream stream,
457     float *samples,
458     int maxSamples)
459 {
460     int numSamples = stream->numOutputSamples;
461     int remainingSamples = 0;
462     short *buffer;
463     int count;
464 
465     if(numSamples == 0) {
466 	return 0;
467     }
468     if(numSamples > maxSamples) {
469 	remainingSamples = numSamples - maxSamples;
470 	numSamples = maxSamples;
471     }
472     buffer = stream->outputBuffer;
473     count = numSamples*stream->numChannels;
474     while(count--) {
475 	*samples++ = (*buffer++)/32767.0f;
476     }
477     if(remainingSamples > 0) {
478 	memmove(stream->outputBuffer, stream->outputBuffer + numSamples*stream->numChannels,
479 	    remainingSamples*sizeof(short)*stream->numChannels);
480     }
481     stream->numOutputSamples = remainingSamples;
482     return numSamples;
483 }
484 
485 /* Read short data out of the stream.  Sometimes no data will be available, and zero
486    is returned, which is not an error condition. */
sonicReadShortFromStream(sonicStream stream,short * samples,int maxSamples)487 int sonicReadShortFromStream(
488     sonicStream stream,
489     short *samples,
490     int maxSamples)
491 {
492     int numSamples = stream->numOutputSamples;
493     int remainingSamples = 0;
494 
495     if(numSamples == 0) {
496 	return 0;
497     }
498     if(numSamples > maxSamples) {
499 	remainingSamples = numSamples - maxSamples;
500 	numSamples = maxSamples;
501     }
502     memcpy(samples, stream->outputBuffer, numSamples*sizeof(short)*stream->numChannels);
503     if(remainingSamples > 0) {
504 	memmove(stream->outputBuffer, stream->outputBuffer + numSamples*stream->numChannels,
505 	    remainingSamples*sizeof(short)*stream->numChannels);
506     }
507     stream->numOutputSamples = remainingSamples;
508     return numSamples;
509 }
510 
511 /* Read unsigned char data out of the stream.  Sometimes no data will be available, and zero
512    is returned, which is not an error condition. */
sonicReadUnsignedCharFromStream(sonicStream stream,unsigned char * samples,int maxSamples)513 int sonicReadUnsignedCharFromStream(
514     sonicStream stream,
515     unsigned char *samples,
516     int maxSamples)
517 {
518     int numSamples = stream->numOutputSamples;
519     int remainingSamples = 0;
520     short *buffer;
521     int count;
522 
523     if(numSamples == 0) {
524 	return 0;
525     }
526     if(numSamples > maxSamples) {
527 	remainingSamples = numSamples - maxSamples;
528 	numSamples = maxSamples;
529     }
530     buffer = stream->outputBuffer;
531     count = numSamples*stream->numChannels;
532     while(count--) {
533 	*samples++ = (char)((*buffer++) >> 8) + 128;
534     }
535     if(remainingSamples > 0) {
536 	memmove(stream->outputBuffer, stream->outputBuffer + numSamples*stream->numChannels,
537 	    remainingSamples*sizeof(short)*stream->numChannels);
538     }
539     stream->numOutputSamples = remainingSamples;
540     return numSamples;
541 }
542 
543 /* Force the sonic stream to generate output using whatever data it currently
544    has.  No extra delay will be added to the output, but flushing in the middle of
545    words could introduce distortion. */
sonicFlushStream(sonicStream stream)546 int sonicFlushStream(
547     sonicStream stream)
548 {
549     int maxRequired = stream->maxRequired;
550     int remainingSamples = stream->numInputSamples;
551     float speed = stream->speed/stream->pitch;
552     float rate = stream->rate*stream->pitch;
553     int expectedOutputSamples = stream->numOutputSamples +
554 	(int)((remainingSamples/speed + stream->numPitchSamples)/rate + 0.5f);
555 
556     /* Add enough silence to flush both input and pitch buffers. */
557     if(!enlargeInputBufferIfNeeded(stream, remainingSamples + 2*maxRequired)) {
558         return 0;
559     }
560     memset(stream->inputBuffer + remainingSamples*stream->numChannels, 0,
561 	2*maxRequired*sizeof(short)*stream->numChannels);
562     stream->numInputSamples += 2*maxRequired;
563     if(!sonicWriteShortToStream(stream, NULL, 0)) {
564 	return 0;
565     }
566     /* Throw away any extra samples we generated due to the silence we added */
567     if(stream->numOutputSamples > expectedOutputSamples) {
568 	stream->numOutputSamples = expectedOutputSamples;
569     }
570     /* Empty input and pitch buffers */
571     stream->numInputSamples = 0;
572     stream->remainingInputToCopy = 0;
573     stream->numPitchSamples = 0;
574     return 1;
575 }
576 
577 /* Return the number of samples in the output buffer */
sonicSamplesAvailable(sonicStream stream)578 int sonicSamplesAvailable(
579    sonicStream stream)
580 {
581     return stream->numOutputSamples;
582 }
583 
584 /* If skip is greater than one, average skip samples together and write them to
585    the down-sample buffer.  If numChannels is greater than one, mix the channels
586    together as we down sample. */
downSampleInput(sonicStream stream,short * samples,int skip)587 static void downSampleInput(
588     sonicStream stream,
589     short *samples,
590     int skip)
591 {
592     int numSamples = stream->maxRequired/skip;
593     int samplesPerValue = stream->numChannels*skip;
594     int i, j;
595     int value;
596     short *downSamples = stream->downSampleBuffer;
597 
598     for(i = 0; i < numSamples; i++) {
599 	value = 0;
600         for(j = 0; j < samplesPerValue; j++) {
601 	    value += *samples++;
602 	}
603 	value /= samplesPerValue;
604         *downSamples++ = value;
605     }
606 }
607 
608 /* Find the best frequency match in the range, and given a sample skip multiple.
609    For now, just find the pitch of the first channel. */
findPitchPeriodInRange(short * samples,int minPeriod,int maxPeriod,int * retMinDiff,int * retMaxDiff)610 static int findPitchPeriodInRange(
611     short *samples,
612     int minPeriod,
613     int maxPeriod,
614     int *retMinDiff,
615     int *retMaxDiff)
616 {
617     int period, bestPeriod = 0, worstPeriod = 255;
618     short *s, *p, sVal, pVal;
619     unsigned long diff, minDiff = 1, maxDiff = 0;
620     int i;
621 
622     for(period = minPeriod; period <= maxPeriod; period++) {
623 	diff = 0;
624 	s = samples;
625 	p = samples + period;
626 	for(i = 0; i < period; i++) {
627 	    sVal = *s++;
628 	    pVal = *p++;
629 	    diff += sVal >= pVal? (unsigned short)(sVal - pVal) :
630 	        (unsigned short)(pVal - sVal);
631 	}
632 	/* Note that the highest number of samples we add into diff will be less
633 	   than 256, since we skip samples.  Thus, diff is a 24 bit number, and
634 	   we can safely multiply by numSamples without overflow */
635 	if(diff*bestPeriod < minDiff*period) {
636 	    minDiff = diff;
637 	    bestPeriod = period;
638 	}
639 	if(diff*worstPeriod > maxDiff*period) {
640 	    maxDiff = diff;
641 	    worstPeriod = period;
642 	}
643     }
644     *retMinDiff = minDiff/bestPeriod;
645     *retMaxDiff = maxDiff/worstPeriod;
646     return bestPeriod;
647 }
648 
649 /* At abrupt ends of voiced words, we can have pitch periods that are better
650    approximated by the previous pitch period estimate.  Try to detect this case. */
prevPeriodBetter(sonicStream stream,int period,int minDiff,int maxDiff,int preferNewPeriod)651 static int prevPeriodBetter(
652     sonicStream stream,
653     int period,
654     int minDiff,
655     int maxDiff,
656     int preferNewPeriod)
657 {
658     if(minDiff == 0 || stream->prevPeriod == 0) {
659 	return 0;
660     }
661     if(preferNewPeriod) {
662 	if(maxDiff > minDiff*3) {
663 	    /* Got a reasonable match this period */
664 	    return 0;
665 	}
666 	if(minDiff*2 <= stream->prevMinDiff*3) {
667 	    /* Mismatch is not that much greater this period */
668 	    return 0;
669 	}
670     } else {
671 	if(minDiff <= stream->prevMinDiff) {
672 	    return 0;
673 	}
674     }
675     return 1;
676 }
677 
678 /* Find the pitch period.  This is a critical step, and we may have to try
679    multiple ways to get a good answer.  This version uses AMDF.  To improve
680    speed, we down sample by an integer factor get in the 11KHz range, and then
681    do it again with a narrower frequency range without down sampling */
findPitchPeriod(sonicStream stream,short * samples,int preferNewPeriod)682 static int findPitchPeriod(
683     sonicStream stream,
684     short *samples,
685     int preferNewPeriod)
686 {
687     int minPeriod = stream->minPeriod;
688     int maxPeriod = stream->maxPeriod;
689     int sampleRate = stream->sampleRate;
690     int minDiff, maxDiff, retPeriod;
691     int skip = 1;
692     int period;
693 
694     if(sampleRate > SONIC_AMDF_FREQ && stream->quality == 0) {
695 	skip = sampleRate/SONIC_AMDF_FREQ;
696     }
697     if(stream->numChannels == 1 && skip == 1) {
698 	period = findPitchPeriodInRange(samples, minPeriod, maxPeriod, &minDiff, &maxDiff);
699     } else {
700 	downSampleInput(stream, samples, skip);
701 	period = findPitchPeriodInRange(stream->downSampleBuffer, minPeriod/skip,
702 	    maxPeriod/skip, &minDiff, &maxDiff);
703 	if(skip != 1) {
704 	    period *= skip;
705 	    minPeriod = period - (skip << 2);
706 	    maxPeriod = period + (skip << 2);
707 	    if(minPeriod < stream->minPeriod) {
708 		minPeriod = stream->minPeriod;
709 	    }
710 	    if(maxPeriod > stream->maxPeriod) {
711 		maxPeriod = stream->maxPeriod;
712 	    }
713 	    if(stream->numChannels == 1) {
714 		period = findPitchPeriodInRange(samples, minPeriod, maxPeriod,
715 		    &minDiff, &maxDiff);
716 	    } else {
717 		downSampleInput(stream, samples, 1);
718 		period = findPitchPeriodInRange(stream->downSampleBuffer, minPeriod,
719 		    maxPeriod, &minDiff, &maxDiff);
720 	    }
721 	}
722     }
723     if(prevPeriodBetter(stream, period, minDiff, maxDiff, preferNewPeriod)) {
724         retPeriod = stream->prevPeriod;
725     } else {
726 	retPeriod = period;
727     }
728     stream->prevMinDiff = minDiff;
729     stream->prevPeriod = period;
730     return retPeriod;
731 }
732 
733 /* Overlap two sound segments, ramp the volume of one down, while ramping the
734    other one from zero up, and add them, storing the result at the output. */
overlapAdd(int numSamples,int numChannels,short * out,short * rampDown,short * rampUp)735 static void overlapAdd(
736     int numSamples,
737     int numChannels,
738     short *out,
739     short *rampDown,
740     short *rampUp)
741 {
742     short *o, *u, *d;
743     int i, t;
744 
745     for(i = 0; i < numChannels; i++) {
746 	o = out + i;
747 	u = rampUp + i;
748 	d = rampDown + i;
749 	for(t = 0; t < numSamples; t++) {
750 #ifdef SONIC_USE_SIN
751 	    float ratio = sin(t*M_PI/(2*numSamples));
752 	    *o = *d*(1.0f - ratio) + *u*ratio;
753 #else
754 	    *o = (*d*(numSamples - t) + *u*t)/numSamples;
755 #endif
756 	    o += numChannels;
757 	    d += numChannels;
758 	    u += numChannels;
759 	}
760     }
761 }
762 
763 /* Overlap two sound segments, ramp the volume of one down, while ramping the
764    other one from zero up, and add them, storing the result at the output. */
overlapAddWithSeparation(int numSamples,int numChannels,int separation,short * out,short * rampDown,short * rampUp)765 static void overlapAddWithSeparation(
766     int numSamples,
767     int numChannels,
768     int separation,
769     short *out,
770     short *rampDown,
771     short *rampUp)
772 {
773     short *o, *u, *d;
774     int i, t;
775 
776     for(i = 0; i < numChannels; i++) {
777 	o = out + i;
778 	u = rampUp + i;
779 	d = rampDown + i;
780 	for(t = 0; t < numSamples + separation; t++) {
781 	    if(t < separation) {
782 		*o = *d*(numSamples - t)/numSamples;
783 		d += numChannels;
784 	    } else if(t < numSamples) {
785 		*o = (*d*(numSamples - t) + *u*(t - separation))/numSamples;
786 		d += numChannels;
787 		u += numChannels;
788 	    } else {
789 		*o = *u*(t - separation)/numSamples;
790 		u += numChannels;
791 	    }
792 	    o += numChannels;
793 	}
794     }
795 }
796 
797 /* Just move the new samples in the output buffer to the pitch buffer */
moveNewSamplesToPitchBuffer(sonicStream stream,int originalNumOutputSamples)798 static int moveNewSamplesToPitchBuffer(
799     sonicStream stream,
800     int originalNumOutputSamples)
801 {
802     int numSamples = stream->numOutputSamples - originalNumOutputSamples;
803     int numChannels = stream->numChannels;
804 
805     if(stream->numPitchSamples + numSamples > stream->pitchBufferSize) {
806 	stream->pitchBufferSize += (stream->pitchBufferSize >> 1) + numSamples;
807 	stream->pitchBuffer = (short *)realloc(stream->pitchBuffer,
808 	    stream->pitchBufferSize*sizeof(short)*numChannels);
809 	if(stream->pitchBuffer == NULL) {
810 	    return 0;
811 	}
812     }
813     memcpy(stream->pitchBuffer + stream->numPitchSamples*numChannels,
814         stream->outputBuffer + originalNumOutputSamples*numChannels,
815 	numSamples*sizeof(short)*numChannels);
816     stream->numOutputSamples = originalNumOutputSamples;
817     stream->numPitchSamples += numSamples;
818     return 1;
819 }
820 
821 /* Remove processed samples from the pitch buffer. */
removePitchSamples(sonicStream stream,int numSamples)822 static void removePitchSamples(
823     sonicStream stream,
824     int numSamples)
825 {
826     int numChannels = stream->numChannels;
827     short *source = stream->pitchBuffer + numSamples*numChannels;
828 
829     if(numSamples == 0) {
830 	return;
831     }
832     if(numSamples != stream->numPitchSamples) {
833 	memmove(stream->pitchBuffer, source, (stream->numPitchSamples -
834 	    numSamples)*sizeof(short)*numChannels);
835     }
836     stream->numPitchSamples -= numSamples;
837 }
838 
839 /* Change the pitch.  The latency this introduces could be reduced by looking at
840    past samples to determine pitch, rather than future. */
adjustPitch(sonicStream stream,int originalNumOutputSamples)841 static int adjustPitch(
842     sonicStream stream,
843     int originalNumOutputSamples)
844 {
845     float pitch = stream->pitch;
846     int numChannels = stream->numChannels;
847     int period, newPeriod, separation;
848     int position = 0;
849     short *out, *rampDown, *rampUp;
850 
851     if(stream->numOutputSamples == originalNumOutputSamples) {
852 	return 1;
853     }
854     if(!moveNewSamplesToPitchBuffer(stream, originalNumOutputSamples)) {
855 	return 0;
856     }
857     while(stream->numPitchSamples - position >= stream->maxRequired) {
858 	period = findPitchPeriod(stream, stream->pitchBuffer + position*numChannels, 0);
859 	newPeriod = period/pitch;
860 	if(!enlargeOutputBufferIfNeeded(stream, newPeriod)) {
861 	    return 0;
862 	}
863 	out = stream->outputBuffer + stream->numOutputSamples*numChannels;
864 	if(pitch >= 1.0f) {
865 	    rampDown = stream->pitchBuffer + position*numChannels;
866 	    rampUp = stream->pitchBuffer + (position + period - newPeriod)*numChannels;
867 	    overlapAdd(newPeriod, numChannels, out, rampDown, rampUp);
868 	} else {
869 	    rampDown = stream->pitchBuffer + position*numChannels;
870 	    rampUp = stream->pitchBuffer + position*numChannels;
871 	    separation = newPeriod - period;
872 	    overlapAddWithSeparation(period, numChannels, separation, out, rampDown, rampUp);
873 	}
874 	stream->numOutputSamples += newPeriod;
875 	position += period;
876     }
877     removePitchSamples(stream, position);
878     return 1;
879 }
880 
881 /* Interpolate the new output sample. */
interpolate(sonicStream stream,short * in,int oldSampleRate,int newSampleRate)882 static short interpolate(
883     sonicStream stream,
884     short *in,
885     int oldSampleRate,
886     int newSampleRate)
887 {
888     short left = *in;
889     short right = in[stream->numChannels];
890     int position = stream->newRatePosition*oldSampleRate;
891     int leftPosition = stream->oldRatePosition*newSampleRate;
892     int rightPosition = (stream->oldRatePosition + 1)*newSampleRate;
893     int ratio = rightPosition - position;
894     int width = rightPosition - leftPosition;
895 
896     return (ratio*left + (width - ratio)*right)/width;
897 }
898 
899 /* Change the rate. */
adjustRate(sonicStream stream,float rate,int originalNumOutputSamples)900 static int adjustRate(
901     sonicStream stream,
902     float rate,
903     int originalNumOutputSamples)
904 {
905     int newSampleRate = stream->sampleRate/rate;
906     int oldSampleRate = stream->sampleRate;
907     int numChannels = stream->numChannels;
908     int position = 0;
909     short *in, *out;
910     int i;
911 
912     /* Set these values to help with the integer math */
913     while(newSampleRate > (1 << 14) || oldSampleRate > (1 << 14)) {
914 	newSampleRate >>= 1;
915 	oldSampleRate >>= 1;
916     }
917     if(stream->numOutputSamples == originalNumOutputSamples) {
918 	return 1;
919     }
920     if(!moveNewSamplesToPitchBuffer(stream, originalNumOutputSamples)) {
921 	return 0;
922     }
923     /* Leave at least one pitch sample in the buffer */
924     for(position = 0; position < stream->numPitchSamples - 1; position++) {
925 	while((stream->oldRatePosition + 1)*newSampleRate >
926 	        stream->newRatePosition*oldSampleRate) {
927 	    if(!enlargeOutputBufferIfNeeded(stream, 1)) {
928 		return 0;
929 	    }
930 	    out = stream->outputBuffer + stream->numOutputSamples*numChannels;
931 	    in = stream->pitchBuffer + position;
932 	    for(i = 0; i < numChannels; i++) {
933 		*out++ = interpolate(stream, in, oldSampleRate, newSampleRate);
934 		in++;
935 	    }
936 	    stream->newRatePosition++;
937 	    stream->numOutputSamples++;
938 	}
939 	stream->oldRatePosition++;
940 	if(stream->oldRatePosition == oldSampleRate) {
941 	    stream->oldRatePosition = 0;
942 	    if(stream->newRatePosition != newSampleRate) {
943 		fprintf(stderr,
944 		    "Assertion failed: stream->newRatePosition != newSampleRate\n");
945 		exit(1);
946 	    }
947 	    stream->newRatePosition = 0;
948 	}
949     }
950     removePitchSamples(stream, position);
951     return 1;
952 }
953 
954 
955 /* Skip over a pitch period, and copy period/speed samples to the output */
skipPitchPeriod(sonicStream stream,short * samples,float speed,int period)956 static int skipPitchPeriod(
957     sonicStream stream,
958     short *samples,
959     float speed,
960     int period)
961 {
962     long newSamples;
963     int numChannels = stream->numChannels;
964 
965     if(speed >= 2.0f) {
966 	newSamples = period/(speed - 1.0f);
967     } else {
968 	newSamples = period;
969 	stream->remainingInputToCopy = period*(2.0f - speed)/(speed - 1.0f);
970     }
971     if(!enlargeOutputBufferIfNeeded(stream, newSamples)) {
972 	return 0;
973     }
974     overlapAdd(newSamples, numChannels, stream->outputBuffer +
975         stream->numOutputSamples*numChannels, samples, samples + period*numChannels);
976     stream->numOutputSamples += newSamples;
977     return newSamples;
978 }
979 
980 /* Insert a pitch period, and determine how much input to copy directly. */
insertPitchPeriod(sonicStream stream,short * samples,float speed,int period)981 static int insertPitchPeriod(
982     sonicStream stream,
983     short *samples,
984     float speed,
985     int period)
986 {
987     long newSamples;
988     short *out;
989     int numChannels = stream->numChannels;
990 
991     if(speed < 0.5f) {
992         newSamples = period*speed/(1.0f - speed);
993     } else {
994         newSamples = period;
995 	stream->remainingInputToCopy = period*(2.0f*speed - 1.0f)/(1.0f - speed);
996     }
997     if(!enlargeOutputBufferIfNeeded(stream, period + newSamples)) {
998 	return 0;
999     }
1000     out = stream->outputBuffer + stream->numOutputSamples*numChannels;
1001     memcpy(out, samples, period*sizeof(short)*numChannels);
1002     out = stream->outputBuffer + (stream->numOutputSamples + period)*numChannels;
1003     overlapAdd(newSamples, numChannels, out, samples + period*numChannels, samples);
1004     stream->numOutputSamples += period + newSamples;
1005     return newSamples;
1006 }
1007 
1008 /* Resample as many pitch periods as we have buffered on the input.  Return 0 if
1009    we fail to resize an input or output buffer.  Also scale the output by the volume. */
changeSpeed(sonicStream stream,float speed)1010 static int changeSpeed(
1011     sonicStream stream,
1012     float speed)
1013 {
1014     short *samples;
1015     int numSamples = stream->numInputSamples;
1016     int position = 0, period, newSamples;
1017     int maxRequired = stream->maxRequired;
1018 
1019     if(stream->numInputSamples < maxRequired) {
1020 	return 1;
1021     }
1022     do {
1023 	if(stream->remainingInputToCopy > 0) {
1024             newSamples = copyInputToOutput(stream, position);
1025 	    position += newSamples;
1026 	} else {
1027 	    samples = stream->inputBuffer + position*stream->numChannels;
1028 	    period = findPitchPeriod(stream, samples, 1);
1029 	    if(speed > 1.0) {
1030 		newSamples = skipPitchPeriod(stream, samples, speed, period);
1031 		position += period + newSamples;
1032 	    } else {
1033 		newSamples = insertPitchPeriod(stream, samples, speed, period);
1034 		position += newSamples;
1035 	    }
1036 	}
1037 	if(newSamples == 0) {
1038 	    return 0; /* Failed to resize output buffer */
1039 	}
1040     } while(position + maxRequired <= numSamples);
1041     removeInputSamples(stream, position);
1042     return 1;
1043 }
1044 
1045 /* Resample as many pitch periods as we have buffered on the input.  Return 0 if
1046    we fail to resize an input or output buffer.  Also scale the output by the volume. */
processStreamInput(sonicStream stream)1047 static int processStreamInput(
1048     sonicStream stream)
1049 {
1050     int originalNumOutputSamples = stream->numOutputSamples;
1051     float speed = stream->speed/stream->pitch;
1052     float rate = stream->rate;
1053 
1054     if(!stream->useChordPitch) {
1055 	rate *= stream->pitch;
1056     }
1057     if(speed > 1.00001 || speed < 0.99999) {
1058 	changeSpeed(stream, speed);
1059     } else {
1060         if(!copyToOutput(stream, stream->inputBuffer, stream->numInputSamples)) {
1061 	    return 0;
1062 	}
1063 	stream->numInputSamples = 0;
1064     }
1065     if(stream->useChordPitch) {
1066 	if(stream->pitch != 1.0f) {
1067 	    if(!adjustPitch(stream, originalNumOutputSamples)) {
1068 		return 0;
1069 	    }
1070 	}
1071     } else if(rate != 1.0f) {
1072 	if(!adjustRate(stream, rate, originalNumOutputSamples)) {
1073 	    return 0;
1074 	}
1075     }
1076     if(stream->volume != 1.0f) {
1077 	/* Adjust output volume. */
1078         scaleSamples(stream->outputBuffer + originalNumOutputSamples*stream->numChannels,
1079 	    (stream->numOutputSamples - originalNumOutputSamples)*stream->numChannels,
1080 	    stream->volume);
1081     }
1082     return 1;
1083 }
1084 
1085 /* Write floating point data to the input buffer and process it. */
sonicWriteFloatToStream(sonicStream stream,float * samples,int numSamples)1086 int sonicWriteFloatToStream(
1087     sonicStream stream,
1088     float *samples,
1089     int numSamples)
1090 {
1091     if(!addFloatSamplesToInputBuffer(stream, samples, numSamples)) {
1092 	return 0;
1093     }
1094     return processStreamInput(stream);
1095 }
1096 
1097 /* Simple wrapper around sonicWriteFloatToStream that does the short to float
1098    conversion for you. */
sonicWriteShortToStream(sonicStream stream,short * samples,int numSamples)1099 int sonicWriteShortToStream(
1100     sonicStream stream,
1101     short *samples,
1102     int numSamples)
1103 {
1104     if(!addShortSamplesToInputBuffer(stream, samples, numSamples)) {
1105 	return 0;
1106     }
1107     return processStreamInput(stream);
1108 }
1109 
1110 /* Simple wrapper around sonicWriteFloatToStream that does the unsigned char to float
1111    conversion for you. */
sonicWriteUnsignedCharToStream(sonicStream stream,unsigned char * samples,int numSamples)1112 int sonicWriteUnsignedCharToStream(
1113     sonicStream stream,
1114     unsigned char *samples,
1115     int numSamples)
1116 {
1117     if(!addUnsignedCharSamplesToInputBuffer(stream, samples, numSamples)) {
1118 	return 0;
1119     }
1120     return processStreamInput(stream);
1121 }
1122 
1123 /* This is a non-stream oriented interface to just change the speed of a sound sample */
sonicChangeFloatSpeed(float * samples,int numSamples,float speed,float pitch,float rate,float volume,int useChordPitch,int sampleRate,int numChannels)1124 int sonicChangeFloatSpeed(
1125     float *samples,
1126     int numSamples,
1127     float speed,
1128     float pitch,
1129     float rate,
1130     float volume,
1131     int useChordPitch,
1132     int sampleRate,
1133     int numChannels)
1134 {
1135     sonicStream stream = sonicCreateStream(sampleRate, numChannels);
1136 
1137     sonicSetSpeed(stream, speed);
1138     sonicSetPitch(stream, pitch);
1139     sonicSetRate(stream, rate);
1140     sonicSetVolume(stream, volume);
1141     sonicSetChordPitch(stream, useChordPitch);
1142     sonicWriteFloatToStream(stream, samples, numSamples);
1143     sonicFlushStream(stream);
1144     numSamples = sonicSamplesAvailable(stream);
1145     sonicReadFloatFromStream(stream, samples, numSamples);
1146     sonicDestroyStream(stream);
1147     return numSamples;
1148 }
1149 
1150 /* This is a non-stream oriented interface to just change the speed of a sound sample */
sonicChangeShortSpeed(short * samples,int numSamples,float speed,float pitch,float rate,float volume,int useChordPitch,int sampleRate,int numChannels)1151 int sonicChangeShortSpeed(
1152     short *samples,
1153     int numSamples,
1154     float speed,
1155     float pitch,
1156     float rate,
1157     float volume,
1158     int useChordPitch,
1159     int sampleRate,
1160     int numChannels)
1161 {
1162     sonicStream stream = sonicCreateStream(sampleRate, numChannels);
1163 
1164     sonicSetSpeed(stream, speed);
1165     sonicSetPitch(stream, pitch);
1166     sonicSetRate(stream, rate);
1167     sonicSetVolume(stream, volume);
1168     sonicSetChordPitch(stream, useChordPitch);
1169     sonicWriteShortToStream(stream, samples, numSamples);
1170     sonicFlushStream(stream);
1171     numSamples = sonicSamplesAvailable(stream);
1172     sonicReadShortFromStream(stream, samples, numSamples);
1173     sonicDestroyStream(stream);
1174     return numSamples;
1175 }
1176