calcFinalPointPosition.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the final position for the specified
% point on the specified body. The difference between the final point 
% position and target positon is calculated.
%
% (struct, struct, struct) -> (Number)
% 


function finalPointPositionErrorMag = calcFinalPointPosition(auxdata, ...
    values, constraintTerm)

pointPosition = calcBodyLocation(values, str2num(constraintTerm.point), ...
    constraintTerm.body, auxdata);

finalPointPositionError = pointPosition(end, :) - ...
    str2num(constraintTerm.target_position);
finalPointPositionErrorMag = norm(finalPointPositionError);
end