Skip to content

Commit b2a1f05

Browse files
iakoviakov
committed
Cleanup code after hot fix
1 parent f41d638 commit b2a1f05

File tree

2 files changed

+89
-95
lines changed

2 files changed

+89
-95
lines changed

‎plugins/robots/common/twoDModel/src/engine/model/model.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -293,7 +293,7 @@ void Model::initPhysics()
293293
connect(this, &model::Model::robotRemoved, mSimplePhysicsEngine, &physics::PhysicsEngineBase::removeRobot);
294294

295295
connect(&mTimeline, &Timeline::tick, this, &Model::recalculatePhysicsParams);
296-
connect(&mTimeline, &Timeline::nextFrame, this, [this](){ mRealisticPhysicsEngine->nextFrame(); });
296+
connect(&mTimeline, &Timeline::nextFrame, mRealisticPhysicsEngine, &physics::PhysicsEngineBase::nextFrame);
297297
}
298298

299299
void Model::recalculatePhysicsParams()

‎plugins/robots/common/twoDModel/src/engine/model/physics/box2DPhysicsEngine.cpp‎

Lines changed: 88 additions & 94 deletions
Original file line numberDiff line numberDiff line change
@@ -31,13 +31,6 @@
3131
#include "parts/box2DRobot.h"
3232
#include "parts/box2DItem.h"
3333

34-
//#define BOX2D_DEBUG_PATH
35-
36-
#ifdef BOX2D_DEBUG_PATH
37-
QGraphicsPathItem *debugPathBox2D = nullptr;
38-
#endif
39-
40-
4134
using namespace twoDModel::model::physics;
4235
using namespace parts;
4336
using namespace mathUtils;
@@ -236,103 +229,111 @@ void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
236229
const int positionIterations = 6;
237230

