TRNORM Normalize a homogeneous transformation. TN = TRNORM(T) Returns a normalized homogeneous tranformation matrix in which the rotation submatrix is a proper orthogonal matrix. The O and V vectors are normalized and the normal vector is formed from O x A. Finite word length arithmetic can cause transforms to become `unnormalized'. See also: OA2TR
0001 %TRNORM Normalize a homogeneous transformation. 0002 % 0003 % TN = TRNORM(T) 0004 % 0005 % Returns a normalized homogeneous tranformation matrix in which the rotation 0006 % submatrix is a proper orthogonal matrix. 0007 % The O and V vectors are normalized and the normal vector is formed from 0008 % O x A. 0009 % 0010 % Finite word length arithmetic can cause transforms to become `unnormalized'. 0011 % 0012 % See also: OA2TR 0013 0014 % Copyright (C) 1993-2008, by Peter I. Corke 0015 % 0016 % This file is part of The Robotics Toolbox for Matlab (RTB). 0017 % 0018 % RTB is free software: you can redistribute it and/or modify 0019 % it under the terms of the GNU Lesser General Public License as published by 0020 % the Free Software Foundation, either version 3 of the License, or 0021 % (at your option) any later version. 0022 % 0023 % RTB is distributed in the hope that it will be useful, 0024 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0025 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0026 % GNU Lesser General Public License for more details. 0027 % 0028 % You should have received a copy of the GNU Leser General Public License 0029 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0030 0031 function r = trnorm(t) 0032 n = cross(t(1:3,2), t(1:3,3)); % N = O x A 0033 o = cross(t(1:3,3), n); % O = A x N 0034 r = [unit(n) unit(t(1:3,2)) unit(t(1:3,3)) t(1:3,4); 0 0 0 1];