calcFootGroundReactions.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function loads the appropriate spring position and velocity
% directions to calculate the corresponding ground reaction forces and
% moments. The ground reaction forces and moments are calculated for both
% the parent (ex. calcaneus) and child body (ex. toes).
%
% (struct, struct, struct, struct) -> (struct)
% Returns ground reaction forces and moments at the parent and child body 


function groundReactions = calcFootGroundReactions(springPositions, ...
    springVelocities, params, bodyLocations)
    
for i = 1:length(params.contactSurfaces)
    [groundReactions.parentForces{i}, groundReactions.parentMoments{i}] = ...
        calcGroundReactionForcesAndMoments(springPositions.parent{i}, ...
        springVelocities.parent{i}, params.contactSurfaces{i}.parentSpringConstants, ...
        bodyLocations.midfootSuperior{i}, params.contactSurfaces{i});
    [groundReactions.childForces{i}, groundReactions.childMoments{i}] = ...
        calcGroundReactionForcesAndMoments(springPositions.child{i}, ...
        springVelocities.child{i}, params.contactSurfaces{i}.childSpringConstants, ...
        bodyLocations.midfootSuperior{i}, params.contactSurfaces{i});
end
end
function [forces, moments] = calcGroundReactionForcesAndMoments(markerPositions, ...
    markerVelocities, springConstants, midfootSuperiorPosition, contactSurface)

markerPositions = reshape(markerPositions, [], 3, size(springConstants, 2));
markerVelocities = reshape(markerVelocities, [], 3, size(springConstants, 2));

for i = 1:size(markerPositions, 1)
markerKinematics.xPosition = squeeze(markerPositions(i, 1, :))';
markerKinematics.height = squeeze(markerPositions(i, 2, :))';
markerKinematics.zPosition = squeeze(markerPositions(i, 3, :))';
markerKinematics.xVelocity = squeeze(markerVelocities(i, 1, :))';
markerKinematics.yVelocity = squeeze(markerVelocities(i, 2, :))';
markerKinematics.zVelocity = squeeze(markerVelocities(i, 3, :))';

springForces = zeros(3, size(markerPositions, 3));

[forces(i, 2), springForces] = calcModeledVerticalGroundReactionForce( ...
    springConstants, contactSurface.dampingFactor, contactSurface.restingSpringLength, ...
    markerKinematics, springForces);

contactSurface.springConstants = springConstants;

[forces(i, 1), forces(i, 3), springForces] = ...
    calcModeledHorizontalGroundReactionForces(contactSurface, ...
    contactSurface.beltSpeed, contactSurface.latchingVelocity, markerKinematics, springForces);

task.midfootSuperiorPosition = midfootSuperiorPosition';
[moments(i, 1), moments(i, 2), moments(i, 3)] = ...
    calcModeledGroundReactionMoments(contactSurface, task, markerKinematics, ...
    springForces, i);
end
end