generate pivot data 2

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; % Z
yaw_W_WtoDRF00 = 0;     % Y
pitch_W_WtoDRF00 = 0;   % X
EAZYX_W_WtoDRF00 = [roll_W_WtoDRF00 yaw_W_WtoDRF00 pitch_W_WtoDRF00 ];
TlXYZ_W_WtoDRF00 = [100, -100, -1200]';
HTfM_W_WtoDRF00 = genHTfM_useEularAngle(EAZYX_W_WtoDRF00,TlXYZ_W_WtoDRF00);
DRF00 = HTfM_W_WtoDRF00*W;

%% 变换（一）
% 动态参考坐标系DRF01，初始状态和DRF00坐标系完全重合
%（1）相对于DRF00的坐标原点，沿着DRF00的X方向平移
roll_W_DRF00toDRF01 = 0;    % Z
yaw_W_DRF00toDRF01 = 0;     % Y
pitch_W_DRF00toDRF01 = 0;   % X
EAZYX_W_DRF00toDRF01 = [roll_W_DRF00toDRF01 yaw_W_DRF00toDRF01 pitch_W_DRF00toDRF01 ];
TlXYZ_W_DRF00toDRF01 = [200,0,0]';
HTfM_W_DRF00toDRF01 = genHTfM_useEularAngle(EAZYX_W_DRF00toDRF01,TlXYZ_W_DRF00toDRF01);


% 右乘（先相对于W变换，然后再做W到DRF00的变换）
HTfM_DRF00_DRF00toDRF01 = HTfM_W_WtoDRF00*HTfM_W_DRF00toDRF01;
DRF01 = HTfM_DRF00_DRF00toDRF01*W;

%% 变换（二）
% 动态参考坐标系DRF02TMP，初始状态和DRF01坐标系完全重合
%（1）DRF02TMP绕着DRF00的Y轴旋转，得到最终的DRF02
roll_W_DRF01toDRF02 = 0;        % Z
yaw_W_DRF01toDRF02 = -pi/4;     % Y
pitch_W_DRF01toDRF02 = 0;       % X
EAZYX_W_DRF01toDRF02 = [roll_W_DRF01toDRF02 yaw_W_DRF01toDRF02 pitch_W_DRF01toDRF02];
TlXYZ_W_DRF01toDRF02 = [0,0,0]';
HTfM_W_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_W_DRF01toDRF02,TlXYZ_W_DRF01toDRF02);

EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

% (1)我们要求得一个变换矩阵HTfM_DRF01_DRF01toDRF02，使得DRF02 = HTfM_DRF01_DRF01toDRF02*W
% 由于变换矩阵最终是左乘坐标系W，所以此处需要目标坐标系DRF02的初始状态DRF02TMP00和坐标系W重合

% (2)把坐标系DRF02TMP00变换到和坐标系DRF01完全重合，得到DRF02TMP01
% {DRF02TMP01 = HTfM_DRF00_DRF00toDRF01*W}

% (3)把坐标系DRF00和坐标系DRF02TMP01，看作一个整体，同时变换。
% 对坐标系DRF02TMP01做，把坐标系DRF00变换到坐标系W的变换，左乘inv(HTfM_W_WtoDRF00)，得到DRF02TMP02
% {DRF02TMP02 = inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01*W}

% (4)此时坐标系W和坐标系DRF00完全重合，在坐标系W处 做DRF02TMP02绕着DRF00的Y轴旋转DRF01到DRF02的变换，得到DRF02TMP03
% 此时坐标系W和坐标系DRF00完全重合，所以相对于W做变换等价于相对于DRF00做变换
% {DRF02TMP03 = HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01*W}

% (5)对此时的坐标系做从W到DRF00的变换，即过程(3)的逆变换，变换矩阵左乘HTfM_W_WtoDRF00
% {DRF02 =  HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01*W}

HTfM_DRF00_DRF01toDRF02 = HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01;
DRF02 = HTfM_DRF00_DRF01toDRF02*W;

%% 变换（三）
% 动态参考坐标系DRF03，初始状态和DRF02坐标系完全重合
%（1）DRF02绕着DRF00的Z轴旋转
roll_W_DRF02toDRF03 = pi/2;        % Z
yaw_W_DRF02toDRF03 = 0;           % Y
pitch_W_DRF02toDRF03 = 0;        % X
EAZYX_W_DRF02toDRF03 = [roll_W_DRF02toDRF03 yaw_W_DRF02toDRF03 pitch_W_DRF02toDRF03];
TlXYZ_W_DRF02toDRF03 = [0,0,0]';
HTfM_W_DRF02toDRF03 = genHTfM_useEularAngle(EAZYX_W_DRF02toDRF03,TlXYZ_W_DRF02toDRF03);


