% ANGLEAXIS2MATRIX - converts angle-axis descriptor to 4x4 homogeneous % transformation matrix % % Usage: T = angleaxis2matrix(t) % % Argument: t - 3-vector giving rotation axis with magnitude equal to the % rotation angle in radians. % Returns: T - 4x4 Homogeneous transformation matrix % % See also: MATRIX2ANGLEAXIS, ANGLEAXISROTATE, NEWANGLEAXIS, NORMALISEANGLEAXIS % Copyright (c) 2008 Peter Kovesi % School of Computer Science & Software Engineering % The University of Western Australia % pk at csse uwa edu au % http://www.csse.uwa.edu.au/ % % Permission is hereby granted, free of charge, to any person obtaining a copy % of this software and associated documentation files (the "Software"), to deal % in the Software without restriction, subject to the following conditions: % % The above copyright notice and this permission notice shall be included in % all copies or substantial portions of the Software. % % The Software is provided "as is", without warranty of any kind. function T = angleaxis2matrix(t) theta = sqrt(t(:)'*t(:)); % = norm(t), but faster if theta < eps % If the rotation is very small... T = [ 1 -t(3) t(2) 0 t(3) 1 -t(1) 0 -t(2) t(1) 1 0 0 0 0 1]; return end % Otherwise set up standard matrix, first setting up some convenience % variables t = t/theta; x = t(1); y = t(2); z = t(3); c = cos(theta); s = sin(theta); C = 1-c; xs = x*s; ys = y*s; zs = z*s; xC = x*C; yC = y*C; zC = z*C; xyC = x*yC; yzC = y*zC; zxC = z*xC; T = [ x*xC+c xyC-zs zxC+ys 0 xyC+zs y*yC+c yzC-xs 0 zxC-ys yzC+xs z*zC+c 0 0 0 0 1];