diff --git a/include/sslworld.h b/include/sslworld.h index 60915835..d96032ee 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -24,6 +24,8 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #include #include #include +#include +#include #include "graphics.h" @@ -76,6 +78,8 @@ class SSLWorld : public QObject Robot *robot) ; void processTeleportBall(SimulatorResponse &simulatorResponse, const TeleportBall &teleBall) const; static void processTeleportRobot(const TeleportRobot &teleBot, Robot *robot); + QTimer* simulationTimer {nullptr}; + QElapsedTimer elapsed; public: dReal customDT; bool isGLEnabled; @@ -83,6 +87,8 @@ class SSLWorld : public QObject ~SSLWorld() override; void glinit(); void step(dReal dt=-1); + void simulate(dReal dt=-1); + void render(); SSL_WrapperPacket* generatePacket(int cam_id=0); void addFieldLinesArcs(SSL_GeometryFieldSize *field); static void addFieldLine(SSL_GeometryFieldSize *field, const std::string &name, float p1_x, float p1_y, float p2_x, float p2_y, float thickness); @@ -124,6 +130,7 @@ public slots: void simControlSocketReady(); void blueControlSocketReady(); void yellowControlSocketReady(); + void stepSimulation(); signals: void fpsChanged(int newFPS); }; diff --git a/src/glwidget.cpp b/src/glwidget.cpp index 46af025d..c424d5e0 100644 --- a/src/glwidget.cpp +++ b/src/glwidget.cpp @@ -346,27 +346,21 @@ void GLWidget::step() const dReal* ballV = dBodyGetLinearVel(ssl->ball->body); double ballSpeed = ballV[0]*ballV[0] + ballV[1]*ballV[1] + ballV[2]*ballV[2]; ballSpeed = sqrt(ballSpeed); + rendertimer.restart(); - m_fps = frames /(time.elapsed()/1000.0); - if (!(frames % ((int)(ceil(cfg->DesiredFPS()))))) { + + m_fps = frames / (time.elapsed() / 1000.0); + if (!(frames % static_cast(ceil(cfg->DesiredFPS())))) { time.restart(); frames = 0; } - if (first_time) {ssl->step();first_time = false;} - else { - if (cfg->SyncWithGL()) - { - dReal ddt=rendertimer.elapsed()/1000.0; - if (ddt>0.05) ddt=0.05; - ssl->step(ddt); - } - else { - ssl->step(cfg->DeltaTime()); - } - } - frames ++; + + ssl->render(); + + frames++; } + void GLWidget::paintGL() { if (!ssl->g->isGraphicsEnabled()) return; diff --git a/src/sslworld.cpp b/src/sslworld.cpp index ab14858f..66defe73 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -20,6 +20,7 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #include #include +#include #include @@ -316,6 +317,14 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 elapsedLastPackageBlue.start(); elapsedLastPackageYellow.start(); + + elapsed.start(); + + simulationTimer = new QTimer(this); + simulationTimer->setInterval(10); + connect(simulationTimer, &QTimer::timeout, + this, &SSLWorld::stepSimulation); + simulationTimer->start(); } int SSLWorld::robotIndex(int robot,int team) { @@ -410,10 +419,30 @@ void SSLWorld::glinit() { p->glinit(); } -void SSLWorld::step(dReal dt) { - if (customDT > 0) dt = customDT; - const auto ratio = m_parent->devicePixelRatio(); - g->initScene(m_parent->width()*ratio,m_parent->height()*ratio,0,0.7,1); +void SSLWorld::stepSimulation() +{ + if (!elapsed.isValid()) { + elapsed.start(); + return; + } + + qint64 nsec = elapsed.nsecsElapsed(); + elapsed.restart(); + + dReal dt = static_cast(nsec) / 1e9; + if (dt <= 0) { + dt = (last_dt > 0) ? last_dt : 0.01; + } + + simulate(dt); +} + +void SSLWorld::simulate(dReal dt) +{ + if (customDT > 0) { + dt = customDT; + } + int ballCollisionTry = 5; for (int kk=0;kk < ballCollisionTry;kk++) { const dReal* ballvel = dBodyGetLinearVel(ball->body); @@ -496,6 +525,21 @@ void SSLWorld::step(dReal dt) { robots[k]->step(); robots[k]->selected = false; } + + sendVisionBuffer(); + frame_num++; +} + +void SSLWorld::render() +{ + if (!g->isGraphicsEnabled()) + return; + + const auto ratio = m_parent->devicePixelRatio(); + g->initScene(m_parent->width() * ratio, + m_parent->height() * ratio, + 0, 0.7, 1); + p->draw(); g->drawSkybox(4 * cfg->Robots_Count() + 6 + 1, //31 for 6 robot 4 * cfg->Robots_Count() + 6 + 2, //32 for 6 robot @@ -504,24 +548,23 @@ void SSLWorld::step(dReal dt) { 4 * cfg->Robots_Count() + 6 + 5, //31 for 6 robot 4 * cfg->Robots_Count() + 6 + 6);//36 for 6 robot - dMatrix3 R; - - if (g->isGraphicsEnabled()) - if (show3DCursor) - { - dRFromAxisAndAngle(R,0,0,1,0); - g->setColor(1,0.9,0.2,0.5); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); - g->drawCircle(cursor_x,cursor_y,0.001,cursor_radius); - glDisable(GL_BLEND); - } + if (show3DCursor) { + dMatrix3 R; + dRFromAxisAndAngle(R,0,0,1,0); + g->setColor(1,0.9,0.2,0.5); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); + g->drawCircle(cursor_x,cursor_y,0.001,cursor_radius); + glDisable(GL_BLEND); + } g->finalizeScene(); +} - - sendVisionBuffer(); - frame_num ++; +void SSLWorld::step(dReal dt) +{ + simulate(dt); + render(); } void SSLWorld::addRobotStatus(Robots_Status& robotsPacket, int robotID, bool infrared, KickStatus kickStatus) {