flatten_to_sphere2.c.new

#include  <internal_volume_io.h>
#include  <bicpl.h>

#undef  USE_CENTROID
#define USE_CENTROID

typedef  float  dtype;

private  void  flatten_polygons(
    int              n_points,
    Point            points[],
    int              n_neighbours[],
    int              *neighbours[],
    Point            init_points[],
    Real             centroid_weight,
    Real             sphere_weight,
    int              n_iters );

int  main(
    int    argc,
    char   *argv[] )
{
    STRING               src_filename, dest_filename, initial_filename;
    int                  n_objects, n_i_objects, n_iters;
    int                  *n_neighbours, **neighbours;
    File_formats         format;
    object_struct        **object_list, **i_object_list;
    polygons_struct      *polygons, *init_polygons;
    Point                *init_points, *points;
    Real                 sphere_weight, centroid_weight;

    initialize_argument_processing( argc, argv );

    if( !get_string_argument( NULL, &src_filename ) ||
        !get_string_argument( NULL, &dest_filename ) )
    {
        print_error( "Usage: %s  input.obj output.obj [cw] [sw] [n_iters]\n",
                     argv[0] );
        return( 1 );
    }

    (void) get_real_argument( 1.0, &centroid_weight );
    (void) get_real_argument( 1.0, &sphere_weight );
    (void) get_int_argument( 100, &n_iters );

    if( input_graphics_file( src_filename, &format, &n_objects,
                             &object_list ) != OK || n_objects != 1 ||
        get_object_type(object_list[0]) != POLYGONS )
        return( 1 );

    polygons = get_polygons_ptr( object_list[0] );

    if( get_string_argument( NULL, &initial_filename ) )
    {
        if( input_graphics_file( initial_filename, &format, &n_i_objects,
                                 &i_object_list ) != OK || n_i_objects != 1 ||
            get_object_type(i_object_list[0]) != POLYGONS )
            return( 1 );

        init_polygons = get_polygons_ptr( i_object_list[0] );
        init_points = init_polygons->points;
        ALLOC( init_polygons->points, 1 );
        delete_object_list( n_i_objects, i_object_list );
    }
    else
    {
        init_points = NULL;
    }

    create_polygon_point_neighbours( polygons, FALSE, &n_neighbours,
                                     &neighbours, NULL, NULL );

    points = polygons->points;
    ALLOC( polygons->points, 1 );
    delete_object_list( 1, object_list );

    flatten_polygons( polygons->n_points, points, n_neighbours, neighbours,
                      init_points, centroid_weight,
                      sphere_weight, n_iters );

    delete_polygon_point_neighbours( polygons, n_neighbours, neighbours,
                                     NULL, NULL );

    if( input_graphics_file( src_filename, &format, &n_objects,
                         &object_list ) != OK || n_objects != 1 ||
        get_object_type(object_list[0]) != POLYGONS )
        return( 1 );

    polygons = get_polygons_ptr( object_list[0] );
    FREE( polygons->points );
    polygons->points = points;

    if( output_graphics_file( dest_filename, format, 1, object_list ) != OK )
        print_error( "Error outputting: %s\n", dest_filename );

    return( 0 );
}