% (1)我们要求得一个变换矩阵 HTfM_DRF00_DRF02toDRF03，使得DRF03 = HTfM_DRF00_DRF02toDRF03*W
% 由于变换矩阵最终是左乘坐标系W，所以此处需要目标坐标系DRF03的初始状态DRF03TMP00和坐标系W重合

% (2)把坐标系DRF03TMP00变换到和坐标系DRF02完全重合，得到DRF03TMP01
% 此处直接借用 变换（二）的结果
% {DRF03TMP01 = HTfM_DRF00_DRF01toDRF02*W}

% (3)把坐标系DRF00和坐标系DRF03TMP01，看作一个整体，同时变换。
% 对坐标系DRF03TMP01做，把坐标系DRF00变换到坐标系W的变换，左乘inv(HTfM_W_WtoDRF00)，得到DRF03TMP02
% {DRF03TMP02 = inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02*W}

% (4)此时坐标系W和坐标系DRF00完全重合，在坐标系W处 做DRF03TMP02绕着DRF00的Z轴旋转DRF02到DRF03的变换，得到DRF03TMP03
% 此时坐标系W和坐标系DRF00完全重合，所以相对于W做变换等价于相对于DRF00做变换
% {DRF03TMP03 = HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02*W}

% (5)对此时的坐标系做从W到DRF00的变换，即过程(3)的逆变换，变换矩阵左乘HTfM_W_WtoDRF00
% {DRF03 = HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02*W}

HTfM_DRF00_DRF02toDRF03 = HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF03*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF01toDRF02;
DRF03 = HTfM_DRF00_DRF02toDRF03*W;


%% 变换（四）
% 动态参考坐标系DRF04TMP，初始状态和DRF03坐标系完全重合
%（1）DRF04TMP绕着DRF03坐标原点旋转，得到DRF04
roll_W_DRF03toDRF04 = pi/3;        % Z
yaw_W_DRF03toDRF04 = pi/4;           % Y
pitch_W_DRF03toDRF04 = pi/2;        % X
EAZYX_W_DRF03toDRF04 = [roll_W_DRF03toDRF04 yaw_W_DRF03toDRF04 pitch_W_DRF03toDRF04];
TlXYZ_W_DRF03toDRF04 = [0,0,0]';
HTfM_W_DRF03toDRF04 = genHTfM_useEularAngle(EAZYX_W_DRF03toDRF04,TlXYZ_W_DRF03toDRF04);


% (1)我们要求得一个变换矩阵 HTfM_DRF00_DRF03toDRF04，使得DRF04 = HTfM_DRF00_DRF03toDRF04*W
% 由于变换矩阵最终是左乘坐标系W，所以此处需要目标坐标系DRF04的初始状态DRF04TMP00和坐标系W重合

% (2)把坐标系DRF04TMP00相对于坐标系W做旋转变换，得到DRF04TMP01
% {DRF04TMP01 = HTfM_W_DRF03toDRF04*W}

% (3)对坐标系DRF04TMP01，做从坐标系W到坐标系DRF03的变换，得到DRF04
% {DRF04 = HTfM_DRF00_DRF02toDRF03*HTfM_W_DRF03toDRF04*W}

