geom.c

Go to the documentation of this file.

#include "geom.h"

#include "error.h"
#include "defines.h"

#include <math.h>
#include <gsl/gsl_linalg.h>

boolean pointLeftOfLine(double px,double py,
                        double x1,double y1,double x2,double y2)
{
  //        |x1 y1 1|
  //  det = |x2 y2 1| >= 0
  //        |px py 1|
  double det;

  ROUNDDOWN;
  det=(x2*py-px*y2)-(x1*py-px*y1)+(x1*y2-x2*y1);
  ROUNDNEAR;

  return(det>=0);
}


boolean pointRightOfLine(double px,double py,
                         double x1,double y1,double x2,double y2)
{
  //        |x1 y1 1|
  //  det = |x2 y2 1| <= 0
  //        |px py 1|
  double det;

  ROUNDUP;
  det=(x2*py-px*y2)-(x1*py-px*y1)+(x1*y2-x2*y1);
  ROUNDNEAR;

  return (det<=0);
}

/*
 * Computes two intervals that delimite the bounding box for a set of 'n' coordinates
 * in the x,y plane
 */
void ComputeBoundingBox(unsigned int n,double *x,double *y,
                        Tinterval *ix,Tinterval *iy)
{ 
  unsigned int i; 
  double x_min,x_max; 
  double y_min,y_max; 

  x_min=x_max=x[0]; 
  y_min=y_max=y[0]; 
  for(i=1;i<n;i++) 
    { 
      if (x[i]>x_max) x_max=x[i]; 
      else {if (x[i]<x_min) x_min=x[i];} 
      if (y[i]>y_max) y_max=y[i]; 
      else {if (y[i]<y_min) y_min=y[i];} 
    }  
  NewInterval(x_min,x_max,ix); 
  NewInterval(y_min,y_max,iy); 
}

void ComputeBoundingBox3d(unsigned int n,double *x,double *y,double *z,
                          Tinterval *ix,Tinterval *iy,Tinterval *iz)
{ 
  unsigned int i; 
  double x_min,x_max; 
  double y_min,y_max; 
  double z_min,z_max; 

  x_min=x_max=x[0]; 
  y_min=y_max=y[0]; 
  z_min=z_max=z[0]; 
  for(i=1;i<n;i++) 
    { 
      if (x[i]>x_max) x_max=x[i]; 
      else {if (x[i]<x_min) x_min=x[i];} 
      if (y[i]>y_max) y_max=y[i]; 
      else {if (y[i]<y_min) y_min=y[i];} 
      if (z[i]>z_max) z_max=z[i]; 
      else {if (z[i]<z_min) z_min=z[i];} 
    }  
  NewInterval(x_min,x_max,ix); 
  NewInterval(y_min,y_max,iy);  
  NewInterval(z_min,z_max,iz); 
}

/*
 * Performs the Clipping between a rectangle and a circle.
 */
boolean RectangleCircleClipping(double r2,
                                Tinterval *x,Tinterval *y,
                                Tinterval *x_new,Tinterval *y_new)
{
  Tinterval d,md,a,b,diameter,rad2;
  boolean full;
  double r;

  /* First, both x,y must be inside the square +/-r 
     This is basically to deal with infinite ranges */
  r=sqrt(r2);
  NewInterval(-r-ZERO,+r+ZERO,&diameter);
  full=((Intersection(x,&diameter,x_new))&&
        (Intersection(y,&diameter,y_new)));

  if (full)
    {
      NewInterval(r2-ZERO,r2+ZERO,&rad2);
      /* y = sqrt(r2 - x^2) */
      IntervalPow(x_new,2,&d);
      IntervalInvert(&d,&d);
      IntervalAdd(&d,&rad2,&d);
      IntervalSqrt(&d,&d);
      IntervalInvert(&d,&md);
  
      Intersection(y_new,&d,&a);
      Intersection(y_new,&md,&b);

      full=Union(&a,&b,y_new);
  
      if (full)
        { 
          /* x = sqrt(r2 - y_new^2) */
          IntervalPow(y_new,2,&d);
          IntervalInvert(&d,&d);
          IntervalAdd(&d,&rad2,&d);
          IntervalSqrt(&d,&d);
          IntervalInvert(&d,&md);
      
          Intersection(x_new,&d,&a);
          Intersection(x_new,&md,&b);

          full=Union(&a,&b,x_new);
        }
    }
  return(full); 
}


