function [COP T] = compute_COP_and_T(F, M, FPorigin, I)

n = size(F,1);

% With respect to the FP origin in the model coordinate system
COP = zeros(n, 3);
COP(I,:) = [M(I,3)./F(I,2), zeros(length(I),1), -M(I,1)./F(I,2)];

T = zeros(n, 3);
T(I,:) = [zeros(length(I),1), M(I,2)-(COP(I,3).*F(I,1) - COP(I,1).*F(I,3)) zeros(length(I),1)];

COP = COP + ones(n,1)*FPorigin';
