point_ii.m

camlaw的ｍａｔｌａｂ程序 [e-mail]

% ********************
% * basic rod method *
% ********************
% inputs : (li,lj,lmq,m,xm,dxm,d2xm,ym,dym,d2ym,xq,dxq,d2xq,yq,dyq,d2yq)
% outputs: [fii,dfii,d2fii,fij,dfij,d2fij,xn,dxn,d2xn,yn,dyn,d2yn]

function [fii,dfii,d2fii,fij,dfij,d2fij,xn,dxn,d2xn,yn,dyn,d2yn] = point_ii(li,lj,lmq,m,xm,dxm,d2xm,ym,dym,d2ym,xq,dxq,d2xq,yq,dyq,d2yq)

if lmq>li+lj|lmq<abs(li-lj)
    fii=nan;
    dfii=nan;
    d2fii=nan;
    fij=nan;
    dfij=nan;
    d2fij=nan;
    xn=nan;
    dxn=nan;
    d2xn=nan;
    yn=nan;
    dyn=nan;
    d2yn=nan;
    return
end

a=2*li*(yq-ym);
b=2*li*(xq-xm);
c=li^2+lmq^2-lj^2;
den1=b+c;


% prevent from M and Q butukaru

%if den1<10e-10
%    den1=10e-10;
%end

fii=2*atan((a+m*sqrt(a^2+b^2-c^2))/den1);
xn=xm+li*cos(fii);
yn=ym+li*sin(fii);
fij=atan((yn-yq)/(xn-xq));

% fi<0 is not sufficient!
if xn-xq<0
    fij=fij+pi;
end

den2=(yn-yq)*(xn-xm)-(yn-ym)*(xn-xq);
% prevent from M and Q butukaru
%if den2<10e-10
%    den2=10e-10;
%end

dfii=((dxq-dxm)*(xn-xq)+(dyq-dym)*(yn-yq))/den2;
dfij=((dxq-dxm)*(xn-xm)+(dyq-dym)*(yn-ym))/den2;

dxn=dxm-dfii*(yn-ym);
dyn=dym+dfii*(xn-xm);

c1=d2xq-d2xm+(xn-xm)*dfii^2-(xn-xq)*dfij^2;
c2=d2yq-d2ym+(yn-ym)*dfii^2-(yn-yq)*dfij^2;

d2fii=(c1*(xn-xq)+c2*(yn-yq))/den2;
d2fij=(c1*(xn-xm)+c2*(yn-ym))/den2;

d2xn=d2xm-(xn-xm)*dfii^2-(yn-ym)*d2fii;
d2yn=d2ym-(yn-ym)*dfii^2-(xn-xm)*d2fii;