ANGVEC2R Convert angle and vector orientation to a 3x3 rotation matrix R = angvec2r(theta, v) Return an orthonormal rotation matrix, R, equivalent to a rotation of theta about the vector v. See also: EUL2R, RPY2R
0001 %ANGVEC2R Convert angle and vector orientation to a 3x3 rotation matrix 0002 % 0003 % R = angvec2r(theta, v) 0004 % 0005 % Return an orthonormal rotation matrix, R, equivalent to a rotation of theta 0006 % about the vector v. 0007 % 0008 % See also: EUL2R, RPY2R 0009 0010 % Copyright (C) 1993-2008, by Peter I. Corke 0011 % 0012 % This file is part of The Robotics Toolbox for Matlab (RTB). 0013 % 0014 % RTB is free software: you can redistribute it and/or modify 0015 % it under the terms of the GNU Lesser General Public License as published by 0016 % the Free Software Foundation, either version 3 of the License, or 0017 % (at your option) any later version. 0018 % 0019 % RTB is distributed in the hope that it will be useful, 0020 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0021 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0022 % GNU Lesser General Public License for more details. 0023 % 0024 % You should have received a copy of the GNU Leser General Public License 0025 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0026 function R = angvec2r(theta, k) 0027 0028 cth = cos(theta); 0029 sth = sin(theta); 0030 vth = (1 - cth); 0031 kx = k(1); ky = k(2); kz = k(3); 0032 0033 R = [ 0034 kx*kx*vth+cth ky*kx*vth-kz*sth kz*kx*vth+ky*sth 0035 kx*ky*vth+kz*sth ky*ky*vth+cth kz*ky*vth-kx*sth 0036 kx*kz*vth-ky*sth ky*kz*vth+kx*sth kz*kz*vth+cth 0037 ];