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

%QUAT2YPR Convert quaternion to yaw, pitch, and roll angles
%   ypr = fusion.internal.frames.quat2ypr(q) returns the yaw, pitch, and
%   roll angles defined by the quaternion, q. The yaw, pitch, and roll
%   angles are returned as the 1-by-3 vector, ypr, as [yaw pitch roll] in
%   radians.
%
%   ypr = fusion.internal.frames.quat2ypr(...,units) optionally specifies
%   the units of the returned yaw, pitch, and roll angles. units can be set
%   to either 'radians' or 'degrees'. If not provided, the angles are
%   returned 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.
%
%   % Example:
%   % Compute the yaw, pitch, and roll angles from the quaternion defining
%   % the orientation of the local coordinate system.
%
%   % Define orientation of the local coordinate system
%   yaw = -33;
%   pitch = -3;
%   roll = 15;
%
%   % Create quaternion from Euler angles
%   q = fusion.internal.frames.ypr2quat([yaw pitch roll],'degrees')
%
%   % Extract the yaw, pitch and roll from the quaternion
%   ypr = fusion.internal.frames.quat2ypr(q,'degrees')
%
%   See also ypr2quat, quat2rotmat.

%   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 ypr = quat2ypr(q,units)

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

ypr = euler(q,'zyx','frame');

if strcmp(units,'degrees')
    ypr = ypr*180/pi;
    bnd360 = [0 360];
    bnd180 = [-180 180];
else
    bnd360 = [0 2*pi];
    bnd180 = [-pi pi];
end
classType = class(parts(q));
bnd360 = cast(bnd360,classType);
bnd180 = cast(bnd180,classType);

% yaw lies in interval [0 360)
ypr(1) = fusion.internal.units.interval(ypr(1),bnd360);

% roll lies in interval [-180 180)
ypr(3) = fusion.internal.units.interval(ypr(3),bnd180);

end
