%This function is for internal use only. It may be removed in the future.

%YPR2ROTMAT Convert yaw, pitch, and roll angles to a frame rotation matrix
%   rot = fusion.internal.frames.ypr2rotmat(ypr) returns the frame rotation
%   matrix, rot, defined by specified the yaw, pitch, and roll angles.
%   The yaw, pitch, and roll angles are specified in the 3-element vector,
%   ypr, as [yaw pitch roll] in radians.
%
%   rot = fusion.internal.frames.ypr2rotmat(ypr,units) optionally
%   specifies the units of the provided yaw, pitch, and roll angles. units
%   can be set to either 'radians' or 'degrees'. If not provided, the
%   angles are in radians.
%
%   Conventions used
%   ----------------
%   The Euler angle ZYX convention is used to specify orientation. The
%   Euler angles are defined as the successive rotations needed to
%   transform the global coordinate system to the local coordinate system.
%   The rotation sequence, ZYX, is the Tait-Bryan angles rotation using the
%   angle names yaw, pitch, and roll. Starting with the global axes,
%   (x,y,z), the global coordinate system is rotated by the yaw angle
%   around the z-axis, yielding a new set of axes, (x',y',z), where the x
%   and y axes have now changed to x' and y' and the z-axis remains
%   unchanged. This new set of axes is then pitched around the y'-axis,
%   yielding a new set of axes, (x'',y',z'). Finally, these axes are rolled
%   around the x''-axis, arriving at the local coordinate system,
%   (x'',y'',z'').
%
%   A positive rotation angle corresponds to a clockwise rotation about the
%   axis of rotation when viewed looking in the positive direction of the
%   rotation axis. This is the same as the right-hand convention where
%   positive rotation is indicated by the direction in which your fingers
%   on your right hand curl when your thumb is pointing in the direction of
%   the axis of rotation.
%
%   Usage notes
%   -----------
%   - The frame rotation matrix can be used to rotate a point defined in
%     the global frame into the local frame by pre-multiplying the point
%     by the rotation matrix:
%       
%       x_local = rot*(x_global-T)
%
%     where T is a column vector denoting the offset of the local frame's
%     origin from the global frame's origin
%
%   - The frame rotation matrix can be used to rotate a covariance matrix
%     defined in the local frame into the global frame by pre-multiplying
%     the covariance matrix by the rotation matrix and then
%     post-multiplying the covariance matrix by the transpose of the
%     rotation matrix:
%
%       C_local = rot*C_global*rot'
%
%   - To rotate a point or covariance matrix from the local frame into the
%     global frame, the transpose of the frame rotation matrix can be
%     substituted in the preceding notes:
%
%       x_global = rot'*x_local+T
%       C_global = rot'*C*rot
%
%   % Example:
%   % Represent an airframe body's local frame in the global NED coordinate
%   % frame
%   
%   % Define simple airframe in body (local) coordinates
%   x = [1 -0.5 -0.5;0 -0.5 -0.5]'; % normal axis
%   y = [0 -1 1;0 0 0]';            % lateral axis
%   z = [0 0 0;0 -0.25 0]';         % longitudinal axis
%   body = [x(:) y(:) z(:)]';
% 
%   % Define orientation of body frame in the global NED coordinate system
%   yaw = 20;
%   pitch = 5;
%   roll = 10;
%
%   % Define the offset of the body frame's origin from the NED frame
%   T = [5 -10 3]';
% 
%   % Create frame rotation matrix defining orientation of body frame using Euler angles
%   rot = fusion.internal.frames.ypr2rotmat([yaw pitch roll],'degrees');
% 
%   % Transform the airframe's body to the NED coordinate frame
%   ned = rot'*body+T;
% 
%   % Plot airframe in global frame
%   figure; axis equal; hold on;
%   xlabel('x (global)'); ylabel('y (global)'); zlabel('z (global)');
%   view(3);
% 
%   x = reshape(ned(2,:),[],2);
%   y = reshape(ned(1,:),[],2);
%   z = reshape(-ned(3,:),[],2);
% 
%   patch(x,y,z,'g');
% 
%   % Plot the body axes in global NED frame
%   quiver3(T(2)*ones(3,1),T(1)*ones(3,1),-T(3)*ones(3,1),...
%       rot(:,2),rot(:,1),-rot(:,3));
%   text(rot(2,2)+T(2),rot(2,1)+T(1),-(rot(2,3)+T(3)),'x (body)');
%   text(rot(1,2)+T(2),rot(1,1)+T(1),-(rot(1,3)+T(3)),'y (body)');
%   text(-rot(3,1)+T(2),-rot(3,2)+T(1),-(rot(3,3)+T(3)),'z (body)');
%
%   See also rotmat2ypr, ypr2quat.

%   Copyright 2017 The MathWorks, Inc.

%   Reference
%   [1] IEEE Std 1278.1-2012 IEEE Standard for Distributed Interactive
%       Simulation -- Application Protocols

% internal function, no error checking is performed

%#codegen

function rot = ypr2rotmat(ypr,units)

narginchk(1,2);
if nargin<2
    units = 'radians';
else
    units = validatestring(lower(units),{'radians' 'degrees'},mfilename);
end

q = fusion.internal.frames.ypr2quat(ypr,units);
rot = fusion.internal.frames.quat2rotmat(q);

end
