% MotionTable
function Motion_Table()
clc

Wp=400; % Weight of Pilots
mp=Wp/32.2; %Mass of Pilots
Wc=300; %Weight of Cockpit
mc=Wc/32.2; %Mass of Cockpit
g=32.2;

ta=.75;
ro=50*(pi/180);
ra=250*(pi/180);
Ixc=1000;
Iyc=1000;
Izc=1000;
[Ixp Iyp Izp]=pmoment(mp);
Xsf=0;
Xsr=26;
Xsl=Xsr;
Xmc=0;
Xmp=0
Ysf=62;
Ysr=18;
Ysl=Ysr;
Ymc=18;
Ymp=0;
Zsf=10;
Zsr=Zsf;
Zsl=Zsf;
Zmc=14;
Zmp=26;

i=0;

for t=-ta:2*ta:ta
    for a=-ra:2*ra:ra
        for o=0:ro
        i=i+1;
% Reaction Matrix for Heave (+Z direction)
RH=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FH=[0;
    0;
    (1+t)*g*(mc+mp);
    (1+t)*g*mc*(Ysf-Ymc)+(1+t)*g*mp*(Ysf);
    0;
    0;
    -(1+t)*g*mc*Ymc;
    0;
    0;
    -(1+t)*g*(mc*(Ymc+Ysr)+mp*Ysr);
    (1+t)*g*Xsr*(mc+mp);
    0];
R{1}(:,i)=pinv(RH)*FH;


% Reaction Matrix for Lateral (+X direction)
RL=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FL=[t*g*(mc+mp);
    0;
    0;
    0;
    -t*g*(mc*(Zmc-Zsf)+mp*(Zmp-Zsf));
    -t*g*(mc*(Ysf-Ymc)+mp*Ysf);
    0;
    -t*g*(mc*Zmc+mp*Zmp);
    t*g*(mc*Ymc);
    0;
    -t*g*(mc*(Zmc-Zsf)+mp*(Zmp-Zsf));
    t*g*(mc*(Ymc+Ysr)+mp*Ysr)];
R{2}(:,i)=pinv(RL)*FL;

% Reaction Matrix for Surge (-Y direction)
RS=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FS=[0;  %1
    t*g*(mc+mp); %2
    0; %3
    -t*g*(mc*(Zmc-Zsf)+mp*(Zmp-Zsf)); %4
    0; %5
    0; %6
    -t*g*(mc*Zmc+mp*Zmp); %7
    0; %8
    0; %9
    -t*g*(mc*(Zmc-Zsr)+mp*(Zmp-Zsr)); %10
    0; %11
    t*g*(mc*Xsr+mp*Xsr)]; %12
R{3}(:,i)=pinv(RS)*FS;



% Reaction Matrix for Constant Angular Velocity (omega)Pitch
ROP=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FOP=[0;  %1
    0; %2
    -mc*o^2*Zmc-mp*o^2*Zmp; %3
    -o^2*Zmc*mc*(Ysf-Ymc)-o^2*Zmp*mp*(Ysf); %4
    0; %5
    0; %6
    o^2*Zmc*mc*Ymc; %7
    0; %8
    0; %9
    o^2*Zmc*mc*(Ymc+Ysr)+o^2*Zmp*mp*Ysr; %10
    -o^2*Zmc*Xsr*mc-o^2*Zmp*Xsr*mp; %11
    0]; %12
R{4}(:,i)=pinv(ROP)*FOP;

% Reaction Matrix for Constant Angular Velocity (omega)Roll
ROR=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FOR=[0;  %1
    0; %2
    -mc*o^2*Zmc-mp*o^2*Zmp; %3
    -o^2*Zmc*mc*(Ysf-Ymc)-o^2*Zmp*mp*(Ysf); %4
    0; %5
    0; %6
    o^2*Zmc*mc*Ymc; %7
    0; %8
    0; %9
    o^2*Zmc*mc*(Ymc+Ysr)+o^2*Zmp*mp*Ysr; %10
    -o^2*Zmc*Xsr*mc-o^2*Zmp*Xsr*mp; %11
    0]; %12
R{5}(:,i)=pinv(ROR)*FOR;

%Reaction Matrix for Constant Angular Velocity (omega)Yaw
ROY=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FOY=[0;  %1
    mc*o^2*Ymc; %2
    0; %3
    -o^2*Ymc*mc*(Zmc-Zsf); %4
    0; %5
    0; %6
    -o^2*Ymc*mc*Zmc; %7
    0; %8
    0; %9
    -o^2*Ymc*mc*(Zmc-Zsr); %10
    0; %11
    o^2*Ymc*mc*Xsr]; %12
R{6}(:,i)=pinv(ROY)*FOY;

