// This version is updated on Jul. 10 2012
    /////////////////////////////////////////////////////////////////////////////
    //////////////////////////////*ellipsoid.h*////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    #ifndef ELLIPSOIDS_H_
    #define ELLIPSOIDS_H_
    #include <stdio.h>
    #include<math.h>
    #include<errno.h>
    #include<stdlib.h>
    #include<complex.h>
    #define pi 4*atan(1.0)
    #define gratio (1.0+sqrt(5.0))*0.5
    #define o1g 1.0/(1.0+gratio)
    #define tolerance 1E-9
    #define delt 1E-5
    void crossP(double *x,double *y,double *z); //
    void norm(double *vec,double *nvec); //
    double mag(double *V); //
    double dotP(double *Vec1,double *Vec2); //Functions
    double plane_int(double); //
    double distance2d(double,double,double,double,double,double); //
    double complex c_cbrt(double complex); //
    double ellipsoids(void);
    double l1i[3],l2i[3],m1i[3],m2i[3],n1i[3],n2i[3],di[3]; //Input vectors
    double a1,a2,b1,b2,c1,c2; //Input semiaxes lenghts
    double d[3],p0[3],p[3],s[3],l1[3],l2[3],m1[3],m2[3],n1[3],n2[3],dxp0[3]; //Normalized vectors
    #endif
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /*Subroutine to calculate the distance of closest approach of two arbitrary ellipsoids*/
    #include "ellipsoids.h"
    double ellipsoids(void)
    {
       int j;
       double sol;
       double a,b,theta1,theta2,theta3,theta4,f1,f4,f3,f5,f6; //Search algorithm variables
       norm(di,d);
       norm(l1i,l1);
       norm(l2i,l2);
       norm(m1i,m1);
       norm(m2i,m2);
       norm(n1i,n1);
       norm(n2i,n2);
       crossP(d,l1,dxp0);
       if(mag(dxp0)<1.E-14)
          crossP(d,m1,dxp0);
       norm(dxp0,p0);
       crossP(p0,d,dxp0);
       theta1=0.0;
       theta2=pi;
       theta3=theta1+o1g*(theta2-theta1);
       theta4=theta2-o1g*(theta2-theta1);
       f1=plane_int(theta1);
       f3=plane_int(theta3);
       f4=plane_int(theta4);
    /////////////////////////////search algorithm loop///////////////////////////////
       while(theta2-theta1>tolerance)
       {
          if((f1<=f3 && f3<=f4) || (f3<=f1 && f1<=f4)) //case a
          {
             theta1=theta3;
             f1=f3;
             theta3=theta4;
             f3=f4;
             theta4=theta2-o1g*(theta2-theta1);
             f4=plane_int(theta4);
          }
          else if((f1<=f4 && f4<=f3) || (f4<=f1 && f1<=f3)) //case b
          {
             theta2=theta4;
             theta4=theta3;
             f4=f3;
             theta3=theta1+o1g*(theta2-theta1);
             f3=plane_int(theta3);
          }
          else //case c
          {
             f5=plane_int(theta1+delt);
             if(f5>f1)
             {
                theta2=theta3;
                theta3=theta1+o1g*(theta2-theta1);
                theta4=theta2-o1g*(theta2-theta1);
                f3=plane_int(theta3);
                f4=plane_int(theta4);
             }
             else
             {
                f6=plane_int(theta1-delt);
                if(f6<f1)
                {
                   theta2=theta3;
                   theta3=theta1+o1g*(theta2-theta1);
                   theta4=theta2-o1g*(theta2-theta1);
                   f3=plane_int(theta3);
                   f4=plane_int(theta4);
                }
                else
                {
                   theta1=theta4;
                   f1=f4;
                   theta3=theta1+o1g*(theta2-theta1);
                   theta4=theta2-o1g*(theta2-theta1);
                   f3=plane_int(theta3);
                   f4=plane_int(theta4);
                }
             }
          }
       }
       sol=f4;
       return sol;
    }
    ////////////////////////////////////////////////////////////////////////////
    ///////////////////////*FUNCTIONS*///////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////
    /*Cross Product of two vectors*/
    void crossP(double x[3],double y[3],double z[3])
    {
       z[0]=x[1]*y[2]-x[2]*y[1];
       z[1]=x[2]*y[0]-x[0]*y[2];
       z[2]=x[0]*y[1]-x[1]*y[0];
    }
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /*Normalization of a vector*/
    void norm(double vec[3],double nvec[3])
    {
       double length;
       length=mag(vec);
       if(length==0.0)
       {
          nvec[0]=0.0;nvec[1]=0.0;nvec[2]=0.0;
       }
       else
       {
       nvec[0]=vec[0]/length;
       nvec[1]=vec[1]/length;
       nvec[2]=vec[2]/length;
       }
    }
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /*Magnitude of a vector*/
    double mag(double V[3])
    {
       return sqrt(V[0]*V[0]+V[1]*V[1]+V[2]*V[2]);
    }
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /*Dot Product of two vectors*/
    double dotP(double Vec1[3],double Vec2[3])
    {
       return Vec1[0]*Vec2[0]+Vec1[1]*Vec2[1]+Vec1[2]*Vec2[2];
    }
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /*Ellipses formed with the intersection of the ellipsoid with the plane*/
    double plane_int(double theta)
    {
       double alpha1,beta1,gamma1,v1,u1,alpha2,beta2,gamma2,v2,u2;
       double e1,e2;
       double l1x,l1y,m1x,m1y,n1x,n1y,l2x,l2y,m2x,m2y,n2x,n2y;
       double a2d1,a2d2,b2d1,b2d2,angle1,angle2; //Ellipses variables
       int j;
       for(j=0;j<3;j++)
          p[j]=cos(theta)*p0[j]+sin(theta)*dxp0[j];
       crossP(p,d,s);
       l1x=dotP(l1,d);
       l1y=dotP(l1,s);
       m1x=dotP(m1,d);
       m1y=dotP(m1,s);
       n1x=dotP(n1,d);
       n1y=dotP(n1,s);
       alpha1=l1x*l1x/(a1*a1)+m1x*m1x/(b1*b1)+n1x*n1x/(c1*c1);
       beta1=l1y*l1x/(a1*a1)+m1y*m1x/(b1*b1)+n1x*n1y/(c1*c1);
       gamma1=l1y*l1y/(a1*a1)+m1y*m1y/(b1*b1)+n1y*n1y/(c1*c1);
       v1=sqrt(4.0*beta1*beta1+(alpha1-gamma1)*(alpha1-gamma1));
       angle1=0.5*atan2(-2.0*beta1,-(alpha1-gamma1));
       u1=gamma1+v1*sin(angle1)*sin(angle1);
       b2d1=1.0/sqrt(u1);
       a2d1=1./sqrt(u1-v1);
       l2x=dotP(l2,d);
       l2y=dotP(l2,s);
       m2x=dotP(m2,d);
       m2y=dotP(m2,s);
       n2x=dotP(n2,d);
       n2y=dotP(n2,s);
       alpha2=l2x*l2x/(a2*a2)+m2x*m2x/(b2*b2)+n2x*n2x/(c2*c2);
       beta2=l2y*l2x/(a2*a2)+m2y*m2x/(b2*b2)+n2x*n2y/(c2*c2);
       gamma2=l2y*l2y/(a2*a2)+m2y*m2y/(b2*b2)+n2y*n2y/(c2*c2);
       v2=sqrt(4.0*beta2*beta2+(alpha2-gamma2)*(alpha2-gamma2));
       angle2=0.5*atan2(-2.0*beta2,-(alpha2-gamma2));
       u2=gamma2+v2*sin(angle2)*sin(angle2);
       b2d2=1.0/sqrt(u2);
       a2d2=1.0/sqrt(u2-v2);
       return distance2d(a2d1,b2d1,a2d2,b2d2,angle1,angle2);
    }
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /*Distance of Closest Approach of two arbitrary ellipses*/
    double distance2d(double a1, double b1, double a2, double b2, double angle1, double angle2)
    {
       double eps1,eps2,k1dotd,k2dotd,k1dotk2,nu,Ap[2][2],
             lambdaplus,lambdaminus,bp2,ap2,cosphi,tanphi2,delta,dp;
       double complex A,B,C,D,E,alpha,beta,gamma,P,Q,U,y,qu;
  //the fix on July 2012 
       if(fabs(angle2-angle1)==pi)
       { angle2=angle1;
       }
      //
       eps1=sqrt(1.0-(b1*b1)/(a1*a1));
       eps2=sqrt(1.0-(b2*b2)/(a2*a2));
       k1dotd=cos(angle1);
       k2dotd=cos(angle2);
       k1dotk2=cos(angle2-angle1);
       nu=a1/b1-1.0;
       Ap[0][0]=b1*b1/(b2*b2)*(1.0+0.5*(1.0+k1dotk2)*
                (nu*(2.0+nu)-eps2*eps2*(1.0+nu*k1dotk2)*(1.0+nu*k1dotk2)));
       Ap[1][1]=b1*b1/(b2*b2)*(1.0+0.5*(1.0-k1dotk2)*
                (nu*(2.0+nu)-eps2*eps2*(1.0-nu*k1dotk2)*(1.0-nu*k1dotk2)));
       Ap[0][1]=b1*b1/(b2*b2)*0.5*sqrt(1.0-k1dotk2*k1dotk2)*
                (nu*(2.0+nu)+eps2*eps2*(1.0-nu*nu*k1dotk2*k1dotk2));
       lambdaplus=0.5*(Ap[0][0]+Ap[1][1])+sqrt(0.25*(Ap[0][0]-Ap[1][1])*
                (Ap[0][0]-Ap[1][1])+Ap[0][1]*Ap[0][1]);
       lambdaminus=0.5*(Ap[0][0]+Ap[1][1])-sqrt(0.25*(Ap[0][0]-Ap[1][1])*
                (Ap[0][0]-Ap[1][1])+Ap[0][1]*Ap[0][1]);
       bp2=1.0/sqrt(lambdaplus);
       ap2=1.0/sqrt(lambdaminus);
       if(fabs(k1dotk2)==1.0)
       {
          if(Ap[0][0]>Ap[1][1])
             cosphi=b1/a1*k1dotd/sqrt(1.0-eps1*eps1*k1dotd*k1dotd);
          else
             cosphi=sqrt(1.0-k1dotd*k1dotd)/sqrt(1.0-eps1*eps1*k1dotd*k1dotd);
       }
       else
       {
          cosphi=1.0/sqrt(2.0*(Ap[0][1]*Ap[0][1]+(lambdaplus-Ap[0][0])*(lambdaplus-Ap[0][0]))*
                (1.0-eps1*eps1*k1dotd*k1dotd))*(Ap[0][1]/sqrt(1.0+k1dotk2)*(b1/a1*k1dotd+
                k2dotd+(b1/a1-1.0)*k1dotd*k1dotk2)+ (lambdaplus-Ap[0][0])/sqrt(1.0-k1dotk2)*
                (b1/a1*k1dotd-k2dotd-(b1/a1-1.0)*k1dotd*k1dotk2));
       }
       delta=ap2*ap2/(bp2*bp2)-1.0;
       if(delta==0.0 || cosphi==0.0)
          dp=1.0+ap2;
       else
       {
          tanphi2=1.0/(cosphi*cosphi)-1.0;
          A=-(1.0+tanphi2)/(bp2*bp2);
          B=-2.0*(1.0+tanphi2+delta)/bp2;
          C=-tanphi2-(1.0+delta)*(1.0+delta)+(1.0+(1.0+delta)*tanphi2)/(bp2*bp2);
          D=2.0*(1.0+tanphi2)*(1.0+delta)/bp2;
          E=(1.0+tanphi2+delta)*(1.0+delta);
          alpha=-3.0*B*B/(8.0*A*A)+C/A;
          beta=B*B*B/(8.0*A*A*A)-B*C/(2.0*A*A)+D/A;
          gamma=-3.0*B*B*B*B/(256.0*A*A*A*A)+C*B*B/(16.0*A*A*A)-B*D/(4.0*A*A)+E/A;
          if(beta==0.0)
          {
             qu=-B/(4.0*A)+csqrt(0.5*(-alpha+csqrt(alpha*alpha-4.0*gamma)));
          }
          else
          {
             P=-alpha*alpha/12.0-gamma;
             Q=-alpha*alpha*alpha/108.0+alpha*gamma/3.0-beta*beta/8.0;
             U=c_cbrt(-Q*0.5+csqrt(Q*Q*0.25+P*P*P/27.0));
             if(U==0.0)
                y=-5.0*alpha/6.0-c_cbrt(Q);
             else
                y=-5.0*alpha/6.0+U-P/(3.0*U);
             qu=-B/(4.0*A)+0.5*(csqrt(alpha+2.0*y)+
                csqrt(-(3.0*alpha+2.0*y+2.0*beta/csqrt(alpha+2.0*y))));
          }
          dp=csqrt((qu*qu-1.0)/delta*(1.0+bp2*(1.0+delta)/qu)*
             (1.0+bp2*(1.0+delta)/qu)+(1.0-(qu*qu-1.0)/delta)* (1.0+bp2/qu)*(1.0+bp2/qu));
       }
       return dp*b1/sqrt(1.0-eps1*eps1*k1dotd*k1dotd);
    }
    /////////////////////////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////
    /* Principal cubic root of a complex number */
    double complex c_cbrt(double complex x)
    {
       double a,b,r,phi,rn;
       a=creal(x);
       b=cimag(x);
       r=sqrt(a*a+b*b);
       phi=atan2(b,a);
       phi/=3.0;
       rn=cbrt(r);
       return rn*cos(phi)+I*rn*sin(phi);
    }