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
  17 public:
  18         BoundingSphere(): radius(0) { }
  19         BoundingSphere(const LinAl::Vector<T, N> &, T);
  20
  21         template<typename Iter>
  22         static BoundingSphere from_point_cloud(const Iter &, const Iter &);
  23
  24         const LinAl::Vector<T, N> get_center() const { return center; }
  25         T get_radius() const { return radius; }
  26 };
  27
  28 template<typename T, unsigned N>
  29 BoundingSphere<T, N>::BoundingSphere(const LinAl::Vector<T, N> &c, T r):
  30         center(c),
  31         radius(r)
  32 { }
  33
  34 template<typename T, unsigned N>
  35 template<typename Iter>
  36 BoundingSphere<T, N> BoundingSphere<T, N>::from_point_cloud(const Iter &begin, const Iter &end)
  37 {
  38         using std::sqrt;
  39
  40         LinAl::Vector<T, N> p1;
  41         T sqdist = 0;
  42         for(Iter i=begin; i!=end; ++i)
  43         {
  44                 T d = inner_product(*i, *i);
  45                 if(d>sqdist)
  46                 {
  47                         p1 = *i;
  48                         sqdist = d;
  49                 }
  50         }
  51
  52         LinAl::Vector<T, N> p2;
  53         sqdist = 0;
  54         for(Iter i=begin; i!=end; ++i)
  55         {
  56                 LinAl::Vector<T, N> v = *i-p1;
  57                 T d = inner_product(v, v);
  58                 if(d>sqdist)
  59                 {
  60                         p2 = *i;
  61                         sqdist = d;
  62                 }
  63         }
  64
  65         sqdist /= 4;
  66         BoundingSphere<T, N> bsphere((p1+p2)/T(2), sqrt(sqdist));
  67         for(Iter i=begin; i!=end; ++i)
  68         {
  69                 LinAl::Vector<T, N> v = *i-bsphere.center;
  70                 T d = inner_product(v, v);
  71                 if(d>sqdist)
  72                 {
  73                         d = sqrt(d);
  74                         bsphere.center += v*(1-bsphere.radius/d);
  75                         bsphere.radius += d/2;
  76                         sqdist = bsphere.radius*bsphere.radius;
  77                 }
  78         }
  79
  80         return bsphere;
  81 }
  82
  83 template<typename T, unsigned N>
  84 BoundingSphere<T, N> operator|(const BoundingSphere<T, N> &bs1, const BoundingSphere<T, N> &bs2)
  85 {
  86         LinAl::Vector<T, N> v = bs2.get_center()-bs1.get_center();
  87         T d = v.norm();
  88         if(d+bs2.get_radius()<bs1.get_radius())
  89                 return bs1;
  90         else if(d+bs1.get_radius()<bs2.get_radius())
  91                 return bs2;
  92         else
  93         {
  94                 T r = (bs1.get_radius()+d+bs2.get_radius())/T(2);
  95                 if(bs1.get_radius()>bs2.get_radius())
  96                         return BoundingSphere<T, N>(bs1.get_center()+v*((r-bs1.get_radius())/d), r);
  97                 else
  98                         return BoundingSphere<T, N>(bs2.get_center()-v*((r-bs2.get_radius())/d), r);
  99         }
 100 }
 101
 102 } // namespace Geometry
 103 } // namespace Msp
 104
 105 #endif