h t fad fdads

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是为了使得坐标轴变长，便于作图时候显示 


%% 变换（零）
% 动态参考坐标系DRF00，初始状态和世界坐标系完全重合
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转，
%（4）相对于W的坐标原点，沿着W的XYZ方向平移
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;


%% 变换（一）
% 动态参考坐标系DRF01，初始状态和世界坐标系完全重合
%（1）绕W的X轴旋转，（2）绕W的Y轴旋转，（3）绕W的Z轴旋转，
%（4）相对于W的坐标原点，沿着W的XYZ方向平移
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;



%% 变换（二）
% DRF02TMP0初始状态和对DRF01完全重合，对DRF02TMP0做如下变换，最终得到DRF02
%（1）DRF02TMP0绕DRF00的X轴旋转roll_DRF00_DRF01toDRF02，得到DRF02TMP1，
%（2）DRF02TMP1绕DRF00的Y轴旋转pitch_DRF00_DRF01toDRF02，得到DRF02TMP2，
%（3）DRF02TMP2绕DRF00的Z轴旋转yaw_DRF00_DRF01toDRF02，得到DRF02TMP3,即最终的DRF02
% 
% 等价于：
% DRF02TMP0初始状态和对DRF01完全重合，对DRF02TMP0做如下变换，最终得到DRF02
% (一)坐标系A绕着坐标系B旋转，就把A和B看作一个整体，A和B同步做相同地变换，使得B和世界坐标系重合
% (1)把DRF02TMP0和DRF00看作一个整体，整体变换到DRF00和W完全重合,此时DRF00的状态记作DRF00TMP1，DRF02的状态记作DRF02TMP4

% (二)由于左乘是绕着世界坐标系旋转，此时B和世界坐标系重合，所以此时左乘即绕着坐标系B旋转。
% (2)DRF02TMP4绕DRF00TMP1的X轴旋转roll_DRF00TMP1_DRF02TMP4toDRF02TMP5，得到DRF02TMP6
% (3)DRF02TMP6绕DRF00TMP1的Y轴旋转pitch_DRF00TMP1_DRF02TMP4toDRF02TMP5，得到DRF02TMP7
% (4)DRF02TMP7绕DRF00TMP1的Z轴旋转raw_DRF00TMP1_DRF02TMP4toDRF02TMP5，得到DRF02TMP8

% （三）把A和B看作一个整体，A和B同步做相同地变换，使得B和B刚开始的位置重合
% (5)把DRF02TMP8和DRF00TMP1看作一个整体，整体变换到DRF00TMP1和DRF00完全重合,此时得到DRF02TMP9，即即最终的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;


% 画图
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')