miniball.H

/*
 * This file is part of the "Archon" framework.
 * (http://files3d.sourceforge.net)
 *
 * Copyright © 2002 by Kristian Spangsege and Brian Kristiansen.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation under the terms of the GNU General Public License is
 * hereby granted. No representations are made about the suitability of
 * this software for any purpose. It is provided "as is" without express
 * or implied warranty. See the GNU General Public License
 * (http://www.gnu.org/copyleft/gpl.html) for more details.
 *
 * The characters in this file are ISO8859-1 encoded.
 *
 * The documentation in this file is in "Doxygen" style
 * (http://www.doxygen.org).
 */



//    Copright (C) 1999
//    $Revision: 1.1 $
//    $Date: 2003/03/15 16:26:26 $
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program; if not, write to the Free Software
//    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA,
//    or download the License terms from prep.ai.mit.edu/pub/gnu/COPYING-2.0.
//
//    Contact:
//    --------
//    Bernd Gaertner
//    Institut f. Informatik
//    ETH Zuerich
//    ETH-Zentrum
//    CH-8092 Zuerich, Switzerland
//    http://www.inf.ethz.ch/personal/gaertner
//


#include <list>

#include <archon/math/vector.H>

namespace Archon
{
  using namespace std;

  namespace Utilities
  {
    template<int N>
    class MinimalBoundingBall
    {
    public:
      // types
      typedef Math::BasicVector<double, N>           Point;
      typedef typename list<Point>::iterator         It;
      typedef typename list<Point>::const_iterator   Cit;

      class Basis
      {
      private:
        // data members
        int    m, s;   // size and number of support points
        double q0[N];
    
        double z[N+1];
        double f[N+1];
        double v[N+1][N];
        double a[N+1][N];
    
        double c[N+1][N];
        double sqr_r[N+1];

        double *current_c;      // points to some c[j]
        double current_sqr_r;
    
        double sqr(double r) const { return r*r; }
    
      public:
        Basis();

        // access
        const double *center() const;
        double       squared_radius() const;
        int          size() const;
        int          support_size() const;
        double       excess(const Point &) const;

        // modification
        void reset(); // generates empty sphere with m=s=0
        bool push(const Point &);
        void pop();

        // checking
        double slack() const;
      };

    private:
      // data members
      list<Point> L;              // STL list keeping the points
      Basis       B;              // basis keeping the current ball
      It          support_end;    // past-the-end iterator of support set
    
      // private methods
      void   mtf_mb(It k);
      void   pivot_mb(It k);
      void   move_to_front(It j);
      double max_excess(It t, It i, It &pivot) const;
      double abs(double r) const { return r>0 ? r : -r; }
      double sqr(double r) const { return r*r; }

    public:
      // construction
      MinimalBoundingBall() {}
      void check_in(const Point &);
      void build(bool pivoting = true);
    
      // access
      Point  center() const;
      double squared_radius() const;
      int    nr_points() const;
      Cit    points_begin() const;
      Cit    points_end() const;
      int    nr_support_points() const;
      Cit    support_points_begin() const;
      Cit    support_points_end() const;
    
      // checking
      double accuracy(double &slack) const;
      bool   is_valid(double tolerance = 1e-15) const;
    };



    template <int N>
    void MinimalBoundingBall<N>::check_in(const Point &p)
    {
      L.push_back(p);
    }

    template<int N>
    void MinimalBoundingBall<N>::build(bool pivoting)
    {
      B.reset();
      support_end = L.begin();
      if(pivoting) pivot_mb(L.end());
      else mtf_mb(L.end());
    }

    template<int N>
    void MinimalBoundingBall<N>::mtf_mb(It i)
    {
      support_end = L.begin();
      if((B.size())==N+1) return;
      for(It k=L.begin(); k!=i;)
      {
        It j=k++;
        if(B.excess(*j) > 0)
        {
          if(B.push(*j))
          {
            mtf_mb (j);
            B.pop();
            move_to_front(j);
          }
        }
      }
    }

    template<int N>
    void MinimalBoundingBall<N>::move_to_front(It j)
    {
      if(support_end == j) support_end++;
      L.splice(L.begin(), L, j);
    }

    template<int N>
    void MinimalBoundingBall<N>::pivot_mb(It i)
    {
      It t = ++L.begin();
      mtf_mb(t);
      double max_e, old_sqr_r = 0;
      do
      {
        It pivot;
        max_e = max_excess(t, i, pivot);
        if(max_e > 0)
        {
          t = support_end;
          if(t==pivot) ++t;
          old_sqr_r = B.squared_radius();
          B.push(*pivot);
          mtf_mb(support_end);
          B.pop();
          move_to_front(pivot);
        }
      }
      while(max_e > 0 && B.squared_radius() > old_sqr_r);
    }

    template<int N>
    double MinimalBoundingBall<N>::max_excess(It t, It i, It &pivot) const
    {
      const double *c = B.center(), sqr_r = B.squared_radius();
      double e, max_e = 0;
      for(It k=t; k!=i; ++k)
      {
        const Point &p = *k;
        e = -sqr_r;
        for(int j=0; j<N; ++j) e += sqr(p[j]-c[j]);
        if(e > max_e)
        {
          max_e = e;
          pivot = k;
        }
      }
      return max_e;
    }

