quadratic loess in C++

#include <iostream>
#include <iterator>
#include <vector>
#include <cmath>
#include <gsl_multifit.h>
#include "NumericUtils.h"

double tricube(double u, double t)
{
    // In our case, u represents a distance and hence should be strictly positive.
    if (u < 0) {
        std::cout << "In tricube(u, t), u < 0" << std::endl;
        std::abort();
    }

    // 0 <= u < t
    if ( (closeAtTolerance(u, 0.0) || 0.0 < u) && (u < t) ) {
        // (1 - (u/t)^3)^3
        return pow( ( 1.0 - pow(u/t, 3.0)), 3.0 );
    }
    // u >= t
    else {
        return 0.0;
    }
}

double getDistance(double x, double xi)
{
    return fabs(x - xi);
}

int main (int argc, char* argv[])
{
    // Loess algorithm parameters.
    double alpha = 0.33;

    // Create test dataset.
    const unsigned n = 21;

    double xValues[n] = { 0.5578196, 2.0217271, 2.5773252, 3.4140288, 4.3014084, 4.7448394, 5.1073781,
                         6.5411662, 6.7216176, 7.2600583, 8.1335874, 9.1224379, 11.9296663, 12.3797674,
                         13.2728619, 14.2767453, 15.3731026, 15.6476637, 18.5605355, 18.5866354, 18.7572812 };

    double yValues[n] = { 18.63654, 103.49646, 150.35391, 190.51031, 208.70115, 213.71135, 228.49353,
                         233.55387, 234.55054, 223.89225, 227.68339, 223.91982, 168.01999, 164.95750,
                         152.61107, 160.78742, 168.55567, 152.42658, 221.70702, 222.69040, 243.18828 };

    // Loess neighborhood parameter.
    // XXX: Careful, alpha is assume to be <= 1 here.
    const unsigned q = floor(n * alpha);

    // Working arrays. Yes we need both distances and sortedDistances.
    double distances[n];
    double sortedDistances[n];
    double ySmooth[n];

    gsl_matrix *X = gsl_matrix_alloc(n, 3);
    gsl_matrix *cov = gsl_matrix_alloc(3, 3);
    gsl_vector *weights = gsl_vector_alloc(n);
    gsl_vector *c = gsl_vector_alloc(3);
    gsl_vector *x = gsl_vector_alloc(3);

    double y, yErr, chisq;

    // Setup the model matrix X for a quadratic fit.
    for (unsigned i = 0; i < n; ++i) {
        gsl_matrix_set(X, i, 0, 1.0);
        gsl_matrix_set(X, i, 1, xValues[i]);
        gsl_matrix_set(X, i, 2, xValues[i] * xValues[i]);
    }

    for (unsigned i = 0; i < n; ++i) {

        // Compute distances.
        for (unsigned j = 0; j < n; ++j) {
            distances[j] = getDistance(xValues[i], xValues[j]);
            sortedDistances[j] = distances[j];
        }

        // Sort distances in order from smallest to largest.
        std::sort(sortedDistances, sortedDistances + n);

        // Compute weights.
        for (unsigned j = 0; j < n; ++j) {
            gsl_vector_set(weights, j, tricube(distances[j], sortedDistances[q]));
        }

        gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(n, 3);
        gsl_multifit_wlinear(X, weights, &gsl_vector_view_array(yValues, n).vector, c, cov, &chisq, work);
        gsl_multifit_linear_free(work);

        gsl_vector_set(x, 0, 1.0);
        gsl_vector_set(x, 1, xValues[i]);
        gsl_vector_set(x, 2, xValues[i] * xValues[i]);
        gsl_multifit_linear_est (x, c, cov, &y, &yErr);

        ySmooth[i] = y;
    }

    std::cout.setf(std::ios::fixed, std::ios::floatfield);
    for (unsigned i = 0; i < n; ++i) {
        std::cout << xValues[i] << " "
                  << yValues[i] << " "
                  << ySmooth[i]
                  << std::endl;
    }

    gsl_matrix_free(X);
    gsl_vector_free(weights);
    gsl_vector_free(c);
    gsl_matrix_free(cov);

    return 0;
}