EllipsoidOverlapTools.c

Go to the documentation of this file.

/*
*  Copyright (C) 2007 Anand Sengupta, Craig Robinson
*
*  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 with program; see the file COPYING. If not, write to the
*  Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
*  MA  02110-1301  USA
*/
 
/*-----------------------------------------------------------------------
 *
 * File Name: EllipsoidOverlapTools.c
 *
 * Author: Anand S. Sengupta and Craig Robinson
 *
 *
 *-----------------------------------------------------------------------
 */
 
#include <lal/EllipsoidOverlapTools.h>
 
static REAL8 fContact (REAL8 x, void *params);
 
/* ---------------------------------------------------------------------------
 * This function return the contact function of two ellipsoids as defined in
 * the paper by Perram & Wertheim, JCP, v58, pp 409-416, eq 3.7 with a
 * negative sign. This is minimised to figure out if the ellipsoids
 * intersect.
 * --------------------------------------------------------------------------*/
 
 
 
static REAL8 fContact (REAL8 x, void *params)
{
    fContactWorkSpace *p
            = (fContactWorkSpace *)params;
 
    gsl_permutation   *p1    = p->p1;
    gsl_vector        *tmpV  = p->tmpV;
    gsl_matrix        *C     = p->C;
    gsl_matrix        *A     = p->tmpA;
    gsl_matrix        *B     = p->tmpB;
 
    INT4     s1;
    REAL8    result;
 
    gsl_matrix_memcpy ( A, p->invQ1);
    gsl_matrix_memcpy ( B, p->invQ2);
 
    gsl_matrix_scale (B, x);
    gsl_matrix_scale (A, (1.0L-x));
 
    gsl_matrix_add (A, B);
 
    gsl_linalg_LU_decomp( A, p1, &s1 );
    gsl_linalg_LU_invert( A, p1, C );
 
    /* Evaluate the product C x r_AB */
    gsl_blas_dsymv (CblasUpper, 1.0, C, p->r_AB, 0.0, tmpV);
 
    /* Now evaluate transpose(r_AB) x (C x r_AB) */
    gsl_blas_ddot (p->r_AB, tmpV, &result);
 
    result *= (x*(1.0L-x));
 
    return  (-result);
}
 
/*
-------------------------------------------------------------------------
 * This function allocates and initialises the memory and parameters for
 * the workspace used for determining whether the two ellipsoids intersect.
 * If the shape matrices a and b are not null, these will be used in the
 * workspace; otherwise they will point to null, and will have to be
 * set by hand.
 * ------------------------------------------------------------------------*/
fContactWorkSpace * XLALInitFContactWorkSpace(
                       UINT4                          n,
                       const gsl_matrix              *a,
                       const gsl_matrix              *b,
                       const gsl_min_fminimizer_type *T,
                       REAL8                          conv
                                             )
{
  fContactWorkSpace *workSpace = NULL;
 
  /* Check the parameters passed in are sensible */
  if ( n <= 0 )
    XLAL_ERROR_NULL( XLAL_EINVAL );
 
  if ( conv <= 0.0 )
    XLAL_ERROR_NULL( XLAL_EINVAL );
 
  if ( !T )
    XLAL_ERROR_NULL( XLAL_EFAULT );
 
  /* The matrices a and b should be both supplied or both null */
  if ((!( a && b )) && ( a || b ))
        XLAL_ERROR_NULL( XLAL_EINVAL );
 
  /* Check the matrices conform to the expected sizes */
  if ( a )
  {
    if ( a->size1 != n || a->size2 != n || b->size1 != n || b->size2 != n )
    {
      XLAL_ERROR_NULL( XLAL_EBADLEN );
    }
  }
 
 
  /* Allocate the workspace */
  workSpace = (fContactWorkSpace *) LALCalloc( 1, sizeof(fContactWorkSpace) );
  if ( !workSpace )
    XLAL_ERROR_NULL( XLAL_ENOMEM );
 
  if ( a )
  {
    workSpace->invQ1         = a;
    workSpace->invQ2         = b;
  }
 
  XLAL_CALLGSL( workSpace->tmpA = gsl_matrix_calloc( n, n ) );
  XLAL_CALLGSL( workSpace->tmpB = gsl_matrix_calloc( n, n ) );
  XLAL_CALLGSL( workSpace->C    = gsl_matrix_calloc( n, n ) );
  XLAL_CALLGSL( workSpace->p1   = gsl_permutation_alloc( n ) );
  XLAL_CALLGSL( workSpace->tmpV = gsl_vector_calloc( n ) );
  XLAL_CALLGSL( workSpace->r_AB = gsl_vector_calloc( n ) );
  XLAL_CALLGSL( workSpace->s    = gsl_min_fminimizer_alloc( T ) );
 
  /* Check all of the above were allocated properly */
  if (!workSpace->tmpA || !workSpace->tmpB || !workSpace->C ||
      !workSpace->p1 || !workSpace->tmpV || !workSpace->r_AB
      || !workSpace->s )
  {
    XLALFreeFContactWorkSpace( workSpace );
    XLAL_ERROR_NULL( XLAL_ENOMEM );
  }
 
  /* Now set the rest of the parameters to the correct values */
  workSpace->T         = T;
  workSpace->convParam = conv;
  workSpace->n         = n;
 
  return workSpace;
}
 
