0001 function [tBreak,a,b,c,m,p]=essential_distMinAnglePair_discontinuityDistance(Q21) 0002 a=Q21(1,1)+Q21(2,2); 0003 b=Q21(1,2)-Q21(2,1); 0004 c=Q21(3,3); 0005 0006 m=norm([a;b]); 0007 p=sign(a)*acos(clip(b/m)); 0008 0009 %tBreak=modAngle(3/2*pi-p); 0010 tBreak=-0.5*pi-p; 0011 0012 function v=clip(v) 0013 v=min(1,max(-1,v));