Хорошо, я просто сгладил каждую координату независимо, и она работает:
void PathControllerGaussian::smooth_path_gaussian(FuturePath & path, unsigned future_steps)
{
smooth_dimension_gaussian(path.pos, 0, future_steps);
smooth_dimension_gaussian(path.pos, 1, future_steps);
smooth_dimension_gaussian(path.pos, 2, future_steps);
}
void PathControllerGaussian::smooth_dimension_gaussian(PathVec & vec, unsigned dim_index, unsigned vec_size)
{
float sum;
for (unsigned i = gaussian_kernel_size / 2; i < vec_size - gaussian_kernel_size / 2; ++i)
{
sum = 0;
for (int j = 0; j < gaussian_kernel_size; ++j)
{
sum += vec[i + j - gaussian_kernel_size / 2][dim_index] * gaussian_kernel[j];
}
vec[i][dim_index] = sum;
}
}
float PathControllerGaussian::gaussian(float value, float sigma)
{
return 1 / sqrt(2 * M_PI * sigma * sigma) * std::exp(- (value * value) / (2 * sigma * sigma));
}
void PathControllerGaussian::prepare_gaussian_kernel(unsigned size, float sigma)
{
gaussian_kernel_size = size;
gaussian_sigma = sigma;
gaussian_kernel.clear();
int mid_point = gaussian_kernel_size / 2;
for (int i = 0; i < gaussian_kernel_size; ++i)
{
gaussian_kernel.push_back(gaussian(i - mid_point, gaussian_sigma));
}
}