private double[] CalculateDistanceToBeacons(GameObject gameObject, TestFieldModel testFieldModel)
{
var beacons = testFieldModel.GameObjects.Where(x => x is Beacon);
var distances = beacons.Select(beacon => CalculateDistanceToBeacon(gameObject, beacon)).ToArray();
return(distances);
}
public virtual void MakeChanges(TestFieldModel testFieldModel)
{
var possibleRobbot = testFieldModel.GameObjects.FirstOrDefault(go => go is Robot);
if (possibleRobbot != null)
{
var robot = (Robot)possibleRobbot;
robot.Angle += _deltaAngle;
robot.Angle %= 360;
var deltaPositionX = (int)(_deltaPosition * Math.Sin(robot.Angle * Math.PI / 180));
var deltaPositionY = (int)(_deltaPosition * Math.Cos(robot.Angle * Math.PI / 180));
robot.X += deltaPositionX;
robot.Y += deltaPositionY;
robot.X = robot.X < 0 ?
0 :
robot.X >= testFieldModel.Width ?
testFieldModel.Width :
robot.X;
robot.Y = robot.Y < 0 ?
0 :
robot.Y >= testFieldModel.Height ?
testFieldModel.Height :
robot.Y;
}
}
private void KillTheWeakHypotheses(TestFieldModel testFieldModel)
{
var hypotheses = testFieldModel.GameObjects
.Where(x => x is Hypothesis)
.Select(x => x as Hypothesis)
.ToList();
var amountToKill = (int)(hypotheses.Count * 0.75);
var killed = hypotheses.OrderBy(h => h.Weight).Take(amountToKill);
testFieldModel.GameObjects = testFieldModel.GameObjects.Except(killed).ToList();
}
public override void MakeChanges(TestFieldModel testFieldModel)
{
lock (_locker) {
base.MakeChanges(testFieldModel);
UpdateHypotheses(testFieldModel);
KillBadHypotheses(testFieldModel);
if (counter == 0)
{
UpdateWeights(testFieldModel);
KillTheWeakHypotheses(testFieldModel);
}
GenerateSomeNewHypotheses(testFieldModel);
counter = (counter + 1) % amountOfStepsBetweenKillingSprees;
}
}
private void KillBadHypotheses(TestFieldModel testFieldModel)
{
var badHypotheses = testFieldModel
.GameObjects
.Where(x => x is Hypothesis)
.Where(h => h.X < 0 ||
h.X >= testFieldModel.Width ||
h.Y < 0 ||
h.Y >= testFieldModel.Height)
.ToList();
testFieldModel.GameObjects = testFieldModel
.GameObjects
.Except(badHypotheses)
.ToList();
}
private void UpdateHypotheses(TestFieldModel testFieldModel)
{
foreach (var h in testFieldModel.GameObjects.Where(x => x is Hypothesis))
{
var hypothesis = h as Hypothesis;
hypothesis.Angle += _deltaAngle;
hypothesis.Angle %= 360;
var deltaPositionX = (int)(_deltaPosition * Math.Sin(hypothesis.Angle * Math.PI / 180));
var deltaPositionY = (int)(_deltaPosition * Math.Cos(hypothesis.Angle * Math.PI / 180));
hypothesis.X += deltaPositionX;
hypothesis.Y += deltaPositionY;
}
}
private void GenerateHypothesesFromScratch(TestFieldModel testFieldModel)
{
var random = new Random();
var hypotheses = new List <Hypothesis>();
for (int i = 0; i < maxHypothesisCount; i++)
{
var newX = (int)(random.NextDouble() * testFieldModel.Width);
var newY = (int)(random.NextDouble() * testFieldModel.Height);
var newAngle = (int)(random.NextDouble() * 360 - 180);
var hypothesis = new Hypothesis(newX, newY, newAngle);
hypothesis.Weight = 1 / maxHypothesisCount;
hypotheses.Add(hypothesis);
}
testFieldModel.GameObjects.AddRange(hypotheses);
}
private void GenerateSomeNewHypotheses(TestFieldModel testFieldModel)
{
var hypotheses = testFieldModel.GameObjects
.Where(x => x is Hypothesis)
.Select(x => x as Hypothesis)
.ToList();
if (hypotheses.Count == 0)
{
GenerateHypothesesFromScratch(testFieldModel);
}
else
{
RegenerateHypotheses(testFieldModel, hypotheses);
}
}
private double GetNewWeight(TestFieldModel testFieldModel, Hypothesis hypothesis, double[] distancesFromSensors)
{
double result = 1;
var distancesForHypothesis = CalculateDistanceToBeacons(hypothesis, testFieldModel);
for (int i = 0; i < distancesFromSensors.Length; i++)
{
var expected = distancesFromSensors[i];
if (expected < 0)
{
continue;
}
var recieved = distancesForHypothesis[i];
result *= GetWeightFromNormalDistribution(recieved, expected, sensorsEps);
}
return(result);
}
private void UpdateWeights(TestFieldModel testFieldModel)
{
var possibleRobot = testFieldModel.GameObjects.FirstOrDefault(go => go is Robot) as Robot;
var distances = CalculateDistanceToBeacons(possibleRobot, testFieldModel);
distances = distances.Select(d => random.NextGaussian(d, sensorsEps + sensorsEpsCalculatedDelta)).ToArray();
distances = distances.Select(d => d > beaconRadius ? -1 : d).ToArray();
var hypotheses = testFieldModel.GameObjects
.Where(x => x is Hypothesis)
.Select(x => x as Hypothesis)
.ToList();
foreach (var hypothesis in hypotheses)
{
hypothesis.Weight = GetNewWeight(testFieldModel, hypothesis, distances);
}
}
public void MakeChanges(TestFieldModel testFieldModel)
{
foreach (var gameObject in testFieldModel.GameObjects)
{
var newPositionX = gameObject.X + _deltaX;
var newPositionY = gameObject.Y + _deltaY;
newPositionX = newPositionX < 0 ?
0 :
newPositionX >= testFieldModel.Width ?
testFieldModel.Width :
newPositionX;
newPositionY = newPositionY < 0 ?
0 :
newPositionY >= testFieldModel.Height ?
testFieldModel.Height :
newPositionY;
gameObject.X = newPositionX;
gameObject.Y = newPositionY;
}
}
private void RegenerateHypotheses(TestFieldModel testFieldModel, List <Hypothesis> existingHypotheses)
{
var random = new Random();
var amountToCreate = maxHypothesisCount - existingHypotheses.Count;
var hypotheses = new List <Hypothesis>();
for (int i = 0; i < amountToCreate; i++)
{
var parentIndex = random.Next(existingHypotheses.Count);
var parent = existingHypotheses[parentIndex];
var newX = (int)(random.NextGaussian(parent.X, 3));
var newY = (int)(random.NextGaussian(parent.Y, 3));
var newAngle = (int)(random.NextGaussian(parent.Angle, 3));
var hypothesis = new Hypothesis(newX, newY, newAngle);
hypotheses.Add(hypothesis);
}
testFieldModel.GameObjects.AddRange(hypotheses);
}
public void MakeChanges(TestFieldModel testFieldModel)
{
foreach (var gameObject in testFieldModel.GameObjects)
{
var xDelta = _random.Next(-8, 9);
var yDelta = _random.Next(-8, 9);
var newPositionX = gameObject.X + xDelta;
var newPositionY = gameObject.Y + yDelta;
newPositionX = newPositionX < 0 ?
0 :
newPositionX >= testFieldModel.Width ?
testFieldModel.Width :
newPositionX;
newPositionY = newPositionY < 0 ?
0 :
newPositionY >= testFieldModel.Height ?
testFieldModel.Height :
newPositionY;
gameObject.X = newPositionX;
gameObject.Y = newPositionY;
}
}
public void MakeChanges(TestFieldModel testFieldModel)
{
var possibleRobbot = testFieldModel.GameObjects.FirstOrDefault(go => go is Robot);
if (possibleRobbot != null)
{
var robot = (Robot)possibleRobbot;
robot.Angle += _deltaAngle;
robot.Angle %= 360;
var deltaPositionX = (int)(_deltaPosition * Math.Sin(robot.Angle * Math.PI / 180));
var deltaPositionY = (int)(_deltaPosition * Math.Cos(robot.Angle * Math.PI / 180));
robot.X += deltaPositionX;
robot.Y += deltaPositionY;
robot.X = robot.X < 0 ?
0 :
robot.X >= testFieldModel.Width ?
testFieldModel.Width :
robot.X;
robot.Y = robot.Y < 0 ?
0 :
robot.Y >= testFieldModel.Height ?
testFieldModel.Height :
robot.Y;
}
var possibleOdometry = testFieldModel.GameObjects.FirstOrDefault(go => go is OdometerEstimation);
if (possibleOdometry != null)
{
var estimation = (OdometerEstimation)possibleOdometry;
var deltaAngle = _deltaAngle + (rand.NextDouble() - 1) * 0.03 * _angleStep;
estimation.Angle += deltaAngle;
estimation.Angle %= 360;
var deltaPosition = _deltaPosition + (rand.NextDouble() - 1) * 0.05 * _positionStep;
var deltaPositionX = (int)(deltaPosition * Math.Sin(estimation.Angle * Math.PI / 180));
var deltaPositionY = (int)(deltaPosition * Math.Cos(estimation.Angle * Math.PI / 180));
estimation.X += deltaPositionX;
estimation.Y += deltaPositionY;
estimation.X = estimation.X < 0 ?
0 :
estimation.X >= testFieldModel.Width ?
testFieldModel.Width :
estimation.X;
estimation.Y = estimation.Y < 0 ?
0 :
estimation.Y >= testFieldModel.Height ?
testFieldModel.Height :
estimation.Y;
var oldX = estimation.X;
var oldY = estimation.Y;
var oldAngle = estimation.Angle;
}
}
