calcTorqueBasedModeledValues.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the position and velocities of the spring
% locations and corresponding ground reaction forces and moments if
% contact surfaces are present. This function also calculates inverse
% dynamic moments.
%
% (struct, struct) -> (struct)
% returns body locations, ground reactions, and inverse dynamic moments


function modeledValues = calcTorqueBasedModeledValues(values, params)
appliedLoads = [zeros(length(values.time), params.numTotalMuscles)];
if ~isempty(params.contactSurfaces)
    clear pointKinematics
    [springPositions, springVelocities] = getSpringLocations( ...
        values.time, values.statePositions, values.stateVelocities, params);
    modeledValues.bodyLocations = getBodyLocations(values.time, ....
        values.statePositions, values.stateVelocities, params);
    groundReactions = calcFootGroundReactions(springPositions, ...
        springVelocities, params, modeledValues.bodyLocations);
    groundReactionsBody = tranferGroundReactionMoments( ...
        modeledValues.bodyLocations, groundReactions, params);
    modeledValues.groundReactionsLab = calcGroundReactionsLab(groundReactions);
    appliedLoads = [appliedLoads groundReactionsBody];
end
modeledValues.inverseDynamicMoments = inverseDynamics(values.time, ...
    values.statePositions, values.stateVelocities, ...
    values.stateAccelerations, params.coordinateNames, appliedLoads, ...
    params.mexModel);
end

function [springPositions, springVelocities] = getSpringLocations(time, ....
    statePositions, stateVelocities, params)

for i = 1:length(params.contactSurfaces)
    [springPositions.parent{i}, springVelocities.parent{i}] = ...
        pointKinematics(time, statePositions, stateVelocities, ...
        params.contactSurfaces{i}.parentSpringPointsOnBody, ...
        params.contactSurfaces{i}.parentBody * ones(1, ...
        size(params.contactSurfaces{i}.parentSpringPointsOnBody, 1)), ...
        params.mexModel, params.coordinateNames);
    [springPositions.child{i}, springVelocities.child{i}] = ...
        pointKinematics(time, statePositions, stateVelocities, ...
        params.contactSurfaces{i}.childSpringPointsOnBody, ...
        params.contactSurfaces{i}.childBody * ones(1, ...
        size(params.contactSurfaces{i}.childSpringPointsOnBody, 1)), ...
        params.mexModel, params.coordinateNames);
end
end

function bodyLocations = getBodyLocations(time, statePositions, ...
    stateVelocities, params)

for i = 1:length(params.contactSurfaces)
    bodyLocations.midfootSuperior{i} = pointKinematics(time, ...
        statePositions, stateVelocities, ...
        params.contactSurfaces{i}.midfootSuperiorPointOnBody, ...
        params.contactSurfaces{i}.midfootSuperiorBody, ...
        params.mexModel, params.coordinateNames);
    bodyLocations.midfootSuperior{i}(:, 2) = 0;
    bodyLocations.parent{i} = pointKinematics(time, statePositions, ...
        stateVelocities, [0 0 0], params.contactSurfaces{i}.parentBody, ...
        params.mexModel, params.coordinateNames);
    bodyLocations.child{i} = pointKinematics(time, statePositions, ...
        stateVelocities, [0 0 0], params.contactSurfaces{i}.childBody, ...
        params.mexModel, params.coordinateNames);
end
end

function groundReactionsBody = tranferGroundReactionMoments( ...
    bodyLocations, groundReactions, params)

groundReactionsBody = [];
for i = 1:length(params.contactSurfaces)
    parentMoment = transferMoments(bodyLocations.midfootSuperior{i}, ...
        bodyLocations.parent{i}, groundReactions.parentMoments{i}, ...
        groundReactions.parentForces{i});
    childMoment = transferMoments(bodyLocations.midfootSuperior{i}, ...
        bodyLocations.child{i}, groundReactions.childMoments{i}, ...
        groundReactions.childForces{i});
    groundReactionsBody = [groundReactionsBody ...
        groundReactions.parentForces{i} groundReactions.childForces{i} ...
        parentMoment childMoment];
end
end

function groundReactionsInLab = calcGroundReactionsLab(groundReactions)

for i = 1:length(groundReactions.parentForces)
    groundReactionsInLab.forces{i} = ...
        groundReactions.parentForces{i} + groundReactions.childForces{i};
    groundReactionsInLab.moments{i} = ...
        groundReactions.parentMoments{i} + groundReactions.childMoments{i};
end
end