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; }