// 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;
}
//===============================================================
//===============================================================
//===============================================================
//===============================================================
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) );
}
float distanceBetween2Points(Pair A, Pair B)
{
float distance, x, y, bValueX, bValueY, aValueX, aValueY;
bValueX = B.getX();
bValueY = B.getY();
aValueX = A.getX();
aValueY = A.getY();
x = pow(bValueX - aValueX,2);
y = pow(bValueY - aValueY,2);
if (x == 0 && y == 0)
{
distance = 0;
}
else
{
distance = sqrt(x+y);
}
return distance;
}
//----------------------------------------------------------------------------
void Defender::singleAssistOffense(RobotIndex ID,
BasePlay* play,
const VisionData& field,
RobocupStrategyData* sd)
{
Pair ballPos = getBallLocation(field);
//run tight defense skill
if(
(ballPos.getX() < (sd->getSystemParams().field.DEFENSE_ZONE_LINE - sd->getSystemParams().general.PLAY_TRANSITION_HISTORISIS)) &&
((ballPos.getY() > (sd->getSystemParams().field.LEFT_MIDDLE_SECTOR + sd->getSystemParams().general.PLAY_TRANSITION_HISTORISIS)) ||
(ballPos.getY() < (sd->getSystemParams().field.RIGHT_MIDDLE_SECTOR - sd->getSystemParams().general.PLAY_TRANSITION_HISTORISIS)))
)
tightDefense(ID, play, field, sd);
else
{
//-------------------
//Display message
//-------------------
sd->setMessage(ID, "DEFENDER Single Assist Offense");
//------------------- //get a handle on skillset for this robot
//-------------------
SkillSet* skills = sd->getStrategyModule().getSkillSet(ID);
//-------------------
//get a handle on loose defense defender skill
//-------------------
TandemDefenderSkill* skillHandle =
(TandemDefenderSkill*)skills->getSkill(TandemDefenderSkill::skillNum);
//-------------------
//initialize skill if it is not initialized
//-------------------
if(!skillHandle->isInitialized())
skillHandle->initialize();
//-------------------
//run skill
//-------------------
skillHandle->run();
}
}
void SimpleKickTest::executePlay(VisionData* vision,
RobocupStrategyData* rsd)
{
Pair result;
result = getBallLocation((*vision));
rsd->getDestination(ID)->setPos(result.getX(),result.getY());
rsd->getDestination(ID)->setKick(KICK_SHOT);
}
//=====================================================
///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");
}
}
//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void SupplementThreeManSkill::execute()
{
///If not initialized, dont do anything!
if(!initialized)
{
return;
}
//if the ball is close enough to defenisve players, move next to
//the defensive player that is closer to the ball
//since he'll become the aggresor and move upfield.
//this way we can slide in and form the defensive wall immediately.
Pair ballLoc = getBallLocation(*currentVisionData);
Pair robotLoc = getLocation(robotID, *currentVisionData, *sp);
RobotIndex closeDefensiveID = NO_ROBOT;
float closestDistance;
RobotIndex tempID;
float tempDistance;
tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER);
if(tempID != NO_ROBOT)
{
closestDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
closeDefensiveID = tempID;
}
tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
if(tempID != NO_ROBOT)
{
tempDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
if(tempDistance < closestDistance)
{
closestDistance = tempDistance;
closeDefensiveID = tempID;
}
if(closeDefensiveID == strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER))
closeDefensiveID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
}
tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
if(tempID != NO_ROBOT)
{
tempDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
if(tempDistance < closestDistance)
{
closestDistance = tempDistance;
closeDefensiveID = tempID;
}
if(closeDefensiveID == strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER))
closeDefensiveID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
}
//if closest defensive player
//a.) exists
//b.) within tolerance, then go to side of him
if(closestDistance < 3.0f*sp->general.PLAYER_RADIUS)
{
Pair defensivePlayer = getLocation(closeDefensiveID, *currentVisionData, *sp);
if(defensivePlayer.getY() > sp->field.SPLIT_LINE)
{
command->setYPos(defensivePlayer.getY() + sp->general.PLAYER_RADIUS + 0.02f);
}
else
{
command->setYPos(defensivePlayer.getY() - sp->general.PLAYER_RADIUS - 0.02f);
}
command->setXPos(defensivePlayer.getX());
command->setRotation(angleBetween(robotLoc, ballLoc));
}
//else acquire the ball
else
{
Skill* skillHandle = skillSet->getSkill(AcquirePossessionSkill::skillNum);
if(!skillHandle->isInitialized())
skillHandle->initialize();
skillHandle->run();
}
}
