public void OnRotation(int angle)
{
double[] rotationPdf = ProbabilityHelper.NormalDistribution(angle / 90, RotationStd);
int offset = rotationPdf.Length / 2;
double[] kernel = new double[4];
for (int i = 0; i < rotationPdf.Length; i++)
{
kernel[MathHelper.GetOneTurn((i - offset) * 90) / 90] = rotationPdf[i];
}
double[] temp = new double[4];
for (int index = 0; index < _belief.Length; index += 4)
{
if (!IsFree(index))
{
continue;
}
MathHelper.TransposedCircularConvolution1d(index, _belief, kernel, temp);
temp.CopyTo(_belief, index);
}
MathHelper.SetLowerBound(_belief, MinProbability);
LocalizationUpdated.Invoke(this, new LocalizationEventArgs()
{
Belief = _belief
});
}
public void OnElevation(int relativeFloor)
{
double[] kernel = ProbabilityHelper.NormalDistribution(relativeFloor, ElevationStd);
int halfKernel = kernel.Length / 2;
Array.Clear(_tempPdf, 0, _tempPdf.Length);
int elevatorLabelIndex = Array.IndexOf(_mapLabels, StaticModel.ElevatorClassName);
for (int index = 0; index < _viewMap.Length; index++)
{
if (_viewMap[index] == elevatorLabelIndex)
{
int z = MathHelper.GetMultiDimensionalIndices(index, WorldShape)[2];
MathHelper.TransposedConvolution1d(_belief, index, z, _strides[2],
WorldShape[2], kernel, _tempPdf);
}
}
_tempPdf.CopyTo(_belief, 0);
MathHelper.SetLowerBound(_belief, MinProbability);
LocalizationUpdated.Invoke(this, new LocalizationEventArgs()
{
Belief = _belief
});
}
public NavigationSystem(World world)
{
Point upper = world.UpperBound;
WorldShape = new int[] { upper.X + 1, upper.Y + 1, upper.Z + 1, 4 };
_strides = MathHelper.GetIndexStrides(WorldShape);
int length = WorldShape[0] * WorldShape[1] * WorldShape[2] * WorldShape[3];
_belief = new double[length];
_viewMap = new int[length];
_occupancyMap = new int[length / 4];
_tempPdf = new double[length];
_gaussianKernel = ProbabilityHelper.NormalDistribution(0, _translationStd);
//Инициализация убеждения равномерным распределением
ProbabilityHelper.SetUniformDistribution(_belief);
FillMaps(world);
}