// This version is updated on Jul. 10 2012 ///////////////////////////////////////////////////////////////////////////// //////////////////////////////*ellipsoid.h*//////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// #ifndef ELLIPSOIDS_H_ #define ELLIPSOIDS_H_ #include #include #include #include #include #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(f6Ap[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); }