// 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) );
}
//----------------------------------------------------------------------------
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();
}
}
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;
}
//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void LookForChipKickSkill::execute()
{
///If not active, dont do anything!
if(!initialized) return;
Pair ballLoc = getBallLocation(*currentVisionData);
float currentRot = angleToBall(robotID,*currentVisionData,*sp);
Pair dest((sp->field.HALF_LINE + sp->field.OUR_GOAL_LINE)/2.0f,
(sp->field.SPLIT_LINE + sp->field.LEFT_SIDE_LINE)/2.0f);
//setup on same side of field as ball
if(ballLoc.getY() < sp->field.SPLIT_LINE)
dest.setY(-dest.getY());
command->setPos(dest);
command->setRotation(currentRot);
}
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 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();
}
}
void PassingChallengePlay::executePlay(VisionData* vision, RobocupStrategyData* rsd)
{
RobotIndex r0=ROBOT0;
RobotIndex r1=ROBOT1;
Pair ballLoc=getBallLocation(*vision);
///----
for(RobotIndex robotID = r0; robotID < ROBOT2; robotID++){
Pair robotLoc=getLocation(robotID,*vision,rsd->getSystemParams());
Pair otherPos;
int side;
if(robotID == r0){
otherPos=getLocation(r1,*vision,rsd->getSystemParams());
side=side0;
}else{
otherPos=getLocation(r0,*vision,rsd->getSystemParams());
side=side1;
}
if((ballLoc.getX() > rsd->getSystemParams().field.HALF_LINE &&
robotID==r0) ||
(ballLoc.getX() < rsd->getSystemParams().field.HALF_LINE &&
robotID==r1))
{
//ball on our side
if(friendlyHasPossession(robotID,rsd->getSystemParams())){
//go kick
if(-robotLoc.getY()*side > (rsd->getSystemParams().field.SPLIT_LINE + .5f)){
//we're there, kick
rsd->getDestination(robotID)->setKick(KICK_PASS);
}else{
//not there yet.
rsd->getDestination(robotID)->setDribble(DRIBBLE_DEFAULT);
rsd->getDestination(robotID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
//head across
float backup=.3f;
if(robotID == r1){
rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE - backup,-side*1.0f);
rsd->getDestination(robotID)->setRotation(0.0f);
}else{
rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE + backup,-side*1.0f);
rsd->getDestination(robotID)->setRotation(PI);
}
}
}else{
//go grab ball
if(!rsd->getStrategyModule().getSkillSet(robotID)->getSkill(AcquirePossessionSkill::skillNum)->isInitialized()){
rsd->getStrategyModule().getSkillSet(robotID)->getSkill(AcquirePossessionSkill::skillNum)->initialize();
}
rsd->getStrategyModule().getSkillSet(robotID)->getSkill(AcquirePossessionSkill::skillNum)->run();
rsd->getDestination(robotID)->setDribble(DRIBBLE_DEFAULT);
rsd->getDestination(robotID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
rsd->getDestination(robotID)->setKick(NO_KICK);
rsd->getDestination(robotID)->setSpeed(GOALIE_SPEED);
}
}else{
int side;
if(ballLoc.getY() > rsd->getSystemParams().field.SPLIT_LINE){
side=1;
}else{
side=-1;
}
if(robotID == r0){
side0=side;
}else{
side1=side;
}
float backup=.5f;
if(robotID == r1){
rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE - backup,otherPos.getY());
rsd->getDestination(robotID)->setRotation(0.0f);
}else{
rsd->getDestination(robotID)->setPos(rsd->getSystemParams().field.HALF_LINE + backup,otherPos.getY());
rsd->getDestination(robotID)->setRotation(PI);
}
}
}
/*
Pair aggressorPos(getLocation(robot1ID,*vision,rsd->getSystemParams()));
Pair creatorPos(getLocation(robot2ID,*vision,rsd->getSystemParams()));
Pair ballLoc(getBallLocation(*vision));
if(ballLoc.getX() > rsd->getSystemParams().field.HALF_LINE)
{
turn = LEFT;
}
else
{
turn = RIGHT;
}
if(state == KICK_RECEIVE && readTimer() > MAX_ELAPSED_TIME)
{
state = MOVE;
uninitializeSkills();
}
if(state == MOVE && dist(getLocation(robot1ID,*vision,rsd->getSystemParams()),dest) < DISTANCE_THRESHOLD &&
dist(getLocation(robot2ID,*vision,rsd->getSystemParams()),dest1) < DISTANCE_THRESHOLD &&
(friendlyHasPossession(rsd->getRobotByPosition(AGGRESSOR),rsd->getSystemParams(),*vision, *rsd,true) ||
friendlyHasPossession(rsd->getRobotByPosition(CREATOR),rsd->getSystemParams(),*vision, *rsd,true)))
{
state = ROTATE;
uninitializeSkills();
}
if(state == ROTATE && ABS(getRotation(robot1ID,*vision,rsd->getSystemParams()) - PI < ROTATION_THRESHOLD &&
ABS(getRotation(robot2ID,*vision,rsd->getSystemParams()) - 0) < ROTATION_THRESHOLD) &&
(friendlyHasPossession(rsd->getRobotByPosition(AGGRESSOR),rsd->getSystemParams(),*vision, *rsd,true) ||
friendlyHasPossession(rsd->getRobotByPosition(CREATOR),rsd->getSystemParams(),*vision, *rsd,true)))
{
state = KICK_RECEIVE;
uninitializeSkills();
}
if(turn == LEFT)
{
if(state == KICK_RECEIVE)
{
kickSkill1->initialize(KICK_PASS);
receiveSkill2->initialize(0.5f,dest1,false,true);
kickSkill1->run();
receiveSkill2->run();
turn = RIGHT;
dest.set(POINT_X,-POINT_Y);
dest1.set(-POINT_X,-POINT_Y);
startTimer();
rsd->setMessage(robot1ID,"Kicking");
rsd->setMessage(robot2ID,"Receiving");
}
if(state == MOVE)
{
rsd->getDestination(robot1ID)->setDribble(DRIBBLE_DEFAULT);
rsd->getDestination(robot1ID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
if(getBallLocation(*vision).getX() < rsd->getSystemParams().field.HALF_LINE)
{
rsd->getDestination(robot2ID)->setPos(dest1);
rsd->setMessage(robot2ID,"Moving to Receive");
}
rsd->setMessage(robot1ID,"Moving with Ball to pass.");
if(!friendlyHasPossession(rsd->getRobotByPosition(AGGRESSOR),rsd->getSystemParams(),*vision, *rsd,true))
{
acquireSkill1->initialize();
acquireSkill1->run();
}
else
{
rsd->getDestination(robot1ID)->setPos(dest);
}
}
if(state == ROTATE)
{
rsd->getDestination(robot2ID)->setRotation(angleBetween(creatorPos,aggressorPos));
rsd->setMessage(robot2ID,"Rotating to receive");
rsd->setMessage(robot1ID,"Rotating to kick");
if(!friendlyHasPossession(rsd->getRobotByPosition(AGGRESSOR),rsd->getSystemParams(),*vision, *rsd,true))
{
acquireSkill1->initialize();
acquireSkill1->run();
rsd->getDestination(robot2ID)->setDribble(DRIBBLE_DEFAULT);
rsd->getDestination(robot2ID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
rsd->getDestination(robot1ID)->setKick(NO_KICK);
}
else
{
rotateSkill1->initialize(dest1);
rotateSkill1->run();
}
}
}
else
{
if(state == KICK_RECEIVE)
{
kickSkill2->initialize(KICK_PASS);
receiveSkill1->initialize(0.5f,dest,false,true);
kickSkill2->run();
receiveSkill1->run();
turn = LEFT;
dest.set(POINT_X,POINT_Y);
dest1.set(-POINT_X,POINT_Y);
startTimer();
rsd->setMessage(robot2ID,"Kicking");
rsd->setMessage(robot1ID,"Receiving");
}
if(state == MOVE)
{
if(getBallLocation(*vision).getX() > rsd->getSystemParams().field.HALF_LINE)
{
rsd->getDestination(robot1ID)->setDribble(DRIBBLE_DEFAULT);
rsd->getDestination(robot1ID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
rsd->getDestination(robot1ID)->setPos(dest);
rsd->setMessage(robot1ID,"Moving to Receive");
}
rsd->setMessage(robot2ID,"Moving with Ball to pass.");
if(!friendlyHasPossession(rsd->getRobotByPosition(CREATOR),rsd->getSystemParams(),*vision, *rsd,true))
{
acquireSkill2->initialize();
acquireSkill2->run();
rsd->getDestination(robot2ID)->setDribble(DRIBBLE_DEFAULT);
rsd->getDestination(robot2ID)->setVerticalDribble(V_DRIBBLE_DEFAULT);
rsd->getDestination(robot2ID)->setKick(NO_KICK);
}
else
{
rsd->getDestination(robot2ID)->setPos(dest1);
}
}
if(state == ROTATE)
{
rsd->getDestination(robot1ID)->setRotation(angleBetween(creatorPos,aggressorPos));
rsd->setMessage(robot1ID,"Rotating to receive");
rsd->setMessage(robot2ID,"Rotating to kick");
if(!friendlyHasPossession(rsd->getRobotByPosition(CREATOR),rsd->getSystemParams(),*vision, *rsd,true))
{
acquireSkill2->initialize();
acquireSkill2->run();
}
else
{
rotateSkill2->initialize(dest1);
rotateSkill2->run();
}
}
}
*/
}
