essential_distMinAnglePair_dfNewton

Support function for essential_distMinAnglePair implementing Newton's search

0001 %Support function for essential_distMinAnglePair implementing Newton's search
0002 function [tMin,fMin]=essential_distMinAnglePair_dfNewton(m1,p1,c1,m2,p2,c2,tMin,tLow,tHigh)
0003 tolDist=1e-8;
0004 for i=1:100
0005 %     d=dfi(m1,p1,c1,tMin)+dfi(m2,p2,c2,tMin);
0006 %     dd=ddfi(m1,p1,c1,tMin)+ddfi(m2,p2,c2,tMin);
0007     %The code below unrolls the following calls
0008     %     f1=fi(m1,p1,c1,tMin);
0009     %     f2=fi(m2,p2,c2,tMin);
0010     %     d=dfi2(m1,p1,f1,tMin)+dfi2(m2,p2,f2,tMin);
0011     %     dd=ddfi2(m1,p1,f1,tMin)+ddfi2(m2,p2,f2,tMin);
0012     mc1=m1*cos(tMin+p1);
0013     mc2=m2*cos(tMin+p2);
0014     f1=acos(clip((m1*sin(tMin+p1)+c1-1)/2));
0015     f2=acos(clip((m2*sin(tMin+p2)+c2-1)/2));
0016     sf1=2*sin(f1);
0017     sf2=2*sin(f2);
0018     d1=-f1*mc1/sf1;
0019     d2=-f2*mc2/sf2;
0020     d=d1+d2;
0021     eztuSq1=(mc1/sf1)^2;
0022     dd1=eztuSq1+f1/2*cot(f1/2)*(1-eztuSq1);
0023     eztuSq2=(mc2/sf2)^2;
0024     dd2=eztuSq2+f2/2*cot(f2/2)*(1-eztuSq2);
0025     dd=dd1+dd2;
0026         
0027             
0028     tOld=tMin;
0029     tMin=max(tLow+tolDist,min(tHigh-tolDist,tOld-d/dd));
0030     if abs(tMin-tOld)<tolDist
0031         break
0032     end
0033 end
0034 fMin=f1^2+f2^2;
0035 
0036 function v=clip(v)
0037 v=min(1,max(-1,v));
0038