CGII/framework/include/cgv/math/sphere.h

214 lines
4.6 KiB
C
Raw Normal View History

2018-05-17 13:50:03 +00:00
#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;
}
}
}