source/geometry/boundingsphere.h

   1 #ifndef MSP_GEOMETRY_BOUNDINGSPHERE_H_
   2 #define MSP_GEOMETRY_BOUNDINGSPHERE_H_
   3
   4 #include <cmath>
   5 #include <msp/linal/vector.h>
   6
   7 namespace Msp {
   8 namespace Geometry {
   9
  10 template<typename T, unsigned N>
  11 class BoundingSphere
  12 {
  13 private:
  14         LinAl::Vector<T, N> center;
  15         T radius;
  16         bool empty;
  17
  18 public:
  19         BoundingSphere();
  20         BoundingSphere(const LinAl::Vector<T, N> &, T);
  21
  22         template<typename Iter>
  23         static BoundingSphere from_point_cloud(const Iter &, const Iter &);
  24
  25         const LinAl::Vector<T, N> get_center() const { return center; }
  26         T get_radius() const { return radius; }
  27         bool is_empty() const { return empty; }
  28 };
  29
  30 template<typename T, unsigned N>
  31 BoundingSphere<T, N>::BoundingSphere():
  32         radius(0),
  33         empty(true)
  34 { }
  35
  36 template<typename T, unsigned N>
  37 BoundingSphere<T, N>::BoundingSphere(const LinAl::Vector<T, N> &c, T r):
  38         center(c),
  39         radius(r),
  40         empty(false)
  41 { }
  42
  43 template<typename T, unsigned N>
  44 template<typename Iter>
  45 BoundingSphere<T, N> BoundingSphere<T, N>::from_point_cloud(const Iter &begin, const Iter &end)
  46 {
  47         using std::sqrt;
  48
  49         if(begin==end)
  50                 return BoundingSphere<T, N>();
  51
  52         LinAl::Vector<T, N> p1;
  53         T sqdist = 0;
  54         for(Iter i=begin; i!=end; ++i)
  55         {
  56                 T d = inner_product(*i, *i);
  57                 if(d>sqdist)
  58                 {
  59                         p1 = *i;
  60                         sqdist = d;
  61                 }
  62         }
  63
  64         LinAl::Vector<T, N> p2;
  65         sqdist = 0;
  66         for(Iter i=begin; i!=end; ++i)
  67         {
  68                 LinAl::Vector<T, N> v = *i-p1;
  69                 T d = inner_product(v, v);
  70                 if(d>sqdist)
  71                 {
  72                         p2 = *i;
  73                         sqdist = d;
  74                 }
  75         }
  76
  77         sqdist /= 4;
  78         BoundingSphere<T, N> bsphere((p1+p2)/T(2), sqrt(sqdist));
  79         for(Iter i=begin; i!=end; ++i)
  80         {
  81                 LinAl::Vector<T, N> v = *i-bsphere.center;
  82                 T d = inner_product(v, v);
  83                 if(d>sqdist)
  84                 {
  85                         d = sqrt(d);
  86                         bsphere.center += v*(1-bsphere.radius/d);
  87                         bsphere.radius += d/2;
  88                         sqdist = bsphere.radius*bsphere.radius;
  89                 }
  90         }
  91
  92         return bsphere;
  93 }
  94
  95 template<typename T, unsigned N>
  96 BoundingSphere<T, N> operator|(const BoundingSphere<T, N> &bs1, const BoundingSphere<T, N> &bs2)
  97 {
  98         if(bs1.is_empty())
  99                 return bs2;
 100         else if(bs2.is_empty())
 101                 return bs1;
 102
 103         LinAl::Vector<T, N> v = bs2.get_center()-bs1.get_center();
 104         T d = v.norm();
 105         if(d+bs2.get_radius()<bs1.get_radius())
 106                 return bs1;
 107         else if(d+bs1.get_radius()<bs2.get_radius())
 108                 return bs2;
 109         else
 110         {
 111                 T r = (bs1.get_radius()+d+bs2.get_radius())/T(2);
 112                 if(bs1.get_radius()>bs2.get_radius())
 113                         return BoundingSphere<T, N>(bs1.get_center()+v*((r-bs1.get_radius())/d), r);
 114                 else
 115                         return BoundingSphere<T, N>(bs2.get_center()-v*((r-bs2.get_radius())/d), r);
 116         }
 117 }
 118
 119 } // namespace Geometry
 120 } // namespace Msp
 121
 122 #endif