h t fad fdads

clc;
clear;
close all;

% rng(0);
% rand(1)*2*pi

%% W: world coordinate system
%  Size 4X4, Homogeneous matrix , Homogeneous coordinate system 
W = eye(4);
W(1:3,1:3)=W(1:3,1:3)*200; %  multiply 200 To make the axis longer , It is convenient to display  


%%  Transformation （ zero ）
%  Dynamic reference coordinate system DRF00, The initial state completely coincides with the world coordinate system 
%（1） Around the W Of X Shaft rotation ,（2） Around the W Of Y Shaft rotation ,（3） Around the W Of Z Shaft rotation ,
%（4） be relative to W The origin of the coordinates , Along W Of XYZ Direction translation 
roll_W_WtoDRF00 = pi/2;
pitch_W_WtoDRF00 = 0;
yaw_W_WtoDRF00 = 0;
EAZYX_W_WtoDRF00 = [yaw_W_WtoDRF00 pitch_W_WtoDRF00 roll_W_WtoDRF00];
TlZYX_W_WtoDRF00 = [200,200,-1000]';
HTfM_W_WtoDRF00= genHTfM_useEularAngle(EAZYX_W_WtoDRF00,TlZYX_W_WtoDRF00);
DRF00 = HTfM_W_WtoDRF00*W;


%%  Transformation （ One ）
%  Dynamic reference coordinate system DRF01, The initial state completely coincides with the world coordinate system 
%（1） Around the W Of X Shaft rotation ,（2） Around the W Of Y Shaft rotation ,（3） Around the W Of Z Shaft rotation ,
%（4） be relative to W The origin of the coordinates , Along W Of XYZ Direction translation 
roll_W_WtoDRF01 = pi/2;
pitch_W_WtoDRF01 = 0;
yaw_W_WtoDRF01 = 0;
EAZYX_W_WtoDRF01 = [yaw_W_WtoDRF01 pitch_W_WtoDRF01 roll_W_WtoDRF01];
TlZYX_W_WtoDRF01 = [100,100,-1010]';
HTfM_W_WtoDRF01= genHTfM_useEularAngle(EAZYX_W_WtoDRF01,TlZYX_W_WtoDRF01);
DRF01 = HTfM_W_WtoDRF01*W;



%%  Transformation （ Two ）
% DRF02TMP0 Initial state and alignment DRF01 Complete coincidence , Yes DRF02TMP0 Do the following transformation , The resulting DRF02
%（1）DRF02TMP0 Around the DRF00 Of X Shaft rotation roll_DRF00_DRF01toDRF02, obtain DRF02TMP1,
%（2）DRF02TMP1 Around the DRF00 Of Y Shaft rotation pitch_DRF00_DRF01toDRF02, obtain DRF02TMP2,
%（3）DRF02TMP2 Around the DRF00 Of Z Shaft rotation yaw_DRF00_DRF01toDRF02, obtain DRF02TMP3, That is, the final DRF02
% 
%  Equivalent to ：
% DRF02TMP0 Initial state and alignment DRF01 Complete coincidence , Yes DRF02TMP0 Do the following transformation , The resulting DRF02
% ( One ) Coordinate system A Around the coordinate system B rotate , Just put A and B As a whole ,A and B Do the same transformation synchronously , bring B Coincide with the world coordinate system 
% (1) hold DRF02TMP0 and DRF00 As a whole , Overall transformation to DRF00 and W Complete coincidence , here DRF00 The status of is recorded as DRF00TMP1,DRF02 The status of is recorded as DRF02TMP4

% ( Two ) Because the left multiplication is rotating around the world coordinate system , here B Coincide with the world coordinate system , So the left multiplication is going around the coordinate system B rotate .
% (2)DRF02TMP4 Around the DRF00TMP1 Of X Shaft rotation roll_DRF00TMP1_DRF02TMP4toDRF02TMP5, obtain DRF02TMP6
% (3)DRF02TMP6 Around the DRF00TMP1 Of Y Shaft rotation pitch_DRF00TMP1_DRF02TMP4toDRF02TMP5, obtain DRF02TMP7
% (4)DRF02TMP7 Around the DRF00TMP1 Of Z Shaft rotation raw_DRF00TMP1_DRF02TMP4toDRF02TMP5, obtain DRF02TMP8

% （ 3、 ... and ） hold A and B As a whole ,A and B Do the same transformation synchronously , bring B and B The initial positions coincide 
% (5) hold DRF02TMP8 and DRF00TMP1 As a whole , Overall transformation to DRF00TMP1 and DRF00 Complete coincidence , Now get DRF02TMP9, That is, the final DRF02



roll_DRF00_DRF01toDRF02 = pi/2;
pitch_DRF00_DRF01toDRF02 = 0;
yaw_DRF00_DRF01toDRF02 = 0;
EAZYX_DRF00_DRF01toDRF02 = [roll_DRF00_DRF01toDRF02 pitch_DRF00_DRF01toDRF02 yaw_DRF00_DRF01toDRF02];
TlZYX_DRF00_DRF01toDRF02 = [0,0,0]';

EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

HTfM_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_DRF00_DRF01toDRF02, TlZYX_DRF00_DRF01toDRF02);
HTfM_WtoDRF02 = HTfM_W_WtoDRF00 * HTfM_DRF01toDRF02 * EAZYX_W_DRF00toW * HTfM_W_WtoDRF01;
DRF02 = HTfM_WtoDRF02*W;


%  drawing 
axis_limit = 1200;
axis_limitA = [-axis_limit,axis_limit,-axis_limit,axis_limit,-1500,300];
figure(6)
title("figure(6)");
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', 'R02', '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')

hold on;
trplot(DRF02, 'frame', 'R07', 'color', 'k', '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),'ko')