gist_ball.cc

Libgist is an implementation of the Generalized Search Tree, a template index structure that makes i

// gist_ball.cc							-*- c++ -*-
// Copyright (c) 1995, David A. White <[e-mail]>
// Copyright (c) 1998, Regents of the University of California
// $Id: gist_ball.cc,v 1.12 2000/03/15 00:24:50 mashah Exp $

#include "gist_compat.h"

#include <memory.h>
#include <math.h>
#include <stddef.h>
#include <stdlib.h>

#include "gist_defs.h"
#include "gist_ball.h"

#ifndef NDEBUG

// VCPORT_B
#ifdef WIN32
#include <iostream>

using namespace std;
#else
#include <iostream.h>
#endif
// VCPORT_E

#define WARN_MSG(str) cerr << str
#else
#define WARN_MSG(str)
#endif

inline uint4
dalign(uint4 len)
{
    return((len + sizeof(double) - 1) & ~(sizeof(double) - 1));
}

inline void*
pdalign(void* p)
{
    return((void*) dalign((uint4) p));
}

static double
CalcDistSqrd(uint4 num_dim, const double* v1, const double* v2)
{
    assert(num_dim > 1);
    assert(v1 != NULL && v2 != NULL);
    double dist;

    uint4 jj;
    double diff;
    diff = v1[0] - v2[0];
    dist = diff * diff;
    for ( jj = 1; jj < num_dim; jj++ )
    {
	diff = v1[jj] - v2[jj];
	dist += diff * diff;
    }
    return dist;
}

//
// This calculates a weighted quadratic distance between vectors
//
static double
CalcDistSqrd(uint4 num_dim, const double* v1, const double* v2,
	     const double* ww)
{
    if (ww == NULL) return CalcDistSqrd(num_dim, v1, v2);
    assert(num_dim > 1);
    assert(v1 != NULL && v2 != NULL);

    uint4 jj;
    double dist, diff;

    diff = v1[0] - v2[0];
    dist = ww[0] * diff * diff;
    for ( jj = 1; jj < num_dim; jj++ )
    {
	diff = v1[jj] - v2[jj];
	dist += ww[jj] * diff * diff;
    }
    return dist;
}

static double
CalcDist(uint4 num_dim, const double* v1, const double* v2)
{
    return sqrt(CalcDistSqrd(num_dim, v1, v2));
}

static double
CalcDist(uint4 num_dim, const double* v1, const double* v2,
	 const double* ww)
{
    return sqrt(CalcDistSqrd(num_dim, v1, v2, ww));
}

//
// This code uses "Numerical Recipes in C" code to do single precision SVD
// Other packages offering SVD, such as fortran packages, would also work,
// but this code will require modifications.  For example, the
// vectors and arrays use with numerical recipies are 1 relative
// instead of 0 relative.
//

inline double
nr_pythag(double a, double b)
{
    a = fabs(a);
    b = fabs(b);
    double c;

    if (a > b) {   // "&& a > 0.0"
	c = b / a;
	return(a * sqrt(1.0 + c * c));
    }
    if (b > 0.0) { // "&& b >= a"
	c = a / b;
	return(b * sqrt(1.0 + c * c));
    }
    return(0.0);
}

inline double
nr_max(double a, double b)
{
    return((a > b) ? a : b);
}

inline double
nr_sign(double a, double b)
{
    return((b) >= 0.0 ? fabs(a) : -fabs(a));
}

inline double*
_vector(int l, int h)
{
    return((new double[(h)-(l)+1]) - l);
}

inline void
free_vector(double* p, int l, int h)
{
    delete [] ((p) + (l));
}