private  Real  evaluate_fit(
    int     n_parameters,
    dtype   parameters[],
    dtype   distances[],
    int     n_neighbours[],
    int     *neighbours[],
    Real    centroid_weight,
    dtype   centroid_weights[],
    Real    sphere_weight )
{
    int    p, n_points, ind, n, neigh;
    Real   fit, dx, dy, dz, dist, diff, act_dist, radius;
    Real   xc, yc, zc, x, y, z, weight;

    fit = 0.0;
    n_points = n_parameters / 3;

    ind = 0;
    for_less( p, 0, n_points )
    {
        for_less( n, 0, n_neighbours[p] )
        {
            neigh = neighbours[p][n];
            if( neigh < p )
                continue;

            dist = (Real) distances[ind];
            ++ind;

            dx = (Real)parameters[IJ(p,0,3)] - (Real)parameters[IJ(neigh,0,3)];
            dy = (Real)parameters[IJ(p,1,3)] - (Real)parameters[IJ(neigh,1,3)];
            dz = (Real)parameters[IJ(p,2,3)] - (Real)parameters[IJ(neigh,2,3)];
            act_dist = dx * dx + dy * dy + dz * dz;
            diff = dist - act_dist;
            fit += diff * diff;
        }
    }

    if( centroid_weight > 0.0 )
    {
        ind = 0;
        for_less( p, 0, n_points )
        {
            xc = 0.0;
            yc = 0.0;
            zc = 0.0;
            for_less( n, 0, n_neighbours[p] )
            {
                neigh = neighbours[p][n];
                weight = (Real) centroid_weights[ind+n];
                xc += (Real) parameters[IJ(neigh,0,3)] * weight;
                yc += (Real) parameters[IJ(neigh,1,3)] * weight;
                zc += (Real) parameters[IJ(neigh,2,3)] * weight;
            }

            x = (Real) parameters[IJ(p,0,3)];
            y = (Real) parameters[IJ(p,1,3)];
            z = (Real) parameters[IJ(p,2,3)];

            dx = x - xc;
            dy = y - yc;
            dz = z - zc;

            diff = dx * dx + dy * dy + dz * dz;

            fit += centroid_weight * diff * diff;
            ind += n_neighbours[p];
        }
    }

    if( sphere_weight > 0.0 )
    {
        radius = (Real) parameters[n_parameters-1];

        for_less( p, 0, n_points )
        {
            dx = (Real) parameters[IJ(p,0,3)];
            dy = (Real) parameters[IJ(p,1,3)];
            dz = (Real) parameters[IJ(p,2,3)];
            act_dist = dx * dx + dy * dy + dz * dz;
            diff = radius * radius - act_dist;
            fit += sphere_weight * diff * diff;
        }
    }

    return( fit );
}

