private PointParams CalculateDirectedAttack(AHEntities.Action direction, Point puckP, Point puckV, double puckR,
double xline, int tableW, int tableH)
{
PointParams finalConditions = new PointParams();
double[] crossParamsAttack = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, puckR, xline, tableW, tableH);
Point XYi = GetCollissionPoint(puckV.X, puckV.Y, xline, crossParamsAttack[0], direction);
// Detemine speeds at the goal.
Point Pg = new Point(1180, 0);
Point speedG = GetGoalSpeed(direction, xline, crossParamsAttack[0], Pg);
double thetaG = -speedG.X / rp; // constrain on the angle speed at the goal
double vG = speedG.Norm();
double vPS;
Point speedPS;
double thetaPS, thetaPSdot;
if (direction != AHEntities.Action.ATTACK_MIDDLE)
{
// calculating angles
double alpha = CalculateAlpha(speedG, AHEntities.Action.ATTACK_RIGHT);
double beta = CalculateBeta(direction, xline, crossParamsAttack[0], tableH, Pg, alpha);
// calculating post strike prameters
vPS = -vG / e * Math.Sin(alpha) / Math.Sin(beta);
speedPS = new Point(vPS * Math.Cos(beta), vPS * Math.Sin(beta));
thetaPS = Math.Atan2(speedPS.Y, speedPS.X);
thetaPSdot = 2 * vG / (rp * e) * Math.Sin(alpha) / Math.Tan(beta) - 3 * vG / rp * Math.Cos(alpha);
}
else // direction == MIDDLE
{
// calculating post strike prameters
vPS = 0;
speedPS = new Point();
thetaPS = 0;
thetaPSdot = 0;
}
// calculating frames rotation angle
double thetaP = Math.Atan2(puckV.Y, puckV.X);
double thetaC = CalculateRotationAngle(thetaPS, thetaPSdot, thetaP, puckR, vPS, puckV.Norm());
// transform puck and ps velocities to impact frame
Point impactVps = TransformFrames(thetaC, speedPS, true);
Point impactVp = TransformFrames(thetaC, puckV, true);
// calculate control velocities
Point impactVm = CalculateImpactVelocities(impactVp, impactVps, thetaP, thetaPS, rp);
// transform to XY
Point vM = TransformFrames(thetaC, impactVm, false);
// create finalConditions.
Point target = CalculateTargetPoint(XYi, new Point(AHEntities.EstimateLineCrossing.attackLine, crossParamsAttack[0]));
finalConditions.AddParameters(target);
finalConditions.AddParameters(vM);
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(crossParamsAttack[1]);
return(finalConditions);
}
private PointParams CalculateNaiveDirectedAttack(AHEntities.Action direction, Point puckPline, double time,
double xline, double puckRadius, double malletRadius, int tableW, int tableH, double velocity)
{
PointParams finalConditions = new PointParams();
Point collisionPoint, targetPoint, targetVelocity;
collisionPoint = GetNaiveCollisionPoint(direction, puckPline, puckRadius - 2, tableW, tableH);
targetPoint = GetNaiveTargetPoint(puckPline, collisionPoint, malletRadius - 2);
targetVelocity = GetNaiveTargetVelocity(collisionPoint, targetPoint, velocity);
finalConditions.AddParameters(targetPoint);
finalConditions.AddParameters(targetVelocity);
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(time);
return(finalConditions);
}
public void PlanNewAction(ActionDirective A, bool isNewPlanning)
{
planTime.Restart();
// target point parameters (for the whole motion)
//if (isNewPlanning)
//bounderyConditions = actionPlanningStrategy.ActionPlanning(A, isNewPlanning);
bounderyConditions = actionPlanningStrategy.ActionPlanning(A, true);
if (bounderyConditions == null)
{
return;
}
state = WM.GetPhysicalState();
Point agentP = new Point(state["AgentX"], state["AgentY"]);
Point agentV = new Point(state["AgentVx"], state["AgentVy"]);
mLogger.AddLogMessage("LowLevel: action planned: agent currently at: " + agentP.ToString() + " and: " + agentV.ToString() + ", target: " + bounderyConditions.ToString());
// initial movement parameters
PointParams initialConditions = new PointParams();
initialConditions.AddParameters(agentP);
initialConditions.AddParameters(agentV);
initialConditions.T = DateTime.Now;
// new trajectory generation
double time = (bounderyConditions.T - initialConditions.T).TotalSeconds;
double[][,] newTrajectory = null;
if ((isNewPlanning) || ((time < maxTime) && (time > 0)))
{
newTrajectory = trajectoryPlanningStrategy.TrajectoryPlanning(initialConditions, bounderyConditions);
}
if (newTrajectory != null)
{
mLogger.AddLogMessage("LowLevel: New Trajectory Designed, Length: " + newTrajectory[0].LongLength.ToString() + " time: " + time.ToString());
commandsQueue.Replace(QueueType.Position, newTrajectory[0]);
// output the whole trajectory to log
//mLogger.AddLogMessage("LowLevel: Trajectory: " + commandsQueue.PositionToString());
commandsQueue.Replace(QueueType.Velocity, newTrajectory[1]);
}
planTime.Stop();
mLogger.AddLogMessage("LowLevel: Planning time was: " + planTime.Elapsed.TotalSeconds.ToString() + " Seconds");
}
public override PointParams ActionPlanning(AHEntities.ActionDirective action, bool isNewPlaning)
{
AHEntities.Action A = action.Action;
PointParams finalConditions = null;
Random random = new Random();
Point puckP, puckV;
physicalState = worldModel.GetPhysicalState();
puckP = new Point(physicalState["PuckX"], physicalState["PuckY"]);
puckV = new Point(physicalState["PuckVx"], physicalState["PuckVy"]);
global = worldModel.GetSize();
Point crossing;
double[] crossParams;
switch (A)
{
case AHEntities.Action.BLOCK:
#region BLOCK
crossParams = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, rp,
AHEntities.EstimateLineCrossing.blockLine, global[0], global[1]);
crossing = new Point(AHEntities.EstimateLineCrossing.blockLine, crossParams[0]);
Point defendPoint = AHEntities.EstimateLineCrossing.CalculateActionPoint(crossing,
new Point(crossParams[2], crossParams[3]), rp, rm);
mLogger.AddLogMessage("LowLevel Planning BLOCK: estimated collission: mallet: " + defendPoint.ToString() +
", puck: " + crossing.ToString());
finalConditions = new PointParams();
finalConditions.AddParameters(defendPoint);
finalConditions.AddParameters(new Point(0, 0));
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(crossParams[1]);
break;
#endregion BLOCK
case AHEntities.Action.LEAVE:
#region LEAVE
finalConditions = null;
break;
#endregion LEAVE
case AHEntities.Action.ATTACK_RIGHT:
#region ATTACK_RIGHT
//finalConditions = CalculateDirectedAttack(AHEntities.Action.ATTACK_RIGHT, puckP, puckV, physicalState["PuckR"],
// AHEntities.EstimateLineCrossing.attackLine, global[0], global[1]);
//break;
#endregion ATTACK_RIGHT
case AHEntities.Action.ATTACK_LEFT:
#region ATTACK_LEFT
//finalConditions = CalculateDirectedAttack(AHEntities.Action.ATTACK_LEFT, puckP, puckV, physicalState["PuckR"],
// AHEntities.EstimateLineCrossing.attackLine, global[0], global[1]);
//break;
#endregion ATTACK_LEFT
case AHEntities.Action.ATTACK_MIDDLE:
#region ATTACK_MIDDLE
crossParams = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, rp,
AHEntities.EstimateLineCrossing.attackLine, global[0], global[1]);
mLogger.AddLogMessage("LowLevel: Puck Estimated at: (" + AHEntities.EstimateLineCrossing.attackLine.ToString() + "," + crossParams[0].ToString() + ") in " + crossParams[1].ToString() + " seconds");
Point puckPline = new Point(AHEntities.EstimateLineCrossing.attackLine, crossParams[0]);
double velocity = 1000;
finalConditions = CalculateNaiveDirectedAttack(A, puckPline, crossParams[1],
AHEntities.EstimateLineCrossing.attackLine, rp, rm, global[0], global[1], velocity);
break;
#endregion ATTACK_MIDDLE
case AHEntities.Action.DEFENSE_ATTACK:
#region DEFENSE_ATTACK
if (Math.Abs(puckV.X) < 0.01)
{
break;
}
crossParams = AHEntities.EstimateLineCrossing.Estimate(puckP, puckV, rp,
AHEntities.EstimateLineCrossing.defenseAttackLine, global[0], global[1]);
if (crossParams == null)
{
finalConditions = null;
break;
}
crossing = new Point(AHEntities.EstimateLineCrossing.defenseAttackLine, crossParams[0]);
Point attackPoint = AHEntities.EstimateLineCrossing.CalculateActionPoint(new Point(AHEntities.EstimateLineCrossing.defenseAttackLine, crossParams[0]),
new Point(crossParams[2], crossParams[3]), rp, rm);
mLogger.AddLogMessage("LowLevel Planning DEFENSE_ATTACK: estimated collission: mallet: " + attackPoint.ToString() +
", puck estimated at: " + crossing.ToString());
finalConditions = new PointParams();
finalConditions.AddParameters(attackPoint);
double yvel = (attackPoint.Y > 0) ? (yvel = -200) : (yvel = 200);
finalConditions.AddParameters(new Point(500, yvel));
finalConditions.T = DateTime.Now + TimeSpan.FromSeconds(crossParams[1]);
break;
#endregion DEFENSE_ATTACK
case AHEntities.Action.PREPARE:
#region PREPARE
finalConditions = new PointParams();
finalConditions.AddParameters(new Point(-1000, 0));
finalConditions.AddParameters(new Point(0, 0));
finalConditions.T = action.TimeStamp + TimeSpan.FromSeconds(0.5);
break;
#endregion PREPARE
case AHEntities.Action.STUCK_ATTACK:
#region STUCK_ATTACK
double stuckTime;
stuckTime = (action.Duration - (DateTime.Now - action.TimeStamp)).TotalSeconds;
Point stuckAttackPoint = AHEntities.EstimateLineCrossing.EstimateStuck(puckP, puckV, rp, global[0], global[1], stuckTime, rm);
if (stuckAttackPoint == null)
{
finalConditions = null;
break;
}
mLogger.AddLogMessage("LowLevel Planning STUCK_ATTACK: estimated collission: mallet: " + stuckAttackPoint.ToString());
finalConditions = new PointParams();
finalConditions.AddParameters(stuckAttackPoint);
finalConditions.AddParameters(new Point(0, 0));
finalConditions.T = action.TimeStamp + TimeSpan.FromSeconds(stuckTime);
break;
#endregion STUCK_ATTACK
default:
#region default
finalConditions = null;
break;
#endregion default
}
//saving current planning (action and parameters)
lastAction = A;
if (finalConditions != null)
{
if (lastPlan == null)
{
lastPlan = new PointParams();
}
lastPlan.Clear();
lastPlan.AddParameters(finalConditions.GetParamsByIndex(1));
lastPlan.AddParameters(finalConditions.GetParamsByIndex(2));
lastPlan.T = finalConditions.T;
}
else
{
lastPlan = null;
}
return(finalConditions);
}
