sphere.h

#pragma once
#include <cgv/math/vec.h>
#include <cgv/math/functions.h>
#include <cgv/math/det.h>
#include <cgv/math/lin_solve.h>
#include <limits>
 
 
namespace cgv{
    namespace math{
 
template <typename T>
T sphere_val(const vec<T>& sphere, const vec<T>& x)
{
 
    assert(sphere.size()-1  == x.size());
 
    T val=0;
    for(unsigned i = 0; i< x.size(); i++)
        val += sqr(x(i)-sphere(i));
    
    return sqrt(val)-sphere(sphere.size()-1);
}
 
template <typename T>
vec<T> sphere_val(const vec<T>& sphere, const mat<T>& xs)
{
 
    assert(sphere.size()-1  == xs.ncols());
 
    vec<T> vals=0;
    for(unsigned i = 0; i< xs.ncols(); i++)
        vals(i) += sphere_val(sphere,xs.col(i));
    
    return vals;
}
 
template <typename T>
T sphere_val2(const vec<T>& sphere, const vec<T>& x)
{
 
    assert(sphere.size()-1  == x.size());
 
    T val=0;
    for(unsigned i = 0; i< x.size(); i++)
        val += sqr(x(i)-sphere(i));
 
    return val-sqr(sphere(sphere.size()-1));
}
 
template <typename T>
vec<T> sphere_val2(const vec<T>& sphere, const mat<T>& xs)
{
 
    assert(sphere.size()-1  == xs.ncols());
 
    vec<T> vals=0;
    for(unsigned i = 0; i< xs.ncols(); i++)
        vals(i) += sphere_val2(sphere,xs.col(i));
    
    return vals;
}
 
template <typename T>
vec<T> sphere_fit(const vec<T>& p1)
{
    vec<T> sphere(4);
    sphere.set(p1(0),p1(1),p1(2),std::numeric_limits<T>::epsilon());
    
    return sphere;
}
 
template <typename T>
vec<T> sphere_fit(const vec<T>& p1,const vec<T>& p2)
{
    vec<T> sphere(4);
    vec<T> center = (T)0.5*(p1+p2);
 
    sphere.set(center(0),center(1),center(2),length(p2-center)+ std::numeric_limits<T>::epsilon());
    
    return sphere;
}
 
template<typename T>
vec<T> sphere_fit(const vec<T>& O,const vec<T>& A,const vec<T>& B)
{
    vec<T> a = A - O;
    vec<T> b = B - O;
    T denominator = (T)2.0 * dot(cross(a , b) , cross(a , b));
    vec<T> o = (dot(b,b) * cross(cross(a , b) , a) +
                dot(a,a) * cross(b , cross(a , b))) / denominator;
 
    T   radius = length(o) + std::numeric_limits<T>::epsilon();
    vec<T>  center = O + o;
    
    return vec<T>(center(0),center(1),center(2),radius);
 
}
 
 
 
 
template <typename T>
vec<T> sphere_fit(const vec<T>& x1,const vec<T>& x2,const vec<T>& x3,const vec<T>& x4)
{
    mat<T> M(4,4);  
    M(0,0) = 1; M(0,1) = x1(0); M(0,2) = x1(1); M(0,3) = x1(2);
    M(1,0) = 1; M(1,1) = x2(0); M(1,2) = x2(1); M(1,3) = x2(2);
    M(2,0) = 1; M(2,1) = x3(0); M(2,2) = x3(1); M(2,3) = x3(2);
    M(3,0) = 1; M(3,1) = x4(0); M(3,2) = x4(1); M(3,3) = x4(2);
    vec<T> b(4);
    b(0) = -dot(x1,x1);
    b(1) = -dot(x2,x2);
    b(2) = -dot(x3,x3);
    b(3) = -dot(x4,x4);
    vec<T> x;
    cgv::math::solve(M,b,x);
    vec<T> center = vec<T>(-x(1)/(T)2.0,-x(2)/(T)2.0,-x(3)/(T)2.0);
    T radius  = sqrt(dot(center,center)-x(0));
    return vec<T>(center(0),center(1),center(2),radius);
 
    
}
 
 
//recursive helper function 
template <typename T>
vec<T> recurse_mini_ball(T** P, unsigned int p, unsigned int b=0)
{
    vec<T> mb;
 
    switch(b)
    {
        case 0:
            mb=vec<T>((T)0,(T)0,(T)0,(T)-1);
            break;
        case 1:
            mb = sphere_fit(vec<T>(3,(const T*)P[-1]));
            break;
        case 2:
            mb = sphere_fit(vec<T>(3,(const T*)P[-1]),vec<T>(3,(const T*)P[-2]));
            break;
        case 3:
            mb = sphere_fit(vec<T>(3,(const T*)P[-1]),vec<T>(3,(const T*)P[-2]),vec<T>(3,(const T*)P[-3]));
            break;
        case 4:
            mb = sphere_fit(vec<T>(3,(const T*)P[-1]),vec<T>(3,(const T*)P[-2]),vec<T>(3,(const T*)P[-3])
                    ,vec<T>(3,(const T*)P[-4]));
            return mb;
        }
 
        for(unsigned int i = 0; i < p; i++)
        {
            if(sphere_val2(mb,vec<T>(3,(const T*)P[i])) > 0)   // Signed square distance to sphere
            {
                for(unsigned int j = i; j > 0; j--)
                {
                    T *a = P[j];
                    P[j] = P[j - 1];
                    P[j - 1] = a;
                }
 
                mb = recurse_mini_ball(P + 1, i, b + 1);
            }
        }
 
        return mb;
    }
 
 
 
 
template <typename T>
vec<T> mini_ball(cgv::math::mat<T>&  points)
{
    unsigned p = points.ncols();
     T **L = new  T*[p];
 
    for(unsigned int i = 0; i < p; i++)
            L[i] = &(points(0,i));
 
    
 
    vec<T> mb = recurse_mini_ball(L, p);
 
    delete[] L;
 
    return mb;
    
}
 
 
    }
}