function [RAC,rOC_in_A]=get_R_r(q)
%
%
% sliding/rolling disk simulation: compute rotation matrix RAC and 
% the position vector rOC in A with given states variable q
%
% Synopsis:  get_R_r(q)
%
%
% Input:     
%            q = 5x1 vector of the generalized coordinate of the disk
%
% Output:    
%            rOC_in_A = 3x1 position vector of the C* in frame A
%            RAC      = rotation matrix from frame A to the body fixed frame C
% 
% Global variable: 
%            radius   = radius of the disk (global variable specified in disk_simulation.m) 
%
%
% MEAM 535 

global radius;

q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5);

% construct rotation matrix: Rot(z,q1),Rot(x,90),Rot(x,-q2),Rot(z,q3)
Rzq1 = [cos(q1) -sin(q1) 0; ...
        sin(q1)  cos(q1) 0; ...
        0        0       1]; % A_R_A'

Rx90 = [1 0        0; ...
        0 cos(pi/2) -sin(pi/2); ...
        0 sin(pi/2)  cos(pi/2)]; % A'_R_E

Rxmq2 = [1 0         0; ...
        0  cos(-q2) -sin(-q2); ...
        0  sin(-q2)  cos(-q2)]; % E_R_B

Rzq3 = [cos(q3) -sin(q3) 0; ...
        sin(q3)  cos(q3) 0; ...
        0        0       1]; % B_R_C

% A_R_C = A_R_A1 * A'_R_E * E_R_B * B_R_C
RAC = Rzq1 * Rx90 * Rxmq2 * Rzq3;

rOC_in_A = [q4 - radius*sin(q1)*sin(q2); ...
            q5 + radius*cos(q1)*sin(q2); ...
            radius*cos(q2)];