private  void  evaluate_fit_derivative(
    int      n_parameters,
    dtype    parameters[],
    dtype    distances[],
    int      n_neighbours[],
    int      *neighbours[],
    Real     centroid_weight,
    dtype    centroid_weights[],
    Real     sphere_weight,
    dtype    deriv[] )
{
    int    p, n_points, ind, n, neigh, p_index, n_index;
    dtype  weight;
    dtype  xc, yc, zc;
    dtype  x_factor, y_factor, z_factor, x_diff, y_diff, z_diff;
    dtype  sfactor, fcentroid_weight;
    dtype  x1, y1, z1, act_dist, diff, radius, radius2;
    dtype  x2, y2, z2;
    dtype  dx, dy, dz, factor, dist;

    for_less( p, 0, n_parameters )
        deriv[p] = (dtype) 0.0;

    n_points = n_parameters / 3;
    if( sphere_weight > 0.0 )
    {
        radius = parameters[n_parameters-1];
        radius2 = radius * radius;
        for_less( p, 0, n_points )
        {
            p_index = IJ(p,0,3);
            x1 = parameters[p_index+0];
            y1 = parameters[p_index+1];
            z1 = parameters[p_index+2];

            act_dist = x1 * x1 + y1 * y1 + z1 * z1;
            diff = act_dist - radius2;
            deriv[p_index+0] += diff * x1;
            deriv[p_index+1] += diff * y1;
            deriv[p_index+2] += diff * z1;
            deriv[n_parameters-1] += -diff * radius;
        }

        sfactor = (dtype) (4.0 * sphere_weight);
        for_less( p, 0, n_parameters )
            deriv[p] *= sfactor;
    }

    ind = 0;
    for_less( p, 0, n_points )
    {
        p_index = IJ(p,0,3);
        x1 = parameters[p_index+0];
        y1 = parameters[p_index+1];
        z1 = parameters[p_index+2];

        for_less( n, 0, n_neighbours[p] )
        {
            neigh = neighbours[p][n];
            if( neigh < p )
                continue;

            dist = distances[ind];
            ++ind;

            n_index = IJ(neigh,0,3);
            x2 = parameters[n_index+0];
            y2 = parameters[n_index+1];
            z2 = parameters[n_index+2];

            dx = x1 - x2;
            dy = y1 - y2;
            dz = z1 - z2;
            act_dist = dx * dx + dy * dy + dz * dz;
            factor = 2.0f * (act_dist - dist);
            deriv[p_index+0] += dx * factor;
            deriv[p_index+1] += dy * factor;
            deriv[p_index+2] += dz * factor;
            deriv[n_index+0] += -dx * factor;
            deriv[n_index+1] += -dy * factor;
            deriv[n_index+2] += -dz * factor;
        }
    }

    if( centroid_weight > 0.0 )
    {
        ind = 0;
        fcentroid_weight = (dtype) centroid_weight;
        for_less( p, 0, n_points )
        {
            p_index = IJ(p,0,3);
            x1 = parameters[p_index+0];
            y1 = parameters[p_index+1];
            z1 = parameters[p_index+2];
            xc = 0.0f;
            yc = 0.0f;
            zc = 0.0f;
            for_less( n, 0, n_neighbours[p] )
            {
                neigh = neighbours[p][n];
                n_index = IJ(neigh,0,3);
#ifdef USE_CENTROID
                xc += parameters[n_index+0];
                yc += parameters[n_index+1];
                zc += parameters[n_index+2];
#else
                weight = centroid_weights[ind+n];
                xc += parameters[n_index+0] * weight;
                yc += parameters[n_index+1] * weight;
                zc += parameters[n_index+2] * weight;
#endif
            }

#ifdef USE_CENTROID
            weight = 1.0f / (dtype) n_neighbours[p];
            xc *= weight;
            yc *= weight;
            zc *= weight;
#endif

            x_diff = xc - x1;
            x_diff = x_diff * x_diff * x_diff;
            y_diff = yc - y1;
            y_diff = y_diff * y_diff * y_diff;
            z_diff = zc - z1;
            z_diff = z_diff * z_diff * z_diff;
            x_factor = fcentroid_weight * 4.0f * x_diff;
            y_factor = fcentroid_weight * 4.0f * y_diff;
            z_factor = fcentroid_weight * 4.0f * z_diff;

            deriv[p_index+0] += -x_factor;
            deriv[p_index+1] += -y_factor;
            deriv[p_index+2] += -z_factor;

            for_less( n, 0, n_neighbours[p] )
            {
                neigh = neighbours[p][n];
                n_index = IJ(neigh,0,3);
#ifndef  USE_CENTROID
                weight = (Real) centroid_weights[ind+n];
#endif
                deriv[n_index+0] += x_factor * weight;
                deriv[n_index+1] += y_factor * weight;
                deriv[n_index+2] += z_factor * weight;
            }

            ind += n_neighbours[p];
        }
    }
}