% Reaction Matrix for Constant Angular Acceleration (Aphla)Pitch
RAP=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FAP=[0;  %1
    0; %2
    0; %3
    (Ixc+mc*sqrt(Ymc^2+Zmc^2)+Ixp+mp*sqrt(Ymp^2+Zmc^2))*a; %4
    0; %5
    0; %6
    (Ixc+mc*sqrt(Ymc^2+Zmc^2)+Ixp+mp*sqrt(Ymp^2+Zmc^2))*a; %7
    0; %8
    0; %9
    (Ixc+mc*sqrt(Ymc^2+Zmc^2)+Ixp+mp*sqrt(Ymp^2+Zmc^2))*a; %10
    0; %11
    0]; %12
R{7}(:,i)=pinv(RAP)*FAP;

% Reaction Matrix for Constant Angular Acceleration (Aphla)Roll
RAR=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FAR=[0;  %1
    0; %2
    0; %3
    0; %4
    (Iyc+mc*sqrt(Xmc^2+Zmc^2)+Iyp+mp*sqrt(Xmp^2+Zmc^2))*a; %5
    0; %6
    0; %7
    (Iyc+mc*sqrt(Xmc^2+Zmc^2)+Iyp+mp*sqrt(Xmp^2+Zmc^2))*a; %8
    0; %9
    0; %10
    (Iyc+mc*sqrt(Xmc^2+Zmc^2)+Iyp+mp*sqrt(Xmp^2+Zmc^2))*a; %11
    0]; %12
R{8}(:,i)=pinv(RAR)*FAR;

% Reaction Matrix for Constant Angular Acceleration (Aphla)Yaw
RAY=[1 1 1 0 0 0 0 0 0;
    0 0 0 1 1 1 0 0 0;
    0 0 0 0 0 0 1 1 1;
    0 0 0 0 0 0 0 (Ysf+Ysr) (Ysf+Ysl);
    0 0 0 0 0 0 0 -Xsr Xsl;
    0 -(Ysf+Ysr) -(Ysf+Ysl) 0 -Xsr Xsl 0 0 0;
    0 0 0 -Zsf -Zsr -Zsl -Ysf Ysr Ysl;
    -Zsf -Zsr -Zsl 0 0 0 0 -Xsr Xsl;
    Ysf -Ysr -Ysl 0 -Xsr Xsl 0 0 0;
    0 0 0 0 0 0 -(Ysf+Ysr) 0 0;
    0 0 0 0 0 0 Xsr 0 (Xsl+Xsr);
    (Ysf+Ysr) 0 0 Xsr 0 (Xsl+Xsr) 0 0 0];
FAY=[0;  %1
    0; %2
    0; %3
    0; %4
    0; %5
    (Izc+mc*sqrt(Xmc^2+Ymc^2)+Izp+mp*sqrt(Xmp^2+Ymc^2))*a; %6
    0; %7
    0; %8
    (Izc+mc*sqrt(Xmc^2+Ymc^2)+Izp+mp*sqrt(Xmp^2+Ymc^2))*a; %9
    0; %10
    0; %11
    (Izc+mc*sqrt(Xmc^2+Ymc^2)+Izp+mp*sqrt(Xmp^2+Ymc^2))*a]; %12
R{9}(:,i)=pinv(RAY)*FAY;
        end
    end
end

WC=zeros(9);
WCH=zeros(9);
WCL=zeros(9);

for k=1:9
for j=1:9
    [FMax, CMax]=max(R{j},[],2);
    [FMin, CMin]=min(R{j},[],2);


    WCH(:,k)=WCH(:,k)+R{j}(:,CMax(k));
    WCL(:,k)=WCL(:,k)+R{j}(:,CMin(k));

end

if abs(WCH(k,k))>abs(WCL(k,k))
   WC(:,k)=WCH(:,k);
else
   WC(:,k)=WCL(:,k);
end

end

WC


% for p=1:12
% Q=RAP;
% W=FAP;
% Q(p,:)=[];
% W(p,:)=[];
% Re= pinv(Q)*W
% p
% end

% Pilot Moment of Inertia
function [Ix Iy Iz]=pmoment(m)
clc



%Head
Hr=3.7; %radius
Hm=.05; %mass fraction
Hx=11; %distance x
Hy=0; %distance y
Hz=18; %distance z
HIx=Hm*m*(.4*Hr^2+sqrt(Hy^2+Hz^2));
HIy=Hm*m*(.4*Hr^2+sqrt(Hx^2+Hz^2));
HIz=Hm*m*(.4*Hr^2+sqrt(Hx^2+Hy^2));

