% velaccRRT.m
% velocity and acceleration RRT dyad 
function out=velaccRRT(rAi_,rAs_,rAj_,...
 vAi_,vAs_,aAi_,aAs_,dtheta,ddtheta)

% rAi_ is the position vector of Ai
% rAs_ is the position vector of As on the sliding direction
% rAj_ is the position vector of Aj=Cj
% vAi_ is the velocity vector of Ai
% vAs_ is the velocity vector of As on the sliding direction
% aAi_ is the acceleration vector of Ai
% aAs_ is the acceleration vector of As on the sliding direction
% dtheta is the angular velocity of the sliding direction
% ddtheta is the angular acceleration of the sliding direction

% components of position, velocity, acceleration of Ai
xAi = rAi_(1);
yAi = rAi_(2);
vAix = vAi_(1);
vAiy = vAi_(2);
aAix = aAi_(1);
aAiy = aAi_(2);

% components of position, velocity, acceleration of As
% position, velocity, acceleration of As
% As is a point on the sliding direction
xAs = rAs_(1);
yAs = rAs_(2);
vAsx = vAs_(1);
vAsy = vAs_(2);
aAsx = aAs_(1);
aAsy = aAs_(2);

% position of Aj
xAj = rAj_(1);
yAj = rAj_(2);

theta = atan((yAj-yAs)/(xAj-xAs));


% calculate velocity of Aj
syms vAjxSol vAjySol
eqRRT1v =...
(xAi-xAj)*(vAix-vAjxSol)+(yAi-yAj)*(vAiy-vAjySol);
eqRRT2v =...
sin(theta)*(vAjxSol-vAsx)+cos(theta)*dtheta*(xAj-xAs)...
-cos(theta)*(vAjySol-vAsy)+sin(theta)*dtheta*(yAj-yAs)';
solRRTv = solve(eqRRT1v, eqRRT2v);
vAjx = eval(solRRTv.vAjxSol);
vAjy = eval(solRRTv.vAjySol);

% calculate acceleration of Aj
syms aAjxSol aAjySol
eqRRT1a =...
(xAi-xAj)*(aAix-aAjxSol)+(vAix-vAjx)^2+...
(yAi-yAj)*(aAiy-aAjySol)+(vAiy-vAjy)^2;
eqRRT2a =...
sin(theta)*(aAjxSol-aAsx)-cos(theta)*(aAjySol-aAsy)...
+(2*cos(theta)*(vAjx-vAsx)-sin(theta)*dtheta*(xAj-xAs)...
+2*sin(theta)*(vAjy-vAsy)...
+cos(theta)*dtheta*(yAj-yAs))*dtheta...
+(cos(theta)*(xAj-xAs)+sin(theta)*(yAj-yAs))*ddtheta;
solRRTa = solve(eqRRT1a, eqRRT2a);
aAjx = eval(solRRTa.aAjxSol);
aAjy = eval(solRRTa.aAjySol);

% components of velocity and acceleration of Aj
out = [vAjx vAjy aAjx aAjy];
end