void Robot::setXY(float x,float y)
{
float xx,yy,zz,kx,ky,kz;
float height = ROBOT_START_Z(cfg);
chassis->getBodyPosition(xx,yy,zz);
chassis->setBodyPosition(x,y,height);
dummy->setBodyPosition(x,y,height);
kicker->box->getBodyPosition(kx,ky,kz);
kicker->box->setBodyPosition(kx-xx+x,ky-yy+y,kz-zz+height);
for (int i=0;i<4;i++)
{
wheels[i]->cyl->getBodyPosition(kx,ky,kz);
wheels[i]->cyl->setBodyPosition(kx-xx+x,ky-yy+y,kz-zz+height);
}
}
SSLWorld::SSLWorld(QGLWidget* parent,ConfigWidget* _cfg,RobotsFomation *form1,RobotsFomation *form2)
: QObject(parent)
{
isGLEnabled = true;
customDT = -1;
_w = this;
cfg = _cfg;
m_parent = parent;
show3DCursor = false;
updatedCursor = false;
framenum = 0;
last_dt = -1;
g = new CGraphics(parent);
g->setSphereQuality(1);
g->setViewpoint(0,-(cfg->Field_Width()+cfg->Field_Margin()*2.0f)/2.0f,3,90,-45,0);
p = new PWorld(0.05,9.81f,g);
ball = new PBall (0,0,0.5,cfg->BallRadius(),cfg->BallMass(), 1,0.7,0);
ground = new PGround(cfg->Field_Rad(),cfg->Field_Length(),cfg->Field_Width(),cfg->Field_Penalty_Rad(),cfg->Field_Penalty_Line(),cfg->Field_Penalty_Point(),cfg->Field_Line_Width(),cfg->Field_Defense_Stretch(),cfg->Field_Defense_Rad(),0);
ray = new PRay(50);
// Bounding walls
const double thick = cfg->Wall_Thickness();
const double increment = cfg->Field_Margin() + cfg->Field_Referee_Margin() + thick / 2;
const double pos_x = cfg->Field_Length() / 2.0 + increment;
const double pos_y = cfg->Field_Width() / 2.0 + increment;
const double pos_z = 0.0;
const double siz_x = 2.0 * pos_x;
const double siz_y = 2.0 * pos_y;
const double siz_z = 0.4;
const double tone = 1.0;
walls[0] = new PFixedBox(thick/2, pos_y, pos_z,
siz_x, thick, siz_z,
tone, tone, tone);
walls[1] = new PFixedBox(-thick/2, -pos_y, pos_z,
siz_x, thick, siz_z,
tone, tone, tone);
walls[2] = new PFixedBox(pos_x, -thick/2, pos_z,
thick, siz_y, siz_z,
tone, tone, tone);
walls[3] = new PFixedBox(-pos_x, thick/2, pos_z,
thick, siz_y, siz_z,
tone, tone, tone);
// Goal walls
const double gthick = cfg->Goal_Thickness();
const double gpos_x = (cfg->Field_Length() + gthick) / 2.0 + cfg->Goal_Depth();
const double gpos_y = (cfg->Goal_Width() + gthick) / 2.0;
const double gpos_z = cfg->Goal_Height() / 2.0;
const double gsiz_x = cfg->Goal_Depth() + gthick;
const double gsiz_y = cfg->Goal_Width();
const double gsiz_z = cfg->Goal_Height();
const double gpos2_x = (cfg->Field_Length() + gsiz_x) / 2.0;
walls[4] = new PFixedBox(gpos_x, 0.0, gpos_z,
gthick, gsiz_y, gsiz_z,
tone, tone, tone);
walls[5] = new PFixedBox(gpos2_x, -gpos_y, gpos_z,
gsiz_x, gthick, gsiz_z,
tone, tone, tone);
walls[6] = new PFixedBox(gpos2_x, gpos_y, gpos_z,
gsiz_x, gthick, gsiz_z,
tone, tone, tone);
walls[7] = new PFixedBox(-gpos_x, 0.0, gpos_z,
gthick, gsiz_y, gsiz_z,
tone, tone, tone);
walls[8] = new PFixedBox(-gpos2_x, -gpos_y, gpos_z,
gsiz_x, gthick, gsiz_z,
tone, tone, tone);
walls[9] = new PFixedBox(-gpos2_x, gpos_y, gpos_z,
gsiz_x, gthick, gsiz_z,
tone, tone, tone);
p->addObject(ground);
p->addObject(ball);
p->addObject(ray);
for (int i=0;i<10;i++)
p->addObject(walls[i]);
const int wheeltexid = 37;
cfg->robotSettings = cfg->blueSettings;
for (int k=0;k<ROBOT_COUNT;k++)
robots[k] = new Robot(p,ball,cfg,-form1->x[k],form1->y[k],ROBOT_START_Z(cfg),ROBOT_GRAY,ROBOT_GRAY,ROBOT_GRAY,k+1,wheeltexid,1);
cfg->robotSettings = cfg->yellowSettings;
for (int k=0;k<ROBOT_COUNT;k++)
robots[k+ROBOT_COUNT] = new Robot(p,ball,cfg,form2->x[k],form2->y[k],ROBOT_START_Z(cfg),ROBOT_GRAY,ROBOT_GRAY,ROBOT_GRAY,k+ROBOT_COUNT+1,wheeltexid,-1);//XXX
p->initAllObjects();
//Surfaces
p->createSurface(ray,ground)->callback = rayCallback;
p->createSurface(ray,ball)->callback = rayCallback;
for (int k=0;k<ROBOT_COUNT * 2;k++)
{
p->createSurface(ray,robots[k]->chassis)->callback = rayCallback;
p->createSurface(ray,robots[k]->dummy)->callback = rayCallback;
}
PSurface ballwithwall;
ballwithwall.surface.mode = dContactBounce | dContactApprox1;// | dContactSlip1;
ballwithwall.surface.mu = 1;//fric(cfg->ballfriction());
ballwithwall.surface.bounce = cfg->BallBounce();
ballwithwall.surface.bounce_vel = cfg->BallBounceVel();
ballwithwall.surface.slip1 = 0;//cfg->ballslip();
PSurface wheelswithground;
PSurface* ball_ground = p->createSurface(ball,ground);
ball_ground->surface = ballwithwall.surface;
ball_ground->callback = ballCallBack;
PSurface ballwithkicker;
ballwithkicker.surface.mode = dContactApprox1;
ballwithkicker.surface.mu = fric(cfg->robotSettings.Kicker_Friction);
ballwithkicker.surface.slip1 = 5;
for (int i = 0; i < WALL_COUNT; i++)
p->createSurface(ball, walls[i])->surface = ballwithwall.surface;
for (int k = 0; k < 2 * ROBOT_COUNT; k++)
{
p->createSurface(robots[k]->chassis,ground);
for (int j = 0; j < WALL_COUNT; j++)
p->createSurface(robots[k]->chassis,walls[j]);
p->createSurface(robots[k]->dummy,ball);
//p->createSurface(robots[k]->chassis,ball);
p->createSurface(robots[k]->kicker->box,ball)->surface = ballwithkicker.surface;
for (int j = 0; j < WHEEL_COUNT; j++)
{
p->createSurface(robots[k]->wheels[j]->cyl,ball);
PSurface* w_g = p->createSurface(robots[k]->wheels[j]->cyl,ground);
w_g->surface=wheelswithground.surface;
w_g->usefdir1=true;
w_g->callback=wheelCallBack;
}
for (int j = k + 1; j < 2 * ROBOT_COUNT; j++)
{
if (k != j)
{
p->createSurface(robots[k]->dummy,robots[j]->dummy); //seams ode doesn't understand cylinder-cylinder contacts, so I used spheres
p->createSurface(robots[k]->chassis,robots[j]->kicker->box);
}
}
}
sendGeomCount = 0;
timer = new QTime();
timer->start();
in_buffer = new char [65536];
}
SSLWorld::SSLWorld(QGLWidget* parent,ConfigWidget* _cfg,RobotsFomation *form1,RobotsFomation *form2)
: QObject(parent)
{
isGLEnabled = true;
customDT = -1;
_w = this;
cfg = _cfg;
m_parent = parent;
show3DCursor = false;
updatedCursor = false;
framenum = 0;
last_dt = -1;
g = new CGraphics(parent);
g->setSphereQuality(1);
g->setViewpoint(0,-(cfg->Field_Width()+cfg->Field_Margin()*2.0f)/2.0f,3,90,-45,0);
p = new PWorld(0.05,9.81f,g);
ball = new PBall (0,0,0.5,cfg->BallRadius(),cfg->BallMass(), 1,0.7,0);
ground = new PGround(cfg->Field_Rad(),cfg->Field_Length(),cfg->Field_Width(),cfg->Field_Penalty_Rad(),cfg->Field_Penalty_Line(),cfg->Field_Penalty_Point(),0);
ray = new PRay(50);
walls[0] = new PFixedBox(0.0,((cfg->Field_Width() + cfg->Field_Margin()) / 2.0) + (cfg->Wall_Thickness() / 2.0),0.0
,(cfg->Field_Length() + cfg->Field_Margin()) / 1.0, cfg->Wall_Thickness() / 1.0, 0.4,
0.7, 0.7, 0.7);
walls[1] = new PFixedBox(0.0,((cfg->Field_Width() + cfg->Field_Margin()) / -2.0) - (cfg->Wall_Thickness() / 2.0) - cfg->Field_Referee_Margin() / 1.0,0.0,
(cfg->Field_Length() + cfg->Field_Margin()) / 1.0, cfg->Wall_Thickness() / 1.0, 0.4,
0.7, 0.7, 0.7);
walls[2] = new PFixedBox(((cfg->Field_Length() + cfg->Field_Margin()) / 2.0) + (cfg->Wall_Thickness() / 2.0),-cfg->Field_Referee_Margin()/2.f ,0.0,
cfg->Wall_Thickness() / 1.0 ,(cfg->Field_Width() + cfg->Field_Margin() + cfg->Field_Referee_Margin()) / 1.0, 0.4,
0.7, 0.7, 0.7);
walls[3] = new PFixedBox(((cfg->Field_Length() + cfg->Field_Margin()) / -2.0) - (cfg->Wall_Thickness() / 2.0) ,-cfg->Field_Referee_Margin()/2.f ,0.0,
cfg->Wall_Thickness() / 1.0 , (cfg->Field_Width() + cfg->Field_Margin() + cfg->Field_Referee_Margin()) / 1.0, 0.4,
0.7, 0.7, 0.7);
walls[4] = new PFixedBox(( cfg->Field_Length() / 2.0) + cfg->Goal_Depth() + cfg->Goal_Thickness()*0.5f ,0.0, 0.0
, cfg->Goal_Thickness(), cfg->Goal_Width() + cfg->Goal_Thickness()*2.0f, cfg->Goal_Height() , 0.1, 0.9, 0.4);
walls[5] = new PFixedBox(( cfg->Field_Length() / 2.0) + cfg->Goal_Depth()*0.5,-cfg->Goal_Width()*0.5f - cfg->Goal_Thickness()*0.5f,0.0
, cfg->Goal_Depth(), cfg->Goal_Thickness(), cfg->Goal_Height(), 0.1, 0.9, 0.4);
walls[6] = new PFixedBox(( cfg->Field_Length() / 2.0) + cfg->Goal_Depth()*0.5,cfg->Goal_Width()*0.5f + cfg->Goal_Thickness()*0.5f, 0.0
, cfg->Goal_Depth(), cfg->Goal_Thickness(), cfg->Goal_Height(), 0.1, 0.9, 0.4);
walls[7] = new PFixedBox(- (( cfg->Field_Length() / 2.0) + cfg->Goal_Depth() + cfg->Goal_Thickness()*0.5f) ,0.0, 0.0
, cfg->Goal_Thickness(), cfg->Goal_Width() + cfg->Goal_Thickness()*2.0f, cfg->Goal_Height() , 0.1, 0.9, 0.4);
walls[8] = new PFixedBox(- (( cfg->Field_Length() / 2.0) + cfg->Goal_Depth()*0.5),-cfg->Goal_Width()*0.5f- cfg->Goal_Thickness()*0.5f,0.0
, cfg->Goal_Depth(), cfg->Goal_Thickness(), cfg->Goal_Height() , 0.1, 0.9, 0.4);
walls[9] = new PFixedBox( -(( cfg->Field_Length() / 2.0) + cfg->Goal_Depth()*0.5),cfg->Goal_Width()*0.5f+ cfg->Goal_Thickness()*0.5f, 0.0
, cfg->Goal_Depth(), cfg->Goal_Thickness(), cfg->Goal_Height() , 0.1, 0.9, 0.4);
p->addObject(ground);
p->addObject(ball);
p->addObject(ray);
for (int i=0;i<10;i++)
p->addObject(walls[i]);
const int wheeltexid = 37;
cfg->robotSettings = cfg->blueSettings;
for (int k=0;k<5;k++)
robots[k] = new Robot(p,ball,cfg,-form1->x[k],form1->y[k],ROBOT_START_Z(cfg),ROBOT_GRAY,ROBOT_GRAY,ROBOT_GRAY,k+1,wheeltexid,1);
cfg->robotSettings = cfg->yellowSettings;
for (int k=0;k<5;k++)
robots[k+5] = new Robot(p,ball,cfg,form2->x[k],form2->y[k],ROBOT_START_Z(cfg),ROBOT_GRAY,ROBOT_GRAY,ROBOT_GRAY,k+6,wheeltexid,-1);
p->initAllObjects();
//Surfaces
p->createSurface(ray,ground)->callback = rayCallback;
p->createSurface(ray,ball)->callback = rayCallback;
for (int k=0;k<10;k++)
{
p->createSurface(ray,robots[k]->chassis)->callback = rayCallback;
p->createSurface(ray,robots[k]->dummy)->callback = rayCallback;
}
PSurface ballwithwall;
ballwithwall.surface.mode = dContactBounce | dContactApprox1;// | dContactSlip1;
ballwithwall.surface.mu = 1;//fric(cfg->ballfriction());
ballwithwall.surface.bounce = cfg->BallBounce();
ballwithwall.surface.bounce_vel = cfg->BallBounceVel();
ballwithwall.surface.slip1 = 0;//cfg->ballslip();
PSurface wheelswithground;
PSurface* ball_ground = p->createSurface(ball,ground);
ball_ground->surface = ballwithwall.surface;
ball_ground->callback = ballCallBack;
PSurface ballwithkicker;
ballwithkicker.surface.mode = dContactApprox1;
ballwithkicker.surface.mu = fric(cfg->robotSettings.Kicker_Friction);
ballwithkicker.surface.slip1 = 5;
for (int i=0;i<10;i++)
p->createSurface(ball,walls[i])->surface = ballwithwall.surface;
for (int k=0;k<10;k++)
{
p->createSurface(robots[k]->chassis,ground);
for (int j=0;j<10;j++)
p->createSurface(robots[k]->chassis,walls[j]);
p->createSurface(robots[k]->dummy,ball);
//p->createSurface(robots[k]->chassis,ball);
p->createSurface(robots[k]->kicker->box,ball)->surface = ballwithkicker.surface;
for (int j=0;j<4;j++)
{
p->createSurface(robots[k]->wheels[j]->cyl,ball);
PSurface* w_g = p->createSurface(robots[k]->wheels[j]->cyl,ground);
w_g->surface=wheelswithground.surface;
w_g->usefdir1=true;
w_g->callback=wheelCallBack;
}
for (int j=k+1;j<10;j++)
{
if (k!=j)
{
p->createSurface(robots[k]->dummy,robots[j]->dummy); //seams ode doesn't understand cylinder-cylinder contacts, so I used spheres
p->createSurface(robots[k]->chassis,robots[j]->kicker->box);
}
}
}
timer = new QTime();
timer->start();
in_buffer = new char [65536];
}
