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!");
}
}
/// <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);
}