238231
model::RobotModel * const robot = mRobots.first();
239-
if (mBox2DRobots[robot]) {
240-
b2Body *rBody = mBox2DRobots[robot]->getBody();
241-
float secondsInterval = timeInterval / 1000.0f;
232+
if (!mBox2DRobots[robot]) {
233+
return;
234+
}
235+
236+
b2Body *rBody = mBox2DRobots[robot]->getBody();
237+
float secondsInterval = timeInterval / 1000.0f;
238+
239+
if (mBox2DRobots[robot]->isStopping()){
240+
mBox2DRobots[robot]->stop();
241+
} else {
242+
// sAdpt is the speed adaptation coefficient for physics engines
243+
const int sAdpt = 10;
244+
const qreal speed1 = pxToM(wheelLinearSpeed(*robot, robot->leftWheel())) / secondsInterval * sAdpt;
245+
const qreal speed2 = pxToM(wheelLinearSpeed(*robot, robot->rightWheel())) / secondsInterval * sAdpt;
242246

243-
if (mBox2DRobots[robot]->isStopping()){
247+
if (qAbs(speed1) + qAbs(speed2) < b2_epsilon) {
244248
mBox2DRobots[robot]->stop();
245-
} else {
246-
// sAdpt is the speed adaptation coefficient for physics engines
247-
const int sAdpt = 10;
248-
const qreal speed1 = pxToM(wheelLinearSpeed(*robot, robot->leftWheel())) / secondsInterval * sAdpt;
249-
const qreal speed2 = pxToM(wheelLinearSpeed(*robot, robot->rightWheel())) / secondsInterval * sAdpt;
250-
251-
if (qAbs(speed1) + qAbs(speed2) < b2_epsilon) {
252-
mBox2DRobots[robot]->stop();
253-
mLeftWheels[robot]->stop();
254-
mRightWheels[robot]->stop();
255-
}
256-
else {
257-
mLeftWheels[robot]->keepConstantSpeed(speed1);
258-
mRightWheels[robot]->keepConstantSpeed(speed2);
259-
}
249+
mLeftWheels[robot]->stop();
250+
mRightWheels[robot]->stop();
251+
}
252+
else {
253+
mLeftWheels[robot]->keepConstantSpeed(speed1);
254+
mRightWheels[robot]->keepConstantSpeed(speed2);
260255
}
256+
}
261257

262-
mPrevPosition = rBody->GetPosition();
263-
mPrevAngle = rBody->GetAngle();
258+
mPrevPosition = rBody->GetPosition();
259+
mPrevAngle = rBody->GetAngle();
264260

265-
mWorld->Step(secondsInterval, velocityIterations, positionIterations);
261+
mWorld->Step(secondsInterval, velocityIterations, positionIterations);
266262

267-
#ifdef BOX2D_DEBUG_PATH
268-
delete debugPathBox2D;
269-
QPainterPath path;
263+
if ("If you want debug BOX2D, fix this expression to be false" && true) {
264+
return;
265+
}
270266

271-
for(QGraphicsItem *item : mBox2DDynamicItems.keys()) {
272-
if (auto solidItem = dynamic_cast<items::SolidItem *>(item)) {
273-
QPolygonF localCollidingPolygon = solidItem->collidingPolygon();
274-
qreal lsceneAngle = angleToScene(mBox2DDynamicItems[item]->getRotation());
275-
QMatrix m;
276-
m.rotate(lsceneAngle);
267+
QPainterPath path;
277268

278-
QPointF firstP = localCollidingPolygon.at(0);
279-
localCollidingPolygon.translate(-firstP.x(), -firstP.y());
269+
for(QGraphicsItem *item : mBox2DDynamicItems.keys()) {
270+
if (auto solidItem = dynamic_cast<items::SolidItem *>(item)) {
271+
QPolygonF localCollidingPolygon = solidItem->collidingPolygon();
272+
qreal lsceneAngle = angleToScene(mBox2DDynamicItems[item]->getRotation());
273+
QMatrix m;
274+
m.rotate(lsceneAngle);
280275

281-
QPainterPath lpath;
282-
lpath.addPolygon(localCollidingPolygon);
283-
QPainterPath lpathTR = m.map(lpath);
284-
lpathTR.translate(firstP.x(), firstP.y());
276+
QPointF firstP = localCollidingPolygon.at(0);
277+
localCollidingPolygon.translate(-firstP.x(), -firstP.y());
285278

286-
path.addPath(lpathTR);
287-
}
279+
QPainterPath lpath;
280+
lpath.addPolygon(localCollidingPolygon);
281+
QPainterPath lpathTR = m.map(lpath);
282+
lpathTR.translate(firstP.x(), firstP.y());
283+
284+
path.addPath(lpathTR);
288285
}
286+
}
289287

290-
qreal angleRobot= angleToScene(mBox2DRobots[robot]->getBody()->GetAngle());
291-
QPointF posRobot = positionToScene(mBox2DRobots[robot]->getBody()->GetPosition());
292-
QGraphicsRectItem *rect1 = new QGraphicsRectItem(-25, -25, 60, 50);
293-
QGraphicsRectItem *rect2 = new QGraphicsRectItem(-10, -6, 20, 10);
294-
QGraphicsRectItem *rect3 = new QGraphicsRectItem(-10, -6, 20, 10);
295-
296-
rect1->setTransformOriginPoint(0, 0);
297-
rect1->setRotation(angleRobot);
298-
rect1->setPos(posRobot);
299-
rect2->setTransformOriginPoint(0, 0);
300-
rect2->setRotation(angleToScene(mBox2DRobots[robot]->getWheelAt(0)->getBody()->GetAngle()));
301-
rect2->setPos(positionToScene(mBox2DRobots[robot]->getWheelAt(0)->getBody()->GetPosition()));
302-
rect3->setTransformOriginPoint(0, 0);
303-
rect3->setRotation(angleToScene(mBox2DRobots[robot]->getWheelAt(1)->getBody()->GetAngle()));
304-
rect3->setPos(positionToScene(mBox2DRobots[robot]->getWheelAt(1)->getBody()->GetPosition()));
305-
mScene->addItem(rect1);
306-
mScene->addItem(rect2);
307-
mScene->addItem(rect3);
308-
QTimer::singleShot(20, [=](){
309-
delete rect1;
310-
delete rect2;
311-
delete rect3;
312-
});
288+
qreal angleRobot= angleToScene(mBox2DRobots[robot]->getBody()->GetAngle());
289+
QPointF posRobot = positionToScene(mBox2DRobots[robot]->getBody()->GetPosition());
290+
QGraphicsRectItem *rect1 = new QGraphicsRectItem(-25, -25, 60, 50);
291+
QGraphicsRectItem *rect2 = new QGraphicsRectItem(-10, -6, 20, 10);
292+
QGraphicsRectItem *rect3 = new QGraphicsRectItem(-10, -6, 20, 10);
293+
294+
rect1->setTransformOriginPoint(0, 0);
295+
rect1->setRotation(angleRobot);
296+
rect1->setPos(posRobot);
297+
rect2->setTransformOriginPoint(0, 0);
298+
rect2->setRotation(angleToScene(mBox2DRobots[robot]->getWheelAt(0)->getBody()->GetAngle()));
299+
rect2->setPos(positionToScene(mBox2DRobots[robot]->getWheelAt(0)->getBody()->GetPosition()));
300+
rect3->setTransformOriginPoint(0, 0);
301+
rect3->setRotation(angleToScene(mBox2DRobots[robot]->getWheelAt(1)->getBody()->GetAngle()));
302+
rect3->setPos(positionToScene(mBox2DRobots[robot]->getWheelAt(1)->getBody()->GetPosition()));
303+
mScene->addItem(rect1);
304+
mScene->addItem(rect2);
305+
mScene->addItem(rect3);
306+
QTimer::singleShot(20, [this, rect1, rect2, rect3](){
307+
for (auto &&rect: {rect1, rect2, rect3}) {
308+
mScene->removeItem(rect);
309+
delete rect;
310+
}
311+
});
313312

314313

315314
// uncomment it for watching mutual position of robot and wheels (polygon form)
316315
// path.addPolygon(mBox2DRobots[robot]->getDebuggingPolygon());
317316
// path.addPolygon(mBox2DRobots[robot]->getWheelAt(0)->mDebuggingDrawPolygon);
318317
// path.addPolygon(mBox2DRobots[robot]->getWheelAt(1)->mDebuggingDrawPolygon);
319318

320-
const QMap<const view::SensorItem *, Box2DItem *> sensors = mBox2DRobots[robot]->getSensors();
321-
for (Box2DItem * sensor : sensors.values()) {
322-
const b2Vec2 position = sensor->getBody()->GetPosition();
323-
QPointF scenePos = positionToScene(position);
324-
path.addEllipse(scenePos, 10, 10);
325-
}
326-
327-
debugPathBox2D = new QGraphicsPathItem(path);
328-
debugPathBox2D->setBrush(Qt::blue);
329-
debugPathBox2D->setPen(QPen(QColor(Qt::red)));
330-
debugPathBox2D->setZValue(101);
331-
mScene->addItem(debugPathBox2D);
332-
mScene->update();
319+
const QMap<const view::SensorItem *, Box2DItem *> sensors = mBox2DRobots[robot]->getSensors();
320+
for (Box2DItem * sensor : sensors.values()) {
321+
const b2Vec2 position = sensor->getBody()->GetPosition();
322+
QPointF scenePos = positionToScene(position);
323+
path.addEllipse(scenePos, 10, 10);
324+
}
333325

334-
#endif
326+
static QGraphicsPathItem *debugPathBox2D = nullptr;
327+
if (debugPathBox2D) {
328+
mScene->removeItem(debugPathBox2D);
329+
delete debugPathBox2D;
335330
}
331+
debugPathBox2D = new QGraphicsPathItem(path);
332+
debugPathBox2D->setBrush(Qt::blue);
333+
debugPathBox2D->setPen(QPen(QColor(Qt::red)));
334+
debugPathBox2D->setZValue(101);
335+
mScene->addItem(debugPathBox2D);
336+
mScene->update();
336337
}
337338

338339
void Box2DPhysicsEngine::wakeUp()
@@ -411,9 +412,7 @@ void Box2DPhysicsEngine::onItemDragged(graphicsUtils::AbstractItem *item)
411412
Box2DItem *box2dItem = new Box2DItem(this, wallItem, pos, angleToBox2D(item->rotation()));
412413
mBox2DResizableItems[item] = box2dItem;
413414
return;
414-
}
415-
416-
if (auto solidItem = dynamic_cast<items::SolidItem *>(item)) {
415+
} else if (auto solidItem = dynamic_cast<items::SolidItem *>(item)) {
417416
QPolygonF collidingPolygon = solidItem->collidingPolygon();
418417
if (itemTracked(item)) {
419418
if (solidItem->bodyType() == items::SolidItem::DYNAMIC) {
@@ -444,14 +443,9 @@ void Box2DPhysicsEngine::onItemDragged(graphicsUtils::AbstractItem *item)
444443

445444
void Box2DPhysicsEngine::itemRemoved(QGraphicsItem * const item)
446445
{
447-
if (mBox2DResizableItems.contains(item)) {
448-
delete mBox2DResizableItems[item];
449-
mBox2DResizableItems.remove(item);
450-
}
451-
452-
if (mBox2DDynamicItems.contains(item)) {
453-
mBox2DDynamicItems.remove(item);
454-
}
446+
delete mBox2DResizableItems.value(item);
447+
mBox2DResizableItems.remove(item);
448+
mBox2DDynamicItems.remove(item);
455449
}
456450

457451
b2World &Box2DPhysicsEngine::box2DWorld()

0 commit comments

Comments
 (0)