//===============================================================
//===============================================================
//===============================================================
//===============================================================
void intersectFastMovingBall(const VisionData& field,
const SystemParameters& params,
float xValueOfDefendingLine,
Destination* command)
{
Line defenderLine, ballVelocityVector;
//-------------------------------
defenderLine.setAposX( xValueOfDefendingLine );
defenderLine.setBposX( xValueOfDefendingLine );
defenderLine.setAposY( -200.0f);
defenderLine.setAposY( 200.0f);
//-------------------------------
ballVelocityVector.setA( getRobocupBall(field).getPos() );
ballVelocityVector.setBposX(
getRobocupBall(field).getXPos() +
(5.0f*getRobocupBall(field).getXVel())
);
ballVelocityVector.setBposY(
getRobocupBall(field).getYPos() +
(5.0f*getRobocupBall(field).getYVel())
);
Pair intersection;
//find the intersection point for where we can best intercept the ball along our line
findLineIntersection(defenderLine,
ballVelocityVector,
&intersection);
//if we cannot find an intersection, just match the ball's position
if(
(intersection.getX() == NEVER_INTERSECT) ||
(intersection.getY() == NEVER_INTERSECT)
)
intersection.setY( getRobocupBall(field).getYPos() );
//always set our x to be the x line we sent in.
intersection.setX( xValueOfDefendingLine );
//---------------------------------------------
//If the interception point is off the field, or the ball is moving slowly,
//just follow the Y position of the ball
if( offField(params, intersection) )
{
if(intersection.getY() > (params.field.LEFT_SIDE_LINE - params.general.PLAYER_RADIUS))
intersection.setY( params.field.LEFT_SIDE_LINE - params.general.PLAYER_RADIUS );
if(intersection.getY() < (params.field.RIGHT_SIDE_LINE + params.general.PLAYER_RADIUS))
intersection.setY( params.field.RIGHT_SIDE_LINE + params.general.PLAYER_RADIUS );
}
//actually set the desination for good =-)
command->setPos( intersection );
//finally, set the rotation to look straight at the ball
command->setRotation( angleToBall( command->getPos(), field) );
}
// Find the width of a clear lane between the two locations passed as pairs.
// Returns 0.0 if the lane is blocked by any other robot.
float laneHalfWidth (
Pair passerLoc, // Position of the passing robot.
Pair passDestination, // location of the pass destination
const VisionData &field, // where everyone else is now
const SystemParameters &rp,// contains necessary game parameters
bool checkOurRobots) //to see if we check for our robots or not...
{
float minPassBuffer = 10000.0f; //Distance from pass line to nearest enemy robot
float tempPassBuffer; //Used when comparing buffer from one robot to min buffer
Pair closestPoint; //Point on pass line closest to robot
bool onSegment; //true if closestPoint is on pass line segment
Pair originalDest(passDestination.getX(),passDestination.getY());
//extend the pass destination point so an enemy a little behind will be considered
float lineLength = dist(passerLoc,passDestination);
passDestination.setX(passDestination.getX() + 2*rp.general.PLAYER_RADIUS*( (passDestination.getX() - passerLoc.getX()) / lineLength));
passDestination.setY(passDestination.getY() + 2*rp.general.PLAYER_RADIUS*( (passDestination.getY() - passerLoc.getY()) / lineLength));
//Check to make sure no enemy robots are in the way
for(RobotIndex i=ROBOT0; i<NUM_ROBOTS; i++) {
//If enemy robot is found, check to see if it is in the way
if(theirRobotFound(i, field, rp)) {
tempPassBuffer = distFromPointToSegment(passerLoc, passDestination, getTheirRobotLocation(i, field, rp), &closestPoint, onSegment)-rp.general.OPPONENT_RADIUS;
//If enemy is on segment, return 0. Pass is occluded
if(onSegment && tempPassBuffer <0.0f ) return 0.0f;
//Replace the min pass buffer if smaller buffer is found.
if (tempPassBuffer < minPassBuffer && onSegment)
minPassBuffer = tempPassBuffer;
}
}
//CODE TO CHECK OUR ROBOTS ALSO. REMEMBER TO IGNORE PASSER & RECEIVER
if(checkOurRobots) {
//Check to make sure no enemy robots are in the way
for(RobotIndex i=ROBOT0; i<NUM_ROBOTS; i++) {
//If our robot is found, and he is not the passer or receiver => check to see if it is in the way
if(robotFound(i, field, rp)&&
(dist(i,passerLoc,field,rp)>rp.general.PLAYER_RADIUS)&&
(dist(i,originalDest,field,rp)>rp.general.PLAYER_RADIUS))
{
tempPassBuffer = distFromPointToSegment(passerLoc, passDestination, getLocation(i, field, rp), &closestPoint, onSegment)- rp.general.PLAYER_RADIUS;
//If robot is on segment, return 0. Pass is occluded
if(onSegment && tempPassBuffer <0.0f) return 0.0f;
//Replace the min pass buffer if smaller buffer is found.
if (tempPassBuffer < minPassBuffer && onSegment)
minPassBuffer = tempPassBuffer;
}
}
}
//Buffer = distance from pass line to edge (not center) of nearest enemy.
float buf = minPassBuffer;
if(buf <= 0.0f) return 0.001f; //to avoid divide-by-zero errors, and so dist>0
else return buf;
}
//=====================================================
///Execute the skill. This is the main part of the skill, where you tell the
///robot how to perform the skill.
void CutGoalSkill::execute()
{
///If not active, dont do anything!
if(!initialized)
{
return;
}
else
{
//grab the ball location
if(!presetBall){
ball = strategy->getCurrentRoboCupFrame()->getDefensiveBallLocation();
}
///Calculate the angle bisector of the area we want to cover
float theta;
float theta1=angleBetween(sp->field.OUR_GOAL_LINE,point1,ball.getX(),ball.getY());
float theta2=angleBetween(sp->field.OUR_GOAL_LINE,point2,ball.getX(),ball.getY());
theta=(theta1+theta2)/2.0f;
theta=normalizeAngle(theta+PI);
float halfAngle=ABS(angleDifference(theta1,theta2)/2.0f);
//calculate midpoint to extend from
Pair midpoint;
midpoint.setY((sp->field.OUR_GOAL_LINE-ball.getX()) * TAN(theta) + ball.getY());
midpoint.setX(sp->field.OUR_GOAL_LINE);
/*debugging helpful stuff
GUI_Record.debuggingInfo.setDebugPoint(robotID,6,midpoint);
GUI_Record.debuggingInfo.setDebugPoint(robotID,7,sp->field.OUR_GOAL_LINE,point1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,8,sp->field.OUR_GOAL_LINE,point2);
Pair ang1(ball.getX()+.2f,ball.getY());
Pair ang2(ball.getX()+.2f,ball.getY());
rotateAboutPoint(ang1,ball,theta1,ang1);
rotateAboutPoint(ang2,ball,theta2,ang2);
GUI_Record.debuggingInfo.setDebugPoint(robotID,3,ang1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,4,ang2);
Pair t(ball.getX()+.2f,ball.getY());
rotateAboutPoint(t,ball,theta,t);
GUI_Record.debuggingInfo.setDebugPoint(robotID,5,t);
*/
// The ideal point we want the robot to be in this circumstances
Pair dest;
float distance = sp->general.PLAYER_RADIUS / SIN(ABS(halfAngle)) ;
//char msg[80]; sprintf(msg,"dist: %5.2f",distance);GUI_Record.debuggingInfo.addDebugMessage(msg);
extendPoint(midpoint,ball,-distance,dest);
// We have to check if the destination point is between the Upper and lower limit
// float slope = (midpoint.getY() - ball.getY()) / (midpoint.getX() - ball.getY()) ;
// If it is above the limit
if(dest.getX() > UPPER_X){
extrapolateForY(midpoint,ball,UPPER_X,dest);
}
// If it is below the limit
if(dest.getX() < LOWER_X){
extrapolateForY(midpoint,ball,LOWER_X,dest);
}
command->setControl(OMNI_NORMAL);
command->setPos(dest);
command->setRotation(angleBetween(getLocation(robotID, *currentVisionData, *sp),
ball));
strategy->getCurrentFrame()->setMessage(robotID,"Covering Goal");
}
}
