From eec1f0b215d724dca9289ea43dd71264cc322eed Mon Sep 17 00:00:00 2001 From: AlexM-dev Date: Tue, 18 Nov 2025 19:53:19 +0300 Subject: [PATCH 1/4] done --- include/sslworld.h | 7 ++++ src/glwidget.cpp | 24 ++++------- src/sslworld.cpp | 101 ++++++++++++++++++++++++++++++++------------- 3 files changed, 88 insertions(+), 44 deletions(-) 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..f53e070b 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); @@ -451,8 +480,8 @@ void SSLWorld::step(dReal dt) { ball->getBodyPosition(bx,by,bz); g->getViewpoint(xyz,hpr); best_dist =(bx-xyz[0])*(bx-xyz[0]) - +(by-xyz[1])*(by-xyz[1]) - +(bz-xyz[2])*(bz-xyz[2]); + +(by-xyz[1])*(by-xyz[1]) + +(bz-xyz[2])*(bz-xyz[2]); } for (int k=0;kRobots_Count() * 2;k++) { @@ -460,8 +489,8 @@ void SSLWorld::step(dReal dt) { { g->getViewpoint(xyz,hpr); dReal dist= (robots[k]->select_x-xyz[0])*(robots[k]->select_x-xyz[0]) - +(robots[k]->select_y-xyz[1])*(robots[k]->select_y-xyz[1]) - +(robots[k]->select_z-xyz[2])*(robots[k]->select_z-xyz[2]); + +(robots[k]->select_y-xyz[1])*(robots[k]->select_y-xyz[1]) + +(robots[k]->select_z-xyz[2])*(robots[k]->select_z-xyz[2]); if (dist < best_dist) { best_dist = dist; best_k = k; @@ -478,16 +507,16 @@ void SSLWorld::step(dReal dt) { { if(best_k >= cfg->Robots_Count()) robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, - ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, + QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, + ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) - ); + ); else robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, - ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, + QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, + ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) - ); + ); } selected = best_k; ball->tag = -1; @@ -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) { From 9cda7508df75cd6937d7e2570fdd52c9a44a5141 Mon Sep 17 00:00:00 2001 From: AlexM-dev Date: Tue, 18 Nov 2025 19:59:37 +0300 Subject: [PATCH 2/4] fix formating --- src/sslworld.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/sslworld.cpp b/src/sslworld.cpp index f53e070b..531067e4 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -480,8 +480,8 @@ void SSLWorld::simulate(dReal dt) ball->getBodyPosition(bx,by,bz); g->getViewpoint(xyz,hpr); best_dist =(bx-xyz[0])*(bx-xyz[0]) - +(by-xyz[1])*(by-xyz[1]) - +(bz-xyz[2])*(bz-xyz[2]); + +(by-xyz[1])*(by-xyz[1]) + +(bz-xyz[2])*(bz-xyz[2]); } for (int k=0;kRobots_Count() * 2;k++) { @@ -489,8 +489,8 @@ void SSLWorld::simulate(dReal dt) { g->getViewpoint(xyz,hpr); dReal dist= (robots[k]->select_x-xyz[0])*(robots[k]->select_x-xyz[0]) - +(robots[k]->select_y-xyz[1])*(robots[k]->select_y-xyz[1]) - +(robots[k]->select_z-xyz[2])*(robots[k]->select_z-xyz[2]); + +(robots[k]->select_y-xyz[1])*(robots[k]->select_y-xyz[1]) + +(robots[k]->select_z-xyz[2])*(robots[k]->select_z-xyz[2]); if (dist < best_dist) { best_dist = dist; best_k = k; @@ -507,16 +507,16 @@ void SSLWorld::simulate(dReal dt) { if(best_k >= cfg->Robots_Count()) robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, - ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, - ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) - ); + QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, + ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, + ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) + ); else robots[best_k]->chassis->setColor( QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, - ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) - ); + ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) + ); } selected = best_k; ball->tag = -1; From e121145d8ec3b02feb17fffa7c99fa33047c9851 Mon Sep 17 00:00:00 2001 From: AlexM-dev Date: Tue, 18 Nov 2025 20:00:38 +0300 Subject: [PATCH 3/4] fix[2] --- src/sslworld.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 531067e4..f751b3f7 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -507,16 +507,16 @@ void SSLWorld::simulate(dReal dt) { if(best_k >= cfg->Robots_Count()) robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, - ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, - ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) - ); + QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, + ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, + ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) + ); else robots[best_k]->chassis->setColor( - QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, - ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, - ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) - ); + QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, + ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, + ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) + ); } selected = best_k; ball->tag = -1; From e601433f1ec8725202a8d4fc8b3e76baec6832d6 Mon Sep 17 00:00:00 2001 From: AlexM-dev Date: Tue, 18 Nov 2025 20:03:19 +0300 Subject: [PATCH 4/4] fix[3] --- src/sslworld.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/sslworld.cpp b/src/sslworld.cpp index f751b3f7..66defe73 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -508,14 +508,14 @@ void SSLWorld::simulate(dReal dt) if(best_k >= cfg->Robots_Count()) robots[best_k]->chassis->setColor( QColor::fromRgbF(ROBOT_YELLOW_CHASSIS_COLOR.redF()*2, - ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, - ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) + ROBOT_YELLOW_CHASSIS_COLOR.greenF()*1.5, + ROBOT_YELLOW_CHASSIS_COLOR.blueF()*1.5) ); else robots[best_k]->chassis->setColor( QColor::fromRgbF(ROBOT_BLUE_CHASSIS_COLOR.redF()*2, - ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, - ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) + ROBOT_BLUE_CHASSIS_COLOR.greenF()*1.5, + ROBOT_BLUE_CHASSIS_COLOR.blueF()*1.5) ); } selected = best_k;