private  void  evaluate_fit_along_line(
    int     n_parameters,
    dtype   parameters[],
    dtype   delta[],
    dtype   distances[],
    int     n_neighbours[],
    int     *neighbours[],
    Real    centroid_weight,
    dtype   centroid_weights[],
    Real    sphere_weight,
    Real    coefs[] )
{
    int     p, n_points, ind, n, neigh, p_index, n_index;
    dtype   dx, dy, dz, dr, dist, radius;
    dtype   dx1, dy1, dz1, x1, y1, z1;
    dtype   x, y, z;
    dtype   a1, b1, c1, a2, b2, c2, a3, b3, c3;
    dtype   ax, ay, az, bx, by, bz, weight;
    dtype   line_coefs[3];
    dtype   d00, d01, d02, d11, d12, d22;
    dtype   s00, s01, s02, s11, s12, s22;

    d00 = 0.0f;
    d01 = 0.0f;
    d02 = 0.0f;
    d11 = 0.0f;
    d12 = 0.0f;
    d22 = 0.0f;

    s00 = 0.0f;
    s01 = 0.0f;
    s02 = 0.0f;
    s11 = 0.0f;
    s12 = 0.0f;
    s22 = 0.0f;

    n_points = n_parameters / 3;

    if( sphere_weight > 0.0 )
    {
        radius = parameters[n_parameters-1];
        dr = delta[n_parameters-1];
    }

    ind = 0;
    for_less( p, 0, n_points )
    {
        p_index = IJ(p,0,3);
        x1 = parameters[p_index+0];
        y1 = parameters[p_index+1];
        z1 = parameters[p_index+2];
        dx1 = delta[p_index+0];
        dy1 = delta[p_index+1];
        dz1 = delta[p_index+2];

        line_coefs[0] = x1 * x1 + y1 * y1 + z1 * z1 - radius * radius;
        line_coefs[1] = x1 * dx1 + y1 * dy1 + z1 * dz1 - radius * dr;
        line_coefs[2] = dx1 * dx1 + dy1 * dy1 + dz1 * dz1 - dr * dr;

        s00 += line_coefs[0] * line_coefs[0];
        s01 += line_coefs[0] * line_coefs[1];
        s02 += line_coefs[0] * line_coefs[2];
        s11 += line_coefs[1] * line_coefs[1];
        s12 += line_coefs[1] * line_coefs[2];
        s22 += line_coefs[2] * line_coefs[2];

        for_less( n, 0, n_neighbours[p] )
        {
            neigh = neighbours[p][n];
            if( neigh < p )
                continue;

            dist = distances[ind];
            ++ind;

            n_index = IJ(neigh,0,3);
            x = x1 - parameters[n_index+0];
            y = y1 - parameters[n_index+1];
            z = z1 - parameters[n_index+2];
            dx = dx1 - delta[n_index+0];
            dy = dy1 - delta[n_index+1];
            dz = dz1 - delta[n_index+2];

            line_coefs[0] = x * x + y * y + z * z - dist;
            line_coefs[1] = x * dx + y * dy + z * dz;
            line_coefs[2] = dx * dx + dy * dy + dz * dz;

            d00 += line_coefs[0] * line_coefs[0];
            d01 += line_coefs[0] * line_coefs[1];
            d02 += line_coefs[0] * line_coefs[2];
            d11 += line_coefs[1] * line_coefs[1];
            d12 += line_coefs[1] * line_coefs[2];
            d22 += line_coefs[2] * line_coefs[2];
        }
    }

    coefs[0] = (Real) d00 + (Real) s00 * sphere_weight;
    coefs[1] = 4.0 * ((Real) d01 + (Real) s01 * sphere_weight);
    coefs[2] = 2.0 * ((Real) d02 + sphere_weight * (Real) s02) +
               4.0 * ((Real) d11 + sphere_weight * (Real) s11);
    coefs[3] = 4.0 * ((Real) d12 + sphere_weight * (Real) s12);
    coefs[4] = ((Real) d22 + sphere_weight * (Real) s22;

    if( centroid_weight > 0.0 )
    {
        s00 = 0.0f;
        s01 = 0.0f;
        s02 = 0.0f;
        s11 = 0.0f;
        s12 = 0.0f;
        s22 = 0.0f;

        ind = 0;
        for_less( p, 0, n_points )
        {
            ax = 0.0f;
            bx = 0.0f;
            ay = 0.0f;
            by = 0.0f;
            az = 0.0f;
            bz = 0.0f;

            for_less( n, 0, n_neighbours[p] )
            {
                neigh = neighbours[p][n];
                n_index = IJ(neigh,0,3);
#ifdef USE_CENTROID
                ax +=      delta[n_index+0];
                bx += parameters[n_index+0];
                ay +=      delta[n_index+1];
                by += parameters[n_index+1];
                az +=      delta[n_index+2];
                bz += parameters[n_index+2];
#else
                weight = centroid_weights[ind+n];
                ax +=      delta[n_index+0] * weight;
                bx += parameters[n_index+0] * weight;
                ay +=      delta[n_index+1] * weight;
                by += parameters[n_index+1] * weight;
                az +=      delta[n_index+2] * weight;
                bz += parameters[n_index+2] * weight;
#endif
            }

#ifdef USE_CENTROID
            weight = 1.0f / (dtype) n_neighbours[p];
            ax *= weight;
            bx *= weight;
            ay *= weight;
            by *= weight;
            az *= weight;
            bz *= weight;
#endif

            p_index = IJ( p, 0, 3 );
            ax +=      -delta[p_index+0];
            bx += -parameters[p_index+0];
            ay +=      -delta[p_index+1];
            by += -parameters[p_index+1];
            az +=      -delta[p_index+2];
            bz += -parameters[p_index+2];

            a1 = ax * ax;
            b1 = ax * bx;
            c1 = bx * bx;

            a2 = ay * ay;
            b2 = ay * by;
            c2 = by * by;

            a3 = az * az;
            b3 = az * bz;
            c3 = bz * bz;

            s00 += c1 * c1 + c2 * c2 + c3 * c3;
            s01 += b1 * c1 + b2 * c2 + b3 * c3;
            s02 += a1 * c1 + a2 * c2 + a3 * c3;
            s11 += b1 * b1 + b2 * b2 + b3 * b3;
            s12 += a1 * b1 + a2 * b2 + a3 * b3;
            s22 += a1 * a1 + a2 * a2 + a3 * a3;

            ind += n_neighbours[p];
        }

        coefs[0] += (Real) s00 * centroid_weight;
        coefs[1] += 4.0 * (Real) s01 * centroid_weight);
        coefs[2] += (2.0 * (Real) s02 + 4.0 * (Real) s11) * centroid_weight;
        coefs[3] += 4.0 * (Real) s12 * centroid_weight;
        coefs[4] += (Real) s22 * centroid_weight;
    }
}

