! This version is updated on Jul 10, 2012. Fixed a bug about when k1 and k2 are
! exactly antiparallel to each other, we rearrage them to be parallel before
! calling the ellispes
    !*********************************************
    !Common Variables
    !*********************************************
    MODULE Common
    Implicit None
    Double Precision::a1, b1, c1, a2, b2 ,c2 ! length of semiaxes
    Double Precision::l1(3), m1(3), n1(3), l2(3), m2(3), n2(3), d(3), p0(3), p0d(3) ! all vectors
    END MODULE Common
    !*******************************************
    !Subroutine to calculate the distance of closest approach of two ellipsoids
    !**************************************
    Subroutine ellipsoids(dist)
    Use Common
    Implicit None
    Double Precision,Parameter::tau=1.61803398
    Double Precision::theta1,theta2,theta3,theta4,pi
    Double Precision::dist,f1,f2,f3,f4,f5,f6,err,h=1.D-05
    Double Precision,External::norm
    pi=atan(1.d0)*4.d0
    !normalize the vectors
    l1=l1/norm(l1)
    m1=m1/norm(m1)
    n1=n1/norm(n1)
    l2=l2/norm(l2)
    m2=m2/norm(m2)
    n2=n2/norm(n2)
    d=d/norm(d)
    !get p0
    call cross(d,l1,p0)
    If(norm(p0)<1.0d-14) then
       call cross(d,m1,p0)
    End If
    p0=p0/norm(p0)
    call cross(p0,d,p0d)
    !initialize my choice of theta
    theta1=0.d0
    theta2=pi
    theta3=theta2-1.d0/tau*(theta2-theta1)
    theta4=theta1+1.d0/tau*(theta2-theta1)
    call plane_ellp(theta1,f1)
    call plane_ellp(theta3,f3)
    call plane_ellp(theta4,f4)
    Do while(abs(theta2-theta1)>1.D-9)
       !******************case a
       If((f1<=f3 .and. f3<=f4) .or. (f3<=f1 .and. f1<=f4)) then
          theta1=theta3
          f1=f3
          theta3=theta4
          f3=f4
          theta4=theta1+1.d0/tau*(theta2-theta1)
          call plane_ellp(theta4,f4)
       !******************case b
       ElseIf((f1<=f4 .and. f4<=f3) .or. (f4<=f1 .and. f1<=f3)) then
          theta2=theta4
          theta4=theta3
          f4=f3
          theta3=theta2-1.d0/tau*(theta2-theta1)
          call plane_ellp(theta3,f3)
       !******************case c
       ElseIf((f3<=f4 .and. f4<=f1) .or. (f4<=f3 .and. f3<=f1)) then
          call plane_ellp(theta1+h,f5)
          If(f5>f1) then
             theta2=theta3
             theta3=theta2-1.d0/tau*(theta2-theta1)
             theta4=theta1+1.d0/tau*(theta2-theta1)
             call plane_ellp(theta3,f3)
             call plane_ellp(theta4,f4)
          Else
             call plane_ellp(theta1-h,f6)
             if(f6<f1) then
                theta2=theta3
                theta3=theta2-1.d0/tau*(theta2-theta1)
                theta4=theta1+1.d0/tau*(theta2-theta1)
                call plane_ellp(theta3,f3)
                call plane_ellp(theta4,f4)
             else
                theta1=theta4
                f1=f4
                theta3=theta2-1.d0/tau*(theta2-theta1)
                theta4=theta1+1.d0/tau*(theta2-theta1)
                call plane_ellp(theta3,f3)
                call plane_ellp(theta4,f4)
             end if
          End if      
        End If
     End Do
    call plane_ellp(theta1,dist)
    End Subroutine ellipsoids
    !******************************************************************
    !Subroutine to identify the intersection between plane and ellipsoids and
    !and return the distance of closest approach of two ellipses
    !*******************************************************************
    Subroutine plane_ellp(theta,dist)
    Use Common
    Implicit None
    Double Precision::theta
    Double Precision::k1(2),k2(2)
    Double Precision::l1x,l1y,m1x,m1y,n1x,n1y,alpha1,beta1,gamma1,v1,u1,phi1
    Double Precision::l2x,l2y,m2x,m2y,n2x,n2y,alpha2,beta2,gamma2,v2,u2,phi2
    Double Precision::ae1,be1,ae2,be2, k1k2,k1d,k2d,dist !arguments needed for closet approach of 2D ellipses
    Double Precision::p(3),s(3) !vectors p and s
    !get normal to the plane
    p=dcos(theta)*p0+dsin(theta)*p0d
    call cross(p,d,s)
    !generating the first ellipse
    call dot(l1,d,l1x)
    call dot(l1,s,l1y)
    call dot(m1,d,m1x)
    call dot(m1,s,m1y)
    call dot(n1,d,n1x)
    call dot(n1,s,n1y)
    alpha1=l1x**2/a1**2+m1x**2/b1**2+n1x**2/c1**2
    beta1=l1y*l1x/a1**2+m1y*m1x/b1**2+n1x*n1y/c1**2
    gamma1=l1y**2/a1**2+m1y**2/b1**2+n1y**2/c1**2
    v1=dsqrt(4.d0*beta1**2+(alpha1-gamma1)**2)
    phi1=0.5d0*datan2(-2*beta1,-(alpha1-gamma1))
    u1=gamma1+v1*(dsin(phi1))**2
    k1(1)=dcos(phi1)
    k1(2)=dsin(phi1)
    be1=1.d0/dsqrt(u1)
    ae1=1.d0/dsqrt(u1-v1)
    !generating the second ellipse
    call dot(l2,d,l2x)
    call dot(l2,s,l2y)
    call dot(m2,d,m2x)
    call dot(m2,s,m2y)
    call dot(n2,d,n2x)
    call dot(n2,s,n2y)
    alpha2=l2x**2/a2**2+m2x**2/b2**2+n2x**2/c2**2
    beta2=l2y*l2x/a2**2+m2y*m2x/b2**2+n2x*n2y/c2**2
    gamma2=l2y**2/a2**2+m2y**2/b2**2+n2y**2/c2**2
    v2=dsqrt(4.d0*beta2**2+(alpha2-gamma2)**2)
    phi2=0.5d0*datan2(-2.*beta2,-(alpha2-gamma2))
    u2=gamma2+v2*(dsin(phi2))**2
    k2(1)=dcos(phi2)
    k2(2)=dsin(phi2)
    be2=1.d0/dsqrt(u2)
    ae2=1.d0/dsqrt(u2-v2)
    !Finding the dot product between the vectors
    k1k2=k1(1)*k2(1)+k1(2)*k2(2)
    k1d=k1(1)
    k2d=k2(1)
    if(k1k2==-1.d0) then
            k1k2=1.d0;
            k2d=k1d;
    end if
    ! call 2D subroutine to get the closest approach of ellipses
    call ellipses(ae1,be1,ae2,be2,k1k2,k1d,k2d,dist)
    End Subroutine plane_ellp
    !*************************************
    !Subroutine to evaluate the dot product of two vectors
    !*************************************
    Subroutine dot(v,u,vdu)
    Implicit None
    Double Precision::v(3),u(3),vdu
    vdu=v(1)*u(1)+v(2)*u(2)+v(3)*u(3)
    End Subroutine dot
    !*************************************
    !Subroutine to evaluate the cross product of two vectors
    !*************************************
    Subroutine cross(v,u,vcu)
    Implicit None
    Double Precision::v(3),u(3),vcu(3)
    vcu(1)=v(2)*u(3)-u(2)*v(3)
    vcu(2)=u(1)*v(3)-v(1)*u(3)
    vcu(3)=v(1)*u(2)-u(1)*v(2)
    End Subroutine cross
    !**************************************
    !Function to calculate the length of a vector
    !**************************************
    Double Precision function norm(v)
    Implicit None
    Double Precision::v(3)
    norm=dsqrt(v(1)**2+v(2)**2+v(3)**2)
    End function norm
    !**************************************
    !Subroutine to calculate the distance of closest approach of two ellipses
    !***************************************
    Subroutine ellipses(a1,b1,a2,b2,k1k2,k1d,k2d,dist)
    Implicit None
    Double Complex,Parameter::in=(1.d0,0.d0)
    !k1k2, k1d,k2d are dot products between the vectors, k1, k2 and d
    Double Precision,Intent(in)::a1,b1,a2,b2,k1k2,k1d,k2d
    Double Precision,Intent(out)::dist
    Double Precision::e1,e2,eta,a11,a12,a22
    Double Precision::lambda1,lambda2,a2p,b2p
    Double Precision::kpmp,t,deltap,A,B,C,D,E,alpha,beta,gamma,P,Q,Rp
    Double Complex::U,y,qq,z,mysqrt
    Double Complex, External::ccbrt
    !eccentricity of ellipses
    e1=1.d0-b1**2/a1**2
    e2=1.d0-b2**2/a2**2
    !component of A'
    eta=a1/b1-1.d0
    a11=b1**2/b2**2*(1.d0+0.5d0*(1.d0+k1k2)*(eta*(2.d0+eta)-e2*(1.d0+eta*k1k2)**2))
    a12=b1**2/b2**2*0.5d0*dsqrt(1.d0-k1k2**2)*(eta*(2.d0+eta)+e2*(1.d0-eta**2*k1k2**2))
    a22=b1**2/b2**2*(1.d0+0.5d0*(1.d0-k1k2)*(eta*(2.d0+eta)-e2*(1.d0-eta*k1k2)**2))
    !eigenvalues of A'
    lambda1=0.5d0*(a11+a22)+0.5d0*dsqrt((a11-a22)**2+4.d0*a12**2)
    lambda2=0.5d0*(a11+a22)-0.5d0*dsqrt((a11-a22)**2+4.d0*a12**2)
    !major and minor axes of transformed ellipse
    b2p=1.d0/dsqrt(lambda1)
    a2p=1.d0/dsqrt(lambda2)
    deltap=a2p**2/b2p**2-1.d0
    If(abs(k1k2)==1.d0) then
       if(a11>a22)then
          kpmp=1.d0/dsqrt(1.d0-e1*k1d**2)*b1/a1*k1d
       elseif(a11<a22) then
          kpmp=dsqrt(1.d0-k1d**2)/dsqrt(1.d0-e1*k1d**2)
       end if
    Else
       kpmp=(a12/dsqrt(1.d0+k1k2)*(b1/a1*k1d+k2d+(b1/a1-1.d0)*k1d*k1k2)+ &
       (lambda1-a11)/dsqrt(1.d0-k1k2)*(b1/a1*k1d-k2d-(b1/a1-1.d0)*k1d*k1k2)) &
       /dsqrt(2.d0*(a12**2+(lambda1-a11)**2)*(1.d0-e1*k1d**2))
    End If
    IF(kpmp==0.d0 .or. deltap==0.0d0) Then
       Rp=a2p+1.d0
    ELSE
       !coefficients of quartic for q
       t=1.d0/kpmp**2-1.d0
       A=-1.d0/b2p**2*(1.d0+t)
       B=-2.d0/b2p*(1.d0+t+deltap)
       C=-t-(1.d0+deltap)**2+1.d0/b2p**2*(1.d0+t+deltap*t)
       D=2.d0/b2p*(1.d0+t)*(1.d0+deltap)
       E=(1.d0+t+deltap)*(1.d0+deltap)
       !solution for quartic
       alpha=-3.d0/8.d0*(B/A)**2+C/A
       beta=(B/A)**3.d0/8.d0-(B/A)*(C/A)/2.d0+D/A
       gamma=-3.d0/256.d0*(B/A)**4+C/A*(B/A)**2/16.d0-(B/A)*(D/A)/4.+E/A
       If(beta==0.) Then
          qq=-B/4.d0/A+cdsqrt((-alpha+cdsqrt(alpha**2-4.d0*gamma*in))/2.)
       Else
          P=-alpha**2/12.d0-gamma
          Q=-alpha**3/108.d0+gamma*alpha/3.d0-beta**2/8.d0
          U=ccbrt(-0.5d0*Q+cdsqrt(Q**2/4.d0+P**3/27.d0*in))
          if(abs(U)/=0.0d0) then
             y=-5.d0/6d0*alpha+U-P/3.d0/U
          else
             y=-5.d0/6.d0*alpha-ccbrt(Q)
          end if
          qq=-B/4.d0/A+0.5d0*(cdsqrt(alpha+2.d0*y)+ &
             cdsqrt(-(3.d0*alpha+2.d0*y+2.d0*beta/cdsqrt(alpha+2.d0*y))))
       End If
       Rp=cdsqrt((qq**2-1.d0)/deltap*(1.d0+b2p*(1.d0+deltap)/qq)**2+ &
          (1.d0-(qq**2-1.d0)/deltap)*(1.d0+b2p/qq)**2)
    END IF
    dist=Rp*b1/dsqrt(1.d0-e1*k1d**2)
    End Subroutine ellipses
    !*************************************
    !Function to evaluate the principal cubic root of a complex number
    !*************************************
    Double Complex function ccbrt(z)
    Implicit None
    Double Complex:: z
    Double Precision::arg,theta,r
    Double Precision, Intrinsic::datan2
    arg=datan2(imag(z),real(z))
    theta = arg / 3.0d0
    r = (real(z)**2+imag(z)**2)**(1.d0/6.d0)
    ccbrt = cmplx(r*dcos(theta), r*dsin(theta))
    End function ccbrt