%Torso
Tw=10;
Th=25.6;
Tb=13.7;
Tm=.49;
Tx=11;
Ty=0;
Tz=1;
TIx=Tm*m*((1/12)*(Tw^2+Th^2)+sqrt(Tz^2+Ty^2));
TIy=Tm*m*((1/12)*(Tb^2+Th^2)+sqrt(Tz^2+Tx^2));
TIz=Tm*m*((1/12)*(Tw^2+Tb^2)+sqrt(Tx^2+Ty^2));

%Upper Arm
UAr=2;
UAl=14.3;
UAm=.03;
UARx=3;
UALx=19;
UAy=4;
UAz=13;
UAIx=UAm*m*(.25*UAr^2+(1/12)*UAl^2+sqrt(UAy^2+UAz^2));
UARIy=UAm*m*(.5*UAr^2+sqrt(UARx^2+UAz^2));
UARIz=UAm*m*(.25*UAr^2+(1/12)*UAl^2+sqrt(UAy^2+UARx^2));
UALIy=UAm*m*(.5*UAr^2+sqrt(UALx^2+UAz^2));
UALIz=UAm*m*(.25*UAr^2+(1/12)*UAl^2+sqrt(UAy^2+UALx^2));


%Lower Arm
LAr=1.8;
LAl=19.4;
LAm=.02;
LARx=3;
LALx=19;
LAy=19;
LAz=13;
LAIx=LAm*m*(.25*LAr^2+(1/12)*LAl^2+sqrt(LAy^2+LAz^2));
LARIy=LAm*m*(.5*LAr^2+sqrt(LARx^2+LAz^2));
LARIz=LAm*m*(.25*LAr^2+(1/12)*LAl^2+sqrt(LAy^2+LARx^2));
LALIy=LAm*m*(.5*LAr^2+sqrt(LALx^2+LAz^2));
LALIz=LAm*m*(.25*LAr^2+(1/12)*LAl^2+sqrt(LAy^2+LALx^2));


%Thigh
THr=3.7;
THl=14;
THm=.12;
THRx=7;
THLx=15;
THy=7;
THz=8;
THIx=THm*m*(.25*THr^2+(1/12)*THl^2+sqrt(THy^2+THz^2));
THRIy=THm*m*(.5*THr^2+sqrt(THRx^2+THz^2));
THRIz=THm*m*(.25*THr^2+(1/12)*THl^2+sqrt(THy^2+THRx^2));
THLIy=THm*m*(.5*THr^2+sqrt(THLx^2+THz^2));
THLIz=THm*m*(.25*THr^2+(1/12)*THl^2+sqrt(THy^2+THLx^2));

%Calf
Cr=2.4;
Cl=19.7;
Cm=.06;
CRx=7;
CLx=15;
Cy=24;
Cz=8;
CIx=Cm*m*(.25*Cr^2+(1/12)*Cl^2+sqrt(Cy^2+Cz^2));
CRIy=Cm*m*(.5*Cr^2+sqrt(CRx^2+Cz^2));
CRIz=Cm*m*(.25*Cr^2+(1/12)*Cl^2+sqrt(Cy^2+CRx^2));
CLIy=Cm*m*(.5*Cr^2+sqrt(CLx^2+Cz^2));
CLIz=Cm*m*(.25*Cr^2+(1/12)*Cl^2+sqrt(Cy^2+CLx^2));

Ix=HIx+TIx+2*(UAIx+LAIx+THIx+CIx);
Iy=HIy+TIy+UARIy+UALIy+LARIy+LALIy+THRIy+THLIy+CRIy+CLIy;
Iz=HIz+TIz+UARIz+UALIz+LARIz+LALIz+THRIz+THLIz+CRIz+CLIz;
Xmp =

     0


WC =

 -246.9932  -93.5146  -93.5146 -246.9932  -93.5146   93.5146  246.9932   93.5146 -246.9932
 -139.0034 -215.7427 -215.7427 -139.0034 -215.7427  215.7427  139.0034  215.7427 -139.0034
 -139.0034 -215.7427 -215.7427 -139.0034 -215.7427  215.7427  139.0034  215.7427 -139.0034
  175.0000  175.0000 -175.0000 -175.0000 -175.0000 -175.0000  175.0000 -175.0000 -175.0000
  210.0967  135.2759 -214.7241 -139.9033 -214.7241 -135.2759  139.9033 -135.2759 -139.9033
  139.9033  214.7241 -135.2759 -210.0967 -135.2759 -214.7241  210.0967 -214.7241 -210.0967
  575.1663  575.1663  432.6663  432.6663  432.6663  432.6663  575.1663  212.3337  212.3337
   53.5999   53.5999  124.8499  124.8499  124.8499  344.0807  272.8307  777.6501  235.0162
  596.2338  596.2338  667.4838  667.4838  667.4838  448.2531  377.0031  235.0162  777.6501