DRF04 = HTfM_DRF00_DRF02toDRF03*HTfM_W_DRF03toDRF04*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;
% 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')
%
% hold on;
% trplot(DRF02, 'frame', 'R02', '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')
%
% hold on;
% trplot(DRF03, 'frame', 'RDRF03', 'color', 'm', '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),'mo')
%
%
% hold on;
% trplot(DRF04, 'frame', 'RDRF04', 'color', 'c', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
% hold on;
% plot3(DRF04(1,4),DRF04(2,4),DRF04(3,4),'co')



%% 变换（五）
% 动态参考坐标系DRF0X，初始状态和DRF02坐标系完全重合
%（1）DRF0X绕着DRF00的Z轴旋转

fid=fopen(['simulate_pivot.txt'], 'w');		% 写入文件路径

for i=0 : -0.01 : -0.5-(-0.05)
    % 变换（二）
    % 动态参考坐标系DRF02，初始状态和DRF01坐标系完全重合
    %（1）DRF02绕着DRF00的Y轴旋转
    roll_W_DRF01toDRF02 = 0;       % Z
    yaw_W_DRF01toDRF02 = i*pi;     % Y
    pitch_W_DRF01toDRF02 = 0;      % X
    EAZYX_W_DRF01toDRF02 = [roll_W_DRF01toDRF02 yaw_W_DRF01toDRF02 pitch_W_DRF01toDRF02];
    TlXYZ_W_DRF01toDRF02 = [0,0,0]';
    HTfM_W_DRF01toDRF02 = genHTfM_useEularAngle(EAZYX_W_DRF01toDRF02,TlXYZ_W_DRF01toDRF02);

    EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

    HTfM_DRF01_DRF01toDRF02 =   HTfM_W_WtoDRF00*HTfM_W_DRF01toDRF02*inv(HTfM_W_WtoDRF00)*HTfM_DRF00_DRF00toDRF01;
    DRF02 = HTfM_DRF01_DRF01toDRF02*W;


    for j=0 : 0.02 : 2-0.02
        % 变换（三）
        % 动态参考坐标系DRF03，初始状态和DRF02坐标系完全重合
        %（1）DRF02绕着DRF00的Z轴旋转

        roll_W_DRF02toDRF0X = j*pi;        % Z
        yaw_W_DRF02toDRF0X = 0;         % Y
        pitch_W_DRF02toDRF0X = 0;       % X
        EAZYX_W_DRF02toDRF0X = [roll_W_DRF02toDRF0X yaw_W_DRF02toDRF0X pitch_W_DRF02toDRF0X];
        TlXYZ_W_DRF02toDRF0X = [0,0,0]';
        HTfM_W_DRF02toDRF0X = genHTfM_useEularAngle(EAZYX_W_DRF02toDRF0X,TlXYZ_W_DRF02toDRF0X);

        EAZYX_W_DRF00toW = inv(HTfM_W_WtoDRF00);

        HTfM_DRF00_DRF02toDRF0X =   HTfM_W_WtoDRF00*HTfM_W_DRF02toDRF0X*inv(HTfM_W_WtoDRF00)*HTfM_DRF01_DRF01toDRF02;
        DRF0X = HTfM_DRF00_DRF02toDRF0X*W;



        for k=0 : 1: 2
            % 变换（四）
            % 动态参考坐标系DRF04TMP，初始状态和DRF03坐标系完全重合
            %（1）DRF04TMP绕着DRF03坐标原点旋转，得到DRF04
            roll_W_DRF03toDRF04 = k*rand()*pi;          % Z
            yaw_W_DRF03toDRF04 = k*rand()*pi;           % Y
            pitch_W_DRF03toDRF04 = k*rand()*pi;         % X
            EAZYX_W_DRF03toDRF04 = [roll_W_DRF03toDRF04 yaw_W_DRF03toDRF04 pitch_W_DRF03toDRF04];
            TlXYZ_W_DRF03toDRF04 = [0,0,0]';
            HTfM_W_DRF03toDRF04 = genHTfM_useEularAngle(EAZYX_W_DRF03toDRF04,TlXYZ_W_DRF03toDRF04);
            DRF0X = HTfM_DRF00_DRF02toDRF0X*HTfM_W_DRF03toDRF04*W;

            %             hold on;
            %             trplot(DRF0X, 'frame', 'RDRF0X', 'color', 'm', 'axis',axis_limitA,  'text_opts',{'FontSize', 10, 'FontWeight', 'light'},  'view','auto',  'thick',1,  'dispar',0.8);
            %             hold on;
            %             plot3(DRF0X(1,4),DRF0X(2,4),DRF0X(3,4),'mo');

            % 求其坐标系DRF0X下某一点，在坐标系W下的坐标和四元数
            % 其四元数和坐标DRF0X坐标原点的四元数相同
            DRF0X_TlXYZ_marker = [5, 20, 10]';

            R = DRF0X(1:3,1:3);
            T = DRF0X(1:3,4);

            W_TlXYZ_marker = R*DRF0X_TlXYZ_marker + T;

            W_quaternion_marker = rotm2quat(R);

            % 写入txt
            for i=1:length(W_quaternion_marker)
                fprintf(fid,'%.4f ',W_quaternion_marker(i));		% 按行输出
            end



            for i=1:length(W_TlXYZ_marker)
                fprintf(fid,'%.4f ',W_TlXYZ_marker(i));		% 按行输出

            end

            for i=1:length(W_TlXYZ_marker)
                fprintf(fid, '%.4f ',W_TlXYZ_marker(i));		% 按行输出

            end

            for i=1:length(W_TlXYZ_marker)
                fprintf(fid, '%.4f ',W_TlXYZ_marker(i));		% 按行输出

            end

            for i=1:length(W_TlXYZ_marker)
                if i==3
                    fprintf(fid,'%.4f ',W_TlXYZ_marker(i));		% 按行输出
                else
                    fprintf(fid,'%.4f ',W_TlXYZ_marker(i));		% 按行输出

                end
            end

            fprintf(fid,  '\r\n');


        end

    end
end

fclose(fid);
a = 0

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