src/libs/Median.h

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   3       Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
   4       Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 template <typename T>
   9 void split(T data[], unsigned int n, T x, unsigned int& i, unsigned int& j)
  10 {
  11   do {
  12     while (data[i] < x) i++;
  13     while (x < data[j]) j--;
  14
  15     if (i <= j) {
  16       T ii = data[i];
  17       data[i] = data[j];
  18       data[j] = ii;
  19       i++; j--;
  20     }
  21   } while (i <= j);
  22 }
  23
  24 // C.A.R. Hoare's Quick Median
  25 template <typename T>
  26 unsigned int quick_median(T data[], unsigned int n)
  27 {
  28   unsigned int l = 0, r = n-1, k = n/2;
  29   while (l < r) {
  30     T x = data[k];
  31     unsigned int i = l, j = r;
  32     split(data, n, x, i, j);
  33     if (j < k) l = i;
  34     if (k < i) r = j;
  35   }
  36   return k;
  37 }