calcTrailingLimb.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the trailing limb angle. The trailing limb angle
% is the angle between OpenSim's vertical axis and a vector created between
% the greater trochanter and the fifth metatarsal head. The location of the
% greater trochanter and the fifth metatarsal is required. The name of the
% bodies that each location is in reference to must also be assigned.  
%
% (struct, struct, 2D matrix, struct) -> (Array of number)
%


function trailingLimbAngle = calcTrailingLimb(costTerm, values, ...
    normalForce, params)

fifthMetarsalLocation = calcBodyLocation(values, str2num( ...
    costTerm.toe_point), costTerm.toe_body, params);
greaterTrochanterLocation = calcBodyLocation(values, str2num( ...
    costTerm.femur_point), costTerm.femur_body, params);

normalForce(normalForce<0) = 0;
slope = diff(normalForce);
toeOffEvent = getToeOffEvent(slope);

if ~isempty(toeOffEvent)
    trailingLimbVector = fifthMetarsalLocation(toeOffEvent, :) - ...
        greaterTrochanterLocation(toeOffEvent, :);
    trailingLimbUnitVector = trailingLimbVector/norm(trailingLimbVector);
    dotProduct = dot(trailingLimbUnitVector, [0 -1 0]);
    trailingLimbAngle = acosd(dotProduct);
else 
    trailingLimbAngle = 0;
end
end