rotvec2qvec.m

来自「This diskette (version 1.0) contains dem」· M 代码 · 共 14 行

function quatvec = rotvec2qvec(rotvec)
%
% converts rotation vector to quaternion vector
%
rvec        = rotvec;
[rows,cols] = size(rvec);
if (cols == 3)&(rows == 1)
   rvec = rvec';
elseif (rows ~= 3)|(cols ~=1)
   error('Error in rotvec2qvec: rotation vector not 3x1 or 1x3');
end;
angle       = norm(rvec);
uvec        = rvec/angle;
quatvec     = [cos(angle/2);sin(angle/2)*uvec];