Пример #1
0
 public DetectedObjectBase(Direction dir, Distance dist)
     : this()
 {
     relPosition = new RelPosition(dir, dist);
 }
        private void drawMission(List<UIElement> relList, List<UIElement> unrotatedList)
        {
            if (_currentRoutePlanner != null && _currentRoutePlanner.mission != null && _currentRoutePlanner.mission.waypoints.Any())
            {
                try
                {
                    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;

                    LocationWp prevWp = null;
                    double xPixPrev=0, yPixPrev=0;

                    foreach(LocationWp curWp in _currentRoutePlanner.mission.waypoints)
                    {
                        Color wpColor;
                        switch (curWp.waypointState)
                        {
                            default:
                            case WaypointState.None:
                                wpColor = Colors.Blue;
                                break;
                            case WaypointState.SelectedAsTarget:
                                wpColor = Colors.Orange;
                                break;
                            case WaypointState.Passed:
                                wpColor = Colors.Green;
                                break;
                            case WaypointState.CouldNotReach:
                                wpColor = Colors.Red;
                                break;
                        }

                        DetectedObjectViewControl dovc = new DetectedObjectViewControl() { color = wpColor, size = curWp.isHome ? 121 : 41 };

                        Direction dirToWp = curWp.directionToWp(mapperVicinity.robotPosition, mapperVicinity.robotDirection);

                        Distance distToWp = curWp.distanceToWp(mapperVicinity.robotPosition);

                        double metersToWaypoint = Math.Round(distToWp.Meters, 1);

                        distToWp /= 10.0d;     // fit to small scale vicinity map

                        RelPosition pos = new RelPosition(dirToWp, distToWp);

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

                        string wptCommand = curWp.id == MAV_CMD.WAYPOINT ? (curWp.isHome ? "HOME" : string.Empty) : curWp.id.ToString();

                        string strSecToWp = string.Empty;

                        if (curWp.waypointState == WaypointState.SelectedAsTarget && curWp.estimatedTimeOfArrival.HasValue)
                        {
                            strSecToWp = string.Format(" {0:#} sec", (curWp.estimatedTimeOfArrival.Value - DateTime.Now).TotalSeconds);
                        }

                        Label labelWp = new Label() {
                            Content = string.Format("WPT{0}: {1} m {2} deg {3}{4}", curWp.number, metersToWaypoint, Math.Round((double)dirToWp.bearingRelative, 1), wptCommand, strSecToWp),
                            Margin = new Thickness(xPix + 10, yPix + 10, 0, 0),
                            FontSize = 14,
                            FontWeight = FontWeights.Bold
                        };

                        relList.Add(labelWp);

                        if (prevWp != null)
                        {
                            //Line lineToWp = new Line() { X1 = xPixPrev, Y1 = yPixPrev, X2 = xPix, Y2 = yPix, Stroke=Brushes.Red, StrokeThickness = 4.0d, StrokeEndLineCap = PenLineCap.Triangle };
                            //relList.Add(lineToWp);

                            Brush brush = Brushes.Blue;
                            double thickness = 4.0d;

                            if (prevWp.waypointState == WaypointState.Passed && curWp.waypointState == WaypointState.Passed)
                            {
                                // leg has been traveled:
                                brush = Brushes.Green;
                            }
                            else if (curWp.waypointState == WaypointState.SelectedAsTarget)
                            {
                                // current leg:
                                brush = Brushes.OrangeRed;
                                thickness = 6.0d;
                            }

                            relList.Add(LineArrowHelper.DrawLinkArrow(new Point(xPixPrev, yPixPrev), new Point(xPix, yPix), brush, thickness));
                        }

                        prevWp = curWp;
                        xPixPrev = xPix;
                        yPixPrev = yPix;
                        i++;
                    }
                }
                catch { }
            }
        }