Make rigid body transform matrix from 6 parameters trans_mat = vb_rigid_trans_matrix(para) --- Input para : [ dX ; th ] rigid body transformation parameter (6x1 vector) dX : Translation parameter of 3 directions (3x1 vector) th : Rotation angle around x, y, z axis [radian] (3x1 vector) --- Output trans_mat : rigid body transform matrix ( 4 x 3 ) Copyright (C) 2011, ATR All Rights Reserved. License : New BSD License(see VBMEG_LICENSE.txt)
0001 function trans_mat = vb_rigid_trans_matrix(para) 0002 % Make rigid body transform matrix from 6 parameters 0003 % trans_mat = vb_rigid_trans_matrix(para) 0004 % --- Input 0005 % para : [ dX ; th ] rigid body transformation parameter (6x1 vector) 0006 % dX : Translation parameter of 3 directions (3x1 vector) 0007 % th : Rotation angle around x, y, z axis [radian] (3x1 vector) 0008 % --- Output 0009 % trans_mat : rigid body transform matrix ( 4 x 3 ) 0010 % 0011 % Copyright (C) 2011, ATR All Rights Reserved. 0012 % License : New BSD License(see VBMEG_LICENSE.txt) 0013 0014 dX = para(1:3); 0015 th = para(4:6); 0016 0017 Rot_x = [1 0 0 ;... 0018 0 cos(th(1)) -sin(th(1));... 0019 0 sin(th(1)) cos(th(1));... 0020 ]; 0021 0022 Rot_y = [ cos(th(2)) 0 -sin(th(2));... 0023 0 1 0 ;... 0024 sin(th(2)) 0 cos(th(2));... 0025 ]; 0026 0027 Rot_z = [ cos(th(3)) -sin(th(3)) 0;... 0028 sin(th(3)) cos(th(3)) 0;... 0029 0 0 1 ;... 0030 ]; 0031 0032 % Rotation matrix 0033 Rot = Rot_z * Rot_y * Rot_x; 0034 0035 % Rotation & Translation 0036 trans_mat = [ Rot ; dX(:)' ]; 0037