animateQuatPos.m

Go to the documentation of this file.

% =========================================================================== %
%> @brief animates a quaternion vector with position data
%>
%> animateQuat(quat, pos) animates the orientation data in the quaternion vector
%> with the position data in the position vector.
%>
%> animateQuat(quat, pos, options) allows to specify additional options for the
%> animation.
%>
%> animateQuat(quat1, pos1, quat2, pos2, ...) animates multiple quaternion and
%> position vectors in the same figure.
%>
%> @param quat - Nx4 numeric vector or Nx1 quaternion vector
%> @param pos - Nx3 numeric vector with position data
%> @param options (optional) - struct with optional input arguments
%>   - names - cell array with names for the orientation data (default: 1:N)
%>   - freq - Sampling frequency of the data [Hz] (default: 200)
%>   - fps - Desired Frames per second [Hz] (default: 30)
%>   - speedup - Speedup factor of the animation (default: 1)
%>   - title - Title of the figure (default: "Orientation and Position Animation")
%>   - position - Position of the figure window (default: [0,0,0.5,0.5])
%>
%> @retval none
%>
%> @file animateQuatPos.m
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository
% =========================================================================== %
 
function animateQuatPos(quat, pos, options)
 
%% argument checking
arguments (Repeating)
    quat (:,:) {mustBeNonmissing}
    pos (:,3) {mustBeNonmissing}
end
 
arguments
    options.names (:,1) string = string.empty(1,0)
    options.freq (1,1) double = 200;
    options.fps (1,1) double = 30;
    options.speedup (1,1) double = 1;
    options.title (1,1) string = "Orientation and Position Animation"
    options.position = [0,0,0.5,0.5]
end
N_quat = numel(quat);
N_samples = 0;
 
% check if quaternion vector is provided
% define max sample size
for i=1:N_quat
    if ~isa(quat{i}, 'quaternion')
        if size(quat{i}, 2) ~= 4
            error('Quaternion vector must be Nx4');
        else
            quat{i} = quaternion(quat{i});
        end
    end
    
    % max sample size
    N_samples = max(N_samples, numel(quat{i}));
end
 
% check if position vector is the same size as the quaternion vector
for i=1:N_quat
    if size(pos{i}, 1) ~= size(quat{i}, 1)
        error('Position vector must be the same size as the quaternion vector');
    end
end
 
% check if names are provided
if isempty(options.names) || numel(options.names) ~= N_quat
    options.names = string(1:N_quat);
end
 
%% set up parameters
% smoothed fps
smoothed_fps = options.fps;
 
% set up timer
t = timer('ExecutionMode', 'fixedRate', 'Period', round(1/options.fps,3), 'TimerFcn', @timer_callback);
t.UserData = 1;
 
%% create figure
fig = figure('Name', options.title, 'NumberTitle', 'off', 'MenuBar', 'none', 'ToolBar', 'figure', 'Units','normalized',"Position",options.position);
for i=1:N_quat
    subplot(1,N_quat,i);
    pp(i) = poseplot();
    title(options.names(i));
    hold on
    plot3(pos{i}(:,1), pos{i}(:,2), pos{i}(:,3), 'k', 'LineWidth', 2);
    hold off
end
 
% add a close request function to the figure
fig.CloseRequestFcn = @my_closereq;
 
% add a time passed indicator to the bottom left of the figure
text_passed = uicontrol('Style', 'text', 'String', sprintf('Time passed: %.2f s', 0), 'Units', 'normalized', 'Position', [0.01 0.01 0.3 0.05], 'FontSize', 12, 'FontWeight', 'bold');
 
% add a fps indicator to the bottom right of the figure
text_fps = uicontrol('Style', 'text', 'String', sprintf('FPS: %d', options.fps), 'Units', 'normalized', 'Position', [0.69 0.01 0.3 0.05], 'FontSize', 12, 'FontWeight', 'bold');
 
% add a line to the top of the figure that indicates the current time
line_curr_time = annotation('line', [0 0], [1,1], 'Units', 'normalized', 'LineWidth', 5, 'Color', 'r');
 
% set up animation
start(t);
 
%% functions
% timer callback function
    function timer_callback(~, ~)
        if t.UserData > N_samples
            stop(t);
        else
            % update poseplot
            for i=1:N_quat
                if t.UserData < numel(quat{i})
                    pp(i).Orientation = quat{i}(t.UserData);
                    pp(i).Position = pos{i}(t.UserData, :);
                else
                    pp(i).PatchFaceColor = 'r';
                end
            end
            
            % update time passed indicator
            text_passed.String = sprintf('Time passed: %.2f s', t.UserData/options.freq);
            
            % update frequency indicator
            curr_fps = 1/(t.InstantPeriod);
            if isnan(curr_fps)
                curr_fps = options.fps;
            end
            
            % Smooth the FPS value using an IIR filter
            alpha = 0.05; % Smoothing factor
            smoothed_fps = alpha * round(curr_fps) + (1 - alpha) * smoothed_fps;
            text_fps.String = sprintf('FPS: %d', round(smoothed_fps));
            
            % calculate steps to skip to keep up with the desired frequency
            skip = round(options.speedup * options.freq/curr_fps);
            
            % update current time indicator
            line_curr_time.X = [0, t.UserData/N_samples];
            
            % update counter
            t.UserData = t.UserData + skip;
        end
    end
 
% close request function
    function my_closereq(~,~)
        stop(t);
        delete(t);
        delete(fig);
    end
 
end