private  void  minimize_along_line(
    int     n_parameters,
    dtype   parameters[],
    dtype   delta[],
    dtype   distances[],
    int     n_neighbours[],
    int     *neighbours[],
    Real    centroid_weight,
    dtype   centroid_weights[],
    Real    sphere_weight )
{
    int    p, s, n_solutions, best_index;
    Real   coefs[5], deriv[4], *test, t, fit, best_fit, solutions[3];
/*
    Real   test_fit;
*/

    ALLOC( test, n_parameters );

    evaluate_fit_along_line( n_parameters, parameters, delta, distances,
                             n_neighbours, neighbours, centroid_weight,
                             centroid_weights, sphere_weight, coefs );

    for_less( p, 0, 4 )
        deriv[p] = (Real) (p+1) * coefs[p+1];

    n_solutions = solve_cubic( deriv[3], deriv[2], deriv[1], deriv[0],
                               solutions );

    if( n_solutions == 0 )
        print( "N solutions = 0\n" );

    best_fit = coefs[0];
    best_index = -1;

/*
    test_fit = evaluate_fit( n_parameters, parameters, distances,
                             n_neighbours, neighbours, sphere_weight );
    print( "## %g %g\n", coefs[0], test_fit );
*/

    for_less( s, 0, n_solutions )
    {
        t = solutions[s];

/*
        for_less( p, 0, n_parameters )
            test[p] = parameters[p] + t * delta[p];
*/

        fit = coefs[0] + t * (coefs[1] + t * (coefs[2] + t * (coefs[3] +
              t * coefs[4])));

/*
        test_fit = evaluate_fit( n_parameters, test, distances,
                                 n_neighbours, neighbours, sphere_weight );

        print( "%g %g\n", fit, test_fit );
*/

        if( fit < best_fit )
        {
            best_fit = fit;
            best_index = s;
        }
    }

    if( best_index >= 0 )
    {
        t = solutions[best_index];

        for_less( p, 0, n_parameters )
            parameters[p] = (dtype) ((Real) parameters[p] + t*(Real) delta[p]);
    }

    if( sphere_weight > 0.0 && parameters[n_parameters-1] < (dtype) 0.0 )
        parameters[n_parameters-1] *= (dtype) -1.0;

    FREE( test );
}

