repsnapper-2.5a4/src/miniball.h

//    Copright (C) 1999-2006, Bernd Gaertner
//    $Revision: 1.3 $
//    $Date: 2006/11/16 08:01:52 $
//
//    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
//    Institute of Theoretical Computer Science
//    ETH Zuerich
//    CAB G32.2
//    CH-8092 Zuerich, Switzerland
//    http://www.inf.ethz.ch/personal/gaertner

#include <cstdlib>
#include <cassert>
#include <cmath>
#include <iostream>
#include <list>

// Functions
// =========
inline double mb_sqr (double r) {return r*r;}

// Class Declarations
// ==================

// smallest enclosing ball of a set of n points in dimension d
template <int d>
class Miniball;

// smallest ball with a set of n <= d+1 points *on the boundary*
template <int d>
class Miniball_b;

// point in dimension d
template <int d>
class Point;


// Class Definitions
// =================

// Miniball
// --------

template <int d>
class Miniball {
public:
	// types
	typedef typename std::list<Point<d> >::iterator         It;
	typedef typename std::list<Point<d> >::const_iterator   Cit;

private:
	// data members
	std::list<Point<d> > L;            // internal point set
	Miniball_b<d>        B;            // 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;

public:
	// creates an empty ball
	Miniball() {}

	// copies p to the internal point set
	void        check_in (const Point<d>& p);

	// builds the smallest enclosing ball of the internal point set
	void        build ();

	// returns center of the ball (undefined if ball is empty)
	Point<d>       center() const;

	// returns squared_radius of the ball (-1 if ball is empty)
	double      squared_radius () const;

	// returns size of internal point set
	int         nr_points () const;

	// returns begin- and past-the-end iterators for internal point set
	Cit         points_begin () const;
	Cit         points_end () const;

	// returns size of support point set; this set has the following properties:
	// - there are at most d+1 support points,
	// - all support points are on the boundary of the computed ball, and
	// - the smallest enclosing ball of the support point set equals the
	//   smallest enclosing ball of the internal point set
	int         nr_support_points () const;

	// returns begin- and past-the-end iterators for internal point set
	Cit         support_points_begin () const;
	Cit         support_points_end () const;

	// assesses the quality of the computed ball. The return value is the
	// maximum squared distance of any support point or point outside the
	// ball to the boundary of the ball, divided by the squared radius of
	// the ball. If everything went fine, this will be less than e-15 and
	// says that the computed ball approximately contains all the internal
	// points and has all the support points on the boundary.
	//
	// The slack parameter that is set by the method says something about
	// whether the computed ball is really the *smallest* enclosing ball
	// of the support points; if everything went fine, this value will be 0;
	// a positive value may indicate that the ball is not smallest possible,
	// with the deviation from optimality growing with the slack
	double      accuracy (double& slack) const;

	// returns true if the accuracy is below the given tolerance and the
	// slack is 0
	bool        is_valid (double tolerance = 1e-15) const;
};


// Miniball_b
// ----------

template <int d>
class Miniball_b {
private:
	// data members
	int                 m, s;   // size and number of support points
	double              q0[d];

	double              z[d+1];
	double              f[d+1];
	double              v[d+1][d];
	double              a[d+1][d];

	double              c[d+1][d];
	double              sqr_r[d+1];

	double*             current_c;      // refers to some c[j]
	double              current_sqr_r;

public:
	Miniball_b() {reset();}

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

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

	// checking
	double              slack() const;
};

// Point (inline)
// -------------
template <int d>
class Point {
private:
	double coord [d];

public:
	// default
	Point()
	{}

	// copy from Point
	Point (const Point& p)
	{
		for (int i=0; i<d; ++i)
			coord[i] = p.coord[i];
	}

	// copy from double*
	Point (const double* p)
	{
		for (int i=0; i<d; ++i)
			coord[i] = p[i];
	}

	// assignment
	Point& operator = (const Point& p)
	{
		if (this != &p)
			for (int i=0; i<d; ++i)
				coord[i] = p.coord[i];
		return *this;
	}

	// coordinate access
	double& operator [] (int i)
	{
		return coord[i];
	}
	const double& operator [] (int i) const
	{
		return coord[i];
	}
	const double* begin() const
	{
		return coord;
	}
	const double* end() const
	{
		return coord+d;
	}
};


// Class Implementations
// =====================

// Miniball
// --------

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


template <int d>
void Miniball<d>::build ()
{
	B.reset();
	support_end = L.begin();
	pivot_mb (L.end());
}


template <int d>
void Miniball<d>::mtf_mb (It i)
{
	support_end = L.begin();
	if ((B.size())==d+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 d>
void Miniball<d>::move_to_front (It j)
{
	if (support_end == j)
		support_end++;
	L.splice (L.begin(), L, j);
}


template <int d>
void Miniball<d>::pivot_mb (It i)
{
	It t = ++L.begin();
	mtf_mb (t);
	double max_e, old_sqr_r = -1;
	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 d>
double Miniball<d>::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 double *p = (*k).begin();
		e = -sqr_r;
		for (int j=0; j<d; ++j)
			e += mb_sqr(p[j]-c[j]);
		if (e > max_e) {
			max_e = e;
			pivot = k;
		}
	}
	return max_e;
}


template <int d>
Point<d> Miniball<d>::center () const
{
	return Point<d>(B.center());
}

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


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

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

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


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

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

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


template <int d>
double Miniball<d>::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 = std::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 d>
bool Miniball<d>::is_valid (double tolerance) const
{
	double slack;
	return ( (accuracy (slack) < tolerance) && (slack == 0) );
}


// Miniball_b
// ----------

template <int d>
const double* Miniball_b<d>::center () const
{
	return current_c;
}

template <int d>
double Miniball_b<d>::squared_radius() const
{
	return current_sqr_r;
}

template <int d>
int Miniball_b<d>::size() const
{
	return m;
}

template <int d>
int Miniball_b<d>::support_size() const
{
	return s;
}

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


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


template <int d>
void Miniball_b<d>::pop ()
{
	--m;
}


template <int d>
bool Miniball_b<d>::push (const Point<d>& p)
{
	int i, j;
	double eps = 1e-32;
	if (m==0) {
		for (i=0; i<d; ++i)
			q0[i] = p[i];
		for (i=0; i<d; ++i)
			c[0][i] = q0[i];
		sqr_r[0] = 0;
	} else {
		// set v_m to Q_m
		for (i=0; i<d; ++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<d; ++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<d; ++j)
				v[m][j] -= a[m][i]*v[i][j];
		}

		// compute z_m
		z[m]=0;
		for (j=0; j<d; ++j)
			z[m] += mb_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<d; ++i)
			e += mb_sqr(p[i]-c[m-1][i]);
		f[m]=e/z[m];

		for (i=0; i<d; ++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 d>
double Miniball_b<d>::slack () const
{
	double l[d+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);
}

// Point
// -----

// Output
template <int d>
std::ostream& operator << (std::ostream& os, const Point<d>& p)
{
	os << "(";
	for (int i=0; i<d-1; ++i)
		os << p[i] << ", ";
	os << p[d-1] << ")";
	return os;
}