/*
-------------------------------------------------------------------------
 * This function minimises fContact defined above using the
 * Brent method. It returns the minima with a negative sign (which then
 * becomes the maxima of the actual contact function. This can be compared
 * to 1 to check if two ellipsoids indeed overlap.
 * ------------------------------------------------------------------------*/
REAL8 XLALCheckOverlapOfEllipsoids (
        const gsl_vector   *ra,
        const gsl_vector   *rb,
        fContactWorkSpace  *workSpace )
{
    gsl_function        F;
    INT4                min_status;
    INT4                iter = 0, max_iter = 100;
    REAL8               m = 0.6180339887;
    REAL8               a = 0.0L, b = 1.0L;
    gsl_min_fminimizer  *s = workSpace->s;
 
    /* Sanity check on input arguments */
    if ( !ra || !rb || !workSpace )
      XLAL_ERROR_REAL8( XLAL_EFAULT );
 
    if ( ra->size != rb->size || ra->size != workSpace->n )
      XLAL_ERROR_REAL8( XLAL_EBADLEN);
 
 
    /* Set r_AB to be rb - ra */
    XLAL_CALLGSL( gsl_vector_memcpy( workSpace->r_AB, rb) );
    XLAL_CALLGSL( gsl_vector_sub (workSpace->r_AB, ra) );
 
    if ( gsl_vector_isnull( workSpace->r_AB ))
    {
      XLALPrintWarning("Position vectors ra and rb are identical.\n");
      return 0;
    }
 
    F.function = &fContact;
    F.params   = workSpace;
 
    XLAL_CALLGSL( min_status = gsl_min_fminimizer_set (s, &F, m, a, b) );
    if ( min_status != GSL_SUCCESS )
      XLAL_ERROR_REAL8( XLAL_EFUNC );
 
    do
    {
        iter++;
        XLAL_CALLGSL( min_status = gsl_min_fminimizer_iterate (s) );
        if (min_status != GSL_SUCCESS )
        {
          if (min_status == GSL_EBADFUNC)
            XLAL_ERROR_REAL8( XLAL_EFUNC | XLAL_EFPINVAL );
          else if (min_status == GSL_EZERODIV)
            XLAL_ERROR_REAL8( XLAL_EFUNC | XLAL_EFPDIV0 );
          else
            XLAL_ERROR_REAL8( XLAL_EFUNC );
        }
 
        m = gsl_min_fminimizer_x_minimum (s);
        a = gsl_min_fminimizer_x_lower (s);
        b = gsl_min_fminimizer_x_upper (s);
 
        XLAL_CALLGSL( min_status = gsl_min_test_interval (a, b, workSpace->convParam, 0.0) );
        if (min_status != GSL_CONTINUE && min_status != GSL_SUCCESS )
          XLAL_ERROR_REAL8( XLAL_EFUNC );
    }
    while (min_status == GSL_CONTINUE && iter < max_iter );
    /* End of minimization routine */
 
    /* Throw an error if max iterations would have been exceeded */
    if ( iter == max_iter && min_status == GSL_CONTINUE )
    {
      XLAL_ERROR_REAL8( XLAL_EMAXITER );
    }
 
    return ( -(s->f_minimum) );
}
 
 
/*
-------------------------------------------------------------------------
 * This function frees the memory allocated using XLALInitFContactWorkSpace.
 * ------------------------------------------------------------------------*/
void XLALFreeFContactWorkSpace( fContactWorkSpace *workSpace )
{
  if ( !workSpace )
    XLAL_ERROR_VOID( XLAL_EFAULT );
 
  /* Free all the allocated memory */
  if (workSpace->tmpA) XLAL_CALLGSL( gsl_matrix_free( workSpace->tmpA ));
  if (workSpace->tmpB) XLAL_CALLGSL( gsl_matrix_free( workSpace->tmpB ));
  if (workSpace->C)    XLAL_CALLGSL( gsl_matrix_free( workSpace->C ));
  if (workSpace->p1)   XLAL_CALLGSL( gsl_permutation_free( workSpace->p1 ));
  if (workSpace->tmpV) XLAL_CALLGSL( gsl_vector_free( workSpace->tmpV ));
  if (workSpace->r_AB) XLAL_CALLGSL( gsl_vector_free( workSpace->r_AB ));
  if (workSpace->s)    XLAL_CALLGSL( gsl_min_fminimizer_free( workSpace->s ));
 
  LALFree( workSpace );
  return;
}