%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% T = quaternion2T(Q, P) Returns an homogeneous transformation matrix corresponding to the orientation defined by quaternion Q and position defined by P See also QPROD, T2QUATERNION. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 21/04/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % T = quaternion2T(Q, P) 0003 % Returns an homogeneous transformation matrix corresponding to the orientation 0004 % defined by quaternion Q and position defined by P 0005 % 0006 % See also QPROD, T2QUATERNION. 0007 % 0008 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: 0009 % arturo.gil@umh.es date: 21/04/2012 0010 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0011 0012 % Copyright (C) 2012, by Arturo Gil Aparicio 0013 % 0014 % This file is part of ARTE (A Robotics Toolbox for Education). 0015 % 0016 % ARTE is free software: you can redistribute it and/or modify 0017 % it under the terms of the GNU Lesser General Public License as published by 0018 % the Free Software Foundation, either version 3 of the License, or 0019 % (at your option) any later version. 0020 % 0021 % ARTE is distributed in the hope that it will be useful, 0022 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0023 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0024 % GNU Lesser General Public License for more details. 0025 % 0026 % You should have received a copy of the GNU Leser General Public License 0027 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0028 function T = quaternion2T(Q, P) 0029 0030 if nargin==1 0031 Q = Q/norm(Q); % Ensure Q has unit norm 0032 0033 %Set up convenience variables 0034 w = Q(1); x = Q(2); y = Q(3); z = Q(4); 0035 w2 = w^2; x2 = x^2; y2 = y^2; z2 = z^2; 0036 xy = x*y; xz = x*z; yz = y*z; 0037 wx = w*x; wy = w*y; wz = w*z; 0038 0039 T = [w2+x2-y2-z2 , 2*(xy - wz) , 2*(wy + xz) , 0 0040 2*(wz + xy) , w2-x2+y2-z2 , 2*(yz - wx) , 0 0041 2*(xz - wy) , 2*(wx + yz) , w2-x2-y2+z2 , 0 0042 0 , 0 , 0 , 1]; 0043 0044 0045 else 0046 0047 Q = Q/norm(Q); % Ensure Q has unit norm 0048 0049 w = Q(1); x = Q(2); y = Q(3); z = Q(4); 0050 w2 = w^2; x2 = x^2; y2 = y^2; z2 = z^2; 0051 xy = x*y; xz = x*z; yz = y*z; 0052 wx = w*x; wy = w*y; wz = w*z; 0053 0054 T = [w2+x2-y2-z2 , 2*(xy - wz) , 2*(wy + xz) , P(1) 0055 2*(wz + xy) , w2-x2+y2-z2 , 2*(yz - wx) , P(2) 0056 2*(xz - wy) , 2*(wx + yz) , w2-x2-y2+z2 , P(3) 0057 0 , 0 , 0 , 1]; 0058 end