static void
svdcmp(double** a, int m, int n, double* w, double** v)
{
	int flag,i,its,j,jj,k,l=-1,nm=-1;
	double c,f,h,s,x,y,z;
	double anorm=0.0,g=0.0,scale=0.0;
	double *rv1;

	assert(m >= n);
	rv1=_vector(1,n);
	for (i=1;i<=n;i++) {
		l=i+1;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m) {
			for (k=i;k<=m;k++) scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -nr_sign(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				if (i != n) {
					for (j=l;j<=n;j++) {
						for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
						f=s/h;
						for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
					}
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -nr_sign(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				if (i != m) {
					for (j=l;j<=m;j++) {
						for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
						for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
					}
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm=nr_max(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++)
					v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
		}
		v[i][i]=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=n;i>=1;i--) {
		l=i+1;
		g=w[i];
		if (i < n)
			for (j=l;j<=n;j++) a[i][j]=0.0;
		if (g) {
			g=1.0/g;
			if (i != n) {
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
					f=(s/a[i][i])*g;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else {
			for (j=i;j<=m;j++) a[j][i]=0.0;
		}
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;l--) {
				nm=l-1;
				if (fabs(rv1[l])+anorm == anorm) {
					flag=0;
					break;
				}
				if (fabs(w[nm])+anorm == anorm) break;
			}
			if (flag) {
				c=0.0;
				s=1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					if (fabs(f)+anorm != anorm) {
						g=w[i];
						h=nr_pythag(f,g);
						w[i]=h;
						h=1.0/h;
						c=g*h;
						s=(-f*h);
						for (j=1;j<=m;j++) {
							y=a[j][nm];
							z=a[j][i];
							a[j][nm]=y*c+z*s;
							a[j][i]=z*c-y*s;
						}
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
				}
				break;
			}
			assert(its != 30);
			x=w[l];
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=nr_pythag(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+nr_sign(g,f)))-h))/x;
			c=s=1.0;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=nr_pythag(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g=g*c-x*s;
				h=y*s;
				y=y*c;
				for (jj=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=nr_pythag(f,h);
				w[j]=z;
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=(c*g)+(s*y);
				x=(c*y)-(s*g);
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	free_vector(rv1,1,n);
}

static void
svbksb(double** u, double w[], double** v, int m, int n, double b[], double x[])
{
	int jj,j,i;
	double s,*tmp;

	tmp=_vector(1,n);
	for (j=1;j<=n;j++) {
		s=0.0;
		if (w[j]) {
			for (i=1;i<=m;i++) s += u[i][j]*b[i];
			s /= w[j];
		}
		tmp[j]=s;
	}
	for (j=1;j<=n;j++) {
		s=0.0;
		for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
		x[j]=s;
	}
	free_vector(tmp,1,n);
}

// This is the percentage error allowed so that a ball is still
// considered enclosed.  The output of the algorithm could be off
// by this much, but it probably won't happen.
const double RAD_SLACK = 0.015;

// This is the percentage difference allowed between points that
// are supposed to be equidistant.  Singularities might cause this error.
// This is only used during debugging to check that calculations are OK...
const double BOUND_RAD_ERR = 0.01;

inline double**
GrabMatrix(double**& ptr_curr, double*& fl_curr,
	   uint4 start1, uint4 end1,
	   uint4 start2, uint4 end2)
{
    double** mat = ptr_curr - start1;
    ptr_curr += end1 - start1 + 1;
    for (uint4 ii=start1; ii<=end1; ii++)
    {
	mat[ii] = fl_curr - start2;
	fl_curr += end2 - start2 + 1;
    }
    return (double**)mat;
}

inline double*
GrabVector(double*& fl_curr, uint4 start1, uint4 end1)
{
    double* vec = fl_curr - start1;
    fl_curr += end1 - start1 + 1;
    return vec;
}

inline uint4*
GrabIntVector(uint4*& i_curr, uint4 start1, uint4 end1)
{
    uint4* vec = i_curr - start1;
    i_curr += end1 - start1 + 1;
    return vec;
}

inline const double**
GrabPtrVector(double**& ptr_curr, uint4 start1, uint4 end1)
{
    const double** vec = (const double**)ptr_curr - start1;
    ptr_curr += end1 - start1 + 1;
    return vec;
}

//
// This routine determines the center of a ball given points
// on it, and weights determining it's ellipsoid shape
//
// This is done as follows:
// 1. Create a set of num_points - 1 linear equations to solve
// 2. Solve, and use to calculate the center point and radius
//
// The work memory required is calculated below...
// The work_size variable is provided for debugging purposes...
//
// NOTE: All points input to this routine must be on the boundary.  If not,
// the results will not be correct.
//
static void
BallFromBoundaryPoints(uint4 num_dim, uint4 num_points,
		       const double*const* centers, double* out_center,
		       double& radius_sqrd, const double* weights=NULL,
		       void* work=NULL, uint4 work_size=0)
{
    assert(num_dim >= num_points-1);
    uint4 ii, jj, kk;

    // Handle the easy cases explicitly...
    if (num_points <= 2)
	switch (num_points)
	{
	  case 0:
	    // Ball centered at origin with zero radius
	    for (ii=0; ii<num_dim; ii++)
		out_center[ii] = 0.0;
	    radius_sqrd = 0.0;
	    return;

	  case 1:
	    // Return the ball centered at that point of minimum size
	    memcpy((void*)out_center, (const void*)centers[0],
		   num_dim*sizeof(double));
	    radius_sqrd = 16*MINDOUBLE;
	    return;

	  case 2: {
	      double dist = 0.0;
	      for (ii=0; ii<num_dim; ii++)
	      {
		  double diff = centers[0][ii] - centers[1][ii];
		  dist += diff*diff;
		  out_center[ii] = (centers[0][ii] + centers[1][ii])/2.0;
	      }
	      radius_sqrd = dist/4.0;
	      return;
	  }
	}

    // Allocate all memory needed
    uint4 red_dim = num_points-1;
    unsigned char* work_mem;
    uint4 ptr_size = dalign(sizeof(double*)*(3 * red_dim));
    uint4 fl_size = sizeof(double)*
	(3*red_dim + (2*red_dim)*red_dim + red_dim*num_dim);
    uint4 mem_size = ptr_size + fl_size;

    assert((work==NULL && work_size==0) || (work!=NULL && work_size>0));
    bool need_alloc = (work==NULL || work_size < mem_size);
    if (need_alloc)
	work_mem = (unsigned char*) new double[dalign(mem_size)/sizeof(double)];
    else
	work_mem = (unsigned char*)work;

    double** ptr_mem = (double**)work_mem;
    double* fl_mem = (double*)(work_mem + ptr_size);

    double* b_vec = GrabVector(fl_mem, 1, red_dim);
    double* ww = GrabVector(fl_mem, 1, red_dim);
    double* x_vec = GrabVector(fl_mem, 1, red_dim);
    double** centers2 =
	GrabMatrix(ptr_mem, fl_mem, 1, red_dim, 1, num_dim);
    double** u_mat = GrabMatrix(ptr_mem, fl_mem, 1, red_dim, 1, red_dim);
    double** v_mat = GrabMatrix(ptr_mem, fl_mem, 1, red_dim, 1, red_dim);
    assert(pdalign(work_mem + ptr_size)==pdalign(ptr_mem));
    assert(pdalign(work_mem + mem_size)==pdalign(fl_mem));

    // Make center2 and b_vec: only mess with weights if needed
    if (weights!=NULL)
	for (ii=1; ii<=red_dim; ii++)
	{
	    double total = 0;
	    for (jj=0; jj<num_dim; jj++)
	    {
		double diff = (centers[ii][jj] - centers[0][jj])*weights[jj];
		centers2[ii][jj+1] = diff;
		total += diff*diff;
	    }
	    b_vec[ii] = total/2.0;
	}
    else
	for (ii=1; ii<=red_dim; ii++)
	{
	    double total = 0;
	    for (jj=0; jj<num_dim; jj++)
	    {
		double diff = centers[ii][jj] - centers[0][jj];
		centers2[ii][jj+1] = diff;
		total += diff*diff;
	    }
	    b_vec[ii] = total/2.0;
	}

    // Fill in u_mat using centers2
    // It is symmetric, so only do half the work...
    for (jj=1; jj<=red_dim; jj++)
	for (kk=1; kk<=jj; kk++)
	{
	    double total = 0;
	    for (ii=1; ii<=num_dim; ii++)
		total += centers2[jj][ii] * centers2[kk][ii];
	    u_mat[jj][kk] = total;
	    u_mat[kk][jj] = total;
	}

    // OK, we now have a system of linear equations, so solve it
    svdcmp(u_mat, red_dim, red_dim, ww, v_mat); // Augmented u_mat...
    double wmax = 0.0;
    for (jj=1; jj<=red_dim; jj++)
	if (ww[jj] > wmax)
	    wmax = ww[jj];
    double wmin = wmax*5.0e-8;
    for (jj=1; jj<=red_dim; jj++)
	if (ww[jj] < wmin)
	{
	    ww[jj] = 0.0;  // Try to "fix" the singularity
	    WARN_MSG("Singularity in matrix, ignoring: try increasing RAD_SLACK.");
	}

    svbksb(u_mat, ww, v_mat, red_dim, red_dim, b_vec, x_vec);

    // OK, now x_vec holds the center in the reduced dimensionality
    // so we just need to project it back...
    if (weights!=NULL)
	for (ii=0; ii<num_dim; ii++)
	{
	    double total = 0;
	    for (jj=1; jj<=red_dim; jj++)
		total += x_vec[jj] * centers2[jj][ii+1];
	    out_center[ii] = centers[0][ii] + total/weights[ii];
	}
    else
	for (ii=0; ii<num_dim; ii++)
	{
	    double total = centers[0][ii];
	    for (jj=1; jj<=red_dim; jj++)
		total += x_vec[jj] * centers2[jj][ii+1];
	    out_center[ii] = total;
	}

    double max_dist = 0;
    for (ii=0; ii<num_points; ii++)
    {
	double dist = CalcDistSqrd(num_dim, out_center, centers[ii], weights);
	assert(max_dist==0 || fabs(max_dist-dist) <= 2.0*BOUND_RAD_ERR*dist);
	if (dist > max_dist) max_dist = dist;
    }
    radius_sqrd = max_dist;

    if (need_alloc) delete[] work_mem;
}

//
// NOTE: All balls input to this routine must be on the boundary.  If not,
// the results will not be correct.  (ie. one ball encloses other balls)
// It is detected in the 2 ball case, but not handled in general.
//