% calculate_free_moment
% This m file calculates the free moment for each force plate, which is
% required for OpenSIM

% Force plate 1 - large plate
Fx=data(:,8);
Fy=data(:,9);
Mz=data(:,13);
COPx=data(:,2)-data(:,5); % stored data is in global CS and needs to be transferred into local plate CS
COPy=data(:,3)-data(:,6);

Tz=Mz-(COPx.*Fy) + COPy.*Fx;
Tx = zeros(1,length(COPx))';
Ty = Tx;
ForcePlate1_moments = [Tx Ty Tz/1000]; % convert Tz to Nm from Nmm


% Force plate 2 - small plate
Fx=data(:,20);
Fy=data(:,21);
Mz=data(:,25);
COPx=data(:,14)-data(:,17); % stored data is in global CS and needs to be transferred into local plate CS
COPy=data(:,15)-data(:,18);

Tz=Mz-(COPx.*Fy) + COPy.*Fx;
Tx = zeros(1,length(COPx))';
Ty = Tx;
ForcePlate2_moments = [Tx Ty Tz/1000]; % convert Tz to Nm from Nmm

% Force plate 3 - small plate
Fx=data(:,32);
Fy=data(:,33);
Mz=data(:,37);
COPx=data(:,26)-data(:,29); % stored data is in global CS and needs to be transferred into local plate CS
COPy=data(:,27)-data(:,30);

Tz=Mz-(COPx.*Fy) + COPy.*Fx;
Tx = zeros(1,length(COPx))';
Ty = Tx;
ForcePlate3_moments = [Tx Ty Tz/1000]; % convert Tz to Nm from Nmm