void canvas_MouseMove(object sender, MouseEventArgs mea)
{
try
{
Point pos = mea.GetPosition(canvasRel);
// first get it in pixels, relative to center point:
double x = pos.X - canvasRel.ActualWidth / 2.0d;
double y = -(pos.Y - canvasRel.ActualHeight / 2.0d);
// and now convert to meters, relative to robot center:
x *= metersPerPixelX;
y *= metersPerPixelY;
MapperVicinity mapperVicinity = CurrentMapper;
GeoPosition gp = new GeoPosition(mapperVicinity.robotPosition);
gp.translate(new Distance(x), new Distance(y));
RoutedEventArgsMouseMoved newEventArgs = new RoutedEventArgsMouseMoved(MapperViewControl.MouseMovedEvent)
{
xMeters = x, yMeters = y, geoPosition = gp
};
RaiseEvent(newEventArgs);
}
catch { }
}
public RoutePlanner(MapperVicinity mapper)
{
_mapper = mapper;
nW = MapperSettings.nW;
nH = MapperSettings.nH;
pathDistanceMax = 0.9d * MapperSettings.elementSizeMeters * Math.Min(nW, nH) / 2.0d; // try not to hit walls - that's why 0.9d
}
private void buttonRight_Click(object sender, RoutedEventArgs e)
{
MapperVicinity mv = mapperViewControl1.CurrentMapper;
mv.translate(0.3048d, 0.0d);
planRoute();
}
public RoutePlanner(MapperVicinity mapper)
{
_mapper = mapper;
nW = MapperSettings.nW;
nH = MapperSettings.nH;
pathDistanceMax = 0.9d * MapperSettings.elementSizeMeters * Math.Min(nW, nH) / 2.0d; // try not to hit walls - that's why 0.9d
}
private void doVicinityPlanningCheckBox_CheckedUnchecked(object sender, RoutedEventArgs e)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if (_mapperVicinity != null)
{
_mapperVicinity.robotState.doVicinityPlanning = (bool)doVicinityPlanningCheckBox.IsChecked;
}
}
private void buttonReset_Click(object sender, RoutedEventArgs e)
{
MapperVicinity mv = new MapperVicinity();
mapperViewControl1.CurrentMapper = mv;
mapperViewControl1.CurrentRoutePlanner = new RoutePlanner(mv);
mapperViewControl1.setRobotPositionAndDirection(robotPositionDefault, robotDirectionDefault);
}
private void manualControlButtonTurn_Click(object sender, RoutedEventArgs e)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if (_mapperVicinity != null)
{
_mapperVicinity.robotState.manualControlCommand = "turn";
}
}
private void buttonReset_Click(object sender, RoutedEventArgs e)
{
MapperVicinity mv = new MapperVicinity();
mapperViewControl1.CurrentMapper = mv;
mapperViewControl1.CurrentRoutePlanner = new RoutePlanner(mv);
mapperViewControl1.setRobotPositionAndDirection(robotPositionDefault, robotDirectionDefault);
}
private void rotateToNorthUp(bool northUp)
{
MapperVicinity mapperVicinity = CurrentMapper;
TransformGroup geort = null;
TransformGroup robotrt = null;
if (northUp)
{
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double geoCellWidth = frameWidth / mapperVicinity.GeoMapWidth;
double geoCellHeight = frameHeight / mapperVicinity.GeoMapHeight;
geort = new TransformGroup();
robotrt = new TransformGroup();
double angle = 0.0d; // degrees
if (mapperVicinity.robotDirection != null && mapperVicinity.robotDirection.heading.HasValue)
{
angle = (double)mapperVicinity.robotDirection.heading;
}
double arads = angle * Math.PI / 180.0d;
double asin = Math.Abs(Math.Sin(arads));
double acos = Math.Abs(Math.Cos(arads));
double scaleY = (geoCellWidth * asin + geoCellHeight * acos) / (geoCellHeight * (asin + acos));
double scaleX = (geoCellWidth * acos + geoCellHeight * asin) / (geoCellWidth * (asin + acos));
RotateTransform rt = new RotateTransform(angle, 0.0d, 0.0d);
TranslateTransform tt1 = new TranslateTransform(-canvasRel.ActualWidth / 2.0d, -canvasRel.ActualHeight / 2.0d);
TranslateTransform tt2 = new TranslateTransform(canvasRel.ActualWidth / 2.0d, canvasRel.ActualHeight / 2.0d);
ScaleTransform st = new ScaleTransform(scaleX, scaleY);
geort.Children.Add(tt1);
geort.Children.Add(st);
geort.Children.Add(rt);
geort.Children.Add(tt2);
RotateTransform rrt = new RotateTransform(angle, 0.0d, 0.0d);
TranslateTransform rtt1 = new TranslateTransform(-canvasRel.ActualWidth / 2.0d, -canvasRel.ActualHeight / 2.0d);
TranslateTransform rtt2 = new TranslateTransform(canvasRel.ActualWidth / 2.0d, canvasRel.ActualHeight / 2.0d);
robotrt.Children.Add(rtt1);
robotrt.Children.Add(rrt);
robotrt.Children.Add(rtt2);
}
canvasRel.RenderTransform = geort;
robotPanel.RenderTransform = robotrt;
}
private void ignoreKinectSkeletonsCheckBox_CheckedUnchecked(object sender, RoutedEventArgs e)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if (_mapperVicinity != null)
{
_mapperVicinity.robotState.ignoreKinectSkeletons = (bool)ignoreKinectSkeletonsCheckBox.IsChecked;
}
}
private void comboBoxStrategy_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if (_mapperVicinity != null)
{
_mapperVicinity.robotState.robotStrategyType = (RobotStrategyType)comboBoxStrategy.SelectedValue;
}
}
public void setMapper(MapperVicinity mapper, RoutePlanner routePlanner)
{
if (mapWindow != null)
{
_mapperVicinity = mapper;
_routePlanner = routePlanner;
mapWindow.mapperViewControl1.CurrentMapper = mapper;
mapWindow.mapperViewControl1.CurrentRoutePlanner = routePlanner;
}
}
private void manualControlCheckBox_CheckedUnchecked(object sender, RoutedEventArgs e)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if (_mapperVicinity != null)
{
_mapperVicinity.robotState.manualControl = (bool)manualControlCheckBox.IsChecked;
TurnState currentTurnState = _mapperVicinity.turnState;
currentTurnState.directionDesired = null;
}
}
private void drawTurnState(List <UIElement> geoList, List <UIElement> unrotatedList)
{
MapperVicinity mapperVicinity = CurrentMapper;
TurnState currentTurnState = mapperVicinity.turnState;
bool inTurn = currentTurnState != null && currentTurnState.inTurn;
string ts = inTurn ? currentTurnState.ToString() : "not in turn";
double x0 = 20.0d;
double y0 = 50.0d;
Label lblTurn = new Label()
{
Content = ts, Margin = new Thickness(x0, y0, 0, 0), FontSize = 12, FontWeight = FontWeights.Normal
};
unrotatedList.Add(lblTurn);
try
{
string controlName = "TurnStateViewControl1";
TurnStateViewControl rsvc = turnPanel.FindName(controlName) as TurnStateViewControl;
if (rsvc == null)
{
double relCellWidth = canvasRel.ActualWidth / mapperVicinity.RelMapWidth;
double relCellHeight = canvasRel.ActualHeight / mapperVicinity.RelMapHeight;
double controlWidth = 300;
double controlHeight = 300;
double controlX = (turnPanel.ActualWidth - controlWidth) / 2.0d;
double controlY = (turnPanel.ActualHeight - controlHeight) / 2.0d;
rsvc = new TurnStateViewControl()
{
Name = controlName, turnState = currentTurnState
};
rsvc.Width = controlWidth;
rsvc.Height = controlHeight;
rsvc.Margin = new Thickness(controlX, controlY, 0, 0);
turnPanel.Children.Add(rsvc);
turnPanel.RegisterName(rsvc.Name, rsvc);
}
else
{
rsvc.turnState = currentTurnState;
rsvc.redraw();
//Console.WriteLine(string.Format("draw() - using existing control - scale: power={0} speed={1}", rsvc.maxAbsPower, rsvc.maxAbsSpeed));
}
}
catch { }
}
private void drawAll()
{
if (this.IsLoaded)
{
List <UIElement> relList = new List <UIElement>();
List <UIElement> geoList = new List <UIElement>();
bool northUp = (bool)northUpCheckBox.IsChecked;
List <UIElement> unrotatedList = northUp ? geoList : relList;
mapperVicinity = CurrentMapper;
labelLat.Content = mapperVicinity.robotPosition.Lat;
labelLng.Content = mapperVicinity.robotPosition.Lng;
relCellWidth = canvasRel.ActualWidth / mapperVicinity.RelMapWidth;
relCellHeight = canvasRel.ActualHeight / mapperVicinity.RelMapHeight;
// compose lists of objects to be drawn later:
drawTurnState(geoList, unrotatedList);
drawRelCells(relList);
drawReferenceCircle(unrotatedList);
drawGeoCells(geoList);
drawDetectedObjects(relList, unrotatedList, 0);
drawMission(relList, unrotatedList);
drawStatusString(unrotatedList);
// now clear canvases and draw the objects from the lists:
canvasRel.Children.Clear();
canvasGeo.Children.Clear();
rotateGeoAndTurnstateCanvas(unrotatedList, northUp);
foreach (UIElement uie in relList)
{
canvasRel.Children.Add(uie);
}
foreach (UIElement uie in geoList)
{
canvasGeo.Children.Add(uie);
}
// draw other elements on the map:
drawRobot();
drawHeadingBearingPointers();
rotateToNorthUp(northUp);
}
}
/// <summary>
/// rotates robot, reading the "textBoxRotate" control
/// </summary>
/// <param name="dir">direction of rotation, 1 - rotates robot to the right, -1 - to the left</param>
private void rotate(int dir)
{
double angle;
if (double.TryParse(textBoxRotate.Text, out angle)) // degrees
{
MapperVicinity mv = mapperViewControl1.CurrentMapper;
mv.rotate(angle * dir);
planRoute();
}
}
public virtual void OnCurrentDirectionChanged(DependencyPropertyChangedEventArgs e)
{
Direction newValue = (Direction)e.NewValue;
if (newValue != null && newValue.heading.HasValue)
{
MapperVicinity mv = CurrentMapper;
mv.rotateTo((double)newValue.heading);
CurrentMapper = mv;
}
}
public void planRoute()
{
MapperVicinity mv = mapperViewControl1.CurrentMapper;
RoutePlanner routePlanner = mapperViewControl1.CurrentRoutePlanner;
RoutePlan plan = routePlanner.planRoute();
bestHeadingLabel.Content = "Best Heading abs=" + (plan.bestHeading.HasValue ? Math.Round((double)plan.bestHeading).ToString() : "N/A") + " rel (turn)=" + (plan.bestHeading.HasValue ? Math.Round((double)plan.bestHeading).ToString() : "N/A");
closestObstacleLabel.Content = "ClosestObstacle at " + (plan.closestObstacleMeters.HasValue ? (Math.Round((double)plan.closestObstacleMeters, 2).ToString() + " m") : "N/A");
legLengthLabel.Content = "Leg Length = " + (plan.legMeters.HasValue ? (Math.Round((double)plan.legMeters, 2).ToString() + " m") : "N/A (turn only)");
timeSpentLabel.Content = plan.timeSpentPlanning.ToString();
mapperViewControl1.RedrawMap();
}
private void MapperMouseMoved_Handler(object sender, RoutedEventArgs e)
{
RoutedEventArgsMouseMoved ee = (RoutedEventArgsMouseMoved)e;
labelXYMeters.Content = "Meters:\r\nx=" + ee.xMeters + "\r\ny=" + ee.yMeters;
labelXYGeo.Content = "Coord:\r\nx=" + ee.geoPosition.Lng + "\r\ny=" + ee.geoPosition.Lat;
MapperVicinity mv = mapperViewControl1.CurrentMapper;
Distance dfX = ee.geoPosition.distanceFrom(mv.robotPosition);
Distance dfXe = ee.geoPosition.distanceFromExact(mv.robotPosition);
labelFree.Content = dfX.Meters.ToString() + "\r\n" + dfXe.Meters.ToString();
}
private void drawRelCells(List <UIElement> relList)
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double relCellWidth = frameWidth / mapperVicinity.RelMapWidth;
double relCellHeight = frameHeight / mapperVicinity.RelMapHeight;
metersPerPixelX = MapperSettings.elementSizeMeters * mapperVicinity.RelMapWidth / frameWidth;
metersPerPixelY = MapperSettings.elementSizeMeters * mapperVicinity.RelMapHeight / frameHeight;
for (int i = 0; i < mapperVicinity.RelMapHeight; i++)
{
for (int j = 0; j < mapperVicinity.RelMapWidth; j++)
{
MapCell cell = mapperVicinity.relCellAt(j, i);
SolidColorBrush myBrush = null;
bool isEmpty = cell.val < 1;
if (!isEmpty)
{
Rectangle rect = new Rectangle();
rect.Width = relCellWidth;
rect.Height = relCellHeight;
rect.RadiusX = 4;
rect.RadiusY = 4;
rect.Margin = new Thickness(j * relCellWidth, i * relCellHeight, 0, 0);
rect.Fill = myBrush;
rect.Stroke = Brushes.Green;
rect.StrokeThickness = 0.5d;
relList.Add(rect);
}
}
}
}
catch { }
}
public RoutePlanner(MapperVicinity mapper)
{
_mapper = mapper;
nW = MapperSettings.nW;
nH = MapperSettings.nH;
pathDistanceMax = 0.9d * MapperSettings.elementSizeMeters * Math.Min(nW, nH) / 2.0d; // try not to hit walls - that's why 0.9d
// reads "QGC WPL 110" mission file prepared by DIY Drones APM Planner:
mission.Init(missionFilename);
if (_mapper.robotPosition.Lat == 0.0d && _mapper.robotPosition.Lng == 0.0d && mission.home != null)
{
// place the robot near the Home:
_mapper.robotPosition.Lat = mission.home.geoPosition.Lat - 5.0 / Distance.METERS_PER_DEGREE;
_mapper.robotPosition.Lng = mission.home.geoPosition.Lng - 5.0 / Distance.METERS_PER_DEGREE;
}
}
public RoutePlanner(MapperVicinity mapper)
{
_mapper = mapper;
nW = MapperSettings.nW;
nH = MapperSettings.nH;
pathDistanceMax = 0.9d * MapperSettings.elementSizeMeters * Math.Min(nW, nH) / 2.0d; // try not to hit walls - that's why 0.9d
// reads "QGC WPL 110" mission file prepared by DIY Drones APM Planner:
mission.Init(missionFilename);
if (_mapper.robotPosition.Lat == 0.0d && _mapper.robotPosition.Lng == 0.0d && mission.home != null)
{
// place the robot near the Home:
_mapper.robotPosition.Lat = mission.home.geoPosition.Lat - 5.0 / Distance.METERS_PER_DEGREE;
_mapper.robotPosition.Lng = mission.home.geoPosition.Lng - 5.0 / Distance.METERS_PER_DEGREE;
}
}
public void setRobotPositionAndDirection(GeoPosition pos, Direction dir)
{
MapperVicinity mapperVicinity = CurrentMapper;
if (pos != null)
{
mapperVicinity.robotPosition = (GeoPosition)pos.Clone();
}
if (dir != null)
{
mapperVicinity.robotDirection = (Direction)dir.Clone();
}
// --------- debug ------------
/*
* GeoPosition pos1 = (GeoPosition)pos.Clone();
*
* pos1.translate(new Distance(1.0d), new Distance(1.0d)); // robot coordinates - right forward
*
* DetectedObstacle dobst1 = new DetectedObstacle(pos1) { color = Colors.Red };
*
* mapperVicinity.AddDetectedObject(dobst1);
*
* GeoPosition pos2 = (GeoPosition)pos.Clone();
*
* pos2.translate(new Distance(-1.0d), new Distance(1.0d)); // robot coordinates - left forward
*
* DetectedObstacle dobst2 = new DetectedObstacle(pos2) { color = Colors.Yellow };
*
* mapperVicinity.AddDetectedObject(dobst2);
*
* mapperVicinity.computeMapPositions();
*/
// --------- end debug ------------
RedrawMap();
}
private void drawRobot()
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
RobotState rs = mapperVicinity.robotState;
string controlName = "robotStateViewControl1";
RobotStateViewControl rsvc = robotPanel.FindName(controlName) as RobotStateViewControl;
if (rsvc == null)
{
double relCellWidth = canvasRel.ActualWidth / mapperVicinity.RelMapWidth;
double relCellHeight = canvasRel.ActualHeight / mapperVicinity.RelMapHeight;
double robotWidth = rs.robotWidthMeters * relCellWidth / MapperSettings.elementSizeMeters;
double robotHeight = rs.robotLengthMeters * relCellHeight / MapperSettings.elementSizeMeters;
double robotX = (robotPanel.ActualWidth - robotWidth) / 2.0d;
double robotY = (robotPanel.ActualHeight - robotHeight) / 2.0d;
rsvc = new RobotStateViewControl()
{
Name = controlName, robotState = rs
};
rsvc.Width = robotWidth;
rsvc.Height = robotHeight;
rsvc.Margin = new Thickness(robotX, robotY, 0, 0);
robotPanel.Children.Add(rsvc);
robotPanel.RegisterName(rsvc.Name, rsvc);
}
else
{
rsvc.robotState = rs;
rsvc.redrawRobot();
//Console.WriteLine(string.Format("drawRobot() - using existing control - scale: power={0} speed={1}", rsvc.maxAbsPower, rsvc.maxAbsSpeed));
}
}
catch { }
}
private void bearingTextBox_KeyUp(object sender, KeyEventArgs e)
{
double newBearing;
if (e.Key == Key.Enter)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if (_mapperVicinity != null && _mapperVicinity.robotDirection != null && _mapperVicinity.robotDirection.heading.HasValue && double.TryParse(bearingTextBox.Text, out newBearing))
{
newBearing = Direction.to360(newBearing);
Direction dir = new Direction()
{
heading = _mapperVicinity.robotDirection.heading, bearing = newBearing, TimeStamp = DateTime.Now.Ticks
};
_mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
mapperViewControl1.RedrawMap();
}
}
}
private void drawReferenceCircle(List <UIElement> relList)
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double relCellWidth = frameWidth / mapperVicinity.RelMapWidth;
double relCellHeight = frameHeight / mapperVicinity.RelMapHeight;
// draw reference circle (usually 2 meters):
Ellipse refCircle = new Ellipse();
refCircle.Width = MapperSettings.referenceCircleRadiusMeters * 2.0d * relCellWidth / MapperSettings.elementSizeMeters;
refCircle.Height = MapperSettings.referenceCircleRadiusMeters * 2.0d * relCellHeight / MapperSettings.elementSizeMeters;
refCircle.Margin = new Thickness((frameWidth - refCircle.Width) / 2.0d, (frameHeight - refCircle.Height) / 2.0d, 0, 0);
refCircle.Stroke = Brushes.Cyan;
refCircle.StrokeThickness = 4;
relList.Add(refCircle);
}
catch { }
}
private void UserControl_Loaded(object sender, RoutedEventArgs e)
{
CurrentMapper = new MapperVicinity();
}
private void drawGeoCells(List <UIElement> geoList)
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
double frameWidth = canvasGeo.ActualWidth;
double frameHeight = canvasGeo.ActualHeight;
double geoCellWidth = frameWidth / mapperVicinity.GeoMapWidth;
double geoCellHeight = frameHeight / mapperVicinity.GeoMapHeight;
metersPerPixelX = MapperSettings.elementSizeMeters * mapperVicinity.GeoMapWidth / frameWidth;
metersPerPixelY = MapperSettings.elementSizeMeters * mapperVicinity.GeoMapHeight / frameHeight;
// Create a couple of SolidColorBrush and use it to paint the rectangles:
SolidColorBrush myBrush0 = new SolidColorBrush(Colors.White);
SolidColorBrush myBrush1 = new SolidColorBrush(Colors.Black);
for (int i = 0; i < mapperVicinity.GeoMapHeight; i++)
{
for (int j = 0; j < mapperVicinity.GeoMapWidth; j++)
{
MapCell cell = mapperVicinity.geoCellAt(j, i);
SolidColorBrush myBrush = null;
bool isEmpty = cell.val < 1;
if (_currentRoutePlanner != null)
{
// see if the cell belongs to any path, and allow to see if that was the best path:
var query = from cp in _currentRoutePlanner.cellPaths
where cp.ContainsCell(cell)
orderby cp.isBest descending
select cp;
if (query.Count() > 0)
{
myBrush = query.First().isBest ? myBrushB : myBrushP;
isEmpty = false; // belongs to a path
}
else
{
myBrush = cell.val < 1 ? myBrush0 : myBrush1;
}
}
else
{
myBrush = cell.val < 1 ? myBrush0 : myBrush1;
}
if (!isEmpty)
{
Rectangle rect = new Rectangle();
rect.Width = geoCellWidth;
rect.Height = geoCellHeight;
rect.RadiusX = 4;
rect.RadiusY = 4;
rect.Margin = new Thickness(j * geoCellWidth, i * geoCellHeight, 0, 0);
bool isCenterCell = i == mapperVicinity.GeoMapHeight / 2 && j == mapperVicinity.GeoMapWidth / 2;
rect.Stroke = isCenterCell ? Brushes.Magenta : Brushes.Red;
rect.StrokeThickness = isCenterCell ? 5 : 0.5d;
rect.Fill = myBrush;
geoList.Add(rect);
}
}
}
}
catch { }
}
private void drawGeoCells(List <UIElement> geoList)
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
double frameWidth = canvasGeo.ActualWidth;
double frameHeight = canvasGeo.ActualHeight;
double geoCellWidth = frameWidth / mapperVicinity.GeoMapWidth;
double geoCellHeight = frameHeight / mapperVicinity.GeoMapHeight;
metersPerPixelX = MapperSettings.elementSizeMeters * mapperVicinity.GeoMapWidth / frameWidth;
metersPerPixelY = MapperSettings.elementSizeMeters * mapperVicinity.GeoMapHeight / frameHeight;
for (int i = 0; i < mapperVicinity.GeoMapHeight; i++)
{
for (int j = 0; j < mapperVicinity.GeoMapWidth; j++)
{
MapCell cell = mapperVicinity.geoCellAt(j, i);
SolidColorBrush myBrush = null;
bool isEmpty = cell.val < 1;
if (_currentRoutePlanner != null)
{
// see if the cell belongs to any path, and allow to see if that was the best path:
var query = from cp in _currentRoutePlanner.cellPaths
where cp.ContainsCell(cell)
orderby cp.isBest descending
select cp;
if (query.Any())
{
myBrush = query.First().isBest ? myBrushB : myBrushP;
isEmpty = false; // belongs to a path
}
}
if (!isEmpty)
{
if (myBrush == null)
{
myBrush = cell.HasHuman ? myBrushH : (cell.val < 1 ? myBrush0 : new SolidColorBrush(cell.DisplayColor));
}
Rectangle rect = new Rectangle();
rect.Width = geoCellWidth;
rect.Height = geoCellHeight;
rect.RadiusX = 4;
rect.RadiusY = 4;
rect.Margin = new Thickness(j * geoCellWidth, i * geoCellHeight, 0, 0);
bool isCenterCell = i == mapperVicinity.GeoMapHeight / 2 && j == mapperVicinity.GeoMapWidth / 2;
rect.Stroke = isCenterCell ? Brushes.Magenta : Brushes.Red;
rect.StrokeThickness = isCenterCell ? 5 : 0.5d;
rect.Fill = myBrush;
geoList.Add(rect);
if (cell.val > 0)
{
double fontSize = 10.0d;
Label cellValueLabel = new Label()
{
FontSize = fontSize, Foreground = Brushes.Black, Background = myBrush
};
cellValueLabel.Content = cell.val;
cellValueLabel.Margin = rect.Margin;
geoList.Add(cellValueLabel);
}
}
}
}
}
catch { }
}
private void rotateGeoAndTurnstateCanvas(List <UIElement> unrotatedList, bool northUp)
{
MapperVicinity mapperVicinity = CurrentMapper;
TransformGroup geort = null;
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double geoCellWidth = frameWidth / mapperVicinity.GeoMapWidth;
double geoCellHeight = frameHeight / mapperVicinity.GeoMapHeight;
// draw compass labels:
double x0 = geoCellWidth + 6;
double y0 = frameHeight / 2 - geoCellHeight * 1.5d;
double cSize = 35;
Label lN = new Label()
{
Content = "N", Margin = new Thickness(x0, y0 - cSize, 0, 0), FontSize = 20, FontWeight = FontWeights.Bold
};
unrotatedList.Add(lN);
Label lS = new Label()
{
Content = "S", Margin = new Thickness(x0, y0 + cSize, 0, 0), FontSize = 20, FontWeight = FontWeights.Bold
};
unrotatedList.Add(lS);
Label lW = new Label()
{
Content = "W", Margin = new Thickness(x0 - cSize, y0, 0, 0), FontSize = 20, FontWeight = FontWeights.Bold
};
unrotatedList.Add(lW);
Label lE = new Label()
{
Content = "E", Margin = new Thickness(x0 + cSize, y0, 0, 0), FontSize = 20, FontWeight = FontWeights.Bold
};
unrotatedList.Add(lE);
double angle = 0.0d; // degrees
if (mapperVicinity.robotDirection != null && mapperVicinity.robotDirection.heading.HasValue)
{
angle = -(double)mapperVicinity.robotDirection.heading;
}
string content = Math.Round(-angle).ToString();
double shift = content.Length * 3.0d - 2.0d;
Label lDir = new Label()
{
Content = content, Margin = new Thickness(x0 - shift, y0 + 2.0d, 0, 0), FontSize = 18, FontWeight = FontWeights.Bold
};
unrotatedList.Add(lDir);
if (!northUp)
{
double arads = angle * Math.PI / 180.0d;
double asin = Math.Abs(Math.Sin(arads));
double acos = Math.Abs(Math.Cos(arads));
double scaleY = (geoCellWidth * asin + geoCellHeight * acos) / (geoCellHeight * (asin + acos));
double scaleX = (geoCellWidth * acos + geoCellHeight * asin) / (geoCellWidth * (asin + acos));
geort = new TransformGroup();
RotateTransform rt = new RotateTransform(angle, 0.0d, 0.0d);
TranslateTransform tt1 = new TranslateTransform(-canvasRel.ActualWidth / 2.0d, -canvasRel.ActualHeight / 2.0d);
TranslateTransform tt2 = new TranslateTransform(canvasRel.ActualWidth / 2.0d, canvasRel.ActualHeight / 2.0d);
ScaleTransform st = new ScaleTransform(scaleX, scaleY);
geort.Children.Add(tt1);
geort.Children.Add(st);
geort.Children.Add(rt);
geort.Children.Add(tt2);
}
canvasGeo.RenderTransform = geort;
turnPanel.RenderTransform = geort;
}
private void drawAll()
{
if (this.IsLoaded)
{
List<UIElement> relList = new List<UIElement>();
List<UIElement> geoList = new List<UIElement>();
bool northUp = (bool)northUpCheckBox.IsChecked;
List<UIElement> unrotatedList = northUp ? geoList : relList;
mapperVicinity = CurrentMapper;
labelLat.Content = mapperVicinity.robotPosition.Lat;
labelLng.Content = mapperVicinity.robotPosition.Lng;
relCellWidth = canvasRel.ActualWidth / mapperVicinity.RelMapWidth;
relCellHeight = canvasRel.ActualHeight / mapperVicinity.RelMapHeight;
// compose lists of objects to be drawn later:
drawTurnState(geoList, unrotatedList);
drawRelCells(relList);
drawReferenceCircle(unrotatedList);
drawGeoCells(geoList);
drawDetectedObjects(relList, unrotatedList, 0);
drawMission(relList, unrotatedList);
drawStatusString(unrotatedList);
// now clear canvases and draw the objects from the lists:
canvasRel.Children.Clear();
canvasGeo.Children.Clear();
rotateGeoAndTurnstateCanvas(unrotatedList, northUp);
foreach(UIElement uie in relList)
{
canvasRel.Children.Add(uie);
}
foreach (UIElement uie in geoList)
{
canvasGeo.Children.Add(uie);
}
// draw other elements on the map:
drawRobot();
drawHeadingBearingPointers();
rotateToNorthUp(northUp);
}
}
public void setMapper(MapperVicinity mapper, RoutePlanner routePlanner)
{
if (mapWindow != null)
{
_mapperVicinity = mapper;
_routePlanner = routePlanner;
mapWindow.mapperViewControl1.CurrentMapper = mapper;
mapWindow.mapperViewControl1.CurrentRoutePlanner = routePlanner;
}
}
private void UserControl_Loaded(object sender, RoutedEventArgs e)
{
CurrentMapper = new MapperVicinity();
}
public void drawHeadingBearingPointers()
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
string controlNameB = "pointerBearing1";
string controlNameH = "pointerHeading1";
if (mapperVicinity.robotDirection != null && mapperVicinity.robotDirection.heading.HasValue && mapperVicinity.robotDirection.bearing.HasValue)
{
Path pointerBearing = robotPanel.FindName(controlNameB) as Path;
if (pointerBearing == null)
{
// create Bearing pointer, add it to the robotPanel:
var dt = (DataTemplate)Resources["TemplateXaml"];
pointerBearing = (Path)dt.LoadContent();
pointerBearing.Name = controlNameB;
pointerBearing.Fill = Brushes.LightGreen;
robotPanel.Children.Add(pointerBearing); // suitable canvas
robotPanel.RegisterName(pointerBearing.Name, pointerBearing);
}
Path pointerHeading = robotPanel.FindName(controlNameH) as Path;
if (pointerHeading == null)
{
// create Heading pointer, add it to the robotPanel:
var dt = (DataTemplate)Resources["TemplateXaml"];
pointerHeading = (Path)dt.LoadContent();
pointerHeading.Name = controlNameH;
robotPanel.Children.Add(pointerHeading); // suitable canvas
robotPanel.RegisterName(pointerHeading.Name, pointerHeading);
}
double bearing = (double)mapperVicinity.robotDirection.bearingRelative;
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double arrowShift = Math.Min(frameWidth, frameHeight) * 0.5d; // how far the pointer will be from the center of the map
TranslateTransform tt = new TranslateTransform(-arrowShift, 0.0d);
TranslateTransform tt1 = new TranslateTransform(frameWidth / 2.0d, frameHeight / 2.0d - 10.0d); // to the center of the control - post-rotate
//bearingLabel.Content = String.Format("{0,6:0.0}", bearing);
RotateTransform rtB = new RotateTransform(bearing + 90, 0, 10);
TransformGroup tgB = new TransformGroup();
tgB.Children.Add(tt);
tgB.Children.Add(st);
tgB.Children.Add(rtB);
tgB.Children.Add(tt1);
pointerBearing.RenderTransform = tgB;
// similarly draw heading arrow:
double heading = -(double)mapperVicinity.robotDirection.heading;
TranslateTransform ttH = new TranslateTransform(-arrowShift * 1.05d, 0.0d);
TranslateTransform ttH1 = new TranslateTransform(frameWidth / 2.0d, frameHeight / 2.0d - 10.0d); // to the center of the control - post-rotate
//headingLabel.Content = String.Format("{0,6:0.0}", heading);
RotateTransform rtH = new RotateTransform(heading + 90, 0, 10);
TransformGroup tgH = new TransformGroup();
tgH.Children.Add(ttH);
tgH.Children.Add(st);
tgH.Children.Add(rtH);
tgH.Children.Add(ttH1);
pointerHeading.RenderTransform = tgH;
}
}
catch { }
}
// degrees, relative to robot direction
public double? bestHeadingRelative(MapperVicinity mapper)
{
return Direction.to180(bestHeading - mapper.robotDirection.heading);
}
private void drawDetectedObjects(List <UIElement> relList, List <UIElement> unrotatedList, int method)
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double metersPerPixelX = MapperSettings.elementSizeMeters / (frameWidth / mapperVicinity.RelMapWidth);
double metersPerPixelY = MapperSettings.elementSizeMeters / (frameHeight / mapperVicinity.RelMapHeight);
double latitudeFactor = Math.Cos(mapperVicinity.robotPosition.Y);
double xMeters = Distance.METERS_PER_DEGREE * latitudeFactor;
int i = 0;
switch (method)
{
case 0:
default:
i = mapperVicinity.Count;
break;
case 1:
lock (mapperVicinity)
{
foreach (IDetectedObject idobj in mapperVicinity)
{
DetectedObjectViewControl dovc = null;
if (idobj.objectType == DetectedObjectType.Mark)
{
dovc = new DetectedObjectViewControl()
{
color = idobj.color, size = 21
};
}
else
{
dovc = new DetectedObjectViewControl()
{
color = idobj.color
};
}
/*
* GeoPosition pos = idobj.geoPosition;
*
* double distXmeters = (pos.Lng - mapperVicinity.robotPosition.Lng) * Distance.METERS_PER_DEGREE * latitudeFactor;
* double distYmeters = (pos.Lat - mapperVicinity.robotPosition.Lat) * Distance.METERS_PER_DEGREE;
*/
RelPosition pos = idobj.relPosition;
double distXmeters = pos.X;
double distYmeters = pos.Y;
double xPix = frameWidth / 2.0d - distXmeters / metersPerPixelX;
double yPix = frameHeight / 2.0d - distYmeters / metersPerPixelY;
dovc.Margin = new Thickness(xPix, yPix, 0, 0);
relList.Add(dovc);
i++;
}
}
break;
}
// draw number of Detected Objects:
double x0 = 20.0d;
double y0 = 20.0d;
Label lN = new Label()
{
Content = string.Format("{0} objects", i), Margin = new Thickness(x0, y0, 0, 0), FontSize = 10, FontWeight = FontWeights.Normal
};
unrotatedList.Add(lN);
}
catch { }
}
public double?bestHeadingRelative(MapperVicinity mapper) // degrees, relative to robot direction
{
return(Direction.to180(bestHeading - mapper.robotDirection.heading));
}
