public MovementTo(PointD coords)
{
Coords = coords;
Next = null;
Previous = null;
Alternative = null;
}
public StoppingAt(PointD coords, double angle)
{
AngleInRadians = angle;
Coords = coords;
Next = null;
Previous = null;
Alternative = null;
}
public static double CalculateAngle(PointD begin, PointD end)
{
var horizontal = new Vector(new PointD(0, 0), new PointD(1, 0));
var current = new Vector(begin, end);
if (current.X == 0)
return current.Y > 0 ? 3 * Math.PI / 2 : Math.PI / 2;
if (current.Y == 0)
return current.X > 0 ? 0 : Math.PI;
var scalarMult = (horizontal.X * current.X) + (horizontal.Y * current.Y);
var angleCos = scalarMult / (horizontal.Length * current.Length);
var angle = Math.Acos(angleCos);
return current.Y < 0 ? angle : 2*Math.PI - angle;
}
public bool Rotate(StrategyTesterReport originalState, Rotate command, List<Polygon> obstacles, bool doFast)
{
bool isMoveSuccessfull = true;
var targetAngle = Angle + command.AngleSpeed * command.Time;
var timeDelta = command.Time / 100;
for (var i = 0.0; i < command.Time - timeDelta; i += timeDelta)
{
Angle += timeDelta * command.AngleSpeed;
if (obstacles.Any(x => x.IsPointIn(Coords)))
{
Angle = originalState.AngleInRadians;
Coords = originalState.Coords;
isMoveSuccessfull = false;
break;
}
if (!doFast) Thread.Sleep((int)(timeDelta * 1000));
}
Angle = targetAngle;
return isMoveSuccessfull;
}
public bool Forward(StrategyTesterReport originalState, Forward command, List<Polygon> obstacles, bool doFast)
{
var isMoveSuccessfull = true;
var timeDelta = command.Time / 100;
var targetCoords = new PointD(Coords.X + command.Time * command.Speed * Math.Cos(Angle),
Coords.Y + command.Time * command.Speed * -Math.Sin(Angle));
for (var i = 0.0; i < command.Time - timeDelta; i += timeDelta)
{
Coords.X += timeDelta * command.Speed * Math.Cos(Angle);
Coords.Y += timeDelta * command.Speed * -Math.Sin(Angle);
if (obstacles.Any(x => x.IsPointIn(Coords)))
{
Angle = originalState.AngleInRadians;
Coords = originalState.Coords;
isMoveSuccessfull = false;
break;
}
if (!doFast) Thread.Sleep((int)(timeDelta * 1000));
}
if (isMoveSuccessfull) Coords = targetCoords;
return isMoveSuccessfull;
}
public Start(PointD coords)
{
Coords = coords;
}
public World(PointD robotCoords, double robotAngle)
{
Moving = false;
Objects = new List<Polygon>();
OurRobot = new Robot(robotCoords, robotAngle);
}
private Forward MakeForward(PointD current, PointD target)
{
var speed = LinearSpeedСoefficient * robotInfo.MaxLinearSpeed;
var length = Math.Sqrt(Math.Pow(target.X - current.X, 2) + Math.Pow(target.Y - current.Y, 2));
return new Forward(speed, length / speed);
}
public Robot(PointD coords, double angle)
{
Coords = coords;
Angle = angle;
}
public Vector(PointD begin, PointD end)
{
X = end.X - begin.X;
Y = end.Y - begin.Y;
Length = Math.Sqrt(X * X + Y * Y);
}
