Esempio n. 1
0
		private void CalculateNextTarget()
		{
		    Point startFrom = LastValidPoint ?? _robot.StartPosition;

            DebugHelper.StoreAsBitmap(string.Format("C:\\debugMinimap-{0}.png", DateTime.Now.Ticks), _mapExplored.Map);

			_waypointQueue.Clear();
			List<Point> route = null;
			var DijkstraHelper = new DijkstraHelper(_mapExplored);

			var goal = GetGoalPoint();
			if (goal != null)
			{
                route = DijkstraHelper.GetPath(startFrom, goal, _pathLog);
			}

			if (route == null || !route.Any())
			{
				EdgeDetectionAlgorithm edgeDetection = new EdgeDetectionAlgorithm();

				var edges = edgeDetection
                                .GroupToEdges(edgeDetection.GetEdgePoints(_mapExplored.Map))
                                .OrderByDescending(edge => edge.Width)
                                .ToList();

               foreach (var edge in edges)
			   {

                   var path = DijkstraHelper.GetPath(startFrom, edge.CenterPoint, _pathLog);
					if (path != null && path.Any())
					{
						route = path;
						break;
					}
				}
			}

			if (route != null && route.Any())
			{
				route.ForEach(wp => _waypointQueue.Enqueue(wp));
			}
			else
			{
                SimulationEngine.EndSimulation("Robot stuck!");
				//throw new ApplicationException("No further valid edges or target found!");
			}
		}
Esempio n. 2
0
        /// <summary>
        /// Robot recognizes collision with obstacle and tells brain.
        /// Brain adds point to mapExplored and calculates new Waypoint.
        /// </summary>
        /// <param name="mo"></param>
        public void Collision(double posX, double posY, MovementObject mo)
        {
			// Only Handle Collision WayDecision if Robot is not driving backwards (which means that Robot already hat Collision)

            _waypointQueue.Clear();

            var DijkstraHelper = new DijkstraHelper(_mapExplored);
            Point NextPoint;

            do{
                NextPoint = _pathLog.Pop();
            } while(!DijkstraHelper.IsPointValid(NextPoint));

            ActiveWayPoint = NextPoint;
            return;

            WayDecision wd;
			
			if (!WayDecision.IgnoreDirection)
			{
				wd = new WayDecisionCollision(posX, posY, mo, _mapExplored);
				WayDecision.IgnoreDirection = true;
			}
			else
			{
				wd = new WayDecisionCollisionBackwards(posX, posY, mo, _mapExplored);
				WayDecision.IgnoreDirection = false;
			}

			// CalculateNextTarget();
			// _mapExplored.WaypointActive = _waypointQueue.Dequeue();

			CreateNextWaypoint(wd);
        }