void RobotsFomation::resetRobots(Robot** r,int team)
{
dReal dir=-1;
if (team==1) dir = 1;
for (int k=0;k<ROBOT_COUNT;k++)
{
r[robotIndex(k,team)]->setXY(x[k]*dir,y[k]);
r[robotIndex(k,team)]->resetRobot();
}
}
void GLWidget::moveCurrentRobot()
{
ssl->show3DCursor = true;
ssl->cursor_radius = cfg->robotSettings.RobotRadius;
state = 1;
moving_robot_id = robotIndex(Current_robot,Current_team);
}
void GLWidget::resetRobot()
{
if (Current_robot!=-1)
{
ssl->robots[robotIndex(Current_robot, Current_team)]->resetRobot();
}
}
void GLWidget::paintGL()
{
if (!ssl->g->isGraphicsEnabled()) return;
if (cammode==1)
{
dReal x,y,z;
int R = robotIndex(Current_robot,Current_team);
if(ssl->robots[R] != NULL)
{
ssl->robots[R]->getXY(x,y);z = 0.3;
ssl->g->setViewpoint(x,y,z,ssl->robots[R]->getDir(),-25,0);
}
}
if (cammode==-1)
{
dReal x,y,z;
if( ssl->robots[lockedIndex] != NULL)
{
ssl->robots[lockedIndex]->getXY(x,y);z = 0.1;
ssl->g->lookAt(x,y,z);
}
}
if (cammode==-2)
{
dReal x,y,z;
ssl->ball->getBodyPosition(x,y,z);
ssl->g->lookAt(x,y,z);
}
step();
QFont font;
for (int i=0;i<ROBOT_COUNT*2;i++)
{
dReal xx,yy;
if(ssl->robots[i] == NULL)
continue;
ssl->robots[i]->getXY(xx,yy);
if (i>=ROBOT_COUNT) qglColor(Qt::yellow);
else qglColor(Qt::cyan);
renderText(xx,yy,0.3,QString::number(i%ROBOT_COUNT),font);
if (!ssl->robots[i]->on){
qglColor(Qt::red);
font.setBold(true);
renderText(xx,yy,0.4,"Off",font);
}
font.setBold(false);
}
}
void GLWidget::keyPressEvent(QKeyEvent *event)
{
if (event->key() == Qt::Key_Control) ctrl = true;
if (event->key() == Qt::Key_Alt) alt = true;
char cmd = event->key();
if (fullScreen) {
if (event->key()==Qt::Key_F2) emit toggleFullScreen(false);
}
const dReal S = 0.30;
const dReal BallForce = 0.2;
int R = robotIndex(Current_robot,Current_team);
switch (cmd) {
case 't': case 'T': ssl->robots[R]->incSpeed(0,-S);ssl->robots[R]->incSpeed(1,S);ssl->robots[R]->incSpeed(2,-S);ssl->robots[R]->incSpeed(3,S);break;
case 'g': case 'G': ssl->robots[R]->incSpeed(0,S);ssl->robots[R]->incSpeed(1,-S);ssl->robots[R]->incSpeed(2,S);ssl->robots[R]->incSpeed(3,-S);break;
case 'f': case 'F': ssl->robots[R]->incSpeed(0,S);ssl->robots[R]->incSpeed(1,S);ssl->robots[R]->incSpeed(2,S);ssl->robots[R]->incSpeed(3,S);break;
case 'h': case 'H': ssl->robots[R]->incSpeed(0,-S);ssl->robots[R]->incSpeed(1,-S);ssl->robots[R]->incSpeed(2,-S);ssl->robots[R]->incSpeed(3,-S);break;
case 'w': case 'W':dBodyAddForce(ssl->ball->body,0, BallForce,0);break;
case 's': case 'S':dBodyAddForce(ssl->ball->body,0,-BallForce,0);break;
case 'd': case 'D':dBodyAddForce(ssl->ball->body, BallForce,0,0);break;
case 'a': case 'A':dBodyAddForce(ssl->ball->body,-BallForce,0,0);break;
case 'k':case 'K': ssl->robots[R]->kicker->kick(4,0);break;
case 'l':case 'L': ssl->robots[R]->kicker->kick(2,2);break;
case 'j':case 'J': ssl->robots[R]->kicker->toggleRoller();break;
case 'i':case 'I': dBodySetLinearVel(ssl->ball->body,2.0,0,0);dBodySetAngularVel(ssl->ball->body,0,2.0/cfg->v_BallRadius->getValue(),0);break;
case ';':
if (kickingball==false)
{
kickingball = true; logStatus(QString("Kick mode On"),QColor("blue"));
}
else
{
kickingball = false; logStatus(QString("Kick mode Off"),QColor("red"));
}
break;
case ']': kickpower += 0.1; logStatus(QString("Kick power = %1").arg(kickpower),QColor("orange"));break;
case '[': kickpower -= 0.1; logStatus(QString("Kick power = %1").arg(kickpower),QColor("cyan"));break;
case ' ':
ssl->robots[R]->resetSpeeds();
break;
case '`':
dBodySetLinearVel(ssl->ball->body,0,0,0);
dBodySetAngularVel(ssl->ball->body,0,0,0);
break;
}
}
void GLWidget::switchRobotOnOff()
{
int k = robotIndex(Current_robot, Current_team);
if (Current_robot!=-1)
{
if (ssl->robots[k]->on==true)
{
ssl->robots[k]->on = false;
onOffRobotAct->setText("Turn &on");
emit robotTurnedOnOff(k,false);
}
else {
ssl->robots[k]->on = true;
onOffRobotAct->setText("Turn &off");
emit robotTurnedOnOff(k,true);
}
}
}
void GLWidget::moveRobotHere()
{
ssl->robots[robotIndex(Current_robot,Current_team)]->setXY(ssl->cursor_x,ssl->cursor_y);
ssl->robots[robotIndex(Current_robot,Current_team)]->resetRobot();
}
void GLWidget::lockCameraToRobot()
{
cammode = -1;
lockedIndex = robotIndex(Current_robot,Current_team);//clicked_robot;
}
void SSLWorld::recvActions()
{
QHostAddress sender;
quint16 port;
grSim_Packet packet;
while (commandSocket->hasPendingDatagrams())
{
int size = commandSocket->readDatagram(in_buffer, 65536, &sender, &port);
if (size > 0)
{
packet.ParseFromArray(in_buffer, size);
int team=0;
if (packet.has_commands())
{
if (packet.commands().has_isteamyellow())
{
if (packet.commands().isteamyellow()) team=1;
}
for (int i=0;i<packet.commands().robot_commands_size();i++)
{
if (!packet.commands().robot_commands(i).has_id()) continue;
int k = packet.commands().robot_commands(i).id();
int id = robotIndex(k, team);
if ((id < 0) || (id >= ROBOT_COUNT*2)) continue;
bool wheels = false;
if (packet.commands().robot_commands(i).has_wheelsspeed())
{
if (packet.commands().robot_commands(i).wheelsspeed()==true)
{
if (packet.commands().robot_commands(i).has_wheel1()) robots[id]->setSpeed(0, packet.commands().robot_commands(i).wheel1());
if (packet.commands().robot_commands(i).has_wheel2()) robots[id]->setSpeed(1, packet.commands().robot_commands(i).wheel2());
if (packet.commands().robot_commands(i).has_wheel3()) robots[id]->setSpeed(2, packet.commands().robot_commands(i).wheel3());
if (packet.commands().robot_commands(i).has_wheel4()) robots[id]->setSpeed(3, packet.commands().robot_commands(i).wheel4());
wheels = true;
}
}
if (!wheels)
{
dReal vx = 0;if (packet.commands().robot_commands(i).has_veltangent()) vx = packet.commands().robot_commands(i).veltangent();
dReal vy = 0;if (packet.commands().robot_commands(i).has_velnormal()) vy = packet.commands().robot_commands(i).velnormal();
dReal vw = 0;if (packet.commands().robot_commands(i).has_velangular()) vw = packet.commands().robot_commands(i).velangular();
robots[id]->setSpeed(vx, vy, vw);
}
dReal kickx = 0 , kickz = 0;
bool kick = false;
if (packet.commands().robot_commands(i).has_kickspeedx())
{
kick = true;
kickx = packet.commands().robot_commands(i).kickspeedx();
}
if (packet.commands().robot_commands(i).has_kickspeedz())
{
kick = true;
kickz = packet.commands().robot_commands(i).kickspeedz();
}
if (kick && ((kickx>0.0001) || (kickz>0.0001)))
robots[id]->kicker->kick(kickx,kickz);
int rolling = 0;
if (packet.commands().robot_commands(i).has_spinner())
{
if (packet.commands().robot_commands(i).spinner()) rolling = 1;
}
robots[id]->kicker->setRoller(rolling);
char status = 0;
status = k;
if (robots[id]->kicker->isTouchingBall()) status = status | 8;
if (robots[id]->on) status = status | 240;
if (team == 0)
blueStatusSocket->writeDatagram(&status,1,sender,cfg->BlueStatusSendPort());
else
yellowStatusSocket->writeDatagram(&status,1,sender,cfg->YellowStatusSendPort());
}
}
if (packet.has_replacement())
{
for (int i=0;i<packet.replacement().robots_size();i++)
{
int team = 0;
if (packet.replacement().robots(i).has_yellowteam())
{
if (packet.replacement().robots(i).yellowteam())
team = 1;
}
if (!packet.replacement().robots(i).has_id()) continue;
int k = packet.replacement().robots(i).id();
dReal x = 0, y = 0, dir = 0;
if (packet.replacement().robots(i).has_x()) x = packet.replacement().robots(i).x();
if (packet.replacement().robots(i).has_y()) y = packet.replacement().robots(i).y();
if (packet.replacement().robots(i).has_dir()) dir = packet.replacement().robots(i).dir();
int id = robotIndex(k, team);
if ((id < 0) || (id >= ROBOT_COUNT*2)) continue;
robots[id]->setXY(x,y);
robots[id]->resetRobot();
robots[id]->setDir(dir);
}
if (packet.replacement().has_ball())
{
dReal x = 0, y = 0, vx = 0, vy = 0;
if (packet.replacement().ball().has_x()) x = packet.replacement().ball().x();
if (packet.replacement().ball().has_y()) y = packet.replacement().ball().y();
if (packet.replacement().ball().has_vx()) vx = packet.replacement().ball().vx();
if (packet.replacement().ball().has_vy()) vy = packet.replacement().ball().vy();
ball->setBodyPosition(x,y,cfg->BallRadius()*1.2);
dBodySetLinearVel(ball->body,vx,vy,0);
dBodySetAngularVel(ball->body,0,0,0);
}
}
}
}
}
