CodeArchive MinEnclosingSphere

Minimum Enclosing Sphere of a bunch of points in 3D

I am waiting for a certain someone to fill this in :P

---

Here you go, the unmodified version. -- Main.MatthewChan - 24 Nov 2005

struct point{
  ld x, y, z;
  point(ld xx=0, ld yy=0, ld zz=0):x(xx), y(yy), z(zz) {}
};

struct sphere{
  ld x, y, z, r2;
  sphere(ld xx = 0, ld yy = 0, ld zz = 0, ld rr = 0): x(xx), y(yy), z(zz), r2(rr) {}
  sphere(const point &p, ld rr=0): x(p.x), y(p.y), z(p.z), r2(rr) {}
  point get_point() const { return point(x, y, z); }
};

const ld ep = 1e-12;

inline ld dist(const point &s, const point &p) {
  return (p.x-s.x)*(p.x-s.x) + (p.y-s.y)*(p.y-s.y) + (p.z-s.z)*(p.z-s.z);
}

#define mac_det(a,b) (m[(a)][(b)]*m[((a)+1)%3][((b)+1)%3] - m[((a)+1)%3][(b)]*m[(a)][((b)+1)%3])

// We need a mini-solver for 3x3 linear system
bool solve_sys(ld m[3][4], ld &x, ld &y, ld&z) { // Solve for this, return true if success
  ld det = m[0][0]*mac_det(1,1) + m[0][1] * mac_det(1, 2) + m[0][2] * mac_det(1, 0);
  if (fabs(det) < ep) return false;
  // Solve it, formula from Maple
  x = mac_det(1,1) * m[0][3] + mac_det(2, 1) * m[1][3] + mac_det(0, 1) * m[2][3];
  y = mac_det(1,2) * m[0][3] + mac_det(2, 2) * m[1][3] + mac_det(0, 2) * m[2][3];
  z = mac_det(1,0) * m[0][3] + mac_det(2, 0) * m[1][3] + mac_det(0, 0) * m[2][3];
  x /= det; y /= det; z /= det;
  return true;
}

inline bool is_colinear(const point& a, const point& b, const point& c) {
  ld x1 = (b.x - a.x), y1 = (b.y - a.y), z1 = (b.z - a.z);
  ld x2 = (c.x - a.x), y2 = (c.y - a.y), z2 = (c.z - a.z);
  ld len = ( x1 * x1 + y1 * y1 + z1 * z1) * ( x2 * x2 + y2 * y2 + z2 * z2);
  bool ret = (x1 * y2 - x2 * y1) * (x1 * y2 - x2 * y1) + (y1*z2 - y2*z1) * (y1*z2 - y2*z1) + (z1*x2 - z2*x1)*(z1*x2-z2*x1) < ep * len;
  return ret;
}

sphere get_sphere2(const point& a, const point& b) {
  return sphere( 0.5 * (a.x + b.x),
      0.5 * (a.y + b.y), 0.5 * (a.z + b.z), 0.25*dist(a,b));
}

#define test_good_sphere(s) {\
  p = s.get_point(); \
  if (dist(a,p) < s.r2 + ep\
      && dist(b,p) < s.r2 + ep\
      && dist(c,p) < s.r2 + ep\
      && dist(d,p) < s.r2 + ep) {\
    return s;}}

#define dot_prod(a,b) (a.x*b.x + a.y*b.y + a.z*b.z)

#define solve2(u,v) if ( dist((u),(v)) > ep ) { result = get_sphere2((u),(v)); test_good_sphere(result) }

#define solve3(u,v,w) if (!is_colinear(u,v,w)) {\
  uu = dot_prod(u,u), vv = dot_prod(v,v), ww = dot_prod(w,w);\
  m[0][0] = 2*(u.x - w.x); m[0][1] = 2*(u.y - w.y); m[0][2] = 2*(u.z - w.z); m[0][3] = uu-ww;\
  m[1][0] = 2*(v.x - w.x); m[1][1] = 2*(v.y - w.y); m[1][2] = 2*(v.z - w.z); m[1][3] = vv-ww;\
  m[2][0] = mac_det(0,1); m[2][1] = mac_det(0,2); m[2][2] = mac_det(0,0);\
  m[2][3] = m[2][0]*w.x + m[2][1]*w.y + m[2][2]*w.z;\
  if( solve_sys(m, p.x, p.y, p.z) ) { result = sphere(p, dist(p, u) ); test_good_sphere(result) }}

//sphere get_sphere(const point& a, const point& b, const point& c, const point& d) {
sphere gets(const point& a, const point& b, const point& c, const point& d) {
  // Return the four point
  // Try using only 1 pt, a
  if (dist(b,a) < ep && dist(c,a) < ep && dist(d,a) < ep) return sphere(a, 0);
  ld m[3][4], tt,uu,vv,ww;
  point p;
  sphere result;
  // Try all two points, 6 cases, ab, ac, ad, bc, bd, cd
  solve2(a,b); solve2(a,c); solve2(a,d); solve2(b,c); solve2(b,d); solve2(c,d);
  // 3 points case
  solve3(a,b,c); solve3(a,b,d); solve3(a,c,d); solve3(b,c,d);
  // 4 points case
  tt = dot_prod(a,a); uu = dot_prod(b,b); vv = dot_prod(c,c); ww = dot_prod(d,d);
  m[0][0] = 2*(b.x - a.x); m[0][1] = 2*(b.y - a.y); m[0][2] = 2*(b.z - a.z); m[0][3] = uu-tt;
  m[1][0] = 2*(c.x - a.x); m[1][1] = 2*(c.y - a.y); m[1][2] = 2*(c.z - a.z); m[1][3] = vv-tt;
  m[2][0] = 2*(d.x - a.x); m[2][1] = 2*(d.y - a.y); m[2][2] = 2*(d.z - a.z); m[2][3] = ww-tt;
  solve_sys(m, p.x, p.y, p.z);
  result = sphere(p, dist(p, a) );
  return result;
}

---

-- Main.SapphireJay - 21 Nov 2005