public void GoalieCalculation(SSL_DetectionBall ball, int id)
{
IRobotInfo final = new RobotParameters(0, 0, 0);
final.Y = 350 * (ball.y / 2025);
final.X = -2975 + (500 * ((ball.x + 3025) / (6050)));
final.W = (float)Math.Atan2((ball.y - final.Y), (ball.x - final.X));
repo.OutData[id].X = final.X;
repo.OutData[id].Y = final.Y;
repo.OutData[id].W = final.W;
//DataHandler.WriteReference(final, id);
}
/// <summary>
/// CMDragons Research Paper implementation for defence with 3 robots so that no one can shoot directly.
/// </summary>
/// <param name="IDG"> Own Goal Keeper ID.</param>
/// <param name="IDD1"> Own Defender 1 ID.</param>
/// <param name="IDD2"> Own Defender 2 ID.</param>
/// <param name="Goalkeeper"> Location of Own Goal keeper.</param>
/// <param name="Defender1"> Location of Own Defender 1.</param>
/// <param name="Defender2"> Location of Own Defender 2.</param>
/// <param name="ball"> Location of Ball.</param>
/// <param name="OwnGoal"> Location of Own Goal.</param>
public void BlockOppLineOfSight(int IDG, int IDD1, int IDD2, IRobotInfo Goalkeeper, IRobotInfo Defender1, IRobotInfo Defender2, SSL_DetectionBall ball, IRobotInfo OwnGoal)
{
List<float> slopes = new List<float>();
IRobotInfo[] final_points = new IRobotInfo[3];
List<IRobotInfo> points_goal = new List<IRobotInfo>();
List<float> final_angles = new List<float>();
int point_with_small_y = 0, point_with_big_y = 2;
points_goal.Add(new RobotParameters(OwnGoal.X, OwnGoal.Y - 232, 0));
points_goal.Add(OwnGoal);
points_goal.Add(new RobotParameters(OwnGoal.X, OwnGoal.Y + 232, 0));
IRobotInfo tmp_point = new RobotParameters(-3025, 0, 0);
//tmp_point.Y = -232;
//points_goal.Add(DataHandler.DeepClone<IRobotInfo>(tmp_point));
//tmp_point.Y = 0;
//points_goal.Add(DataHandler.DeepClone<IRobotInfo>(tmp_point));
//tmp_point.Y = 232;
//points_goal.Add(DataHandler.DeepClone<IRobotInfo>(tmp_point));
slopes.Add((OwnGoal.Y - 232 - ball.y) / (OwnGoal.X - ball.x));
slopes.Add((OwnGoal.Y + 0 - ball.y) / (OwnGoal.X - ball.x));
slopes.Add((OwnGoal.Y + 232 - ball.y) / (OwnGoal.X - ball.x));
float x = 0, y = 0;
float ratio = 0.1666666666f;
for (int i = 0; i < 3; i++)
{
if (float.IsInfinity(slopes[i]))
slopes[i] = 300;
if (i == 1)
{
while (Distance(x, 0, y, 0) < 375)
//while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 375)
{
if (slopes[i] > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * slopes[i]);
}
else
{
x = x + 1;
y = y + slopes[i];
}
}
}
else
{
while (Distance(x, 0, y, 0) < 800)
//while (Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2)) < 800)
{
if (slopes[i] > 9)
{
x = (float)(x + ratio);
y = (float)(y + ratio * slopes[i]);
}
else
{
x = x + 1;
y = y + slopes[i];
}
}
}
tmp_point.X = points_goal[i].X + x;
tmp_point.Y = points_goal[i].Y + y;
final_points[i] = Statics.DeepClone<IRobotInfo>(tmp_point);
x = 0;
y = 0;
}
final_angles.Add((float)Math.Atan2(ball.y - (OwnGoal.Y - 232), ball.x - OwnGoal.X));
final_angles.Add((float)Math.Atan2(ball.y - (OwnGoal.Y - 0), ball.x - OwnGoal.X));
final_angles.Add((float)Math.Atan2(ball.y - (OwnGoal.Y + 232), ball.x - OwnGoal.X));
for (int i = 0; i < 3; i++)
{
final_points[i].W = final_angles[i];
}
if (final_points[2].Y < final_points[0].Y)
{
point_with_small_y = 2;
point_with_big_y = 0;
}
if (Defender1.Y < Defender2.Y)
{
repo.OutData[IDD1].X = final_points[point_with_small_y].X;
repo.OutData[IDD1].Y = final_points[point_with_small_y].Y;
repo.OutData[IDD1].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], IDD1);
repo.OutData[IDG].X = final_points[1].X; //Techincally this should be outside as it is same in if and else both
repo.OutData[IDG].Y = final_points[1].Y;
repo.OutData[IDG].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], IDG);
repo.OutData[IDD2].X = final_points[point_with_big_y].X;
repo.OutData[IDD2].Y = final_points[point_with_big_y].Y;
repo.OutData[IDD2].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], IDD2);
}
else
{
repo.OutData[IDD1].X = final_points[point_with_big_y].X;
repo.OutData[IDD1].Y = final_points[point_with_big_y].Y;
repo.OutData[IDD1].W = final_points[point_with_big_y].W;
//DataHandler.WriteReference(final_points[point_with_big_y], IDD1);
repo.OutData[IDG].X = final_points[1].X;
repo.OutData[IDG].Y = final_points[1].Y;
repo.OutData[IDG].W = final_points[1].W;
//DataHandler.WriteReference(final_points[1], IDG);
repo.OutData[IDD2].X = final_points[point_with_small_y].X;
repo.OutData[IDD2].Y = final_points[point_with_small_y].Y;
repo.OutData[IDD2].W = final_points[point_with_small_y].W;
//DataHandler.WriteReference(final_points[point_with_small_y], IDD2);
}
}
/// <summary>
/// Converts SSL detection Ball to Robot Info. Normally to use the location of ball in functions.
/// </summary>
/// <param name="ball"> SSL Detection ball to be converted.</param>
/// <returns> returns the converted ball of type RobotInfo.</returns>
public IRobotInfo BallToIRobotInfo(SSL_DetectionBall ball)
{
return new RobotParameters(ball.x, ball.y, 0);
}
/// <summary>
/// Used to send two robots i.e. AttackSupport and FreeRole to make space a head for pass/Attack.
/// </summary>
/// <param name="IDAS"> Attack Support Robot Id.</param>
/// <param name="IDFR"> Free Role Robot Id.</param>
/// <param name="AttackSupport"> Attack Support Robot Position.</param>
/// <param name="FreeRole"> Free Role Robot Position.</param>
/// <param name="ball"> Ball Postion.</param>
/// <param name="OppMean"> Mean of Opponents All Robots.</param>
/// <param name="OppSD"> Standard Deviation of Opponents All Robots.</param>
/// <returns> Returns true if function Executes Successfully.</returns>
public bool MakeFreeSpace(int IDAS, int IDFR, IRobotInfo AttackSupport, IRobotInfo FreeRole, SSL_DetectionBall ball, PointF OppMean, PointF OppSD)
{
IRobotInfo Final_Point_Support = new RobotParameters();
IRobotInfo Final_Point_Mid = new RobotParameters();
if (OppMean.Y > 0)
{
if (Distance(AttackSupport.X, OppMean.X, AttackSupport.Y, OppMean.Y) < Distance(FreeRole.X, OppMean.X, FreeRole.Y, OppMean.Y))
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = OppMean.Y;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = -9 * OppSD.Y / 5;
}
else
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = -9 * OppSD.Y / 5;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = OppMean.Y;
}
}
else
{
if (Distance(AttackSupport.X, OppMean.X, AttackSupport.Y, -OppMean.Y) < Distance(FreeRole.X, OppMean.X, FreeRole.Y, -OppMean.Y))
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = -OppMean.Y;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = 9 * OppSD.Y / 5;
}
else
{
Final_Point_Support.X = OppMean.X;
Final_Point_Support.Y = 9 * OppSD.Y / 5;
Final_Point_Mid.X = OppMean.X;
Final_Point_Mid.Y = -OppMean.Y;
}
}
Final_Point_Support.W = (float)Math.Atan2(ball.y - Final_Point_Support.Y, ball.x - Final_Point_Support.X);
Final_Point_Mid.W = (float)Math.Atan2(ball.y - Final_Point_Mid.Y, ball.x - Final_Point_Mid.X);
if (Distance(AttackSupport.X, Final_Point_Support.X, AttackSupport.Y, Final_Point_Support.Y) > 100 || Distance(FreeRole.X, Final_Point_Mid.X, FreeRole.Y, Final_Point_Mid.Y) > 100)
{
repo.OutData[IDAS].X = Final_Point_Support.X;
repo.OutData[IDAS].Y = Final_Point_Support.Y;
repo.OutData[IDAS].W = Final_Point_Support.W;
//DataHandler.WriteReference(Final_Point_Support, IDAS);
repo.OutData[IDFR].X = Final_Point_Mid.X;
repo.OutData[IDFR].Y = Final_Point_Mid.Y;
repo.OutData[IDFR].W = Final_Point_Mid.W;
//DataHandler.WriteReference(Final_Point_Mid, IDFR);
return false;
}
else
return true;
}
/// <summary>
/// Used to check if the ball is withing kicking radius and kicks the ball.
/// </summary>
/// <param name="ID"> Id of robot to kick the ball.</param>
/// <param name="source"> Location of robot that will kick the ball.</param>
/// <param name="ball"> Location of ball.</param>
/// <param name="KickSpeed"> Speed by which ball will be kicked.</param>
/// <returns> returns true of ball is in radius of kicking and is kicked otherwise false.</returns>
public bool KickBall(int ID, IRobotInfo source, SSL_DetectionBall ball, float KickSpeed)
{
if (Distance(source.X, ball.x, source.Y, ball.y) < 105)
{
repo.OutData[ID].KickSpeed = KickSpeed;
//DataHandler.WriteKickSpeed(KickSpeed, ID);
return true;
}
else
{ return false; }
}
/// <summary>
/// Used to go to ball turn on the dribbler to grab the ball.
/// </summary>
/// <param name="ID"> Id of robot to move.</param>
/// <param name="source"> Location of Robot to be moved.</param>
/// <param name="ball"> Location of ball to grab.</param>
/// <returns>returns true when reached and grabbed ball else returns fale.</returns>
public bool GrabBall(int ID, IRobotInfo source, SSL_DetectionBall ball)
{
if (Distance(source.X, ball.x, source.Y, ball.y) > 105)
{
GotoPoint(ID, source, new RobotParameters(ball.x, ball.y, (float)Math.Atan2((ball.y - source.Y), (ball.x - source.X))), 0);
return false;
}
else
{
repo.OutData[ID].Grab = true;
//DataHandler.Dribble[ID] = true;
return true;
}
}
