public static float GetFreeSpace()
{
if (ObstacleMap != null)
{
PositionInfo offset = Robot.Radar.AntennaPosition;
PositionInfo position = Robot.Position;
float phi = position.Angle / 180.0f * (float)Math.PI;
PositionInfo radarPos = new PositionInfo(
position.X + offset.X * (float)Math.Cos(phi) - offset.Y * (float)Math.Sin(phi),
position.Y + offset.X * (float)Math.Sin(phi) + offset.Y * (float)Math.Cos(phi),
(position.Angle + offset.Angle) % 360);
return (float)ObstacleMap.GetFreeSpace(radarPos);
}
else return 2.55f;
}
public DriveInfo(PositionInfo position,
float runtime,
float speedL, float speedR,
float distanceL, float distanceR,
int driveStatus,
int motorStatusL, int motorStatusR
)
{
Position = position;
Runtime = runtime;
SpeedL = speedL;
SpeedR = speedR;
DistanceL = distanceL;
DistanceR = distanceR;
DriveStatus = driveStatus;
MotorStatusL = motorStatusL;
MotorStatusR = motorStatusR;
}
/// <summary>
/// Aktualisiert die View, d.h. es wird in ein Bitmap gezeichnet und
/// anschliessend das Bitmap in der PictureBox dargestellt.
/// Folgende Objekte werden gezeichnet:
/// - Hindernis
/// - Koordinaten-Netz
/// - Roboter
/// - Radar
/// </summary>
private void UpdateView()
{
// Verhindert Exception falls Fenster auf 0 verkleinert wird.
if (pictureBox.Width == 0 || pictureBox.Height == 0) return;
// Verhindert Designer-Absturz falls ViewPort auf 0 gesetzt wird.
if (viewPort.Width == 0 || viewPort.Height == 0) return;
// Bitmap erstellen auf das die WorldView gezeichnet werden kann
if ((plot == null) || (plot.Size != pictureBox.Size))
{
plot = new Bitmap(pictureBox.Width, pictureBox.Height);
}
using (Graphics g = Graphics.FromImage(plot))
{
// Hintergrund löschen
g.Clear(Color.White);
#region Hindernisse zeichnen
ObstacleMap obstMap = World.ObstacleMap;
if (World.ObstacleMap != null)
{
Bitmap bmp = obstMap.Image;
RectangleF area = obstMap.Area;
int rx1 = XtoScreen(area.Left);
int ry1 = YtoScreen(area.Bottom);
int rx2 = XtoScreen(area.Right);
int ry2 = YtoScreen(area.Top);
g.DrawImage(
World.ObstacleMap.Image,
new Rectangle(rx1, ry1, rx2 - rx1, ry2 - ry1),
new Rectangle(0, 0, bmp.Width, bmp.Height),
GraphicsUnit.Pixel);
}
#endregion
#region Koordinaten-Netz zeichnen
//draw vertical grid lines
for (double x = viewPort.xMin-(viewPort.xMin%0.5); x <= viewPort.xMax; x = x + 0.5)
{
g.DrawLine((x == 0 ? penGrid1 : penGrid2),
Convert.ToInt32(XtoScreen(x)),
Convert.ToInt32(YtoScreen(viewPort.yMax)),
Convert.ToInt32(XtoScreen(x)),
Convert.ToInt32(YtoScreen(viewPort.yMin)));
}
//draw horizontal grid lines
for (double y = viewPort.yMin - (viewPort.yMin % 0.5); y <= viewPort.yMax; y = y + 0.5)
{
g.DrawLine((y == 0 ? penGrid1 : penGrid2),
Convert.ToInt32(XtoScreen(viewPort.xMin)),
Convert.ToInt32(YtoScreen(y)),
Convert.ToInt32(XtoScreen(viewPort.xMax)),
Convert.ToInt32(YtoScreen(y)));
}
#endregion
#region Roboter
Robot robot = World.Robot;
if (robot != null)
{
PositionInfo robotPosition = robot.Drive.Position;
#region Parkplatz zeichnen
if (robot.Finished && !robot.Running)
{
//double _parkingX = robotPosition.X - Math.Abs(robotPosition.X % 0.5);
//double _parkingY = robotPosition.Y + Math.Abs(robotPosition.Y % 0.5);
double _parkingX = Convert.ToInt32(robotPosition.X / 0.5) * 0.5;
double _parkingY = Convert.ToInt32(robotPosition.Y / 0.5) * 0.5;
g.FillRectangle(new SolidBrush(Color.LightBlue), XtoScreen(_parkingX), YtoScreen(_parkingY), WidthToScreen(0.5f), HeightToScreen(0.5f));
}
#endregion
#region Roboterstrecke zeichnen
PositionInfo[] _posInfo = new PositionInfo[robot.positions.Capacity];
robot.positions.CopyTo(_posInfo);
foreach(PositionInfo _info in _posInfo) {
g.FillEllipse(new SolidBrush(robot.Color), XtoScreen(_info.X) - 2, YtoScreen(_info.Y) - 2, 5, 5);
}
#endregion
#region Roboter zeichnen
g.FillEllipse(new SolidBrush(robot.Color), XtoScreen(robotPosition.X - robot.Width), YtoScreen(robotPosition.Y + robot.Height), WidthToScreen(robot.Width*2), HeightToScreen(robot.Height*2));
#endregion
#region Radar zeichnen
// Roboter.Radar
float phi = robot.Drive.Position.Angle;
PositionInfo radarOffset = robot.Radar.AntennaPosition;
PositionInfo radarPos = new PositionInfo(
robotPosition.X + radarOffset.X * (float)Math.Cos(phi) - radarOffset.Y * (float)Math.Sin(phi),
robotPosition.Y + radarOffset.X * (float)Math.Sin(phi) + radarOffset.Y * (float)Math.Cos(phi),
(robotPosition.Angle + radarOffset.Angle) % 360);
double radarPhi = radarPos.Angle / 180.0 * Math.PI;
double distance = robot.Radar.Distance;
// Radarstrahl zeichnen...
g.DrawLine(penRadar, XtoScreen(radarPos.X), YtoScreen(radarPos.Y),
XtoScreen(radarPos.X + distance * Math.Cos(radarPhi)),
YtoScreen(radarPos.Y + distance * Math.Sin(radarPhi)));
#endregion
}
#endregion
}
pictureBox.Image = plot;
}
/// <summary>
/// Liefert die Information, wie weit das nächste Hindernist entfernt ist.
/// </summary>
/// <param name="position">die aktuelle (eigene) Position inkl. Blickrichtung</param>
/// <returns>Die Distanz zum nächsten Hindernis in Blickrichtung</returns>
public double GetFreeSpace(PositionInfo position)
{
lock (SyncRoot)
{
// Hindernis-Suche mit Bresenham-Algorithmus
int x1 = xToIndex(position.X);
int y1 = yToIndex(position.Y);
int x2 = xToIndex(position.X + maxLength * Math.Cos(position.Angle / 180 * Math.PI));
int y2 = yToIndex(position.Y + maxLength * Math.Sin(position.Angle / 180 * Math.PI));
int dx = x2 - x1;
int dy = y2 - y1;
int absDx = Math.Abs(dx);
int absDy = Math.Abs(dy);
int incX = Math.Sign(dx);
int incY = Math.Sign(dy);
int x = x1, y = y1, err = 0;
if (absDx >= absDy)
{
err = -absDx / 2;
for (x = x1; x != x2; x = x + incX)
{
if ((x >= 0) && (x < imageWidth) &&
(y >= 0) && (y < imageHeight) &&
obstaclePixel[y, x])
break;
else
{
err += absDy;
if (err >= 0)
{
y += incY;
err -= absDx;
}
}
}
}
else
{
err = -absDy / 2;
for (y = y1; y != y2; y = y + incY)
{
if ((x >= 0) && (x < imageWidth) &&
(y >= 0) && (y < imageHeight) &&
obstaclePixel[y, x])
break;
else
{
err += absDx;
if (err >= 0)
{
x += incX;
err -= absDy;
}
}
}
}
double xSpace = (x - x1) * area.Width / imageWidth;
double ySpace = (y - y1) * area.Height / imageHeight;
return Math.Sqrt(xSpace * xSpace + ySpace * ySpace);
}
}
/// <summary>
/// Aktualisiert die View, d.h. es wird in ein Bitmap gezeichnet und
/// anschliessend das Bitmap in der PictureBox dargestellt.
/// Folgende Objekte werden gezeichnet:
/// - Hindernis
/// - Koordinaten-Netz
/// - Roboter
/// - Radar
/// </summary>
private void UpdateView()
{
// Verhindert Exception falls Fenster auf 0 verkleinert wird.
if (pictureBox.Width == 0 || pictureBox.Height == 0) return;
// Verhindert Designer-Absturz falls ViewPort auf 0 gesetzt wird.
if (viewPort.Width == 0 || viewPort.Height == 0) return;
// Bitmap erstellen auf das die WorldView gezeichnet werden kann
if ((plot == null) || (plot.Size != pictureBox.Size))
{
plot = new Bitmap(pictureBox.Width, pictureBox.Height);
}
using (Graphics g = Graphics.FromImage(plot))
{
// Hintergrund löschen
g.Clear(Color.White);
// Map
ObstacleMap obstMap = World.ObstacleMap;
if (World.ObstacleMap != null)
{
Bitmap bmp = obstMap.Image;
RectangleF area = obstMap.Area;
int rx1 = XtoScreen(area.Left);
int ry1 = YtoScreen(area.Bottom );
int rx2 = XtoScreen(area.Right );
int ry2 = YtoScreen(area.Top );
g.DrawImage(
bmp,
new Rectangle(rx1, ry1, (rx2 - rx1), (ry2 - ry1)),
new Rectangle(0, 0, bmp.Width, bmp.Height),
GraphicsUnit.Pixel);
}
#region Koordinaten-Netz zeichnen
// TODO Grid zeichnen
// Vertikale Linien die >viewPort.xMin bzw. <viewPort.xMax sind zeichnen...
for (int i = (int)Math.Ceiling(viewPort.xMin); i <( (int)Math.Floor(this.viewPort.xMax))*2; i++)
{
Pen pen = i == 0 ? penGrid0 : penGrid2;
g.DrawLine(pen, XtoScreen(i/2.0f), 0, XtoScreen(i/2.0f), this.Height);
}
// Horizontale Linien >viewPort.yMin bzw. <viewPort.yMax sind zeichnen...
for (int i = (int)Math.Ceiling(viewPort.yMin); i < ((int)Math.Floor(this.viewPort.yMax))*2; i++)
{
Pen pen = i == 0 ? penGrid1 : penGrid2;
g.DrawLine(pen, 0, YtoScreen(i/2.0f), this.Width, YtoScreen(i/2.0f));
}
#endregion
Robot robot = World.Robot;
double phi = (robot.Position.Angle) * (Math.PI / 180.0);
if (robot != null)
{
#region Roboter zeichnen
g.FillEllipse(new SolidBrush(robot.Color),
(int)(this.XtoScreen(robot.Position.X) - (this.WidthToScreen(Constants.Width) / 2.0)),
(int)(this.YtoScreen(robot.Position.Y) - (this.HeightToScreen(Constants.Width) / 2.0)),
this.WidthToScreen(Constants.Width),
this.HeightToScreen(Constants.Width)
);
g.DrawLine(new Pen(Color.Black, 1.0f),
(int)this.XtoScreen(robot.Position.X), //+ (int)(this.WidthToScreen(Constants.Width) / 2.0),
(int)this.YtoScreen(robot.Position.Y), //+ (int)(this.HeightToScreen(Constants.Width) / 2.0),
(int)(this.XtoScreen(robot.Position.X) + Math.Cos(phi) * (this.WidthToScreen(Constants.Width) / 2.0)),
(int)(this.YtoScreen(robot.Position.Y) + Math.Sin(phi) * (this.HeightToScreen(Constants.Width) / 2.0))
);
#endregion
}
// Roboter.Radar
PositionInfo radarOffset = robot.Radar.AntennaPosition;
PositionInfo pos = robot.Position;
PositionInfo radarPos = new PositionInfo(
pos.X + radarOffset.X * (float)Math.Cos(phi) - radarOffset.Y * (float)Math.Sin(phi),
pos.Y + radarOffset.X * (float)Math.Sin(phi) + radarOffset.Y * (float)Math.Cos(phi),
(pos.Angle + radarOffset.Angle) % 360);
double radarPhi = radarPos.Angle / 180.0 * Math.PI;
double distance = robot.Radar.Distance;
// Radarstrahl zeichnen...
g.DrawLine(penRadar, XtoScreen(radarPos.X), YtoScreen(radarPos.Y),
XtoScreen(radarPos.X + distance * Math.Cos(radarPhi)),
YtoScreen(radarPos.Y + distance * Math.Sin(radarPhi)));
//Pfad zeichnen
PositionInfo[] path = robot.GetPath();
Pen penPath = new Pen(Color.Blue,1);
for (int i = 0; i < path.Length-1; i++)
{
int x1 = XtoScreen(path[i].X);
int y1 = YtoScreen(path[i].Y);
int x2 = XtoScreen(path[i + 1].X);
int y2 = YtoScreen(path[i + 1].Y);
g.DrawLine(penPath, x1, y1, x2, y2);
}
}
pictureBox.Image = plot;
}
public string getLine()
{
PositionInfo pos = cmd.getPosition();
return(DateTime.Now.ToString("dd/MM/yyyy-hh:mm:ss.fff") + ";" + pos.X + ";" + pos.Y);
}
