/*----------------------------------------------------------------------*/
void AzSvDataS::mergeData(const char *x_fn,
const char *x_names_fn,
const char *fn_template,
const char *str,
bool doSparse,
int digits,
const char *out_x_fn,
const char *out_n_fn,
int num,
const char *names[])
{
AzSvDataS dataset;
dataset.read_features_only(x_fn, x_names_fn);
mergeData(dataset.feat(), dataset.featInfo(), fn_template, str,
doSparse, digits, out_x_fn, out_n_fn, num, names);
}
/* static */
void AzSvDataS::mergeData(int argc,
const char *argv[])
{
if (argc < 9) {
cout << "x_fn x_names_fn fn_template str doSparse digits out_x_fn out_n_fn nm1 [nm2 ...]"<<endl;
return;
}
int argx = 0;
const char *x_fn = argv[argx++];
const char *x_names_fn = argv[argx++];
const char *fn_template = argv[argx++];
const char *str = argv[argx++];
const char *doSparse_str = argv[argx++];
int digits = atol(argv[argx++]);
const char *out_x_fn = argv[argx++];
const char *out_n_fn = argv[argx++];
int num = argc-argx;
const char **names = argv+argx;
bool doSparse = false;
if (*doSparse_str == '1' || *doSparse_str == 'Y') doSparse = true;
mergeData(x_fn, x_names_fn, fn_template, str, doSparse, digits, out_x_fn, out_n_fn, num, names);
}
int DataMergeGui::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QDialog::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: addMergeList(); break;
case 1: removeMergeList(); break;
case 2: { bool _r = mergeData();
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 3: { bool _r = selectionChangedSlot();
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 4: { bool _r = upButtonSlot();
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 5: { bool _r = downButtonSlot();
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
default: ;
}
_id -= 6;
}
return _id;
}
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;
}
