essential_distMinAnglePair_base

0001 function [tMin,fMin,tBreak1,tBreak2,tMinAll]=essential_distMinAnglePair_base(Q211,Q212)
0002 flagCheckFirstDer=true;
0003 flagUseNewton=true;     %Note: requires flagCheckFirstDer=true
0004 tolMZero=1e-15;
0005 tMinAll=[];
0006 
0007 [tBreak1,~,~,c1,m1,p1]=essential_distMinAnglePair_discontinuityDistance(Q211);
0008 [tBreak2,~,~,c2,m2,p2]=essential_distMinAnglePair_discontinuityDistance(Q212);
0009 
0010 %check for the degenerate case where the cost is constant
0011 if abs(m1)<tolMZero && abs(m2)<tolMZero
0012     tMin=0;
0013     fMin=2*pi^2;
0014     tMinAll=0;
0015 else
0016     %ft=@(t)  acos((m1*sin(t+p1)+c1-1)/2)^2+acos((m2*sin(t+p2)+c2-1)/2)^2;
0017 
0018     if abs(modAngle(tBreak1-tBreak2))<1e-8
0019         tMin=tBreak1+pi;
0020         fMin=0;
0021 %         theta1=@(t) acos((m1*sin(t+p1)+c1-1)/2);
0022 %         theta2=@(t) acos((m2*sin(t+p2)+c2-1)/2);
0023 %
0024 %         ft=@(t) 0.5*(theta1(t)^2+theta2(t)^2);
0025 %         [tMin,fMin]=fminbnd(ft,tBreak1,tBreak1+2*pi);
0026     else
0027         tSearch1=tBreak1;
0028         tSearch2=tBreak2;
0029         if tSearch1>tSearch2
0030             tSearch1=tSearch1-2*pi;
0031         end
0032 
0033         if flagCheckFirstDer
0034             %compute derivatives of each term at discontinuity points
0035             df1Break1=essential_distMinAnglePair_computeDfBreak(tBreak1,Q211);
0036             df2Break2=essential_distMinAnglePair_computeDfBreak(tBreak2,Q212);
0037 %             disp('[df1Break1 df2Break2]')
0038 %             disp([df1Break1 df2Break2])
0039             %compute derivative of each term at other's discontinuity
0040             %(unroll two calls to dfi)
0041             theta1Break2=acos(clip((m1*sin(tBreak2+p1)+c1-1)/2));
0042             df1Break2=-theta1Break2*(m1*cos(tBreak2+p1))/(2*sin(theta1Break2));
0043             theta2Break1=acos(clip((m2*sin(tBreak1+p2)+c2-1)/2));
0044             df2Break1=-theta2Break1*(m2*cos(tBreak1+p2))/(2*sin(theta2Break1));
0045 
0046             %compute left and right derivatives of sum of the two terms
0047             dfBreak1n=+df1Break1+df2Break1;
0048             dfBreak1p=-df1Break1+df2Break1;
0049             dfBreak2n=+df2Break2+df1Break2;
0050             dfBreak2p=-df2Break2+df1Break2;
0051 
0052             flagSearch1=false;
0053         %     plot([tBreak1 tBreak2],[dfBreak1p dfBreak2p],'cx','MarkerSize',10)
0054         %     plot([tBreak1 tBreak2],[dfBreak1n dfBreak2n],'mx','MarkerSize',10)
0055             if sign(dfBreak1p)~=sign(dfBreak2n)
0056                 if flagUseNewton
0057                     %parabolic prediction of min
0058                     tMin0=tSearch1-dfBreak1p*(tSearch2-tSearch1)/(dfBreak2n-dfBreak1p);
0059                     %tMin0=(tSearch1+tSearch2)/2;
0060                     [tMin,fMin]=essential_distMinAnglePair_dfNewton(m1,p1,c1,m2,p2,c2,tMin0,tSearch1,tSearch2);
0061                     %fMin=essential_distMinAnglePair_ft(m1,p1,c1,m2,p2,c2,tMin);
0062                 else
0063                     [tMin,fMin]=fminbnd(essential_distMinAnglePair_ft,tSearch1,tSearch2);
0064                 end
0065                 tMinAll=[tMinAll tMin];
0066                 flagSearch1=true;
0067             end
0068             tSearch1=tSearch1+2*pi;
0069             if sign(dfBreak2p)~=sign(dfBreak1n)
0070                 if flagUseNewton
0071                     %parabolic prediction of min
0072                     tMin0=tSearch2-dfBreak2p*(tSearch1-tSearch2)/(dfBreak1n-dfBreak2p);
0073                     %tMin0=(tSearch1+tSearch2)/2;
0074                     [tMin2,fMin2]=essential_distMinAnglePair_dfNewton(m1,p1,c1,m2,p2,c2,tMin0,tSearch2,tSearch1);
0075                     %fMin2=essential_distMinAnglePair_ft(m1,p1,c1,m2,p2,c2,tMin2);
0076                 else
0077                     [tMin2,fMin2]=fminbnd(essential_distMinAnglePair_ft,tSearch2,tSearch1);
0078                 end
0079                 if ~flagSearch1 || (flagSearch1 && fMin2<fMin)
0080                     tMin=tMin2;
0081                     fMin=fMin2;
0082                 end
0083                 tMinAll=[tMinAll tMin2];
0084             end
0085         else
0086             [tMin1,fMin1]=fminbnd(essential_distMinAnglePair_ft,tSearch1,tSearch2);
0087             tSearch1=tSearch1+2*pi;
0088             [tMin2,fMin2]=fminbnd(essential_distMinAnglePair_ft,tSearch2,tSearch1);
0089             if fMin1<fMin2
0090                 tMin=tMin1;
0091                 fMin=fMin1;
0092             else
0093                 tMin=tMin2;
0094                 fMin=fMin2;
0095             end
0096         end
0097     end
0098 end
0099 
0100 function v=clip(v)
0101 v=min(1,max(-1,v));
0102 
0103 
0104 % function f=fi(m,p,c,t)
0105 % f=acos((m*sin(t+p)+c-1)/2);
0106 %
0107 % function d=dfi2(m,p,theta,t)
0108 % dtheta= -(m*cos(t+p))/(2*sin(theta));
0109 % d=theta*dtheta;
0110 %
0111 % function dd=ddfi2(m,p,theta,t)
0112 % eztuSq=(m*cos(t+p)/(2*sin(theta)))^2;
0113 % dd=eztuSq+theta/2*cot(theta/2)*(1-eztuSq);
0114 %
0115 % function d=dfi(m,p,c,t)
0116 % theta=acos((m*sin(t+p)+c-1)/2);
0117 % dtheta= -(m*cos(t+p))/(2*sin(theta));
0118 % d=theta*dtheta;
0119 %
0120 % function dd=ddfi(m,p,c,t)
0121 % theta=acos((m*sin(t+p)+c-1)/2);
0122 % eztuSq=(m*cos(t+p)/(2*sin(theta)))^2;
0123 % dd=eztuSq+theta/2*cot(theta/2)*(1-eztuSq);
0124 
0125 
0126