Home > vbmeg > functions > common > coordinate > vb_rigid_trans_matrix.m

vb_rigid_trans_matrix

PURPOSE ^

Make rigid body transform matrix from 6 parameters

SYNOPSIS ^

function trans_mat = vb_rigid_trans_matrix(para)

DESCRIPTION ^

 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)

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Mon 22-May-2023 06:53:56 by m2html © 2005