TacticTest::TacticTest(WorldModel *worldmodel, QObject *parent) :
Tactic("TacticTest", worldmodel, parent)
{
circularBorder.assign(Vector2D(1500,0),1700/2);
circularBorderOut.assign(Vector2D(1500,0),2100/2);// a circle may use to push balls with some risks
circularMid.assign(Vector2D(1500,0),720); // a circle which is between holes and border circle
hole1.assign(Vector2D(1500,1700/4),250);
hole2.assign(Vector2D(1500,-1700/4),250);
Vector2D Cdist = (hole1.center() - circularBorder.center());
double deltaAngle=1.1*asin(hole1.radius()/(Cdist.length())); // 1.1 is safety factor
Uangle1=Cdist.dir().radian() + deltaAngle ;
Dangle1=Uangle1 - 2*deltaAngle;
Cdist = (hole2.center() - circularBorder.center());
Uangle2=Cdist.dir().radian() + deltaAngle ;
Dangle2=Uangle2 - 2*deltaAngle;
state=0;
}
Vector2D Knowledge::PredictDestination(Vector2D sourcePos, Vector2D targetPos, double sourceSpeed, Vector2D targetSpeed)
{
double factor = _wm->var[3] / 250.0;
if(factor < 0)
{
factor = 0;
}
double Vm = sourceSpeed;
double k = Vm / targetSpeed.length();
double gamaT = targetSpeed.dir().radian();
Vector2D delta;
delta = targetPos - sourcePos;
double landa = atan2(delta.y, delta.x);
double theta = gamaT - landa;
if (theta > AngleDeg::PI)
{
theta -= 2 * AngleDeg::PI;
}
if (theta < - AngleDeg::PI)
{
theta += 2 * AngleDeg::PI;
}
double dlta = 0;
if(k != 0 && fabs(sin(theta) / k) < 1)
{
dlta = asin(sin(theta)/k);
}
// No solution.
else
{
//qDebug() << "Prediction: No solution.";
return targetPos;//Vector2D::INVALIDATED;
}
double tf = factor * (delta.length() / 1000) / (Vm*cos(dlta) - targetSpeed.length() * cos(theta));
// No solution.
if(tf < 0)
{
//qDebug() << "Prediction: No solution.";
return targetPos; //Vector2D(0,0); //INVALIDATED;
}
double catchDist = targetSpeed.length() * tf * 1000;
Vector2D catchDiff(catchDist * cos(gamaT), catchDist * sin(gamaT));
Vector2D Pred_pos=targetPos + catchDiff;
//_wm->predict_pos.append(Pred_pos);
return Pred_pos;
}
// =================================================================================
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;
}
RobotCommand tacticControl::getCommand()
{
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
////moving on width direction to ball////////////////////////////////////////////
// switch (state) {
// case 0:
// rc.fin_pos.loc = Vector2D(0,-1700);
// if((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length()<100) state=1;
// break;
// case 1:
// rc.fin_pos.loc=Vector2D(0,1700);
// if((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length()<100) state=0;
// break;
// }
// Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
// rc.fin_pos.dir=a.dir().radian();
/////////////////////////////////////////////////////////////////////////////////
////going to center & loking to ball/////////////////////////////////////////////
rc.fin_pos.loc = {0,0};
Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
rc.fin_pos.dir=a.dir().radian();
/////////////////////////////////////////////////////////////////////////////////
////just looking at ball/////////////////////////////////////////////////////////
// Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
// rc.fin_pos.loc = wm->ourRobot[this->id].pos.loc;
// rc.fin_pos.dir=a.dir().radian();
/////////////////////////////////////////////////////////////////////////////////
////just looking at ball/////////////////////////////////////////////////////////
// Vector2D a =wm->ball.pos.loc - wm->ourRobot[id].pos.loc;
// Vector2D b ;
// b = {1,1};
// b.setLength(200);
// b.setDir(sin(alfa/100)*180);
// alfa++;
// rc.fin_pos.dir=a.dir().radian();
// rc.fin_pos.loc = wm->ball.pos.loc + b;
/////////////////////////////////////////////////////////////////////////////////
rc.maxSpeed=2.5;
rc.useNav = true;false;
rc.isBallObs = true;
rc.isKickObs = true;
return rc;
}
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 TacticTransferObject::getCommand()
{
AllInMargin=true;
RobotCommand rc;
if(!wm->ourRobot[id].isValid) return rc;
rc.useNav=true;
rc.maxSpeed = 1;
rc.fin_pos.loc=wm->endPoint;//Vector2D(300,0);
int object;
addData();
mergeData();
sortData();
// if(wm->balls.size() > 0)
// {
// qDebug()<< " BALLL . X = " << wm->balls.at(0)->pos.loc.x << " BALLL . Y = " << wm->balls.at(0)->pos.loc.y;
// qDebug() << " MAX x : " << region[1].maxX() << " MIN X : " << region[1].minX() ;
// qDebug() << " MAX y : " << region[1].maxY() << " MIN y : " << region[1].minY() ;
// if(region[0].IsInside(wm->balls.at(0)->pos.loc)) qDebug() << " THE BALLLLL ISSS INNNNN SIDE !!!!!!!!!!!!!!!!!!!!!!1";
// }
index = -1;
for(int i=0;i<mergedList.size();i++)
{
// qDebug() << i << " AT : (" << mergedList.at(i).pos.x << "," << mergedList.at(i).pos.y << ")";
temp=0;
if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos) && !IsInmargins(mergedList.at(i).pos,300))
{
//qDebug() <<" OBJECT : " << mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
// AllInMargin=false;
index=i;
goalRegion=mergedList.at(i).goalRegion;
temp=1;
break;
}
}
for(int i=0; i<mergedList.size(); i++)
{
if(!IsInmargins(mergedList.at(i).pos,300))
{
AllInMargin=false;
}
}
if(AllInMargin)
{
for(int i=0;i<mergedList.size();i++)
{
if(!region[mergedList.at(i).goalRegion].IsInside(mergedList.at(i).pos))
{
index=i;
goalRegion=mergedList.at(i).goalRegion;
break;
}
}
}
// if(index ==-1)
// {
// for(int i=0;i<wm->Chasbideh.size(); i++)
// {
// if(!region[0].IsInside(wm->Chasbideh.at(i).position) && !region[1].IsInside(wm->Chasbideh.at(i).position))
// {
// //qDebug() <<" OBJECT : " << mergedList.at(i).pos.x << " ------ Y = " << mergedList.at(i).pos.y;// TOOOOOOOOOOOOOOOOOOOSHE !!!!!!!" << index ;
// index=i;
// goalRegion=0;//mergedList.at(i).goalRegion;
// temp=1;
// break;
// }
// }
// }
// qDebug() << mergedList.size() << " MERGED SIZE " ;
if(index != -1)
{
Vector2D point2 = mergedList.at(index).pos;
Vector2D diff2 = region[goalRegion].center() - point2;
bool reach=false;
if(temp!=0)
{
switch(state)
{
case 0:{ //Go Behind the Object
Vector2D space2=diff2;
space2.setLength(300);
rc.maxSpeed=1.4;
rc.useNav = true;
rc.fin_pos.loc=point2 - space2;
rc.fin_pos.dir=diff2.dir().radian();
object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS+150));
rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,150);
if(reach) state = 1;
}
break;
case 1:{//Ready to Push
rc.useNav = false;
rc.maxSpeed=1.2;
rc.fin_pos.loc.x=point2.x - (100 + ROBOT_RADIUS)*(diff2.x)/(diff2.length()); // 100 >> Rounded Radius
rc.fin_pos.loc.y=point2.y - (100 + ROBOT_RADIUS)*(diff2.y)/(diff2.length());
rc.fin_pos.dir=diff2.dir().radian();
if(((wm->ourRobot[id].pos.loc-point2).length())>400) state=0;
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100);
if(reach)
state = 2;
}
break;
case 2:{//Push
//Vector2D diff2 = region2.center() - wm->ourRobot[id].pos.loc ;
rc.useNav = false;
rc.maxSpeed=1;
//if(diff2.length() > 1500) diff2.setLength(1500);
// if(((wm->ourRobot[id].pos.loc-point2).length())>400) state=0;
diff2.setLength(300);
if(((wm->ourRobot[id].pos.loc-point2).length())>600) state=0;
if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<50) state=0;
Vector2D o2r = ( point2 - wm->ourRobot[id].pos.loc );
if(fabs(wm->ourRobot[id].pos.dir - o2r.dir().radian()) > AngleDeg::deg2rad(40))
{
qDebug() << " !!!! Out OF Direction !!!! " ;
state=1;//4;
}
rc.fin_pos.loc=point2 + diff2;//5;
rc.fin_pos.dir=diff2.dir().radian();
reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,10);
if(reach)
state = 3;
}
break;
case 3:{//Release
if(region[goalRegion].IsInside(point2))
{
//qDebug() << " INNNNNNNNNNNN SIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIDE !!!";
//if(index==agentsR1.size()) rc.fin_pos.loc=Vector2D(0,0);
if(temp==0)
{
rc.fin_pos.loc=Vector2D(0,0);
break;
}
//agentsR1.takeFirst();
//index++;
}
//if(reach)
state = 0;
}
break;
case 4:{// back behind the object
}
break;
}
}
Vector2D dlta;
double mrgn=300;
if(IsInmargins(point2,mrgn))
{
// qDebug() << " IS IN MARGIN !!!!!!!!!";
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(side*10,side*(ROBOT_RADIUS+/*mergedShapeList.at(index).roundedRadios+*/50));}
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+/*mergedShapeList.at(index).roundedRadios+*/50),side*10);}
switch(statemargin)
{
case 0:{
rc.fin_pos.loc=point2+dlta;
// object=findnearestObject(mergedShapeList,wm->ourRobot[id].pos.loc);
// if(object!=-1) ObsC=Circle2D(mergedShapeList.at(object).position,(mergedShapeList.at(object).roundedRadios+ROBOT_RADIUS));
// rc.fin_pos.loc=AvoidtoEnterCircle(ObsC,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);
// int rad = mergedShapeList.at(index).roundedRadios+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;
}
}
// qDebug() << rc.fin_pos.loc.x << " ------- Y = " << rc.fin_pos.loc.y << " STATE = " << state;
// qDebug() << "STATE = " << state;
}
//rc.maxSpeed = 1.2;//rc.maxSpeed;
// rc.fin_pos.loc.x=rcpast.x + 0.1*(rc.fin_pos.loc.x-rcpast.x);
// rc.fin_pos.loc.y=rcpast.y + 0.1*(rc.fin_pos.loc.y-rcpast.y);
// rcpast=rc.fin_pos.loc;
// qDebug() << " INDEX = " << index ;
rc.maxSpeed/=1.4;
if(IsInmargins(wm->ourRobot[id].pos.loc,500)) rc.maxSpeed /= 1.5 ;
rc.fin_pos.loc=KeepInField(rc);
// qDebug() << " This Object Is For Region " << goalRegion ;
rc.useNav=false;
rc.isBallObs = false;
rc.isKickObs = true;
return rc;
}
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;
}
