% pvaR.m
% position, velocity, and acceleration

function out = pvaR(rAi_, vAi_, aAi_, li, phi, omega, alpha)

% rAi_ is the position vector of Ai
% vAi_ is the velocity vector of Ai
% aAi_ is the acceleration vector of Ai
% li is the length from Ai to P: li=AiP
% phi is the angle of link (i) with x-axis
% omega is the angular velocity of  link (i),  AiP
% alpha is the angular acceleration of link (i),  AiP

omega_ = [0 0 omega]; % angular velocity vector of AiP
alpha_ = [0 0 alpha]; % angular acceleration vector of AiP

% position of P
xP = rAi_(1)+li*cos(phi);
yP = rAi_(2)+li*sin(phi);
rP_ = [xP, yP, 0]; % position vector of P

% velocity of P
vP_ = vAi_ + cross(omega_, rP_-rAi_);
vPx = vP_(1); % x-component of velocity of P
vPy = vP_(2); % y-component of velocity of P
% acceleration of P
aP_ = aAi_ + cross(alpha_, rP_-rAi_)-omega^2*(rP_-rAi_);
aPx = aP_(1); % x-component of acceleration of P
aPy = aP_(2); % y-component of acceleration of P
out = [xP, yP, vPx, vPy, aPx, aPy];
end
