clc;
clear;
close all;
% rng(0);
% rand(1)*2*pi
%% W: world coordinate system
% 尺寸4X4,齐次矩阵,齐次坐标系
W = eye(4);
W(1:3,1:3)=W(1:3,1:3)*200; % 乘以200是为了使得坐标轴变长,便于作图时候显示
%% 变换(零)
% 动态参考坐标系DRF,初始状态和世界坐标系完全重合
%(1)绕W的X轴旋转,(2)绕W的Y轴旋转,(3)绕W的Z轴旋转,
%(4)相对于W的坐标原点,沿着W的XYZ方向平移
roll_WtoDRF00 = pi/2;
pitch_WtoDRF00 = 0;
yaw_WtoDRF00 = 0;
EA_zyx_WtoDRF00 = [yaw_WtoDRF00 pitch_WtoDRF00 roll_WtoDRF00];
Tl_WtoDRF00 = [200,200,-1000]';
HTfM_WtoDRF00= genHTfM_useEularAngle(EA_zyx_WtoDRF00,Tl_WtoDRF00);
DRF00 = HTfM_WtoDRF00*W;
%% 变换(一)
% 动态参考坐标系DRF00,由变换(一)得到
% (0) DRF00平移回到和W完全重合。只有平移,不旋转
%(1)绕W的X轴旋转,(2)绕W的Y轴旋转,(3)绕W的Z轴旋转,
% (4) 把DRF00平移到,变换(一)结束时候的位置,得到DRF01。只有平移,不旋转
% 效果等价于,DRF00固定自己的坐标原点,沿着W的XYZ轴旋转
roll_DRF00toDRF01 = pi/2;
pitch_DRF00toDRF01 = 0;
yaw_DRF00toDRF01 = 0;
EA_zyx_DRF00toDRF01 = [yaw_DRF00toDRF01 pitch_DRF00toDRF01 roll_DRF00toDRF01];
R_DRF00toDRF01 = eul2rotm(EA_zyx_DRF00toDRF01,'ZYX');
HTfM_WtoDRF01 = HTfM_WtoDRF00;
HTfM_WtoDRF01(1:3,1:3) = R_DRF00toDRF01*HTfM_WtoDRF01(1:3,1:3);
DRF01 = HTfM_WtoDRF01*W;
% 画图
axis_limit = 1000;
axis_limitA = [-axis_limit,axis_limit,-axis_limit,axis_limit,-1500,300];
figure(1)
trplot(W,'frame', 'RW','color','r', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')
hold on;
trplot(DRF00,'frame', 'R00','color','g', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')
hold on;
trplot(DRF01,'frame', 'R01','color','b', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(DRF01(1,4),DRF01(2,4),DRF01(3,4),'bo')
%% 变换(二)
% 动态参考坐标系DRF00,由变换(一)得到。对DRF00做一下操作,得到DRF02
%(1)绕W的X轴旋转,(2)绕W的Y轴旋转,(3)绕W的Z轴旋转
% DRF00在旋转的过程中,其坐标原点在世界坐标系下的位置也会随之旋转。
roll_DRF00toDRF02 = pi/10;
pitch_DRF00toDRF02 = 0;
yaw_DRF00toDRF02 = 0;
EA_zyx_DRF00toDRF02 = [yaw_DRF00toDRF02 pitch_DRF00toDRF02 roll_DRF00toDRF02];
Tl_DRF00toDRF02 = [0,0,0]';
HTfM_DRF00toDRF02= genHTfM_useEularAngle(EA_zyx_DRF00toDRF02,Tl_DRF00toDRF02);
HTfM_WtoDRF02 = HTfM_DRF00toDRF02*HTfM_WtoDRF00;
DRF02 = HTfM_WtoDRF02*W;
figure(2)
trplot(W,'frame', 'RW','color','r', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')
hold on;
trplot(DRF00,'frame', 'R00','color','g', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')
hold on;
trplot(DRF02,'frame', 'R02','color','m', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(DRF02(1,4),DRF02(2,4),DRF02(3,4),'mo')
%% 变换(三)
% 动态参考坐标系DRF00,由变换(一)得到。对DRF00做一下操作,得到DRF03
%(1)DRF00相对于W的坐标原点,在原有平移的基础上,沿着W的XYZ方向平移,得到DRF03
roll_DRF00toDRF03 = 0;
pitch_DRF00toDRF03 = 0;
yaw_DRF00toDRF03 = 0;
EA_zyx_DRF00toDRF03 = [yaw_DRF00toDRF03 pitch_DRF00toDRF03 roll_DRF00toDRF03];
Tl_DRF00toDRF03 = [0,240,0]';
HTfM_DRF00toDRF03= genHTfM_useEularAngle(EA_zyx_DRF00toDRF03,Tl_DRF00toDRF03);
HTfM_WtoDRF03 = HTfM_DRF00toDRF03*HTfM_WtoDRF00;
DRF03 = HTfM_WtoDRF03*W;
figure(3)
trplot(W,'frame', 'RW','color','r', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(W(1,4),W(2,4),W(3,4),'ro')
hold on;
trplot(DRF00,'frame', 'R00','color','g', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(DRF00(1,4),DRF00(2,4),DRF00(3,4),'go')
hold on;
trplot(DRF03, 'frame', 'R03', 'color', 'k', 'axis',axis_limitA, 'text_opts',{'FontSize', 10, 'FontWeight', 'light'}, 'view','auto', 'thick',1, 'dispar',0.8);
hold on;
plot3(DRF03(1,4),DRF03(2,4),DRF03(3,4),'ko')
function HTfM= genHTfM_useRotM(rotationMatrix,translation)
% HTfM homogeneous transform matrix
HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;
end
function HTfM= genHTfM_useEularAngle(eularAngle_zyx,translation)
% HTfM homogeneous transform matrix
rotationMatrix=eul2rotm(eularAngle_zyx,'ZYX');
HTfM = eye(4,4);
HTfM(1:3,1:3) = rotationMatrix;
HTfM(1:3,4) = translation;
end