214 lines
4.6 KiB
C++
214 lines
4.6 KiB
C++
#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{
|
|
|
|
/**
|
|
* A sphere is defined as a vector (a,b,c,r) => center (a,b,c) and radius r
|
|
*/
|
|
|
|
|
|
///evaluate implicit sphere equation at x =(x1,x2,x3)
|
|
///returns signed dist
|
|
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);
|
|
}
|
|
|
|
///evaluate implicit sphere equation at x =(x1,x2,x3)
|
|
///returns signed dist
|
|
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;
|
|
}
|
|
|
|
///evaluate implicit sphere equation at x =(x1,x2,x3)
|
|
///returns signed squared dist
|
|
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));
|
|
}
|
|
|
|
///evaluate implicit sphere equation at x =(x1,x2,x3)
|
|
///returns signed dist
|
|
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;
|
|
}
|
|
|
|
///construct smallest enclosing sphere of one point
|
|
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;
|
|
}
|
|
|
|
///sphere through 2 points
|
|
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;
|
|
}
|
|
|
|
///sphere through 3 points
|
|
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);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
///sphere through 4 points
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
|
|
///compute smallest enclosing sphere of points
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|