- Main Page
- Classes
- Files

- File List

# C:/Users/Andreas Schiffler/Desktop/SVN/NewGamePhysics/NewGamePhysics/Physics/ButterworthFilter.cs

    00001 ﻿//-----------------------------------------------------------------------------
    00002 // ButterworthFilter.cs
    00003 // (A. Schiffler, 2009)
    00004 //-----------------------------------------------------------------------------
    00005
    00006 namespace NewGamePhysics.Physics
    00007 {
    00008     using System;
    00009     using System.Collections.Generic;
    00010     using System.Linq;
    00011     using System.Text;
    00012
    00016     public enum ButterworthFilterType
    00017     {
    00021         LowPass,
    00022
    00026         HighPass,
    00027     }
    00028
    00037     public class ButterworthFilter
    00038     {
    00042         static public double DefaultResonance = Math.Sqrt(2.0);
    00043
    00047         private double a1;
    00048
    00052         private double a2;
    00053
    00057         private double a3;
    00058
    00062         private double b1;
    00063
    00067         private double b2;
    00068
    00085         public ButterworthFilter(
    00086             ButterworthFilterType filterType,
    00087             double sampleRate,
    00088             double cutoffFrequency) : this(filterType, sampleRate, cutoffFrequency, DefaultResonance)
    00089         {
    00090         }
    00091
    00109         public ButterworthFilter(
    00110             ButterworthFilterType filterType,
    00111             double sampleRate,
    00112             double cutoffFrequency,
    00113             double resonance)
    00114         {
    00115             // Check input parameters
    00116             if (sampleRate <= 0.0)
    00117             {
    00118                 throw new ArgumentException("The sample rate cannot be zero or negative.");
    00119             }
    00120
    00121             if ((cutoffFrequency <= 0.0) ||
    00122                 (cutoffFrequency > (sampleRate / 2.0)))
    00123             {
    00124                 throw new ArgumentException("The cutoff frequency should be greater than " +
    00125                     "zero, but less than half the sample rate");
    00126             }
    00127
    00128             if ((resonance < 0.1) || (resonance > DefaultResonance))
    00129             {
    00130                 throw new ArgumentException("The resonance parameter should be in " +
    00131                     " the range 0.1 to sqrt(2)");
    00132             }
    00133
    00134             // Calculate filter parameters
    00135             switch (filterType)
    00136             {
    00137                 case ButterworthFilterType.LowPass:
    00138                     this.InitializeLowPass(sampleRate, cutoffFrequency, resonance);
    00139                     break;
    00140                 case ButterworthFilterType.HighPass:
    00141                     this.InitializeHighPass(sampleRate, cutoffFrequency, resonance);
    00142                     break;
    00143             }
    00144         }
    00145
    00152         private void InitializeLowPass(
    00153             double sampleRate,
    00154             double cutoffFrequency,
    00155             double resonance)
    00156         {
    00157             double c = 1.0 / Math.Tan(Math.PI * cutoffFrequency / sampleRate);
    00158             this.a1 = 1.0 / (1.0 + resonance * c + c * c);
    00159             this.a2 = 2 * this.a1;
    00160             this.a3 = this.a1;
    00161             this.b1 = 2.0 * (1.0 - c * c) * this.a1;
    00162             this.b2 = (1.0 - resonance * c + c * c) * this.a1;
    00163         }
    00164
    00171         private void InitializeHighPass(
    00172             double sampleRate,
    00173             double cutoffFrequency,
    00174             double resonance)
    00175         {
    00176             double c = Math.Tan(Math.PI * cutoffFrequency / sampleRate);
    00177             this.a1 = 1.0 / (1.0 + resonance * c + c * c);
    00178             this.a2 = -2 * this.a1;
    00179             this.a3 = this.a1;
    00180             this.b1 = 2.0 * (c * c - 1.0) * this.a1;
    00181             this.b2 = (1.0 - resonance * c + c * c) * this.a1;
    00182         }
    00183
    00192         public double[] Calculate(
    00193             double[] samples)
    00194         {
    00195             if (samples == null)
    00196             {
    00197                 throw new ArgumentNullException("samples");
    00198             }
    00199
    00200             int nMax = samples.Length;
    00201             if (nMax < 3)
    00202             {
    00203                 throw new ArgumentException(
    00204                     "samples",
    00205                     "The samples array must contain at least 3 values.");
    00206             }
    00207
    00208             double[] filteredSamples = new double[nMax];
    00209             filteredSamples[0] = 0.0;
    00210             filteredSamples[1] = 0.0;
    00211             for (int n = 2; n < nMax; n++)
    00212             {
    00213                 filteredSamples[n] =
    00214                     this.a1 * samples[n] +
    00215                     this.a2 * samples[n - 1] +
    00216                     this.a3 * samples[n - 2] -
    00217                     this.b1 * filteredSamples[n - 1] -
    00218                     this.b2 * filteredSamples[n - 2];
    00219             }
    00220
    00221             return filteredSamples;
    00222         }
    00223     }
    00224 }