/*
 * Performs the Clipping between a 3d box and a sphere.
 */
boolean BoxSphereClipping(double r2,
                          Tinterval *x,Tinterval *y,Tinterval *z,
                          Tinterval *x_new,Tinterval *y_new,Tinterval *z_new)
{
  Tinterval d,md,a,b,rad2,diameter;
  boolean full;
  double r;

  /* First, both x,y must be inside the square +/-r 
     This is basically to deal with infinite ranges */
  r=sqrt(r2);
  NewInterval(-r-ZERO,+r+ZERO,&diameter);
  full=((Intersection(x,&diameter,x_new))&&
        (Intersection(y,&diameter,y_new))&&
        (Intersection(z,&diameter,z_new)));

  if (full)
    {
      /* z = sqrt(r2 - x^2 - y^2) */
      NewInterval(r2-ZERO,r2+ZERO,&rad2);
      IntervalPow(x_new,2,&a);
      IntervalPow(y_new,2,&b);
      IntervalAdd(&a,&b,&d);
      IntervalInvert(&d,&d);
      IntervalAdd(&d,&rad2,&d);
      IntervalSqrt(&d,&d);
      IntervalInvert(&d,&md);
  
      Intersection(z_new,&d,&a);
      Intersection(z_new,&md,&b);
      full=Union(&a,&b,z_new);
  
      if (full)
        { 
          /* y = sqrt(r2 - x^2 - z_new^2) */
          IntervalPow(x_new,2,&a);
          IntervalPow(z_new,2,&b);
          IntervalAdd(&a,&b,&d);
          IntervalInvert(&d,&d);
          IntervalAdd(&d,&rad2,&d);
          IntervalSqrt(&d,&d);
          IntervalInvert(&d,&md);
      
          Intersection(y_new,&d,&a);
          Intersection(y_new,&md,&b);
          full=Union(&a,&b,y_new);

          if (full)
            {
              /* x = sqrt(r2 - y_new^2 - z_new^2) */
              IntervalPow(y_new,2,&a);
              IntervalPow(z_new,2,&b);
              IntervalAdd(&a,&b,&d);
              IntervalInvert(&d,&d);
              IntervalAdd(&d,&rad2,&d);
              IntervalSqrt(&d,&d);
              IntervalInvert(&d,&md);
          
              Intersection(x_new,&d,&a);
              Intersection(x_new,&md,&b);
              full=Union(&a,&b,x_new);
            }
        }
    }
  return(full);
}

/*
 * Clipping of a 2d box with the function  \alpha y=x^2
*/
boolean RectangleParabolaClipping(Tinterval *x,double alpha,Tinterval *y,
                                  Tinterval *x_new,Tinterval *y_new)
{
  Tinterval d,md,a,b,s;
  boolean full;
  double s1,s2;

  /* y = (1/alpha) * x^2 */
  ROUNDDOWN;
  s1=1/alpha;
  ROUNDUP;
  s2=1/alpha;
  ROUNDNEAR;
  NewInterval(s1,s2,&s);

  IntervalPow(x,2,&d);
  IntervalProduct(&d,&s,&d);

  full=Intersection(y,&d,y_new);

  if (full)
    { 
      /* x = +/- sqrt(alpha * y_new)*/
      IntervalScale(y_new,alpha,&d);
      IntervalSqrt(&d,&d);
      IntervalInvert(&d,&md);
      
      Intersection(x,&d,&a);
      Intersection(x,&md,&b);
      full=Union(&a,&b,x_new);
    }
  
  return(full);
}

/*
  A saddle is a function of the form
     z = x * y.
  This implements the clipping between a saddle and a 3d box
*/
boolean BoxSaddleClipping(Tinterval *x,Tinterval *y,double alpha,Tinterval *z,
                          Tinterval *x_new,Tinterval *y_new,Tinterval *z_new)

