gist_ball.cc

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

static void
BallFromBoundaryBalls(uint4 num_dim, uint4 num_points,
		      const double*const* centers, const double* radii,
		      double* out_center, double& radius,
		      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...
    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 = 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 = radii[0];
	    return;

	  case 2: {
	      double dist = CalcDist(num_dim, centers[0], centers[1], weights);

	      // Handle rare cases where one boundary covers the other side
	      uint4 low_rad, high_rad;
	      if (radii[0] >= radii[1]) { low_rad = 1; high_rad = 0; }
	      else { low_rad = 0; high_rad = 1; }

	      if (radii[high_rad] > dist + radii[low_rad])
	      {
#if 0
		  WARN_MSG("Weird case: one ball encloses all");
#endif
		  memcpy((void*)out_center, (const void*)centers[0],
			 sizeof(double)*num_dim);
		  radius = radii[high_rad];
	      }

	      double final_dist = (radii[0] + radii[1] + dist)/2.0;

	      // .5 is between centers
	      // 0 is at point1
	      // 1 is at point2
	      // Other values are as expected...
	      double center_ratio;
	      if (dist < MINDOUBLE*4)
	      {
		  center_ratio = 0.5;
		  final_dist += MINDOUBLE*4;
	      }
	      else
		  center_ratio = (dist - radii[0] + radii[1])/(2.0*dist);

	      for (jj=0; jj<num_dim; jj++)
		  out_center[jj] = centers[0][jj] +
		      (centers[1][jj] - centers[0][jj]) * center_ratio;

	      radius = final_dist;
	      return;
	      }
	}

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

    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* line_vec = GrabVector(fl_mem, 1, red_dim+1);
    double* temp_vec = GrabVector(fl_mem, 1, red_dim+1);
    double* x_vec = GrabVector(fl_mem, 1, red_dim+1);
    double* b_vec = GrabVector(fl_mem, 1, red_dim+1);
    double* ww = GrabVector(fl_mem, 1, red_dim+1);
    double* base_vec = GrabVector(fl_mem, 1, num_dim);
    double* off_vec = GrabVector(fl_mem, 1, num_dim);
    double** centers2 = GrabMatrix(ptr_mem, fl_mem, 1, red_dim, 1, num_dim);
    double** temp_mat = GrabMatrix(ptr_mem, fl_mem, 1, red_dim, 1, red_dim+1);
    double** u_mat = GrabMatrix(ptr_mem, fl_mem, 1, red_dim+1, 1, red_dim+1);
    double** v_mat = GrabMatrix(ptr_mem, fl_mem, 1, red_dim+1, 1, red_dim+1);
    assert(pdalign(work_mem + ptr_size)==pdalign(ptr_mem));
    assert(pdalign(work_mem + mem_size)==pdalign(fl_mem));

    double radius0sqrd = radii[0] * radii[0];

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

    // Fill in temp_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];
	    temp_mat[jj][kk] = total;
	    temp_mat[kk][jj] = total;
	}
    for (jj=1; jj<=red_dim; jj++)
	temp_mat[jj][red_dim+1] = radii[0] - radii[jj];

    // To make this solvable by SVD, multiply both sides by
    // the transpose of tempmat
    for (ii=1; ii<=red_dim+1; ii++)
    {
	double total = 0;
	for (kk=1; kk<=red_dim; kk++)
	    total += temp_vec[kk]*temp_mat[kk][ii];
	b_vec[ii] = total;
	for (jj=1; jj<=red_dim+1; jj++)
	{
	    double total = 0;
	    for (kk=1; kk<=red_dim; kk++)
		total += temp_mat[kk][ii]*temp_mat[kk][jj];
	    u_mat[ii][jj] = total;
	}
    }

    // OK, we now have a system of linear equations, so find the
    // solution space (which is one dimensional in this case)
    svdcmp(u_mat, red_dim+1, red_dim+1, ww, v_mat); // Augmented u_mat...
    double wmax = 0.0;
    for (jj=1; jj<=red_dim+1; jj++)
	if (ww[jj] > wmax) wmax = ww[jj];
    double wmin = wmax*5.0e-8;
    bool found_singularity = 0;
    for (jj=1; jj<=red_dim+1; jj++)
	if (ww[jj] < wmin)
	{
	    ww[jj] = 0.0;
	    if (found_singularity)
	    {
		WARN_MSG("Extra singularity: Automatically fixing: Try increasing RAD_SLACK");
		for (ii=1; ii<=red_dim+1; ii++)
		    line_vec[ii] = v_mat[ii][jj]*v_mat[red_dim+1][jj] +
			line_vec[ii]*line_vec[red_dim+1];
	    }
	    else
	    {
		found_singularity = 1;
		for (ii=1; ii<=red_dim+1; ii++)
		    line_vec[ii] = v_mat[ii][jj];
	    }
	}
    assert(found_singularity);  // Must find at least one singularity
    assert(fabs(line_vec[red_dim+1]) > MINDOUBLE*16);

    // Multiply vector so the r is 1 (Assume mult is slower than division)
    double factor = 1/line_vec[red_dim+1];
    for (ii=1; ii<=red_dim; ii++)
	line_vec[ii] *= factor;
    // line_vec[red_dim+1] = 1.0;

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

    // Subtract the line_vec so the r is 0
    for (ii=1; ii<=red_dim; ii++)
	x_vec[ii] -= line_vec[ii] * x_vec[red_dim+1];
    // x_vec[red_dim+1] = 0.0;  // This is ignored also, but should be 0.0

    // Project into full dimensionality
    for (ii=1; ii<=num_dim; ii++)
    {
	double base_total = 0;
	double off_total = 0;
	for (jj=1; jj<=red_dim; jj++)
	{
	    base_total += centers2[jj][ii] * x_vec[jj];
	    off_total += centers2[jj][ii] * line_vec[jj];
	}
	base_vec[ii] = base_total;
	off_vec[ii] = off_total;
    }

    double a_sqrd_sum=0;
    double b_sqrd_sum=0;
    double ab_sum=0;
    for (ii=1; ii<=num_dim; ii++)
    {
	a_sqrd_sum += off_vec[ii] * off_vec[ii];
	b_sqrd_sum += base_vec[ii] * base_vec[ii];
	ab_sum += base_vec[ii] * off_vec[ii];
    }

    double aa = 1 - a_sqrd_sum;
    double bb = -2.0 * (ab_sum + radii[0]);
    double cc = radii[0]*radii[0] - b_sqrd_sum;

    // Quadratic equation
    double sqrt_term = bb*bb - 4.0 * aa * cc;
    assert(sqrt_term >= 0.0);
    double rr;
    assert(aa != 0.0);
    if (aa>0)
	rr = (sqrt(sqrt_term) - bb)/(2.0*aa);
    else
	rr = (-sqrt(sqrt_term) - bb)/(2.0*aa);
    assert(rr>0.0);

    // Reuse x_vec[] for the values of the final center
    if (weights!=NULL)
	for (ii=1; ii<=num_dim; ii++)
	{
	    double weighted = base_vec[ii] + rr * off_vec[ii];
	    out_center[ii-1] = centers[0][ii-1] + weighted/weights[ii-1];
	}
    else
	for (ii=1; ii<=num_dim; ii++)
	    out_center[ii-1] =
		centers[0][ii-1] + base_vec[ii] + rr * off_vec[ii];

    // Double check the solution
    double max_dist = 0;
    for (ii=0; ii<num_points; ii++)
    {
	double dist =
	    radii[ii] + CalcDist(num_dim, out_center, centers[ii], weights);
	assert(fabs(rr - dist) <= BOUND_RAD_ERR*rr);
	if (dist > max_dist)
	    max_dist = dist;
    }
	
    // If the solution is incorrect, then there probably is a ball
    // included that is not on the outside of the bounding ball.
    // Therefore, we could try throwing away a point and redoing
    // the calculations, but I may not need this, so I'll put it off...
    radius = max_dist;
    if (need_alloc) delete[] work_mem;
}


