% angvelacc.m
% angular velocity and acceleration

function out=angvelacc(rP_, rQ_, vP_, vQ_, aP_, aQ_)
% P and Q the input points
% rP_ is the position vector of P
% rQ_ is the position vector of Q

% vP_ is the velocity vector of P
% vQ_ is the velocity vector of Q

% aP_ is the acceleration vector of P
% aQ_ is the acceleration vector of Q

% coordinates position, velocity, acceleration of P
xP = rP_(1);
yP = rP_(2);
vPx = vP_(1);
vPy = vP_(2);
aPx = aP_(1);
aPy = aP_(2);

% coordinates position, velocity, acceleration of Q
xQ = rQ_(1);
yQ = rQ_(2);
vQx = vQ_(1);
vQy = vQ_(2);
aQx = aQ_(1);
aQy = aQ_(2);

% angle between PQ and x-axis
theta = atan((yP-yQ)/(xP-xQ));

% angular velocity of PQ
dtheta = ...
 (cos(theta)*(vPy-vQy)-sin(theta)*(vPx-vQx))/...
 (sin(theta)*(yP-yQ)+cos(theta)*(xP-xQ));

% angular acceleration of PQ
ddtheta = ...
 (cos(theta)*(aPy-aQy)-sin(theta)*(aPx-aQx)...
 -(cos(theta)*dtheta*(yP-yQ)+2*sin(theta)*(vPy-vQy)...
 -sin(theta)*dtheta*(xP-xQ)+2*cos(theta)*(vPx-vQx))*dtheta)/...
 (cos(theta)*(xP-xQ)+sin(theta)*(yP-yQ));

out = [dtheta ddtheta];
end