{
  Tinterval d,s;
  boolean full;
  double s1,s2;

  /* z=(1/alpha)*x*y */
  ROUNDDOWN;
  s1=1/alpha;
  ROUNDUP;
  s2=1/alpha;
  ROUNDNEAR;
  NewInterval(s1,s2,&s);

  IntervalProduct(x,y,&d);
  IntervalProduct(&s,&d,&d);

  full=Intersection(z,&d,z_new);

  if (full)
    { 
      /* x=alpha*z_new/y */
      if (IsInside(0,y))
        CopyInterval(x_new,x);
      else
        {
          IntervalDivision(z_new,y,&d);
          IntervalScale(&d,alpha,&d);
          full=Intersection(x,&d,x_new);
        }

      if (full)
        { 
          /* y=alpha*z_new/x_new */
          if (IsInside(0,x_new))
            CopyInterval(y_new,y);
          else
            {
              IntervalDivision(z_new,x_new,&d);
              IntervalScale(&d,alpha,&d);
              full=Intersection(y,&d,y_new);
            }
        }
    }
   return(full);
}

void DefineNormalVector(double *v,double *n)
{
  unsigned int i;
  double norm;

  if (v[0]!=0.0)
    {
      n[1]=n[2]=1.0;
      n[0]=-(v[1]+v[2])/v[0];
    }
  else
    {
      if (v[1]!=0.0)
        {
          n[0]=n[2]=1.0;
          n[1]=-(v[0]+v[2])/v[1];
        }
      else
        { 
          if (v[2]!=0.0)
            {
              n[0]=n[1]=1.0;
              n[2]=-(v[0]+v[1])/v[2];
            }
          else
            Error("Null input vector in DefineNormalVector");
        }
    }
  norm=0.0;
  for(i=0;i<3;i++)
    norm=norm+v[i]*v[i];
  norm=sqrt(norm);
  for(i=0;i<3;i++)
    v[i]/=norm;
}

void CrossProduct(double *v1,double *v2,double *v3)
{
  double a,b,c;

  a= (v1[1]*v2[2])-(v1[2]*v2[1]);
  b=-(v1[0]*v2[2])+(v1[2]*v2[0]);
  c= (v1[0]*v2[1])-(v1[1]*v2[0]);

  v3[0]=a;
  v3[1]=b;
  v3[2]=c;
}

double DotProduct(double *v1,double *v2)
{
  return(v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]);
}


int tolerant_SV_decomp(int M,int N,gsl_matrix *U,gsl_matrix *V,gsl_vector *S) 
{
  gsl_matrix* VT;
  gsl_vector* S2;
  gsl_matrix* UT ;
  gsl_vector* temp;
  int i, j;
  int err;

  if (M<N) 
    {
      temp=gsl_vector_calloc(M);
      S2=gsl_vector_calloc(M);
      UT=gsl_matrix_calloc(M,M);
      VT=gsl_matrix_calloc(N,M);
      for(i=0;i<M;i++) 
        {
          for(j=0;j<N;j++) 
            gsl_matrix_set(VT,j,i,gsl_matrix_get(U,i,j));
        }
 
      err=gsl_linalg_SV_decomp(VT,UT,S2,temp);

      if (!err)
        {
          for(i=0;i<M;i++) 
            {
              for(j=0;j<N;j++) 
                {
                  gsl_matrix_set(V,j,i,gsl_matrix_get(VT,j,i));
                  gsl_matrix_set(U,i,j,(j<M ? gsl_matrix_get(UT,i,j):0));
                }
              gsl_vector_set(S,i,gsl_vector_get(S2,i));
            }
        }

      gsl_matrix_free(VT);
      gsl_matrix_free(UT);
      gsl_vector_free(temp);
      gsl_vector_free(S2);
    } 
  else 
    {
      temp=gsl_vector_calloc(N);
      err=gsl_linalg_SV_decomp(U,V,S,temp);
      gsl_vector_free(temp);
    }
  return(err);
}