% ********************
% * basic rod method *
% ********************
% inputs : (len,ang,fi,dfi,d2fi,x,dx,d2x,y,dy,d2y)
% outputs: [xm,dxm,d2xm,ym,dym,d2ym]

function [xm,dxm,d2xm,ym,dym,d2ym] = point_i(len,ang,fi,dfi,d2fi,x,dx,d2x,y,dy,d2y)

xm=x+len*cos(fi+ang);
ym=y+len*sin(fi+ang);

dxm=dx-len*dfi*sin(fi+ang); % all is vector to vector
dym=dy+len*dfi*cos(fi+ang); % unlike vector to scalar

d2xm=d2x-len*(dfi^2*cos(fi+ang)+d2fi*sin(fi+ang));
d2ym=d2y-len*(dfi^2*sin(fi+ang)-d2fi*cos(fi+ang));