File content as of revision 1:05e1d9336ca8:

#include "SphereFitting.hpp"

float const EPSILON =  1e-8;

bool is_zero(float val);

template<typename T>
Sphere<T>::Sphere(T arg_cx, T arg_cy, T arg_cz)
{
    cx = arg_cx;
    cy = arg_cy;
    cz = arg_cz;
    r = 1;
}

template<typename T>
SphereFitting<T>::SphereFitting(T arg_cx, T arg_cy, T arg_cz)
{
    P = new Sphere<T>(arg_cx, arg_cy, arg_cz);
    for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) X[i][j] = 0.0;
    for (int i = 0; i < 4; i++) Y[i] = 0.0;
}

template<typename T>
SphereFitting<T>::~SphereFitting()
{
    delete(P);
}

template<typename T>
void SphereFitting<T>::addsample(T x, T y, T z)
{
    X[0][0] += x * x;
    X[0][1] += x * y;
    X[0][2] += x * z;
    X[0][3] += x;

    X[1][0] += y * x;
    X[1][1] += y * y;
    X[1][2] += y * z;
    X[1][3] += y;

    X[2][0] += z * x;
    X[2][1] += z * y;
    X[2][2] += z * z;
    X[2][3] += z;

    X[3][0] += x;
    X[3][1] += y;
    X[3][2] += z;
    X[3][3] += 1;

    Y[0] += x *(x * x + y * y + z * z);
    Y[1] += y *(x * x + y * y + z * z);
    Y[2] += z *(x * x + y * y + z * z);
    Y[3] += x * x + y * y + z * z;
}

template<typename T>
void SphereFitting<T>::solve()
{
    float temp;

    // Completing making expression
    for (int i = 0; i < 4; i++) 
        for (int j = 0; j < 4; j++) 
            X[i][j] *= 2;
    
    //  Linear Algebrical Procedure
    for(int i = 0 ; i < 4 ; i++ ) {
        for(int j = i+1 ; j < 4 ; j++ ) {
            // Each proc lasts until X[i][j] is sufficiently small
            if(-EPSILON < X[i][j] && X[i][j] < EPSILON)
                break;

            for(int k = 0 ; k < 4 ; k++ )
                X[i][k] += X[j][k];
            Y[i] += Y[j];
        }

        temp = X[i][i];
        for(int j = i ; j < 4; j++ )
            X[i][j] /= temp;
        Y[i] /= temp;

        for(int j = i+1 ; j < 4 ; j++ ) {
            temp = X[j][i];

            for(int k = i ; k < 4 ; k++ )
                X[j][k] -= temp * X[i][k];
            Y[j] -= temp * Y[i];
        }
    }

    for(int i = 4-1 ; i >= 0 ; i-- )
        for(int j = i - 1 ; j >= 0 ; j-- )
            Y[j] -= X[j][i] * Y[i];

    P->cx = Y[0];
    P->cy = Y[1];
    P->cz = Y[2];
    P->r = (short)sqrt((double)(Y[0]*Y[0] + Y[1]*Y[1] + Y[2]*Y[2] + 2*Y[3]));
}


template<typename T>
void SphereFitting<T>::copy_param_to(T* ret_cx, T* ret_cy, T* ret_cz, T* ret_r)
{
    *ret_cx = P->cx;
    *ret_cy = P->cy;
    *ret_cz = P->cz;
    if (ret_r != NULL)
        *ret_r  = P->r;
}