public void UpdateSensordata(MapElementStatus[,] arrMap, Point posRobot)
{
int widthSensorMap = arrMap.GetLength(0);
int heightSensorMap = arrMap.GetLength(1);
int offsetLeft = (widthSensorMap - 800) / 2;
int offsetTop = (heightSensorMap - 600) / 2;
for (int i = offsetLeft; i < 800 + offsetLeft; i++)
{
for (int j = offsetTop; j < 600 + offsetTop; j++)
{
// nur überschreiben wenn Undiscovered
if (_map[i - offsetLeft, j - offsetTop] == MapElementStatus.Undiscovered)
{
MapElementStatus status = offsetLeft < 0 || offsetTop < 0 ? MapElementStatus.Undiscovered : arrMap[i, j];
if (status == MapElementStatus.TargetShadowed)
{
}
if (_conversationDictionary.Keys.Any(k => k == status))
{
_map[i - offsetLeft, j - offsetTop] = _conversationDictionary[status];
}
}
}
}
}
public void Dispose()
{
_map = new MapElementStatus[800, 600];
_waypointActive = null;
}
public bool PathAvailable(Point target, Point positionRobot)
{
double shiftX = target.X - positionRobot.X;
double shiftY = target.Y - positionRobot.Y;
int dirX = shiftX > 0 ? 1 : -1;
int dirY = shiftY > 0 ? 1 : -1;
shiftX = Math.Abs(shiftX);
shiftY = Math.Abs(shiftY);
double stepX;
double stepY;
int loopCount = 0;
if (shiftX >= shiftY)
{
loopCount = (int)shiftX;
stepX = 1 * dirX;
stepY = (1 / shiftX * shiftY) * dirY;
}
else
{
loopCount = (int)shiftY;
stepX = (1 / shiftY * shiftX) * dirX;
stepY = 1 * dirY;
}
bool wayClear = true;
// step through array to goal (direct line) and check if there is a collision on its way
/*
* R x 0 0 0 0 0 0 0
* 0 0 x x 0 0 0 0 0
* 0 0 0 0 x x 0 0 0
* 0 0 0 0 0 0 x x 0
* 0 0 0 0 0 0 0 0 T
*
* x = path
*/
double x = positionRobot.X;
double y = positionRobot.Y;
for (int i = 0; i < loopCount; i++)
{
x += stepX;
y += stepY;
MapElementStatus status = GetStatus((int)x, (int)y);
if (status == MapElementStatus.Blocked || status == MapElementStatus.BlockedShadowed)
{
wayClear = false;
break;
}
}
return wayClear;
}
