/* calculates new movementPoint based on next waypoint
/****************************************************************/
private MovementObject GetNextMovementPoint(double x, double y)
{
var movementObject = new MovementObject();
double movementX = _mapExplored.WaypointActive.X - x;
double movementY = _mapExplored.WaypointActive.Y - y;
double posX;
double posY;
int dirX = movementX >= 0 ? 1 : -1;
int dirY = movementY >= 0 ? 1 : -1;
movementX = Math.Abs(movementX);
movementY = Math.Abs(movementY);
if (movementX >= movementY)
{
posX = x + 1 * dirX;
posY = y + (1 / movementX * movementY) * dirY;
}
else
{
posY = y + 1 * dirY;
posX = x + (1 / movementY * movementX) * dirX;
}
movementObject.X = posX;
movementObject.Y = posY;
return movementObject;
}
public MovementObject GetNextMove(double posX, double posY, double currentDirection)
{
AllowToRescan();
// check if waypoint is reached
if (ActiveWayPoint == null || HasReachedWayPoint(posX, posY))
{
LastValidPoint = ActiveWayPoint;
_pathLog.Push(LastValidPoint);
WayDecision.IgnoreDirection = false;
if (!_waypointQueue.Any())
{
CalculateNextTarget();
}
if (_waypointQueue.Count > 0)
{
ActiveWayPoint = _waypointQueue.Dequeue();
}
else
{
ActiveWayPoint = _pathLog.Pop();
}
//ActiveWayPoint = _waypointQueue.Dequeue();
}
MovementObject settingNew = new MovementObject(posX, posY, currentDirection);
double targetDirection = CalculateTargetDirection(posX, posY, _mapExplored.WaypointActive);
// either change direction or position
if (IsPointingAtTarget(currentDirection, targetDirection) || WayDecision.IgnoreDirection)
{
MovementObject positionNew = GetNextMovementPoint(posX, posY);
settingNew.X = positionNew.X;
settingNew.Y = positionNew.Y;
}
else
{
settingNew.Direction = AdjustDirection(currentDirection, targetDirection);
}
// add new movement point to map explored and mark as VISITED
if (_mapExplored.GetStatus(settingNew.X, settingNew.Y) == MapElementStatus.Undiscovered)
{
_mapExplored.SetStatus(settingNew.X, settingNew.Y, MapElementStatus.Visited);
}
return settingNew;
}
/// <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);
}
private bool IsValidMove(MovementObject mo)
{
Robot collisionDummy = null;
try
{
IsCollidable = false;
collisionDummy = (Robot)this.Clone();
collisionDummy.ApplyTo(_mapArea);
Map.Add(collisionDummy);
collisionDummy.Bind(Map);
// set new position
collisionDummy.SetPos(mo.X, mo.Y);
if (collisionDummy.IsOverlapping())
{
_brain.Collision(_positionX, _positionY, mo);
return false;
}
return true;
}
finally
{
IsCollidable = true;
if (collisionDummy != null)
{
collisionDummy.Dispose();
collisionDummy.Remove(_mapArea);
}
}
}