static int
int_comp(const void* p1, const void* p2)
{
    return *((long*)p1) - *((long*)p2);
}

static void
IntSort(uint4* bound_arr, uint4 num_entries)
{
    qsort((void*)bound_arr, num_entries, sizeof(uint4), int_comp);
}

void
EnclosingBall(uint4 num_dim, uint4 num_points,
	      const double*const* centers, const double* radii,
	      double* out_center, double& out_radius,
	      const double* weights, uint4* boundary_arr,
	      uint4 in_boundary_size, uint4* out_boundary_size,
	      void* work, uint4 work_size)
{
    assert(num_points>0);
    assert(num_dim>1);
    assert(centers!=NULL);
    assert(out_center!=NULL);
    assert((out_boundary_size==NULL) || (boundary_arr!=NULL));
    uint4 ii;
    double common_radius = 0;

    // Directly solve the easy cases
    if (num_points <= 2)
    {
	if (radii==NULL)
	{
	    BallFromBoundaryPoints(num_dim, num_points, centers,
				   out_center, out_radius, weights);
	    out_radius = sqrt(out_radius);
	}
	else
	    BallFromBoundaryBalls(num_dim, num_points, centers, radii,
				  out_center, out_radius, weights);
	if (out_boundary_size)
	{
	    *out_boundary_size = 1;
	    boundary_arr[0]=0;
	}
	return;
    }

    // If all the same radius, then just do the point calculation
    // and add on the extra radius at the end...
    if (radii!=NULL)
    {
	for (ii=1; ii<num_points; ii++)
	    if (radii[ii]!=radii[0])
		break;
	if (ii==num_points)
	{
	    common_radius = radii[0];
	    radii = NULL;
	}
    }

    // Maximum boundary points and level of recursion
    uint4 max_level = num_points > num_dim+1 ? num_dim+1: num_points;

    uint4 int_size = dalign(sizeof(uint4)*(2*num_points + 2*max_level));
    uint4 ptr_size = dalign(sizeof(double*)*max_level);
    uint4 fl_size = sizeof(double)*(radii!=NULL?max_level:0);
    uint4 mem_size = ptr_size + int_size + fl_size;
    uint4 extra_size;
    unsigned char* work_mem;
    void* extra_mem;
    assert((work==NULL && work_size==0) || (work!=NULL && work_size>0));
    bool need_alloc = (work==NULL || work_size < mem_size);
    if (need_alloc)
    {
	extra_size = sizeof(double*)*4*max_level +
	    sizeof(double)*(5*max_level + 2*num_dim + max_level*num_dim +
			   3*max_level*max_level);
	// Shouldn't happen, but handle this case
	if (mem_size > (1<<13))
	    extra_size = 0;
	else if (extra_size + mem_size > (1<<14))
	    extra_size = (1<<14) - mem_size;  // Limit total to 16K
	work_mem = (unsigned char*)
	    new double[dalign(mem_size + extra_size)/sizeof(double)];
	extra_mem = (void*)(work_mem + mem_size);
    }
    else
    {
	work_mem = (unsigned char*)work;
	extra_mem = (void*)(work_mem + mem_size);
	extra_size = work_size - mem_size;
    }

    double** ptr_mem = (double**)work_mem;
    uint4* int_mem = (uint4*)(work_mem + ptr_size);
    double* fl_mem = (double*)(work_mem + ptr_size + int_size);

    // Used to create permuted next_elem
    uint4* array = GrabIntVector(int_mem, 0, num_points-1);
    // Linked list of elements
    uint4* next_elem = GrabIntVector(int_mem, 0, num_points-1);
    // Array of boundary ball elements
    uint4* bound_elem = GrabIntVector(int_mem, 0, max_level-1);
    uint4* last_stack = GrabIntVector(int_mem, 0, max_level-1);
    const double** bound_centers = GrabPtrVector(ptr_mem, 0, max_level-1);
    double* bound_radii =
	radii!=NULL ? GrabVector(fl_mem, 0, max_level-1): (double*) NULL; 

    assert(pdalign(work_mem + ptr_size)==pdalign(ptr_mem));
    assert(pdalign(work_mem + ptr_size + int_size)==pdalign(int_mem));
    assert(pdalign(work_mem + mem_size)==pdalign(fl_mem));

    uint4 first;	// First in list of elements
    uint4 bound_size;
    uint4 curr_level;
    uint4 prev, curr, last;
    double radius, true_radius;	// Use local variable so a register can be used
    // BTW, radius is radius squared if dealing with points
#ifdef SHOW_WORK
    uint4 old_first_elem=1000000;
#endif

    for (ii=0; ii<num_points; ii++)
	array[ii] = ii;

    // If a boundary was given, make it the first elements
    assert(in_boundary_size <= num_points);
    if (in_boundary_size > 0)
    {
	// Sort to avoid swapping problems
	IntSort(boundary_arr, in_boundary_size);
	for (ii=0; ii<in_boundary_size; ii++)
	{
	    uint4 other = boundary_arr[ii];
	    assert(other < num_points);  // Valid element
	    assert(ii==0 || other!=boundary_arr[ii-1]); // No repeats...
	    uint4 tmp = array[ii];
	    array[ii] = array[other];
	    array[other] = tmp;
	}
    }

    // Permute the ordering other elements...
    for (ii=in_boundary_size; ii<num_points-1; ii++)
    {
	uint4 pick = rand()%(num_points-ii);
	uint4 tmp = array[ii];
	array[ii] = array[ii+pick];
	array[ii+pick] = tmp;
    }

    first = array[0];
    for (ii=1; ii<num_points; ii++)
	next_elem[array[ii-1]] = array[ii];
    next_elem[array[num_points-1]] = num_points;  // End list terminator

    // Init Center and boundary to enclose the first point
    // This avoid some special cases below
    memset((void*)out_center, 0, num_dim*sizeof(double));
    radius = MINDOUBLE;
    true_radius = MINDOUBLE;
    bound_elem[0] = first;
    bound_centers[0] = centers[first];
    if (radii!=NULL)
	bound_radii[0] = radii[first];
    bound_size = 1;

    curr_level = 0;
    prev = first;
    curr = next_elem[first];
    last = num_points;

    while (curr!=num_points)
    {
	while (1)
	{
	    // Is it in the ball...
	    double total_dist = (radii != NULL) ?
		CalcDist(num_dim, out_center, centers[curr], weights) +
		    radii[curr]:
		// Here total_dist and radius are squared
		CalcDistSqrd(num_dim, out_center, centers[curr], weights);

	    // Quit loop if enclosed by ball...
	    if (total_dist <= true_radius) break;
	    if (total_dist <= radius)
	    {
		// Yes quit loop but update true_radius
		true_radius = total_dist;
		break;
	    }

	    // This shouldn't happen, but might happen due to numeric error...
	    // I'll handle this later if necessary
	    assert(curr_level<=max_level);

	    bound_elem[curr_level] = curr;
	    bound_centers[curr_level] = centers[curr];
	    if (radii!=NULL)
		bound_radii[curr_level] = radii[curr];
	    bound_size = curr_level+1;

	    if (radii!=NULL)
	    {
		BallFromBoundaryBalls(num_dim, bound_size, bound_centers,
				      bound_radii, out_center,
				      true_radius, weights,
				      extra_mem, extra_size);
		// Add a little extra space (avoid singularities)
		radius = true_radius*(1.0 + RAD_SLACK);
	    }
	    else
	    {
		BallFromBoundaryPoints(num_dim, bound_size, bound_centers,
				       out_center, true_radius, weights,
				       extra_mem, extra_size);
		// Add a little extra space (avoid singularities)
		// This is squared dist so the error tolerance is doubled
		radius = true_radius*(1.0 + 2.0*RAD_SLACK);
	    }

	    if (curr==first) break;
	    last_stack[curr_level] = last;
	    curr_level++;
	    last = prev;
	    curr = first;
	}

#ifdef SHOW_WORK
	if (curr_level==0 && bound_size>1 && bound_elem[0]!=old_first_elem)
	{
	    for (ii=0; ii<bound_size; ii++)
		cout << bound_elem[ii] << " ";
	    cout << endl;
	    old_first_elem = bound_elem[0];
	}
#endif

	prev = curr;
	curr = next_elem[curr];

	uint4 test_prev = prev;
	while (test_prev==last)
	{
	    assert(curr_level != 0);
	    curr_level--;
	    last = last_stack[curr_level];
	    uint4 next = next_elem[curr];

	    // Now do move to front...
	    assert(curr!=first);
	    next_elem[prev] = next;
	    next_elem[curr] = first;
	    test_prev = first = curr;
	    curr = next;
	}
    }
    assert(curr_level==0);

    const double FLOAT_ERROR_SAFETY = 1.001;
    if (radii!=NULL)
	out_radius = true_radius*FLOAT_ERROR_SAFETY;
    else
	out_radius = (sqrt(true_radius) + common_radius)*FLOAT_ERROR_SAFETY;

    // Output boundary points
    if (boundary_arr!=NULL && out_boundary_size!=NULL)
    {
	if (bound_size < *out_boundary_size)
	    *out_boundary_size = bound_size;
	uint4 out_size = *out_boundary_size;
	for (ii=0; ii<out_size; ii++)
	    boundary_arr[ii] = bound_elem[ii];
    }

#ifndef NDEBUG
    if (radii==NULL)
	for (ii=0; ii<num_points; ii++)
	{
	    double dist = CalcDist(num_dim, out_center, centers[ii], weights);
	    if (dist > out_radius) {
		cerr << "EnclosingBall: increasing from "
		     << out_radius << " to "
		     << dist << endl;
		out_radius = dist;
	    }
	    assert(dist <= out_radius);
	}
    else
	for (ii=0; ii<num_points; ii++)
	{
	    double dist = CalcDist(num_dim, out_center, centers[ii], weights);
	    dist += radii[ii];
	    if (dist > out_radius) {
		cerr << "EnclosingBall: increasing from "
		     << out_radius << " to "
		     << dist << endl;
		out_radius = dist;
	    }
	    assert(dist <= out_radius);
	}
#endif

    if (need_alloc) delete[] work_mem;
}