%% Interpolate Trajectory Between Rotation Matrices
% Define two rotation matrix waypoints to interpolate between.

r0 = [1 0 0; 0 1 0; 0 0 1];
rF = [0 0 1; 1 0 0; 0 0 0];

% Specify a vector of times to sample the quaternion trajectory.

tvec = 0:0.1:1;

% Generate the trajectory. Plot the results using plotTransforms. Convert the rotation matrices to quaternions and specify zero translation. The figure shows all the intermediate rotations of the coordinate frame.

[rInterp1,w1,a1] = rottraj(r0,rF,[0 1],tvec);

rotations = rotm2quat(rInterp1);
zeroVect = zeros(length(rotations),1);
translations = [zeroVect,zeroVect,zeroVect];

plotTransforms(translations,rotations)
xlabel('X')
ylabel('Y')
zlabel('Z')

% Copyright 2018 The MathWorks, Inc.