    template<int N>
    typename MinimalBoundingBall<N>::Point MinimalBoundingBall<N>::center() const
    {
      return Point(B.center());
    }

    template<int N>
    double MinimalBoundingBall<N>::squared_radius() const
    {
      return B.squared_radius();
    }

    template<int N>
    int MinimalBoundingBall<N>::nr_points() const
    {
      return L.size();
    }

    template<int N>
    typename MinimalBoundingBall<N>::Cit MinimalBoundingBall<N>::points_begin() const
    {
      return L.begin();
    }

    template<int N>
    typename MinimalBoundingBall<N>::Cit MinimalBoundingBall<N>::points_end() const
    {
      return L.end();
    }

    template<int N>
    int MinimalBoundingBall<N>::nr_support_points() const
    {
      return B.support_size();
    }

    template<int N>
    typename MinimalBoundingBall<N>::Cit MinimalBoundingBall<N>::support_points_begin() const
    {
      return L.begin();
    }

    template<int N>
    typename MinimalBoundingBall<N>::Cit MinimalBoundingBall<N>::support_points_end() const
    {
      return support_end;
    }

    template<int N>
    double MinimalBoundingBall<N>::accuracy(double &slack) const
    {
      double e, max_e = 0;
      int n_supp=0;
      Cit i;
      for(i=L.begin(); i!=support_end; ++i,++n_supp)
        if((e = abs(B.excess(*i))) > max_e)
          max_e = e;
   
      // you've found a non-numerical problem if the following ever fails
      assert(n_supp == nr_support_points());
   
      for(i=support_end; i!=L.end(); ++i)
        if((e = B.excess(*i)) > max_e)
          max_e = e;
   
      slack = B.slack();
      return max_e/squared_radius();
    }

    template<int N>
    bool MinimalBoundingBall<N>::is_valid(double tolerance) const
    {
      double slack;
      return accuracy(slack) < tolerance && slack == 0;
    }


    // MinimalBoundingBall<N>::Basis

    template<int N>
    const double *MinimalBoundingBall<N>::Basis::center() const
    {
      return current_c;
    }

    template<int N>
    double MinimalBoundingBall<N>::Basis::squared_radius() const
    {
      return current_sqr_r;
    }

    template<int N>
    int MinimalBoundingBall<N>::Basis::size() const
    {
      return m;
    }

    template<int N>
    int MinimalBoundingBall<N>::Basis::support_size() const
    {
      return s;
    }

    template<int N>
    double MinimalBoundingBall<N>::Basis::excess(const Point &p) const
    {
      double e = -current_sqr_r;
      for(int k=0; k<N; ++k) e += sqr(p[k]-current_c[k]);
      return e;
    }

    template<int N>
    void MinimalBoundingBall<N>::Basis::reset()
    {
      m = s = 0;
      // we misuse c[0] for the center of the empty sphere
      for(int j=0; j<N; ++j) c[0][j]=0;
      current_c = c[0];
      current_sqr_r = -1;
    }

    template<int N>
    MinimalBoundingBall<N>::Basis::Basis()
    {
      reset();
    }

    template<int N>
    void MinimalBoundingBall<N>::Basis::pop()
    {
      --m;
    }

    template<int N>
    bool MinimalBoundingBall<N>::Basis::push(const Point &p)
    {
      int i, j;
      double eps = 1e-32;
      if(m==0)
      {
        for(i=0; i<N; ++i) q0[i] = p[i];
        for(i=0; i<N; ++i) c[0][i] = q0[i];
        sqr_r[0] = 0;
      }
      else
      {
        // set v_m to Q_m
        for(i=0; i<N; ++i) v[m][i] = p[i]-q0[i];

        // compute the a_{m,i}, i< m
        for(i=1; i<m; ++i)
        {
          a[m][i] = 0;
          for(j=0; j<N; ++j) a[m][i] += v[i][j] * v[m][j];
          a[m][i]*=(2/z[i]);
        }
   
        // update v_m to Q_m-\bar{Q}_m
        for(i=1; i<m; ++i)
          for(j=0; j<N; ++j)
            v[m][j] -= a[m][i]*v[i][j];
   
        // compute z_m
        z[m]=0;
        for(j=0; j<N; ++j) z[m] += sqr(v[m][j]);
        z[m]*=2;
   
        // reject push if z_m too small
        if(z[m]<eps*current_sqr_r) return false;
   
        // update c, sqr_r
        double e = -sqr_r[m-1];
        for(i=0; i<N; ++i) e += sqr(p[i]-c[m-1][i]);
        f[m]=e/z[m];
   
        for(i=0; i<N; ++i) c[m][i] = c[m-1][i]+f[m]*v[m][i];
        sqr_r[m] = sqr_r[m-1] + e*f[m]/2;
      }
      current_c = c[m];
      current_sqr_r = sqr_r[m];
      s = ++m;
      return true;
    }

    template<int N>
    double MinimalBoundingBall<N>::Basis::slack() const
    {
      double l[N+1], min_l=0;
      l[0] = 1;
      for(int i=s-1; i>0; --i)
      {
        l[i] = f[i];
        for(int k=s-1; k>i; --k) l[i]-=a[k][i]*l[k];
        if(l[i] < min_l) min_l = l[i];
        l[0] -= l[i];
      }
      if(l[0] < min_l) min_l = l[0];
      return min_l < 0 ? -min_l : 0;
    }
  }
}

---