/src/util/select.c

/*
 * This file is part of libtrilateration.
 *
 *
 * Copyright 2014 by Marcel Kyas
 *
 * libtrilateration 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.
 *
 * libtrilateration 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with libtrilateration.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#ifdef HAVE_CONFIG_H
#  include "libtrilateration/config.h"
#endif

#include <assert.h>
#include <stdlib.h>
#include <string.h>

#include <libtrilateration/util.h>

/*!
 * Find the kth element in sorted order of an array.
 *
 * Adapted from Wirth, Niklaus. "Algorithms + data structures = programs",
 * Englewood Cliffs: Prentice-Hall, 1976.
 */
double
trilateration_fselect(double const *const values, const size_t N,
		      const int k)
{
    double a[N];
    memcpy(a, values, N * sizeof(double));

    int l = 0, m = (int) N - 1;

    while (l < m) {
	double x = a[k];
	int i = l;
	int j = m;
	do {
	    while (a[i] < x) {
		i++;
	    }
	    while (x < a[j]) {
		j--;
	    }
	    if (i <= j) {
		assert(0 <= i && i < (int) N);
		assert(0 <= j && j < (int) N);
		double t = a[i];
		a[i] = a[j];
		a[j] = t;
		i++;
		j--;
	    }
	}
	while (i <= j);
	if (j < k) {
	    l = i;
	}
	if (k < i) {
	    m = j;
	}
    }
    return a[k];
}





double trilateration_fmedian(double const *const values, const size_t N)
{
    const int k = (N / 2) - ((N & 1) ? 0 : 1);
    return trilateration_fselect(values, N, k);
}