private  void  flatten_polygons(
    int              n_points,
    Point            points[],
    int              n_neighbours[],
    int              *neighbours[],
    Point            init_points[],
    Real             centroid_weight,
    Real             sphere_weight,
    int              n_iters )
{
    int              p, n, point, dim1, dim2, max_neighbours;
    int              n_parameters, total_neighbours, total_edges;
    Real             gg, dgg, gam, current_time, last_update_time;
    Real             fit;
    Real             len, radius, *weights[3][3], *x_flat, *y_flat, *z_flat;
    Point            centroid, *plane_points;
#ifndef   USE_CENTROID
    int              neigh;
    Vector           offset, normal, hor, vert;
#endif
    int              iter, ind, update_rate;
    dtype            *g, *h, *xi, *parameters, *unit_dir, *distances;
    dtype            *centroid_weights;
    BOOLEAN          init_supplied;

    init_supplied = (init_points != NULL);
    if( !init_supplied )
        init_points = points;

    if( sphere_weight > 0.0 )
    {
        radius = 0.0;
        get_points_centroid( n_points, init_points, &centroid );
        for_less( point, 0, n_points )
        {
            radius += distance_between_points( &points[point], &centroid );
        }
        radius /= (Real) n_points;
    }

    total_neighbours = 0;
    total_edges = 0;
    for_less( point, 0, n_points )
    {
        total_neighbours += n_neighbours[point];
        for_less( n, 0, n_neighbours[point] )
            if( neighbours[point][n] > point )
                ++total_edges;
    }

    if( centroid_weight > 0.0 )
    {
        max_neighbours = 0;
        for_less( point, 0, n_points )
            max_neighbours = MAX( max_neighbours, n_neighbours[point] );

        for_less( dim1, 0, 3 )
        for_less( dim2, 0, 3 )
            ALLOC( weights[dim1][dim2], max_neighbours );
        ALLOC( x_flat, max_neighbours );
        ALLOC( y_flat, max_neighbours );
        ALLOC( z_flat, max_neighbours );
        ALLOC( plane_points, max_neighbours );

        ALLOC( centroid_weights, total_neighbours );
        ind = 0;
        for_less( point, 0, n_points )
        {
#ifdef USE_CENTROID
            for_less( n, 0, n_neighbours[point] )
                centroid_weights[ind+n] = (dtype)
                            (1.0 / (Real) n_neighbours[point]);
#else
            for_less( n, 0, n_neighbours[point] )
                plane_points[n] = points[neighbours[point][n]];

            find_polygon_normal( n_neighbours[point], plane_points, &normal );
            create_two_orthogonal_vectors( &normal, &hor, &vert );

            for_less( n, 0, n_neighbours[point] )
            {
                neigh = neighbours[point][n];
                SUB_POINTS( offset, points[neigh], points[point] );
                x_flat[n] = DOT_VECTORS( offset, hor );
                y_flat[n] = DOT_VECTORS( offset, vert );
                z_flat[n] = 0.0;
            }

            if( !get_prediction_weights_3d( 0.0, 0.0, 0.0,
                                            n_neighbours[point],
                                            x_flat, y_flat, z_flat,
                                            weights[0], weights[1], weights[2]))

            {
                print_error( "Error in interpolation weights.\n" );
            }

            for_less( n, 0, n_neighbours[point] )
                centroid_weights[ind+n] = (dtype) weights[0][n][n];
#endif

            ind += n_neighbours[point];
        }

        for_less( dim1, 0, 3 )
        for_less( dim2, 0, 3 )
            FREE( weights[dim1][dim2] );
        FREE( x_flat );
        FREE( y_flat );
        FREE( z_flat );
        FREE( plane_points );
    }

    ALLOC( distances, total_edges );
    ind = 0;

    for_less( point, 0, n_points )
    {
        for_less( n, 0, n_neighbours[point] )
        {
            if( neighbours[point][n] > point )
            {
                distances[ind] = (dtype) sq_distance_between_points(
                                       &points[point],
                                       &points[neighbours[point][n]] );
                ++ind;
            }
        }
    }

    n_parameters = 3 * n_points;
    if( sphere_weight > 0.0 )
        ++n_parameters;
    ALLOC( parameters, n_parameters );
    ALLOC( g, n_parameters );
    ALLOC( h, n_parameters );
    ALLOC( xi, n_parameters );
    ALLOC( unit_dir, n_parameters );

    for_less( point, 0, n_points )
    {
        parameters[IJ(point,0,3)] = (dtype) Point_x(init_points[point] );
        parameters[IJ(point,1,3)] = (dtype) Point_y(init_points[point] );
        parameters[IJ(point,2,3)] = (dtype) Point_z(init_points[point] );
    }

    if( init_supplied )
        FREE( init_points );

    if( sphere_weight > 0.0 )
        parameters[n_parameters-1] = (dtype) radius;

    sphere_weight *= (Real) total_edges / (Real) n_points;
    centroid_weight *= (Real) total_edges / (Real) n_points;

    fit = evaluate_fit( n_parameters, parameters, distances,
                        n_neighbours, neighbours,
                        centroid_weight, centroid_weights, sphere_weight );

    print( "Initial  %g\n", fit );
    (void) flush_file( stdout );

    evaluate_fit_derivative( n_parameters, parameters, distances,
                             n_neighbours, neighbours, centroid_weight,
                             centroid_weights, sphere_weight, xi );

    for_less( p, 0, n_parameters )
    {
        g[p] = -xi[p];
        h[p] = g[p];
        xi[p] = g[p];
    }

    update_rate = 1;
    last_update_time = current_cpu_seconds();

    for_less( iter, 0, n_iters )
    {
        len = 0.0;
        for_less( p, 0, n_parameters )
            len += (Real) xi[p] * (Real) xi[p];

        len = sqrt( len );
        for_less( p, 0, n_parameters )
            unit_dir[p] = (dtype) ((Real) xi[p] / len);

        minimize_along_line( n_parameters, parameters, unit_dir, distances,
                             n_neighbours, neighbours,
                             centroid_weight, centroid_weights, sphere_weight );

        if( ((iter+1) % update_rate) == 0 || iter == n_iters - 1 )
        {
            fit = evaluate_fit( n_parameters, parameters, distances,
                                n_neighbours, neighbours, centroid_weight,
                                centroid_weights, sphere_weight );

            print( "%d: %g", iter+1, fit );
            if( sphere_weight > 0.0 )
                print( "\t Radius: %g", (Real) parameters[n_parameters-1] );
            print( "\n" );

            (void) flush_file( stdout );
            current_time = current_cpu_seconds();
            if( current_time - last_update_time < 1.0 )
                update_rate *= 10;

            last_update_time = current_time;
        }

        evaluate_fit_derivative( n_parameters, parameters, distances,
                                 n_neighbours, neighbours, centroid_weight,
                                 centroid_weights, sphere_weight, xi );

        gg = 0.0;
        dgg = 0.0;
        for_less( p, 0, n_parameters )
        {
            gg += (Real) g[p] * (Real) g[p];
            dgg += ((Real) xi[p] + (Real) g[p]) * (Real) xi[p];
/*
            dgg += ((Real) xi[p] * (Real) xi[p];
*/
        }

        if( len == 0.0 )
            break;

        gam = dgg / gg;

        for_less( p, 0, n_parameters )
        {
            g[p] = -xi[p];
            h[p] = (dtype) ((Real) g[p] + gam * (Real) h[p]);
            xi[p] = h[p];
        }
    }

    for_less( point, 0, n_points )
    {
        fill_Point( points[point],
                    parameters[IJ(point,0,3)],
                    parameters[IJ(point,1,3)],
                    parameters[IJ(point,2,3)] );
    }

    if( centroid_weight > 0.0 )
        FREE( centroid_weights );
    FREE( distances );
    FREE( parameters );
    FREE( xi );
    FREE( g );
    FREE( h );
    FREE( unit_dir );
}