/// <summary>
/// Update function of map. Calls Bayes filtering and handles the rotation and translation of all map objects.
/// </summary>
private void Update(object sender, EventArgs e)
{
lock (robot.mapLock)
{
Bayes(true, preySensor, ref preyProbability);
Bayes(false, obstacleSensor, ref obstacleProbability);
bBayes = new Bitmap(bMap.Size.Width, bMap.Size.Height);
SetNewRovioPosition();
// Run AStar if there is a suitable destination and draw it on the map.
using (graphics = Graphics.FromImage(bBayes))
{
AStar astar = new AStar(finalMap.GetLength(0), finalMap.GetLength(1));
astar.Build(finalMap, new DPoint(destination.X, destination.Y), new DPoint((picBoxRovio.Location.X / 10) + (picBoxRovio.Width / 10 / 2), picBoxRovio.Location.Y / 10));
aStarPath = astar.path;
for (int i = 0; i < maxX; i++)
{
for (int j = 0; j < maxY; j++)
{
graphics.FillRectangle(new SolidBrush(DColor.FromArgb((int)(preyProbability[i, j] * 255), System.Drawing.Color.DarkRed)), new DRectangle(i * 10, j * 10, 10, 10));
graphics.FillRectangle(new SolidBrush(DColor.FromArgb((int)(obstacleProbability[i, j] * 255), System.Drawing.Color.DarkBlue)), new DRectangle(i * 10, j * 10, 10, 10));
if (astar.inPath[i, j])
graphics.FillRectangle(new SolidBrush(DColor.Red), new DRectangle(i * 10, j * 10, 10, 10));
}
}
}
picBoxBayes.Image = bBayes;
// Check which cells are within the viewing cone.
if (viewConePoints != null)
{
for (int i = 0; i < maxX; i++)
{
for (int j = 0; j < maxY; j++)
{
DPoint a = new DPoint(i * 10 + 1, j * 10 + 1); // Top left
DPoint b = new DPoint((i + 1) * 10 - 1, j * 10 + 1); // Top right
DPoint c = new DPoint(i * 10 + 1, (j + 1) * 10 - 1); // Bottom left
DPoint d = new DPoint((i + 1) * 10 - 1, (j + 1) * 10 - 1); // Bottom right
if (!(PointInPolygon(a, viewConePoints) || PointInPolygon(b, viewConePoints)
|| PointInPolygon(c, viewConePoints) || PointInPolygon(d, viewConePoints)))
{
isCellVisible[i, j] = false;
}
else
{
preySensor[i, j] = false;
obstacleSensor[i, j] = false;
isCellVisible[i, j] = true;
}
}
}
}
if (robot.GetType() == typeof(Rovio.PredatorMap))
{
if (robot.IsPreySeen())
{
FindRelativeLocation(picBoxPrey, robot.preyRectangle, robot.GetPreyDistance(), 1, 1);
try
{
preySensor[(int)((picBoxPrey.Location.X / 10) + 1.5), (int)((picBoxPrey.Location.Y / 10) + 1.5)] = true;
} catch { }
}
else
picBoxPrey.Hide();
if ((robot as Rovio.BaseArena).IsObstacleSeen())
{
// Find obstacle position relative to the Rovio's position.
FindRelativeLocation(picBoxObstacle, robot.obstacleRectangle, robot.GetObstacleDistance(), 40, 3);
try
{
int p = (int)((picBoxObstacle.Location.X / 10) + 0.5);
int q = (int)((picBoxObstacle.Location.Y / 10) + 0.5);
// Populate 3x3 area with the obstacle.
for (int i = -1; i <= 1; i++)
for (int j = -1; j <= 1; j++)
obstacleSensor[(int)((picBoxObstacle.Location.X / 10) + i), (int)((picBoxObstacle.Location.Y / 10) + j)] = true;
}
catch { }
}
else
picBoxObstacle.Hide();
}
// Rotate the Rovio icon to the angle that the robot physically faces.
System.Drawing.Drawing2D.Matrix matrixRovio = new System.Drawing.Drawing2D.Matrix();
matrixRovio.RotateAt((float)robot.cumulativeAngle, new System.Drawing.Point(picBoxRovio.Location.X + (picBoxRovio.Size.Width / 2), picBoxRovio.Location.Y + (picBoxRovio.Size.Height / 2)));
matrixRovio.Translate(0f, -0f);
DPoint[] rovioMovementPoints = {new DPoint(picBoxRovio.Location.X+(picBoxRovio.Size.Width/2), picBoxRovio.Location.Y + (picBoxRovio.Size.Height/2)),
new DPoint(picBoxRovio.Location.X+(picBoxRovio.Size.Width/2) - 69, picBoxRovio.Location.Y-150),
new DPoint(picBoxRovio.Location.X+(picBoxRovio.Size.Width/2) + 69, picBoxRovio.Location.Y-150)};
matrixRovio.TransformPoints(rovioMovementPoints);
if ((robot as Rovio.BaseArena).IsObstacleSeen())
{
// Get the left and right points of the obstacle in the viewing cone and orientate accordingly.
DPoint leftPoint = new DPoint(picBoxObstacle.Location.X - (picBoxObstacle.Width / 2), picBoxObstacle.Location.Y - 15);
DPoint rightPoint = leftPoint;
using (matrix = new System.Drawing.Drawing2D.Matrix())
{
matrix.RotateAt((float)robot.cumulativeAngle, leftPoint);
matrix.Translate(0, -0f);
DPoint[] tempPointArr = { leftPoint };
matrix.TransformPoints(tempPointArr);
leftPoint = tempPointArr[0];
// Transform the right point relative to the position of the left.
matrix = new System.Drawing.Drawing2D.Matrix();
matrix.RotateAt((float)robot.cumulativeAngle, leftPoint);
matrix.Translate(30, 0);
tempPointArr[0] = new DPoint(0, 0);
matrix.TransformPoints(tempPointArr);
rightPoint = tempPointArr[0];
}
// Check if all points are still within the viewing cone. If not, skip over them.
if (PointInPolygon(leftPoint, rovioMovementPoints) && PointInPolygon(rightPoint, rovioMovementPoints))
rovioMovementPoints = new DPoint[] { rovioMovementPoints[0], rovioMovementPoints[1], leftPoint, rightPoint, rovioMovementPoints[2] };
}
viewConePoints = rovioMovementPoints;
Bitmap rotated = new Bitmap(bRovio.Width + 70, bRovio.Height + 70);
rotated.SetResolution(bRovio.HorizontalResolution, bRovio.VerticalResolution);
using (graphics = Graphics.FromImage(rotated))
{
graphics.TranslateTransform(bRovio.Width / 2, bRovio.Height / 2);
graphics.RotateTransform((float)robot.cumulativeAngle);
graphics.TranslateTransform(-bRovio.Width / 2, -bRovio.Height / 2);
graphics.DrawImage(bRovio, new DPoint(0, 0));
picBoxRovio.Size = new Size(27, 24);
picBoxRovio.Image = rotated;
picBoxCone.Location = new DPoint(picBoxRovio.Location.X - (bCone.Width / 2) + (bRovio.Width / 2), picBoxRovio.Location.Y - bCone.Height + (bRovio.Height / 2));
picBoxCone.Size = new Size(900, 500);
picBoxCone.Image = rotated;
picBoxCone.Location = new DPoint(0, 0);
picBoxCone.Size = new Size(260, 300);
}
Bitmap newCone = new Bitmap(260, 300);
using (graphics = Graphics.FromImage(newCone))
{
graphics.FillPolygon(new SolidBrush(DColor.FromArgb(100, DColor.ForestGreen)), viewConePoints);
picBoxCone.Image = newCone;
}
}
}