www.gusucode.com > robotcore 工具箱 matlab源码程序 > robotcore/quat2axang.m

    function axang = quat2axang(q)
%QUAT2AXANG Convert quaternion to axis-angle rotation representation
%   AXANG = QUAT2AXANG(Q) converts a unit quaternion, Q, into the equivalent
%   axis-angle representation, AXANG. The input, Q, is an N-by-4 matrix
%   containing N quaternions. Each quaternion represents a 3D rotation
%   and is of the form q = [w x y z], with a scalar number as the first
%   value. Each element of Q must be a real number.
%   AXANG is an N-by-4 matrix of N axis-angle rotations. The first
%   three element of every row specify the rotation axis and the last
%   element defines the angle (in radians, in the interval [-pi pi]).
%
%   Example:
%      % Convert a quaternion to axis-angle
%      q = [0.7071 0.7071 0 0];
%      axang = quat2axang(q)
%
%   See also axang2quat

%   Copyright 2014-2015 The MathWorks, Inc.

%#codegen

robotics.internal.validation.validateNumericMatrix(q, 'quat2axang', 'q', ...
    'ncols', 4);

% Normalize the quaternions
q = robotics.internal.normalizeRows(q);

% Normalize and generate the rotation vector and angle sequence
% For a single quaternion q = [w x y z], the formulas are as follows:
% (axis) v = [x y z] / norm([x y z]);
% (angle) theta = 2 * acos(w)
v = robotics.internal.normalizeRows(q(:,2:4));
theta = robotics.internal.wrapToPi(2*acos(q(:,1)));

% Handle the zero rotation degenerate case
% Can return an arbitrary axis, but fix on z-axis rotation
zeroIdx = abs(theta) < 10 * eps(class(q));

numZeroIdx = sum(zeroIdx);
assert(numZeroIdx <= length(zeroIdx));

if any(zeroIdx)
    v(zeroIdx,:) = repmat([0 0 1], numZeroIdx, 1);
    theta(zeroIdx) = 0;
end

axang = cat(2, v, theta);

end