void GLWidget::paintGL()
{
glDrawBuffer(GL_BACK);
glClearColor(0.6f, 0.8f, 1.0f, 0.0f);
glClearDepth(1.0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
/*Angulo ratio znear, zfar*/
gluPerspective(50.0, width / height, 1.0, 50000.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/*pos cam pto central vector up*/
gluLookAt(this->glcam_pos_X, this->glcam_pos_Y, this->glcam_pos_Z,
this->glcam_foa_X, this->glcam_foa_Y, this->glcam_foa_Z,
0., 0., 1.);
// absolute axis
glLineWidth(3.0f);
glColor3f(0.7, 0., 0.);
glBegin(GL_LINES);
v3f(0.0, 0.0, 0.0);
v3f(10.0, 0.0, 0.0);
glEnd();
glColor3f(0., 0.7, 0.);
glBegin(GL_LINES);
v3f(0.0, 0.0, 0.0);
v3f(0.0, 10.0, 0.0);
glEnd();
glColor3f(0., 0., 0.7);
glBegin(GL_LINES);
v3f(0.0, 0.0, 0.0);
v3f(0.0, 0.0, 10.0);
glEnd();
glLineWidth(1.0f);
//SUELO
glColor3f(0.6, 0.6, 0.6);
glLineWidth(2.0f);
glBegin(GL_LINES);
for (int i = 0; i < ((int) MAXWORLD + 1); i++) {
v3f(-(int) MAXWORLD * 10 / 2. + (float) i * 10, -(int) MAXWORLD * 10 / 2., 0.);
v3f(-(int) MAXWORLD * 10 / 2. + (float) i * 10, (int) MAXWORLD * 10 / 2., 0.);
v3f(-(int) MAXWORLD * 10 / 2., -(int) MAXWORLD * 10 / 2. + (float) i * 10, 0.);
v3f((int) MAXWORLD * 10 / 2., -(int) MAXWORLD * 10 / 2. + (float) i * 10, 0.);
}
glEnd();
UpdateRobot();
//robot
drawRobot();
}
/*
* Tick -
*
* This funciton is the guts of the simulator. It should be called on each simulation tick.
* (Hence the name :-). The function calculates the new position for the robots and ball
* based on their kinematics and dynamics. It then accounts for the collisions between
* all the objects and the objects and the field.
*/
int Simulator::Tick(void)
{
lock();
double secs = timePerFrame;
/* wait on the runstate mutex to prevent conflicts */
runstate_mutex.lock();
//处理暂停
// If paused then still signal a new frame.
if (running_state == RUNSTATE_PAUSE)
{
runstate_mutex.unlock();
unlock();
return frameCount;
}
//处理单步
else if (running_state == RUNSTATE_STEPFORWARD)
{
running_state = RUNSTATE_PAUSE;
}
runstate_mutex.unlock();
commands_mutex.lock();
// turn of the goal score flag
goalscored = 0;
/* Update each team of Robots with their new velocities */
for(int i = 0; i < MAX_TEAM_ROBOTS; i++)
{
if(yellow_robots[i].conf==0.0)continue;
#ifdef USE_VQUEUE
//读取一条命令
yellow_robots[i].vqueue.GetCommand(yellow_robots[i].vcmd, frameCount);
#endif
UpdateRobot(yellow_robots[i], secs);
}
/* Update each team of Robots with their new velocities */
for(int i = 0; i < MAX_TEAM_ROBOTS; i++)
{
if(blue_robots[i].conf==0.0)continue;
#ifdef USE_VQUEUE
//读取一条命令
blue_robots[i].vqueue.GetCommand(blue_robots[i].vcmd, frameCount);
#endif
UpdateRobot(blue_robots[i], secs);
}
// Update Ball
/* bound the ball off the wall */
ball.oldpos = ball.pos;
MyVector2d v = ball.vel;
////如果球出现范围超界,速度除以sqrt(2)
//if ((ball.pos.x < minX) || (ball.pos.x > maxX))
//{
// v.x *= M_SQRT1_2;
//}
//if ((ball.pos.y < minY-FieldConstantsRoboCup2012::boundary_width_) || (ball.pos.y > maxY+FieldConstantsRoboCup2012::boundary_width_))
//{
// v.y *= M_SQRT1_2;
//}
ball.pos += ball.vel * secs;
/* decay the ball velocity with friction */
//计算摩擦力影响速度后的结果
double dv_mag = ballCarpetFrictionCoef * GRAVITY * secs;
double vmag = ball.vel.length();
MyVector2d dv = -ball.vel;
if (vmag > dv_mag)
{
dv *= dv_mag / vmag;
}
ball.vel += dv;
// Check for Walls
//
// (mhb) Updated for 45' walls.
// See: http://dept.physics.upenn.edu/courses/gladney/
// mathphys/java/sect4/subsubsection4_1_4_3.html
//球撞墙后反弹
bool bOutSide=false;
//上下出界
if (fabs(ball.pos.y) > FIELD_WIDTH_H)
{
if (fabs(ball.pos.y) >= field_max.y)
{
ball.vel.set(0,0);
}
bOutSide=true;
//ball.vel.y -= FSGN(ball.pos.y) * secs * M_SQRT1_2 * GRAVITY * 5.0/7.0;
}
//左右出界并且没有进入球门
if ((fabs(ball.pos.x) > FIELD_WIDTH_H) && (fabs(ball.pos.y) > GOAL_WIDTH_H))
{
if((fabs(ball.pos.x) >= field_max.x))
{
ball.vel.set(0,0);
}
bOutSide=true;
//ball.vel.x -= FSGN(ball.pos.x) * secs * M_SQRT1_2 * GRAVITY * 5.0/7.0;
}
//球进入门中的处理
if ( fabs(ball.pos.x) > FIELD_LENGTH_H && fabs(ball.pos.y) <= GOAL_WIDTH_H)
{
if(fabs(ball.pos.x) >= FIELD_LENGTH_H+180.0)
{
ball.vel.set(0,0);
}
bOutSide=true;
}
static int OutSideDelay=120;
if(bOutSide)
{
if(OutSideDelay==0)
{
//ball.pos.set(0,0);
}
else
{
OutSideDelay--;
}
}
else
{
OutSideDelay=120;
}
/* now check for robot collisions with the ball
* original code by S Lenser
*/
//限制蓝队队员运动范围
for(int i = 0; i < MAX_TEAM_ROBOTS; i++)
{
if(blue_robots[i].conf==0.0)continue;
RobotBallCollisionRadius(blue_robots[i], ball, secs);
// bound the ball position
blue_robots[i].pos.p.x = bound(blue_robots[i].pos.p.x, -field_max.x, field_max.x);
blue_robots[i].pos.p.y = bound(blue_robots[i].pos.p.y, -field_max.y, field_max.y);
}
//限制黄队队员运动范围
for(int i = 0; i < MAX_TEAM_ROBOTS; i++)
{
if(yellow_robots[i].conf==0.0)continue;
RobotBallCollisionRadius(yellow_robots[i], ball, secs);
// bound the ball position
yellow_robots[i].pos.p.x = bound(yellow_robots[i].pos.p.x, -field_max.x, field_max.x);
yellow_robots[i].pos.p.y = bound(yellow_robots[i].pos.p.y, -field_max.y, field_max.y);
}
// bound the ball position
//限制球的运动范围
ball.pos.x = bound(ball.pos.x, -field_max.x, field_max.x);
ball.pos.y = bound(ball.pos.y, -field_max.y, field_max.y);
// check if the ball is in the goal
//计分处理
if (usegoals)
{
if ((fabs(ball.pos.x) > FIELD_LENGTH_H + BALL_RADIUS * 2.0) &&
fabs(ball.pos.y) < GOAL_WIDTH_H)
{
goalscored = ((ball.pos.x > 0) ? 1 : -1);
printf("Goal scored on goal %i!!!!\n", ball.pos.x > 0);
ball.pos.set(0, 0);
ball.vel.set(0, 0);
}
}
// generate noise input on confidences
//在可信度上叠加噪声
if (usenoise) {
// crappy for now
ball.conf = (double) rand() / (double) RAND_MAX;
for (int i = 0; i < num_yellow; i++)
{
yellow_robots[i].conf = (double) rand() / (double) RAND_MAX;
}
for (int i = 0; i < num_blue; i++)
{
blue_robots[i].conf = (double) rand() / (double) RAND_MAX;
}
}
/* increment the ol frame counter */
frameCount++;
// take care of semaphores
commands_mutex.unlock();
//run
//double timestamp = GetTimeSec();
// get any new radio commands
//接收无线电命令
unlock();
return frameCount;
}
