/* ---------------------------------------------------------------------- */
Vector2D Circle2D::nearestpoint(Vector2D pnt)
{
Vector2D diff = pnt - this->center();
diff.setLength(this->radius());
Vector2D dest=this->center()+diff;
return dest;
}
Position Knowledge::AdjustKickPoint(Vector2D ballPos, Vector2D target, int kickSpeed)
{
Position p;
Vector2D dir = (ballPos - target);//.normalizedVector();
dir.setLength(/*scale(*/ROBOT_RADIUS - (35 /*- kickSpeed*/));
p.loc = ballPos + dir;
p.dir = (-dir).dir().radian();
return p;
}
// =================================================================================
Vector2D TacticPush2Goal::GoOncircle(Vector2D center, double radius)//, Vector2D Object)
{
//for (int k=0;k<1000;k++)
Vector2D diff = wm->ourRobot[id].pos.loc - center;
diff.setDir(diff.dir()+1.1);//50);//.radian() + AngleDeg::deg2rad(5));
diff.setLength(radius);
Vector2D point = center + diff;
return point;
}
bool SkillCircle::execute(RobotCommand &rc)
{
if(_rid==-1) return false;
Vector2D vc;
vc = _wm->ourRobot[_rid].pos.loc;
vc.setLength(500);
vc.rotate(10);
rc.FinalPos.loc=vc;
rc.TargetPos.loc=vc;
rc.FinalPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian();
rc.TargetPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian();
rc.Speed=1;
return true;
}
/*!
*/
bool
Bhv_SetPlayGoalKick::doKickToFarSide( PlayerAgent * agent )
{
const WorldModel & wm = agent->world();
Vector2D target_point( ServerParam::i().ourPenaltyAreaLineX() - 5.0,
ServerParam::i().penaltyAreaHalfWidth() );
if ( wm.ball().pos().y > 0.0 )
{
target_point.y *= -1.0;
}
double ball_move_dist = wm.ball().pos().dist( target_point );
double ball_first_speed = calc_first_term_geom_series_last( 0.7,
ball_move_dist,
ServerParam::i().ballDecay() );
ball_first_speed = std::min( ServerParam::i().ballSpeedMax(),
ball_first_speed );
ball_first_speed = std::min( wm.self().kickRate() * ServerParam::i().maxPower(),
ball_first_speed );
Vector2D accel = target_point - wm.ball().pos();
accel.setLength( ball_first_speed );
double kick_power = std::min( ServerParam::i().maxPower(),
accel.r() / wm.self().kickRate() );
AngleDeg kick_angle = accel.th();
dlog.addText( Logger::TEAM,
__FILE__" (doKickToFarSide) target=(%.2f %.2f) dist=%.3f ball_speed=%.3f",
target_point.x, target_point.y,
ball_move_dist,
ball_first_speed );
dlog.addText( Logger::TEAM,
__FILE__" (doKickToFarSide) kick_power=%f kick_angle=%.1f",
kick_power,
kick_angle.degree() );
agent->doKick( kick_power, kick_angle - wm.self().body() );
agent->setNeckAction( new Neck_ScanField() );
return true;
}
void SimpleBirdController::affect(float ms)
{
Entity *entity = getEntity();
if(currentPoint >= 0) {
Vector2D position = entity->getPosition();
Vector2D target = points[currentPoint];
Vector2D velocity = target - position;
velocity.setLength(entity->getSpeed());
Vector2D destination = position + velocity * ms;
/* We cross the target. */
if(destination.getDistance(target) >
position.getDistance(target) ||
position.getDistance(target) < 0.00001f)
currentPoint = (currentPoint + 1) % points.size();
entity->setVelocity(velocity);
}
}
OperatingPosition Knowledge::AdjustKickPointB(Vector2D ballLoc, Vector2D target, Position robotPos)
{
OperatingPosition KickPos;
Vector2D KickDir = (target - ballLoc);
bool shoot_sensor = true;
double DirErr;
double DistErr;
double BallDir = _wm->ball.vel.loc.dir().radian();
double DAngel = AngleDeg ::deg2rad(80);
if(_wm->ball.vel.loc.length()>.2 && (-KickDir.dir().radian()-DAngel)<BallDir && (-KickDir.dir().radian()+DAngel)>BallDir)
{
KickPos.pos.dir = BallDir-M_PI;
if (KickPos.pos.dir > M_PI) KickPos.pos.dir -= 2 * M_PI;
if (KickPos.pos.dir < -M_PI) KickPos.pos.dir += 2 * M_PI;
KickPos.pos.loc = ballLoc ;
}
else
{
//possession point >>navigation : ON
KickDir.setLength( ROBOT_RADIUS + BALL_RADIUS*2);
KickPos.useNav = true ;
KickPos.pos.dir = KickDir.dir().radian();
KickPos.pos.loc = ballLoc - KickDir;
//possession point check
DirErr = AngleDeg::rad2deg(fabs(KickPos.pos.dir - robotPos.dir));
if(DirErr > 360.0) DirErr = 360.0 - DirErr ;
DistErr = (KickPos.pos.loc - robotPos.loc).length();
if(DirErr < 15 && DistErr < BALL_RADIUS*2) kickPermission = true;
if(DirErr > 19 && DistErr > BALL_RADIUS*3) kickPermission = false;
}
if(kickPermission)
{
Vector2D ball_vel_change;
ball_vel_change =_wm->ball.vel.loc - last_ball_vell;
last_ball_vell = _wm->ball.vel.loc ;
if(ball_vel_change.length()>0.02)
shoot_sensor = false;
//control point >>navigation : OFF
KickDir.setLength( ROBOT_RADIUS- BALL_RADIUS);
KickPos.useNav = false ;
KickPos.pos.dir = KickDir.dir().radian();//(ballLoc - robotPos.loc).dir().radian();//
KickPos.pos.loc = ballLoc - KickDir;
DirErr = AngleDeg::rad2deg(fabs(KickPos.pos.dir - robotPos.dir));
if(DirErr > 360.0) DirErr = 360.0 - DirErr ;
DistErr = (KickPos.pos.loc - robotPos.loc).length();
//qDebug()<<"A"<<DirErr<<DistErr<<ball_vel_change.length();
//#kick distance and angel limits
if(shoot_sensor)//kicking limits when shooting with sensor
{
}
else//kicking limits when shooting without sensor
{
if(DirErr < 10 && DistErr < 45)
{
KickPos.readyToShoot = true;
qDebug()<<"shooooooooooooooooooooooooooooooooooooooot";
}
else
{
KickPos.readyToShoot = false;
}
}
//##kick distance and angel limits
}
return KickPos;
}
RobotCommand TacticPush2Goal::getCommand()
{
oppIsValid = wm->ourRobot[0].isValid;//wm->theirRobot.IsValid;// opposite robot
addData();
sortData();
if(!oppIsValid) opp = Vector2D(1000000,1000000);
opp = wm->ourRobot[0].pos.loc;//wm->theirRobot.position;//wm->ourRobot[8].pos.loc;
OppIsKhoraak=!circularBorder2.contains(opp);//out of his field
bool reach=false;
Avoided=false;
AllIn=true;
AnyIn=false;
AllInCorner=true;
AllUnAccessible=true;
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
rc.fin_pos.loc=Vector2D(400,0);//circularBorder.center();
rc.maxSpeed = 1.2;
index=-1;
int tah=balls.size()-1;
if(tah != -1)
{
rc.fin_pos.loc=circularBorder2.nearestpoint(balls.at(tah)->pos.loc);
rc.fin_pos.dir=(circularBorder.center()-rc.fin_pos.loc).dir().radian();
}
FindBall(); // find approtiate ball
if( index != -1 )
{
point2 = balls.at(index)->pos.loc;
FindHole();
Vector2D nrstpnt = (circularBorder.center()-point2); //nearest Point
nrstpnt=nrstpnt.setLength(nrstpnt.length()-circularBorder2.radius());
diff2 = nrstpnt;
nrstpnt=point2+nrstpnt;
// state2=0;
switch(state)
{
case 0:{ //Go Behind the Object
vec2goal.setLength(250);
// qDebug()<<"VEC 2 GOAL LENGTH = " << vec2goal.length();
rc.maxSpeed=1.3;
rc.useNav = false;//true;
rc.isBallObs = false;//true;
rc.isKickObs = false;//true;
rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2 - vec2goal,
rc.maxSpeed,balls.at(index)->vel.loc);//point2 - vec2goal;
// wm->kn->PredictDestination()
int rad = 150+ROBOT_RADIUS;
Circle2D c(point2,rad);
rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// if(Avoided) rc.maxSpeed=rc.maxSpeed;
rc.fin_pos.dir=vec2goal.dir().radian();
// KeepInField(rc);
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
if(reach && !Avoided) state = 1;
}
break;
case 1:{//Push
rc.useNav = false;
rc.isBallObs = false;
rc.isKickObs = false;
rc.maxSpeed=1.1;
vec2goal.setLength(100);
rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2 + vec2goal,
rc.maxSpeed,balls.at(index)->vel.loc);
rc.fin_pos.dir=vec2goal.dir().radian();
KeepInField(rc);
if(((wm->ourRobot[id].pos.loc-point2).length())>800) state=0;
if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;
if(hole1.contains(rc.fin_pos.loc) || hole2.contains(rc.fin_pos.loc))
{
// vec2goal.setLength(100);
rc.fin_pos.loc=point2 ;//+ vec2goal;
// rc.fin_pos.loc=point2;
}
// reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,40);
// if(reach)
// state2 = 0;
}
break;
}
int mrgn=200;
Vector2D dlta;
if(IsInmargins(point2,mrgn))
{
int side = ((point2.x-mean_x)/abs(point2.x-mean_x))*((point2.y-mean_y)/abs(point2.y-mean_y));
if(point2.x > MAX_X-mrgn || point2.x < MIN_X+mrgn) {
side *= ((point2.y-mean_y)/abs(point2.y-mean_y));
dlta=Vector2D(0,side*(ROBOT_RADIUS+20));}
else if(point2.y > MAX_Y-mrgn || point2.y < MIN_Y+mrgn) {
side *=((point2.x-mean_x)/abs(point2.x-mean_x));
dlta=Vector2D(side*(ROBOT_RADIUS+20),0);}
switch(statemargin)
{
case 0:{
rc.fin_pos.loc=point2+dlta;
int rad = 70+ROBOT_RADIUS;
Circle2D c(point2,rad);
rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
qDebug()<< "In Margins Pos : ball = ( " << point2.x << ","<< point2.y << ")";
qDebug()<< "In Margins Pos : delta = ( " << dlta.x << ","<< dlta.y << ")";
qDebug()<< "In Margins Pos : fin_pos = ( " << rc.fin_pos.loc.x << ","<<rc.fin_pos.loc.y << ")";
qDebug()<< "In Margins Pos : Robot = ( " << wm->ourRobot[id].pos.loc.x << ","<<wm->ourRobot[id].pos.loc.y << ")";
rc.fin_pos.dir=dlta.dir().radian()-side*M_PI/2;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,7);
// wm->ourRobot[id].pos.loc,rc.fin_pos.loc,200);
qDebug() << "dist To final Pos : " << (wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length();
qDebug() << " Avoided : " << Avoided << " reach" << reach;
if(reach) statemargin = 1;
}
break;
case 1:{
rc.fin_pos.dir = dlta.dir().radian() - side*0.9*M_PI ;
rc.fin_pos.loc=point2-dlta;
qDebug() << "Fin_POS . dir = " << AngleDeg::rad2deg(rc.fin_pos.dir) << " ROBOT . dir = " << AngleDeg::rad2deg(wm->ourRobot[id].pos.dir);
if(((wm->ourRobot[id].pos.loc-point2).length())>300) statemargin=0;
double delta_ang=wm->ourRobot[id].pos.dir-rc.fin_pos.dir;
if (delta_ang > M_PI) delta_ang -= (M_PI * 2);
if (delta_ang < -M_PI) delta_ang += (M_PI * 2);
if(fabs(delta_ang) < AngleDeg::deg2rad(10)) statemargin=0;
rc.maxSpeed=1.7;
// bool chighz=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,10);
// if(chighz) statemargin=0;
// if((wm->ourRobot[id].pos.loc.dir()-rc.fin_pos.dir)<AngleDeg::deg2rad(10)) statemargin=0;
// if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0;
}
break;
}
}
}
// if(DontEnterCircle && circularBorderDANGER.contains(wm->ourRobot[id].pos.loc) && circularBorder2.contains(point2))//circularBorderDANGER.contains(rc.fin_pos.loc))
// {
// rc.fin_pos.loc=circularBorderDANGER.nearestpoint(point2);//wm->ourRobot[id].pos.loc;//AvoidtoEnterCircle(circularBorderDANGER,point2);
// rc.maxSpeed=1.1;
// }
// qDebug()<< "BEFOR ANY CHANGE " << "fin_pos.x " << rc.fin_pos.loc.x << " Y "<<rc.fin_pos.loc.y<< " ------------------------------ STATE = " << state << " STATE 2 =" << state2;
// qDebug()<< " " << "ROBOT POS.x " << wm->ourRobot[id].pos.loc.x << " Y "<< wm->ourRobot[id].pos.loc.y;
// qDebug() << "Distance To Fin_Pos = " << (rc.fin_pos.loc-wm->ourRobot[id].pos.loc).length();
rc.fin_pos.loc=KeepInField(rc);
if(/*((wm->ourRobot[id].pos.loc-opp).length() < 700)*/!OppIsKhoraak && oppIsValid )
{
rc.fin_pos.loc=AvoidtoEnterCircle(circularBorderDANGER,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);//rc.fin_pos.loc);
}
rc.fin_pos.loc=AvoidtoEnterCircle(hole1_Offset,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
rc.fin_pos.loc=AvoidtoEnterCircle(hole2_Offset,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
if(oppIsValid && OppIsKhoraak) rc.fin_pos.loc=opp;
qDebug() << "State Margin = " << statemargin;
// if(!OppIsKhoraak)
// {
// Circle2D c(opp,2*ROBOT_RADIUS+350);
// rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// }
// bool rich = wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,10,5);
// if(rich) rc.fin_pos.loc=wm->ourRobot[id].pos.loc;
// rc.fin_pos.loc=Vector2D(60000,60000);
// rc.fin_pos.loc=KeepInField(rc);
// rc.fin_pos.loc=Vector2D(MAX_X,MAX_Y);
// qDebug() << " DIRECTION ERROR = " << (rc.fin_pos.dir-wm->ourRobot[id].pos.dir)*(180/3.14);
//rc.useNav = true;
// rc.maxSpeed=1.8*rc.maxSpeed;
// rc.isBallObs = false;
// rc.isKickObs = false;
qDebug()<< "INDEX = " << index << "fin_pos.x " << rc.fin_pos.loc.x << " Y "<<rc.fin_pos.loc.y<< " ------------------------------ STATE = " << state << " STATE 2 =" << state2;
//qDebug()<< " " << "ROBOT POS.x " << wm->ourRobot[id].pos.loc.x << " Y "<< wm->ourRobot[id].pos.loc.y;
//qDebug() << "Distance To Fin_Pos = " << (rc.fin_pos.loc-wm->ourRobot[id].pos.loc).length();
// qDebug() << " IS INSIDE = " << IsInside << " UN ACCESSIBLE :" << unAccessible;
// qDebug() << " BALL SIZE : " << wm->balls.size();
// qDebug() << " ROBOT IS IN CORNER " << IsInCorner(wm->ourRobot[id].pos.loc,70) << " Robot Is In Margin : " << IsInmargins(wm->ourRobot[id].pos.loc,70) ;
rc.maxSpeed=1.21*rc.maxSpeed;
return rc;
}
