calcSpringConstantsErrorFromNeighbors.m

% This function is part of the NMSM Pipeline, see file for full license.
%
%
%
% (Array of double, struct, struct) -> (struct)
% Optimize ground contact parameters according to Jackson et al. (2016)


function error = calcSpringConstantsErrorFromNeighbors(springConstants, ...
    gaussianWeights)
error = zeros(1, size(gaussianWeights, 1) * size(gaussianWeights, 2));
for i = 1:size(gaussianWeights, 1)
    for j = 1:size(gaussianWeights, 2)
        totalNeighborSpringValue = 0;
        totalNeighborWeight = 0;
        for k = 1:size(gaussianWeights, 3)
            totalNeighborSpringValue = totalNeighborSpringValue + ...
                gaussianWeights(i, j, k) * springConstants(k);
            totalNeighborWeight = totalNeighborWeight + ...
                gaussianWeights(i, j, k);
        end
        error((i - 1) * size(gaussianWeights, 2) + j) = ...
            springConstants(j) - ...
            (totalNeighborSpringValue / totalNeighborWeight);
    end
end
end