Open Ephys GUI
 All Classes Functions Variables
Utilities.h
1 /*******************************************************************************
2 
3 "A Collection of Useful C++ Classes for Digital Signal Processing"
4  By Vincent Falco
5 
6 Official project location:
7 http://code.google.com/p/dspfilterscpp/
8 
9 See Documentation.cpp for contact information, notes, and bibliography.
10 
11 --------------------------------------------------------------------------------
12 
13 License: MIT License (http://www.opensource.org/licenses/mit-license.php)
14 Copyright (c) 2009 by Vincent Falco
15 
16 Permission is hereby granted, free of charge, to any person obtaining a copy
17 of this software and associated documentation files (the "Software"), to deal
18 in the Software without restriction, including without limitation the rights
19 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
20 copies of the Software, and to permit persons to whom the Software is
21 furnished to do so, subject to the following conditions:
22 
23 The above copyright notice and this permission notice shall be included in
24 all copies or substantial portions of the Software.
25 
26 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
27 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
28 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
29 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
30 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
31 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
32 THE SOFTWARE.
33 
34 *******************************************************************************/
35 
36 #ifndef DSPFILTERS_UTILITIES_H
37 #define DSPFILTERS_UTILITIES_H
38 
39 #include "Common.h"
40 
41 namespace Dsp {
42 
43 /*
44  * Utilities
45  *
46  * These routines are handy for manipulating buffers of samples.
47  *
48  */
49 
50 //------------------------------------------------------------------------------
51 
52 // Add src samples to dest, without clip or overflow checking.
53 template <class Td,
54  class Ts>
55 void add (int samples,
56  Td* dest,
57  Ts const* src,
58  int destSkip = 0,
59  int srcSkip = 0)
60 {
61  if (srcSkip !=0 || destSkip != 0)
62  {
63  ++srcSkip;
64  ++destSkip;
65  while (--samples >= 0)
66  {
67  *dest = static_cast<Td>(*src);
68  dest += destSkip;
69  src += srcSkip;
70  }
71  }
72  else
73  {
74  while (--samples >= 0)
75  *dest++ += static_cast<Td>(*src++);
76  }
77 }
78 
79 // Multichannel add
80 template <typename Td,
81  typename Ts>
82 void add (int channels,
83  int samples,
84  Td* const* dest,
85  Ts const* const* src)
86 {
87  for (int i = channels; --i >= 0;)
88  add (samples, dest[i], src[i]);
89 }
90 
91 //--------------------------------------------------------------------------
92 
93 // Copy samples from src to dest, which may not overlap. Performs an implicit
94 // type conversion if Ts and Td are different (for example, float to double).
95 template <typename Td,
96  typename Ts>
97 void copy (int samples,
98  Td* dest,
99  Ts const* src,
100  int destSkip = 0,
101  int srcSkip = 0)
102 {
103  if (srcSkip != 0)
104  {
105  if (destSkip != 0)
106  {
107  ++srcSkip;
108  ++destSkip;
109  while (--samples >= 0)
110  {
111  *dest++ = *src++;
112  dest += destSkip;
113  src += srcSkip;
114  }
115  }
116  else
117  {
118  ++srcSkip;
119  while (--samples >= 0)
120  {
121  *dest++ = *src++;
122  src += srcSkip;
123  }
124  }
125  }
126  else if (destSkip != 0)
127  {
128  ++destSkip;
129  while (--samples >= 0)
130  {
131  *dest = *src++;
132  dest += destSkip;
133  }
134  }
135  else
136  {
137  while (--samples >= 0)
138  *dest++ = *src++;
139  }
140 }
141 
142 // Wrapper that uses memcpy if there is no skip and the types are the same
143 template <typename Ty>
144 void copy (int samples,
145  Ty* dest,
146  Ty const* src,
147  int destSkip = 0,
148  int srcSkip = 0)
149 {
150  if (destSkip != 0 || srcSkip != 0)
151  copy<Ty,Ty> (samples, dest, src, destSkip, srcSkip);
152  else
153  ::memcpy (dest, src, samples * sizeof(src[0]));
154 }
155 
156 // Copy a set of channels from src to dest, with implicit type conversion.
157 template <typename Td,
158  typename Ts>
159 void copy (int channels,
160  int samples,
161  Td* const* dest,
162  Ts const* const* src,
163  int destSkip = 0,
164  int srcSkip = 0)
165 {
166  for (int i = channels; --i >= 0;)
167  copy (samples, dest[i], src[i], destSkip, srcSkip);
168 }
169 
170 //--------------------------------------------------------------------------
171 
172 // Deinterleave channels. Performs implicit type conversion.
173 template <typename Td, typename Ts>
174 void deinterleave (int channels,
175  int samples,
176  Td* const* dest,
177  Ts const* src)
178 {
179  assert (channels > 1);
180 
181  switch (channels)
182  {
183  case 2:
184  {
185  Td* l = dest[0];
186  Td* r = dest[1];
187  int n = (samples + 7) / 8;
188  switch (samples % 8)
189  {
190  case 0: do
191  {
192  *l++ = *src++; *r++ = *src++;
193  case 7: *l++ = *src++; *r++ = *src++;
194  case 6: *l++ = *src++; *r++ = *src++;
195  case 5: *l++ = *src++; *r++ = *src++;
196  case 4: *l++ = *src++; *r++ = *src++;
197  case 3: *l++ = *src++; *r++ = *src++;
198  case 2: *l++ = *src++; *r++ = *src++;
199  case 1: *l++ = *src++; *r++ = *src++;
200  }
201  while (--n > 0);
202  }
203  }
204  break;
205 
206  default:
207  {
208  for (int i = channels; --i >= 0;)
209  copy (samples, dest[i], src + i, 0, channels - 1);
210  }
211  break;
212  };
213 }
214 
215 // Convenience for a stereo pair of channels
216 template <typename Td,
217  typename Ts>
218 void deinterleave (int samples,
219  Td* left,
220  Td* right,
221  Ts const* src)
222 {
223  Td* dest[2];
224  dest[0] = left;
225  dest[1] = right;
226  deinterleave (2, samples, dest, src);
227 }
228 
229 //--------------------------------------------------------------------------
230 
231 // Fade src into dest
232 template <typename Td,
233  typename Ts,
234  typename Ty>
235 void fade (int samples,
236  Td* dest,
237  Ts const* src,
238  Ty start = 0,
239  Ty end = 1)
240 {
241  Ty t = start;
242  Ty dt = (end - start) / samples;
243 
244  while (--samples >= 0)
245  {
246  *dest++ = static_cast<Td>(*dest + t * (*src++ - *dest));
247  t += dt;
248  }
249 }
250 
251 // Fade src channels into dest channels
252 template <typename Td,
253  typename Ts,
254  typename Ty>
255 void fade (int channels,
256  int samples,
257  Td* const* dest,
258  Ts const* const* src,
259  Ty start = 0,
260  Ty end = 1)
261 {
262  for (int i = channels; --i >= 0;)
263  fade (samples, dest[i], src[i], start, end);
264 }
265 
266 //--------------------------------------------------------------------------
267 
268 // Interleave separate channels from source pointers to destination
269 // (Destination requires channels*frames samples of storage). Performs
270 // implicit type conversion.
271 template <typename Td,
272  typename Ts>
273 void interleave (int channels,
274  size_t samples,
275  Td* dest,
276  Ts const* const* src)
277 {
278  assert (channels>1);
279 
280  if (samples==0)
281  return;
282 
283  switch (channels)
284  {
285  case 2:
286  {
287  const Ts* l = src[0];
288  const Ts* r = src[1];
289 
290  // note that Duff's Device only works when samples>0
291  int n = (samples + 7) / 8;
292  switch (samples % 8)
293  {
294  case 0: do
295  {
296  *dest++ = *l++; *dest++ = *r++;
297  case 7: *dest++ = *l++; *dest++ = *r++;
298  case 6: *dest++ = *l++; *dest++ = *r++;
299  case 5: *dest++ = *l++; *dest++ = *r++;
300  case 4: *dest++ = *l++; *dest++ = *r++;
301  case 3: *dest++ = *l++; *dest++ = *r++;
302  case 2: *dest++ = *l++; *dest++ = *r++;
303  case 1: *dest++ = *l++; *dest++ = *r++;
304  }
305  while (--n > 0);
306  }
307  }
308  break;
309 
310  default:
311  {
312  for (int i = channels; --i >= 0;)
313  copy (samples, dest + i, src[i], channels - 1, 0);
314  }
315  break;
316  };
317 }
318 
319 //--------------------------------------------------------------------------
320 
321 // Convenience for a stereo channel pair
322 template <typename Td,
323  typename Ts>
324 void interleave (int samples,
325  Td* dest,
326  Ts const* left,
327  Ts const* right)
328 {
329  const Ts* src[2];
330  src[0] = left;
331  src[1] = right;
332  interleave (2, samples, dest, src);
333 }
334 
335 //--------------------------------------------------------------------------
336 
337 // Multiply samples by a constant, without clip or overflow checking.
338 template <typename Td,
339  typename Ty>
340 void multiply (int samples,
341  Td* dest,
342  Ty factor,
343  int destSkip = 0)
344 {
345  if (destSkip != 0)
346  {
347  ++destSkip;
348  while (--samples >= 0)
349  {
350  *dest = static_cast<Td>(*dest * factor);
351  dest += destSkip;
352  }
353  }
354  else
355  {
356  while (--samples >= 0)
357  *dest++ = static_cast<Td>(*dest * factor);
358  }
359 }
360 
361 // Multiply a set of channels by a constant.
362 template <typename Td,
363  typename Ty>
364 void multiply (int channels,
365  int samples,
366  Td* const* dest,
367  Ty factor,
368  int destSkip = 0)
369 {
370  for (int i = channels; --i >= 0;)
371  multiply (samples, dest[i], factor, destSkip);
372 }
373 
374 //--------------------------------------------------------------------------
375 
376 // Copy samples from src to dest in reversed order. Performs implicit
377 // type conversion. src and dest may not overlap.
378 template <typename Td,
379  typename Ts>
380 void reverse (int samples,
381  Td* dest,
382  Ts const* src,
383  int destSkip = 0,
384  int srcSkip = 0 )
385 {
386  src += (srcSkip + 1) * samples;
387 
388  if (srcSkip != 0 || destSkip == 0)
389  {
390  ++srcSkip;
391  ++destSkip;
392  while (--samples >= 0)
393  {
394  src -= srcSkip;
395  *dest = *src;
396  dest += destSkip;
397  }
398  }
399  else
400  {
401  while (--samples >= 0)
402  *dest++ = *--src;
403  }
404 }
405 
406 template <typename Td, typename Ts>
407 void reverse (int channels, size_t frames, Td* const* dest, const Ts* const* src)
408 {
409  for (int i = channels; --i >= 0;)
410  reverse (frames, dest[i], src[i]);
411 }
412 
413 //--------------------------------------------------------------------------
414 
415 template <typename Tn>
416 void to_mono (int samples, Tn* dest, Tn const* left, Tn const* right)
417 {
418 #if 1
419  while (samples-- > 0)
420  *dest++ = (*left++ + *right++) * Tn(0.70710678118654752440084436210485);
421 #else
422  while (samples-- > 0)
423  *dest++ = (*left++ + *right++) * Tn(0.5);
424 #endif
425 }
426 
427 //--------------------------------------------------------------------------
428 
429 template <typename T>
430 void validate (int numChannels, int numSamples, T const* const* src)
431 {
432  for (int i = 0; i < numChannels; ++i)
433  {
434  T const* p = src [i];
435  for (int j = numSamples; j > 0; --j)
436  {
437  T v = *p++;
438  assert (v < 2 && v > -2);
439  }
440  }
441 }
442 
443 //--------------------------------------------------------------------------
444 
445 #if 0
446 /*
447  * this stuff all depends on is_pod which is not always available
448  *
449  */
450 namespace detail {
451 
452 template <typename Ty,
453  bool isPod>
454 struct zero
455 {
456  static void process (int samples,
457  Ty* dest,
458  int destSkip)
459  {
460  if (destSkip != 0)
461  {
462  ++destSkip;
463  while (--samples >= 0)
464  {
465  *dest = Ty();
466  dest += destSkip;
467  }
468  }
469  else
470  {
471  std::fill (dest, dest + samples, Ty());
472  }
473  }
474 };
475 
476 template <typename Ty>
477 struct zero<Ty, true>
478 {
479  static void process (int samples,
480  Ty* dest,
481  int destSkip)
482  {
483  if (destSkip != 0)
484  zero<Ty,false>::process (samples, dest, destSkip);
485  else
486  ::memset (dest, 0, samples * sizeof(dest[0]));
487  }
488 };
489 
490 }
491 
492 // Fill a channel with zeros. This works even if Ty is not a basic type.
493 template <typename Ty>
494 void zero (int samples,
495  Ty* dest,
496  int destSkip = 0)
497 {
498  detail::zero<Ty, tr1::is_pod<Ty>::value>::process (samples, dest, destSkip );
499 }
500 
501 #else
502 // Fill a channel with zeros. This works even if Ty is not a basic type.
503 template <typename Ty>
504 void zero (int samples,
505  Ty* dest,
506  int destSkip = 0)
507 {
508  if (destSkip != 0)
509  {
510  ++destSkip;
511  while (--samples >= 0)
512  {
513  *dest = Ty();
514  dest += destSkip;
515  }
516  }
517  else
518  {
519  std::fill (dest, dest + samples, Ty());
520  }
521 }
522 
523 #endif
524 
525 // Fill a set of channels with zero.
526 template <typename Ty>
527 void zero (int channels,
528  int samples,
529  Ty* const* dest,
530  int destSkip = 0)
531 {
532  for (int i = channels; --i >= 0;)
533  zero (samples, dest[i], destSkip);
534 }
535 
536 //------------------------------------------------------------------------------
537 
538 // Implementation of Brent's Method provided by
539 // John D. Cook (http://www.johndcook.com/)
540 // The return value of Minimize is the minimum of the function f.
541 // The location where f takes its minimum is returned in the variable minLoc.
542 // Notation and implementation based on Chapter 5 of Richard Brent's book
543 // "Algorithms for Minimization Without Derivatives".
544 //
545 // Reference:
546 // http://www.codeproject.com/KB/recipes/one_variable_optimize.aspx?msg=2779038
547 
548 template <class TFunction>
549 double BrentMinimize
550 (
551  TFunction& f, // [in] objective function to minimize
552  double leftEnd, // [in] smaller value of bracketing interval
553  double rightEnd, // [in] larger value of bracketing interval
554  double epsilon, // [in] stopping tolerance
555  double& minLoc // [out] location of minimum
556 )
557 {
558  double d, e, m, p, q, r, tol, t2, u, v, w, fu, fv, fw, fx;
559  static const double c = 0.5*(3.0 - ::std::sqrt(5.0));
560  static const double SQRT_DBL_EPSILON = ::std::sqrt(DBL_EPSILON);
561 
562  double& a = leftEnd;
563  double& b = rightEnd;
564  double& x = minLoc;
565 
566  v = w = x = a + c*(b - a);
567  d = e = 0.0;
568  fv = fw = fx = f(x);
569  int counter = 0;
570 loop:
571  counter++;
572  m = 0.5*(a + b);
573  tol = SQRT_DBL_EPSILON*::fabs(x) + epsilon;
574  t2 = 2.0*tol;
575  // Check stopping criteria
576  if (::fabs(x - m) > t2 - 0.5*(b - a))
577  {
578  p = q = r = 0.0;
579  if (::fabs(e) > tol)
580  {
581  // fit parabola
582  r = (x - w)*(fx - fv);
583  q = (x - v)*(fx - fw);
584  p = (x - v)*q - (x - w)*r;
585  q = 2.0*(q - r);
586  (q > 0.0) ? p = -p : q = -q;
587  r = e;
588  e = d;
589  }
590  if (::fabs(p) < ::fabs(0.5*q*r) && p < q*(a - x) && p < q*(b - x))
591  {
592  // A parabolic interpolation step
593  d = p/q;
594  u = x + d;
595  // f must not be evaluated too close to a or b
596  if (u - a < t2 || b - u < t2)
597  d = (x < m) ? tol : -tol;
598  }
599  else
600  {
601  // A golden section step
602  e = (x < m) ? b : a;
603  e -= x;
604  d = c*e;
605  }
606  // f must not be evaluated too close to x
607  if (::fabs(d) >= tol)
608  u = x + d;
609  else if (d > 0.0)
610  u = x + tol;
611  else
612  u = x - tol;
613  fu = f(u);
614  // Update a, b, v, w, and x
615  if (fu <= fx)
616  {
617  (u < x) ? b = x : a = x;
618  v = w;
619  fv = fw;
620  w = x;
621  fw = fx;
622  x = u;
623  fx = fu;
624  }
625  else
626  {
627  (u < x) ? a = u : b = u;
628  if (fu <= fw || w == x)
629  {
630  v = w;
631  fv = fw;
632  w = u;
633  fw = fu;
634  }
635  else if (fu <= fv || v == x || v == w)
636  {
637  v = u;
638  fv = fu;
639  }
640  }
641  goto loop; // Yes, the dreaded goto statement. But the code
642  // here is faithful to Brent's orginal pseudocode.
643  }
644  return fx;
645 }
646 
647 //------------------------------------------------------------------------------
648 
649 // Tracks the peaks in the signal stream using the attack and release parameters
650 template <int Channels=2, typename Value=float>
652 {
653 public:
655  {
656  for (int i = 0; i < Channels; i++)
657  m_env[i]=0;
658  }
659 
660  Value operator[] (int channel) const
661  {
662  return m_env[channel];
663  }
664 
665  void Setup (int sampleRate, double attackMs, double releaseMs)
666  {
667  m_a = pow (0.01, 1.0 / (attackMs * sampleRate * 0.001));
668  m_r = pow (0.01, 1.0 / (releaseMs * sampleRate * 0.001));
669  }
670 
671  void Process (size_t samples, const Value** src)
672  {
673  for( int i=0; i<Channels; i++ )
674  {
675  const Value* cur = src[i];
676 
677  double e = m_env[i];
678  for (int n = samples; n; n--)
679  {
680  double v = ::fabs(*cur++);
681  if (v > e)
682  e = m_a * (e - v) + v;
683  else
684  e = m_r * (e - v) + v;
685  }
686  m_env[i]=e;
687  }
688  }
689 
690  double m_env[Channels];
691 
692 protected:
693  double m_a;
694  double m_r;
695 };
696 
697 }
698 
699 #endif