DSP.cs 38 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Text;
  4. using System.IO.IsolatedStorage;
  5. using System.IO;
  6. // http://afni.nimh.nih.gov/pub/dist/src/Haar.c
  7. namespace DSP
  8. {
  9. public class MFCC
  10. {
  11. private static double[] melWorkingFrequencies = new double[11] { 10.0, 20.0, 90.0, 300.0, 680.0, 1270.0, 2030.0, 2970.0, 4050.0, 5250.0, 6570.0 };
  12. public static int numMelFilters(int Nyquist)
  13. {
  14. //System.Diagnostics.Debug.WriteLine("Nyquist:" + Convert.ToString(Nyquist));
  15. double frequency = Nyquist;
  16. double delta = mel(frequency);
  17. int numFilters = 0;
  18. while (frequency > 10)
  19. {
  20. ++numFilters;
  21. frequency -= (delta / 2);
  22. delta = DSP.MFCC.mel(frequency);
  23. //System.Diagnostics.Debug.WriteLine("Frequency:" + Convert.ToString(frequency));
  24. }
  25. return numFilters;
  26. }
  27. /// <summary>
  28. /// Init
  29. /// </summary>
  30. /// <param name="sampleSize"></param>
  31. ///
  32. public static void initMelFrequenciesRange(int Nyquist)
  33. {
  34. //System.Diagnostics.Debug.WriteLine("Nyquist:" + Convert.ToString(Nyquist));
  35. double frequency = Nyquist;
  36. double delta = mel(frequency);
  37. int numFilters = numMelFilters(Nyquist);
  38. melWorkingFrequencies = new double[numFilters];
  39. frequency = Nyquist;
  40. delta = mel(frequency);
  41. int i = 0;
  42. double cFreq = 0;
  43. while (frequency > 10)
  44. {
  45. frequency -= (delta / 2);
  46. delta = mel(frequency);
  47. cFreq = Math.Round(frequency);
  48. //melWorkingFrequencies[numFilters] = Math.Round(frequency);
  49. melWorkingFrequencies[numFilters-1-i] = 10;
  50. while (melWorkingFrequencies[numFilters-1-i] < (cFreq - 10)) melWorkingFrequencies[numFilters-1-i] += 10;
  51. // System.Diagnostics.Debug.WriteLine("Frequency:" + Convert.ToString(melWorkingFrequencies[numFilters-1-i]));
  52. ++i;
  53. }
  54. }
  55. public static double[] compute(ref double[] signal)
  56. {
  57. double[] result = new double[melWorkingFrequencies.Length];
  58. double[] mfcc = new double[melWorkingFrequencies.Length];
  59. int segment = 0;
  60. int start = 0;
  61. int end = 0;
  62. for (int i = 0; i < melWorkingFrequencies.Length; i++)
  63. {
  64. result[i] = 0;
  65. segment = (int) Math.Round(mel( melWorkingFrequencies[i] ) / 10 );
  66. /*System.Diagnostics.Debug.WriteLine("slot #" + Convert.ToString(i));
  67. System.Diagnostics.Debug.WriteLine("freq:" + Convert.ToString(melWorkingFrequencies[i]));
  68. System.Diagnostics.Debug.WriteLine("mel:" + Convert.ToString(mel(melWorkingFrequencies[i])));
  69. System.Diagnostics.Debug.WriteLine("segment:"+Convert.ToString(segment));*/
  70. start = (segment - (int)Math.Floor(segment / 2));
  71. end = (segment + (segment / 2));
  72. //System.Diagnostics.Debug.WriteLine("\tstart:" + Convert.ToString(start) + "\tend:" + Convert.ToString(end));
  73. for (int j = start; j < end; j++)
  74. {
  75. // System.Diagnostics.Debug.WriteLine("\t\tfilter slopet:" + Convert.ToString(DSP.Filters.Triangular(j-start, segment)));
  76. result[i] += signal[j] * DSP.Filters.Triangular(j, segment);
  77. }
  78. //System.Diagnostics.Debug.WriteLine("result[i]:" + Convert.ToString(result[i]));
  79. result[i] = (result[i] > 0) ? Math.Log10( Math.Abs(result[i]) ) : 0;
  80. }
  81. for (int i = 0; i < melWorkingFrequencies.Length; i++)
  82. {
  83. for (int j = 0; j < melWorkingFrequencies.Length; j++)
  84. {
  85. mfcc[i] += result[i] * Math.Cos(((Math.PI * i) / melWorkingFrequencies.Length) * (j - 0.5));
  86. }
  87. mfcc[i] *= Math.Sqrt(2.0 / (double) melWorkingFrequencies.Length);
  88. //System.Diagnostics.Debug.WriteLine("result[i]:" + Convert.ToString(mfcc[i]));
  89. }
  90. return mfcc;
  91. }
  92. public static double mel(double value)
  93. {
  94. return (2595.0 * (double)Math.Log10(1.0 + value / 700.0));
  95. }
  96. public static double melinv(double value)
  97. {
  98. return (700.0 * ((double)Math.Pow(10.0, value / 2595.0) - 1.0));
  99. }
  100. }
  101. public class FourierTransform
  102. {
  103. public const int Raw = 1;
  104. public const int Decibel = 2;
  105. public const int FREQUENCYSLOTCOUNT = 21;
  106. private static int[] _meterFrequencies = new int[FREQUENCYSLOTCOUNT] { 20, 30, 55, 80, 120, 155, 195, 250, 375, 500, 750, 1000, 1500, 2000, 3000, 4000, 6000, 8000, 12000, 16000, 20000 };
  107. private static int _frequencySlotCount = FREQUENCYSLOTCOUNT;
  108. /// <summary>
  109. /// Changes the Frequency Bands to analyze.
  110. /// Affects the other static methods
  111. /// </summary>
  112. /// <param name="meterFrequencies"></param>
  113. public static void SetMeterFrequencies(int[] meterFrequencies)
  114. {
  115. _meterFrequencies = meterFrequencies;
  116. _frequencySlotCount = meterFrequencies.Length;
  117. }
  118. public static double[] Spectrum(ref double[] x, int method = Raw)
  119. {
  120. //uint pow2Samples = FFT.NextPowerOfTwo((uint)x.Length);
  121. double[] xre = new double[x.Length];
  122. double[] xim = new double[x.Length];
  123. Compute((uint)x.Length, x, null, xre, xim, false);
  124. double[] decibel = new double[xre.Length / 2];
  125. for (int i = 0; i < decibel.Length; i++)
  126. decibel[i] = (method == Decibel) ? 10.0 * Math.Log10((float)(Math.Sqrt((xre[i] * xre[i]) + (xim[i] * xim[i])))) : (float)(Math.Sqrt((xre[i] * xre[i]) + (xim[i] * xim[i])));
  127. return decibel;
  128. }
  129. /// <summary>
  130. /// Get Number of bits needed for a power of two
  131. /// </summary>
  132. /// <param name="PowerOfTwo">Power of two number</param>
  133. /// <returns>Number of bits</returns>
  134. public static UInt32 NumberOfBitsNeeded(UInt32 PowerOfTwo)
  135. {
  136. if (PowerOfTwo > 0)
  137. {
  138. for (UInt32 i = 0, mask = 1; ; i++, mask <<= 1)
  139. {
  140. if ((PowerOfTwo & mask) != 0)
  141. return i;
  142. }
  143. }
  144. return 0; // error
  145. }
  146. /// <summary>
  147. /// Reverse bits
  148. /// </summary>
  149. /// <param name="index">Bits</param>
  150. /// <param name="NumBits">Number of bits to reverse</param>
  151. /// <returns>Reverse Bits</returns>
  152. public static UInt32 ReverseBits(UInt32 index, UInt32 NumBits)
  153. {
  154. UInt32 i, rev;
  155. for (i = rev = 0; i < NumBits; i++)
  156. {
  157. rev = (rev << 1) | (index & 1);
  158. index >>= 1;
  159. }
  160. return rev;
  161. }
  162. /// <summary>
  163. /// Return index to frequency based on number of samples
  164. /// </summary>
  165. /// <param name="Index">sample index</param>
  166. /// <param name="NumSamples">number of samples</param>
  167. /// <returns>Frequency index range</returns>
  168. public static Double IndexToFrequency(UInt32 Index, UInt32 NumSamples)
  169. {
  170. if (Index >= NumSamples)
  171. return 0.0;
  172. else if (Index <= NumSamples / 2)
  173. return (double)Index / (double)NumSamples;
  174. return -(double)(NumSamples - Index) / (double)NumSamples;
  175. }
  176. /// <summary>
  177. /// Compute FFT
  178. /// </summary>
  179. /// <param name="NumSamples">NumSamples Number of samples (must be power two)</param>
  180. /// <param name="pRealIn">Real samples</param>
  181. /// <param name="pImagIn">Imaginary (optional, may be null)</param>
  182. /// <param name="pRealOut">Real coefficient output</param>
  183. /// <param name="pImagOut">Imaginary coefficient output</param>
  184. /// <param name="bInverseTransform">bInverseTransform when true, compute Inverse FFT</param>
  185. public static void Compute(UInt32 NumSamples, Double[] pRealIn, Double[] pImagIn,
  186. Double[] pRealOut, Double[] pImagOut, Boolean bInverseTransform)
  187. {
  188. UInt32 NumBits; /* Number of bits needed to store indices */
  189. UInt32 i, j, k, n;
  190. UInt32 BlockSize, BlockEnd;
  191. double angle_numerator = 2.0 * Utilities.DDC_PI;
  192. double tr, ti; /* temp real, temp imaginary */
  193. if (pRealIn == null || pRealOut == null || pImagOut == null)
  194. {
  195. // error
  196. throw new ArgumentNullException("Null argument");
  197. }
  198. if (!Utilities.IsPowerOfTwo(NumSamples))
  199. {
  200. // error
  201. throw new ArgumentException("Number of samples must be power of 2");
  202. }
  203. if (pRealIn.Length < NumSamples || (pImagIn != null && pImagIn.Length < NumSamples) ||
  204. pRealOut.Length < NumSamples || pImagOut.Length < NumSamples)
  205. {
  206. // error
  207. throw new ArgumentException("Invalid Array argument detected");
  208. }
  209. if (bInverseTransform)
  210. angle_numerator = -angle_numerator;
  211. NumBits = NumberOfBitsNeeded(NumSamples);
  212. /*
  213. ** Do simultaneous data copy and bit-reversal ordering into outputs...
  214. */
  215. for (i = 0; i < NumSamples; i++)
  216. {
  217. j = ReverseBits(i, NumBits);
  218. pRealOut[j] = pRealIn[i];
  219. pImagOut[j] = (double)((pImagIn == null) ? 0.0 : pImagIn[i]);
  220. }
  221. /*
  222. ** Do the FFT itself...
  223. */
  224. BlockEnd = 1;
  225. for (BlockSize = 2; BlockSize <= NumSamples; BlockSize <<= 1)
  226. {
  227. double delta_angle = angle_numerator / (double)BlockSize;
  228. double sm2 = Math.Sin(-2 * delta_angle);
  229. double sm1 = Math.Sin(-delta_angle);
  230. double cm2 = Math.Cos(-2 * delta_angle);
  231. double cm1 = Math.Cos(-delta_angle);
  232. double w = 2 * cm1;
  233. double ar0, ar1, ar2;
  234. double ai0, ai1, ai2;
  235. for (i = 0; i < NumSamples; i += BlockSize)
  236. {
  237. ar2 = cm2;
  238. ar1 = cm1;
  239. ai2 = sm2;
  240. ai1 = sm1;
  241. for (j = i, n = 0; n < BlockEnd; j++, n++)
  242. {
  243. ar0 = w * ar1 - ar2;
  244. ar2 = ar1;
  245. ar1 = ar0;
  246. ai0 = w * ai1 - ai2;
  247. ai2 = ai1;
  248. ai1 = ai0;
  249. k = j + BlockEnd;
  250. tr = ar0 * pRealOut[k] - ai0 * pImagOut[k];
  251. ti = ar0 * pImagOut[k] + ai0 * pRealOut[k];
  252. pRealOut[k] = (pRealOut[j] - tr);
  253. pImagOut[k] = (pImagOut[j] - ti);
  254. pRealOut[j] += (tr);
  255. pImagOut[j] += (ti);
  256. }
  257. }
  258. BlockEnd = BlockSize;
  259. }
  260. /*
  261. ** Need to normalize if inverse transform...
  262. */
  263. if (bInverseTransform)
  264. {
  265. double denom = (double)(NumSamples);
  266. for (i = 0; i < NumSamples; i++)
  267. {
  268. pRealOut[i] /= denom;
  269. pImagOut[i] /= denom;
  270. }
  271. }
  272. }
  273. /// <summary>
  274. /// Calculate normal (power spectrum)
  275. /// </summary>
  276. /// <param name="NumSamples">Number of sample</param>
  277. /// <param name="pReal">Real coefficient buffer</param>
  278. /// <param name="pImag">Imaginary coefficient buffer</param>
  279. /// <param name="pAmpl">Working buffer to hold amplitude Xps(m) = | X(m)^2 | = Xreal(m)^2 + Ximag(m)^2</param>
  280. public static void Norm(UInt32 NumSamples, Double[] pReal, Double[] pImag, Double[] pAmpl)
  281. {
  282. if (pReal == null || pImag == null || pAmpl == null)
  283. {
  284. // error
  285. throw new ArgumentNullException("pReal,pImag,pAmpl");
  286. }
  287. if (pReal.Length < NumSamples || pImag.Length < NumSamples || pAmpl.Length < NumSamples)
  288. {
  289. // error
  290. throw new ArgumentException("Invalid Array argument detected");
  291. }
  292. // Calculate amplitude values in the buffer provided
  293. for (UInt32 i = 0; i < NumSamples; i++)
  294. {
  295. pAmpl[i] = pReal[i] * pReal[i] + pImag[i] * pImag[i];
  296. }
  297. }
  298. public static double normalizeFFTValue(double value)
  299. {
  300. return (value < 0.1 && value > -0.1) ? 0 : value;
  301. }
  302. /// <summary>
  303. /// Compute 2D FFT
  304. /// </summary>
  305. /// <param name="width">Width of the Matrix (must be power two)</param>
  306. /// <param name="height">Height of the Matrix (must be power two)</param>
  307. /// <param name="pRealIn">Real samples</param>
  308. /// <param name="pImagIn">Imaginary (optional, may be null)</param>
  309. /// <param name="pRealOut">Real coefficient output</param>
  310. /// <param name="pImagOut">Imaginary coefficient output</param>
  311. /// <param name="bInverseTransform">bInverseTransform when true, compute Inverse FFT</param>
  312. public static void Compute2D(UInt32 width, UInt32 height, ref Double[] pRealIn, Double[] pImagIn, ref Double[] pRealOut, ref Double[] pImagOut, Boolean bInverseTransform = false)
  313. {
  314. double[] row = new double[width];
  315. double[] column = new double[height];
  316. double[] irow = new double[width];
  317. double[] icolumn = new double[height];
  318. double[] xre = new double[width];
  319. double[] xim = new double[width];
  320. if (!bInverseTransform)
  321. {
  322. for (UInt32 y = 0; y < height; y++)
  323. {
  324. Array.Copy(pRealIn, (int)(y * width), row, 0, (int)width);
  325. DSP.FourierTransform.Compute(width, row, null, xre, xim, bInverseTransform);
  326. Array.Copy(xre, 0, pRealOut, (int)(y * width), (int)width);
  327. Array.Copy(xim, 0, pImagOut, (int)(y * width), (int)width);
  328. }
  329. for (int x = 0; x < width; x++)
  330. {
  331. for (int y = 0; y < height; y++)
  332. {
  333. column[y] = pRealOut[x + (y * width)];
  334. icolumn[y] = pImagOut[x + (y * width)];
  335. }
  336. DSP.FourierTransform.Compute(height, column, icolumn, xre, xim, bInverseTransform);
  337. for (int y = 0; y < height; y++)
  338. {
  339. pRealOut[x + (y * width)] = xre[y];
  340. pImagOut[x + (y * width)] = xim[y];
  341. }
  342. }
  343. }
  344. else
  345. {
  346. for (int x = 0; x < width; x++)
  347. {
  348. for (int y = 0; y < height; y++)
  349. {
  350. column[y] = pRealIn[x + (y * width)];
  351. icolumn[y] = pImagIn[x + (y * width)];
  352. }
  353. DSP.FourierTransform.Compute(height, column, icolumn, xre, xim, bInverseTransform);
  354. for (int y = 0; y < height; y++)
  355. {
  356. pRealOut[x + (y * width)] = xre[y];
  357. pImagOut[x + (y * width)] = xim[y];
  358. }
  359. }
  360. for (UInt32 y = 0; y < height; y++)
  361. {
  362. Array.Copy(pRealOut, (int)(y * width), row, 0, (int)width);
  363. Array.Copy(pImagOut, (int)(y * width), irow, 0, (int)width);
  364. DSP.FourierTransform.Compute(width, row, irow, xre, xim, bInverseTransform);
  365. Array.Copy(xre, 0, pRealOut, (int)(y * width), (int)width);
  366. }
  367. }
  368. }
  369. /// <summary>
  370. /// Find Peak frequency in Hz
  371. /// </summary>
  372. /// <param name="NumSamples">Number of samples</param>
  373. /// <param name="pAmpl">Current amplitude</param>
  374. /// <param name="samplingRate">Sampling rate in samples/second (Hz)</param>
  375. /// <param name="index">Frequency index</param>
  376. /// <returns>Peak frequency in Hz</returns>
  377. public static Double PeakFrequency(UInt32 NumSamples, Double[] pAmpl, Double samplingRate, ref UInt32 index)
  378. {
  379. UInt32 N = NumSamples >> 1; // number of positive frequencies. (numSamples/2)
  380. if (pAmpl == null)
  381. {
  382. // error
  383. throw new ArgumentNullException("pAmpl");
  384. }
  385. if (pAmpl.Length < NumSamples)
  386. {
  387. // error
  388. throw new ArgumentException("Invalid Array argument detected");
  389. }
  390. double maxAmpl = -1.0;
  391. double peakFreq = -1.0;
  392. index = 0;
  393. for (UInt32 i = 0; i < N; i++)
  394. {
  395. if (pAmpl[i] > maxAmpl)
  396. {
  397. maxAmpl = (double)pAmpl[i];
  398. index = i;
  399. peakFreq = (double)(i);
  400. }
  401. }
  402. return samplingRate * peakFreq / (double)(NumSamples);
  403. }
  404. public static byte[] GetPeaks(double[] leftChannel, double[] rightChannel, int sampleFrequency)
  405. {
  406. byte[] peaks = new byte[_frequencySlotCount];
  407. byte[] channelPeaks = GetPeaksForChannel(leftChannel, sampleFrequency);
  408. ComparePeaks(peaks, channelPeaks);
  409. return peaks;
  410. }
  411. private static void ComparePeaks(byte[] overallPeaks, byte[] channelPeaks)
  412. {
  413. for (int i = 0; i < _frequencySlotCount; i++)
  414. {
  415. overallPeaks[i] = Math.Max(overallPeaks[i], channelPeaks[i]);
  416. }
  417. }
  418. private static byte[] GetPeaksForChannel(double[] normalizedArray, int sampleFrequency)
  419. {
  420. double maxAmpl = (32767.0 * 32767.0);
  421. byte[] peaks = new byte[_frequencySlotCount];
  422. // update meter
  423. int centerFreq = (sampleFrequency / 2);
  424. byte peak;
  425. for (int i = 0; i < _frequencySlotCount; ++i)
  426. {
  427. if (_meterFrequencies[i] > centerFreq)
  428. {
  429. peak = 0;
  430. }
  431. else
  432. {
  433. int index = (int)(_meterFrequencies[i] * normalizedArray.Length / sampleFrequency);
  434. peak = (byte)Math.Max(0, (17.0 * Math.Log10(normalizedArray[index] / maxAmpl)));
  435. }
  436. peaks[i] = peak;
  437. }
  438. return peaks;
  439. }
  440. }
  441. public class Wavelet
  442. {
  443. // Haar Wavelet
  444. /// <summary>
  445. /// Calculate Haar in-place forward fast wavelet transform in 1-dimension.
  446. /// </summary>
  447. /// <param name="n">Elements to compute</param>
  448. /// <param name="signal">Signal to compute</param>
  449. static void Haar_ip_FFWT_1d(int n, ref double[] signal)
  450. {
  451. double a;
  452. double c;
  453. int i; int j; int k; int l; int m;
  454. i = 1;
  455. j = 2;
  456. m = (int) Utilities.NextPowerOfTwo( (uint) n );
  457. for (l = n - 1; l >= 0; l--)
  458. {
  459. //System.Diagnostics.Debug.WriteLine("l = " + l);
  460. m /= 2;
  461. for (k = 0; k < m; k++)
  462. {
  463. a = (signal[j * k] + signal[j * k + i]) / 2.0;
  464. c = (signal[j * k] - signal[j * k + i]) / 2.0;
  465. signal[j * k] = a;
  466. signal[j * k + i] = c;
  467. }
  468. i *= 2;
  469. j *= 2;
  470. }
  471. }
  472. /// <summary>
  473. /// Calculate Haar in-place inverse fast wavelet transform in 1-dimension.
  474. /// </summary>
  475. /// <param name="n">Elements to compute</param>
  476. /// <param name="signal">Signal to compute (must be power two)</param>
  477. static void Haar_ip_IFWT_1d(int n, ref double[] signal)
  478. {
  479. double a0;
  480. double a1;
  481. int i;
  482. int j;
  483. int k;
  484. int l;
  485. int m;
  486. i = (int) Utilities.NextPowerOfTwo((uint) (n - 1) );
  487. j = 2 * i;
  488. m = 1;
  489. for (l = 1; l <= n; l++)
  490. {
  491. //System.Diagnostics.Debug.WriteLine("l = " + l);
  492. for (k = 0; k < m; k++)
  493. {
  494. a0 = signal[j * k] + signal[j * k + i];
  495. a1 = signal[j * k] - signal[j * k + i];
  496. signal[j * k] = a0;
  497. signal[j * k + i] = a1;
  498. }
  499. i /= 2;
  500. j /= 2;
  501. m *= 2;
  502. }
  503. }
  504. /// <summary>
  505. /// Calculate one iteration of the Haar forward FWT in 1-dimension.
  506. /// </summary>
  507. /// <param name="n">Elements to compute</param>
  508. /// <param name="signal">Signal to compute (must be power two)</param>
  509. static void Haar_forward_pass_1d(int n, ref double[] signal)
  510. {
  511. int i;
  512. int npts;
  513. npts = (int) Utilities.NextPowerOfTwo( (uint) n );
  514. double[] a = new double[ npts / 2 ];
  515. double[] c = new double[ npts / 2 ];
  516. for (i = 0; i < npts / 2; i++)
  517. {
  518. a[i] = (signal[2 * i] + signal[2 * i + 1]) / 2.0;
  519. c[i] = (signal[2 * i] - signal[2 * i + 1]) / 2.0;
  520. }
  521. for (i = 0; i < npts / 2; i++)
  522. {
  523. signal[i] = a[i];
  524. signal[i + npts / 2] = c[i];
  525. }
  526. }
  527. /// <summary>
  528. /// Calculate the Haar forward fast wavelet transform in 1-dimension.
  529. /// </summary>
  530. /// <param name="signal">Signal to compute (must be power two)</param>
  531. public static void Haar_forward_FWT_1d(ref double[] signal)
  532. {
  533. int m;
  534. int npts;
  535. npts = (int) Utilities.NextPowerOfTwo((uint) signal.Length);
  536. for (m = signal.Length - 1; m >= 0; m--)
  537. {
  538. Wavelet.Haar_forward_pass_1d(m + 1, ref signal);
  539. //Wavelet.Haar_ip_FFWT_1d(m + 1, ref signal);
  540. }
  541. /*int i = 0;
  542. int w = signal.Length;
  543. double[] vecp = new double[signal.Length];
  544. for (i = 0; i < signal.Length; i++)
  545. vecp[i] = 0;
  546. //while (w > 1)
  547. // {
  548. w /= 2;
  549. for (i = 0; i < w; i++)
  550. {
  551. vecp[i] = (signal[2 * i] + signal[2 * i + 1]) / Math.Sqrt(2.0);
  552. vecp[i + w] = (signal[2 * i] - signal[2 * i + 1]) / Math.Sqrt(2.0);
  553. }
  554. for (i = 0; i < (w * 2); i++)
  555. signal[i] = vecp[i];
  556. // }*/
  557. }
  558. /// <summary>
  559. /// Calculate one iteration of the Haar inverse FWT in 1-dimension.
  560. /// </summary>
  561. /// <param name="n">Elements to compute</param>
  562. /// <param name="signal">Signal to compute (must be power two)</param>
  563. static void Haar_inverse_pass_1d(int n, ref double[] signal)
  564. {
  565. int i;
  566. int npts = (int) Utilities.NextPowerOfTwo( (uint) n);
  567. double[] r = new double[npts];
  568. for (i = 0; i < npts / 2; i++)
  569. {
  570. r[2 * i] = signal[i] + signal[i + npts / 2];
  571. r[2 * i + 1] = signal[i] - signal[i + npts / 2];
  572. }
  573. for (i = 0; i < npts; i++)
  574. {
  575. signal[i] = r[i];
  576. }
  577. }
  578. /// <summary>
  579. /// Calculate the Haar inverse fast wavelet transform in 1-dimension.
  580. /// </summary>
  581. /// <param name="signal">Signal to compute (must be power two)</param>
  582. public static void Haar_inverse_FWT_1d(ref double[] signal)
  583. {
  584. for (int m = 2; m <= signal.Length; m++)
  585. {
  586. Wavelet.Haar_inverse_pass_1d(m, ref signal);
  587. }
  588. }
  589. /// <summary>
  590. /// Calculate one iteration of the Haar forward FWT in 2-dimensions.
  591. /// </summary>
  592. /// <param name="n">Elements to compute</param>
  593. /// <param name="signal">Signal to compute (must be power two)</param>
  594. /* static void Haar_forward_pass_2d(int n, ref double[][] signal)
  595. {
  596. int i, j;
  597. int npts;
  598. npts = (int) Utilities.NextPowerOfTwo( (uint) n);
  599. for (i = 0; i < npts; i++)
  600. {
  601. Wavelet.Haar_forward_pass_1d(n, ref signal[i]);
  602. }
  603. double[] c = new double [npts];
  604. for (j = 0; j < npts; j++)
  605. {
  606. for (i = 0; i < npts; i++)
  607. c[i] = signal[i][j];
  608. Wavelet.Haar_forward_pass_1d(n, ref c);
  609. for (i = 0; i < npts; i++)
  610. signal[i][j] = c[i];
  611. }
  612. }*/
  613. /// <summary>
  614. /// Calculate the Haar forward fast wavelet transform in 2-dimensions.
  615. /// </summary>
  616. /// <param name="n">Elements to compute</param>
  617. /// <param name="signal">Signal to compute (must be power two)</param>
  618. public static void Haar_forward_FWT_2d(int width, int height, ref double[] signal)
  619. {
  620. /*for (int m = n - 1; m >= 0; m--)
  621. {
  622. Haar_forward_pass_2d(m + 1, ref signal);
  623. }*/
  624. double[] row = new double[Utilities.NextPowerOfTwo( (uint) width)];
  625. double[] column = new double[Utilities.NextPowerOfTwo( (uint) height)];
  626. for (UInt32 y = 0; y < height; y++)
  627. {
  628. Array.Copy(signal, (int)(y * width), row, 0, (int)width);
  629. DSP.Wavelet.Haar_forward_FWT_1d(ref row);
  630. Array.Copy(row, 0, signal, (int)(y * width), (int)width);
  631. }
  632. for (int x = 0; x < width; x++)
  633. {
  634. for (int y = 0; y < height; y++)
  635. {
  636. column[y] = signal[x + (y * width)];
  637. }
  638. DSP.Wavelet.Haar_forward_FWT_1d(ref column);
  639. for (int y = 0; y < height; y++)
  640. {
  641. signal[x + (y * width)] = column[y];
  642. }
  643. }
  644. }
  645. /// <summary>
  646. /// Calculate one iteration of the Haar inverse FWT in 2-dimensions.
  647. /// </summary>
  648. /// <param name="signal">Signal to compute (must be power two)</param>
  649. /* static void Haar_inverse_pass_2d(int n, ref double[][] signal)
  650. {
  651. int i, j;
  652. int npts;
  653. npts = (int) Utilities.NextPowerOfTwo((uint) n);
  654. for (i = 0; i < npts; i++)
  655. {
  656. Wavelet.Haar_inverse_pass_1d(n, ref signal[i]);
  657. }
  658. double[] c = new double[npts];
  659. for (j = 0; j < npts; j++)
  660. {
  661. for (i = 0; i < npts; i++)
  662. c[i] = signal[i][j];
  663. Wavelet.Haar_inverse_pass_1d(n, ref c);
  664. for (i = 0; i < npts; i++)
  665. signal[i][j] = c[i];
  666. }
  667. }*/
  668. /// <summary>
  669. /// Calculate the Haar inverse fast wavelet transform in 2-dimensions.
  670. /// </summary>
  671. /// <param name="signal">Signal to compute (must be power two)</param>
  672. public static void Haar_inverse_FWT_2d(int width, int height, ref double[] signal)
  673. {
  674. /*for (int m = 1; m <= signal.Length; m++)
  675. {
  676. Haar_inverse_pass_2d(m, ref signal);
  677. }*/
  678. double[] row = new double[Utilities.NextPowerOfTwo((uint)width)];
  679. double[] column = new double[Utilities.NextPowerOfTwo((uint)height)];
  680. for (int x = 0; x < width; x++)
  681. {
  682. for (int y = 0; y < height; y++)
  683. {
  684. column[y] = signal[x + (y * width)];
  685. }
  686. DSP.Wavelet.Haar_inverse_FWT_1d(ref column);
  687. for (int y = 0; y < height; y++)
  688. {
  689. signal[x + (y * width)] = column[y];
  690. }
  691. }
  692. for (UInt32 y = 0; y < height; y++)
  693. {
  694. Array.Copy(signal, (int)(y * width), row, 0, (int)width);
  695. DSP.Wavelet.Haar_inverse_FWT_1d(ref row);
  696. Array.Copy(row, 0, signal, (int)(y * width), (int)width);
  697. }
  698. }
  699. // The 1D Haar Transform
  700. public static void haar1d(ref double[] vec, int n)
  701. {
  702. int i = 0;
  703. int w = n;
  704. double[] vecp = new double[n];
  705. for (i = 0; i < n; i++)
  706. vecp[i] = 0;
  707. while (w > 1)
  708. {
  709. w /= 2;
  710. for (i = 0; i < w; i++)
  711. {
  712. vecp[i] = (vec[2 * i] + vec[2 * i + 1]) / Math.Sqrt(2.0);
  713. vecp[i + w] = (vec[2 * i] - vec[2 * i + 1]) / Math.Sqrt(2.0);
  714. }
  715. for (i = 0; i < (w * 2); i++)
  716. vec[i] = vecp[i];
  717. }
  718. }
  719. // A Modified version of 1D Haar Transform, used by the 2D Haar Transform function
  720. static void haar1(ref double[] vec, int n, int w)
  721. {
  722. int i = 0;
  723. double[] vecp = new double[n];
  724. for (i = 0; i < n; i++)
  725. vecp[i] = 0;
  726. w /= 2;
  727. for (i = 0; i < w; i++)
  728. {
  729. vecp[i] = (vec[2 * i] + vec[2 * i + 1]) / Math.Sqrt(2.0);
  730. vecp[i + w] = (vec[2 * i] - vec[2 * i + 1]) / Math.Sqrt(2.0);
  731. }
  732. for (i = 0; i < (w * 2); i++)
  733. vec[i] = vecp[i];
  734. }
  735. // The 2D Haar Transform
  736. public static void haar2(ref double[] matrix, int rows, int cols)
  737. {
  738. double[] temp_row = new double[cols];
  739. double[] temp_col = new double[rows];
  740. int i = 0, j = 0;
  741. int w = cols, h = rows;
  742. while (w > 1 || h > 1)
  743. {
  744. if (w > 1)
  745. {
  746. for (i = 0; i < h; i++)
  747. {
  748. for (j = 0; j < cols; j++)
  749. temp_row[j] = matrix[i + (j * cols)];
  750. haar1(ref temp_row, cols, w);
  751. for (j = 0; j < cols; j++)
  752. matrix[i + (j * cols)] = temp_row[j];
  753. }
  754. }
  755. if (h > 1)
  756. {
  757. for (i = 0; i < w; i++)
  758. {
  759. for (j = 0; j < rows; j++)
  760. temp_col[j] = matrix[j + (i * cols)];
  761. haar1(ref temp_col, rows, h);
  762. for (j = 0; j < rows; j++)
  763. matrix[j + (i * cols)] = temp_col[j];
  764. }
  765. }
  766. if (w > 1)
  767. w /= 2;
  768. if (h > 1)
  769. h /= 2;
  770. }
  771. }
  772. }
  773. public class Utilities
  774. {
  775. public const Double DDC_PI = 3.14159265358979323846;
  776. public static double MSE(ref double[] signal_1, ref double[] signal_2, int SizeToCompare)
  777. {
  778. double result = 0;
  779. if (signal_1.Length < SizeToCompare) return -1;
  780. if (signal_2.Length < SizeToCompare) return -1;
  781. for (int i = 0; i < signal_1.Length; i++)
  782. {
  783. result += Math.Pow(signal_1[i] - signal_2[i], 2);
  784. }
  785. return (result / signal_1.Length);
  786. }
  787. /// <summary>
  788. /// Verifies a number is a power of two
  789. /// </summary>
  790. /// <param name="x">Number to check</param>
  791. /// <returns>true if number is a power two (i.e.:1,2,4,8,16,...)</returns>
  792. public static Boolean IsPowerOfTwo(UInt32 x)
  793. {
  794. return ((x != 0) && (x & (x - 1)) == 0);
  795. }
  796. /// <summary>
  797. /// Get Next power of number.
  798. /// </summary>
  799. /// <param name="x">Number to check</param>
  800. /// <returns>A power of two number</returns>
  801. public static UInt32 NextPowerOfTwo(UInt32 x)
  802. {
  803. x = x - 1;
  804. x = x | (x >> 1);
  805. x = x | (x >> 2);
  806. x = x | (x >> 4);
  807. x = x | (x >> 8);
  808. x = x | (x >> 16);
  809. return x + 1;
  810. }
  811. public static Double[] triangularExtraction(ref Double[] value, uint width, uint height, uint num , int fill = -1) {
  812. Double[] result = new Double[num*2];
  813. uint sidew = (uint) Math.Round(Math.Sqrt(num*2));
  814. if (sidew > height) sidew = height;
  815. uint sideh = sidew;
  816. int index = 0;
  817. string _match = "";
  818. for (int y = 0; y < sideh; y++)
  819. {
  820. for (int x = 0; x < sidew; x++)
  821. {
  822. result[index] = value[x + (y * width)];
  823. _match += "[" + Math.Round(result[index]) + "],";
  824. if (fill != -1)
  825. {
  826. value[x + (y * width)] = fill;
  827. value[width-1-x + (y * width)] = fill;
  828. value[x + ((height - y -1 ) * width)] = fill;
  829. value[width - 1 - x + ((height - y - 1) * width)] = fill;
  830. }
  831. index++;
  832. //if (index >= num) break;
  833. }
  834. System.Diagnostics.Debug.WriteLine(_match); _match = "";
  835. --sidew;
  836. }
  837. return result;
  838. }
  839. static public void saveSignal(ref double[] signal, string fileName)
  840. {
  841. IsolatedStorageFileStream fileStream = null;
  842. IsolatedStorageFile myIsolatedStorage = IsolatedStorageFile.GetUserStoreForApplication();
  843. //create new file
  844. using (IsolatedStorageFile store = IsolatedStorageFile.GetUserStoreForApplication())
  845. {
  846. fileStream = new IsolatedStorageFileStream(fileName, FileMode.Create, FileAccess.ReadWrite, myIsolatedStorage);
  847. using (StreamWriter writeFile = new StreamWriter(fileStream))
  848. {
  849. string _signal = "";
  850. for (int i = 0; i < signal.Length; i++)
  851. {
  852. _signal += Convert.ToString(signal[i]);
  853. if (i < signal.Length - 1) _signal += ";";
  854. }
  855. writeFile.WriteLine(_signal);
  856. writeFile.Close();
  857. }
  858. }
  859. }
  860. static public double[] loadSignal(string fileName)
  861. {
  862. double[] signal;
  863. IsolatedStorageFile myIsolatedStorage = IsolatedStorageFile.GetUserStoreForApplication();
  864. if (!myIsolatedStorage.FileExists(fileName)) return null;
  865. IsolatedStorageFileStream fileStream = null;
  866. using (IsolatedStorageFile store = IsolatedStorageFile.GetUserStoreForApplication())
  867. {
  868. fileStream = new IsolatedStorageFileStream(fileName, FileMode.Open, FileAccess.Read, myIsolatedStorage);
  869. using (StreamReader readFile = new StreamReader(fileStream))
  870. {
  871. string _signal = readFile.ReadLine();
  872. string[] __signal = _signal.Split(';');
  873. signal = new double[__signal.Length];
  874. for (int i = 0; i < __signal.Length; i++)
  875. {
  876. signal[i] = Convert.ToDouble(__signal[i]);
  877. }
  878. readFile.Close();
  879. }
  880. }
  881. return signal;
  882. }
  883. }
  884. public class Filters
  885. {
  886. public static double Triangular(double value, double samples)
  887. {
  888. return (2 / (samples + 1)) * (((samples + 1) / 2) - Math.Abs(value - ((samples - 1) / 2)));
  889. }
  890. public static double[] EnhanceHighFrequencies(ref double[] signal)
  891. {
  892. double[] result = new double[signal.Length];
  893. for (int i = 1; i < signal.Length; i++)
  894. {
  895. result[i] = signal[i] - 0.95 * signal[i - 1];
  896. }
  897. return result;
  898. }
  899. }
  900. }