Skip to content

Commit 3ceb345

Browse files
MinyazevRMinyazevR
authored
Update Box2D (#1942)
* Transfer Box2D to the current master erincatto/box2d * Test update of the submodule for the build * Increase simulate accuracy * Correct operation with Joint LocalFrame
1 parent 0cd485d commit 3ceb345

File tree

11 files changed

+313
-364
lines changed

11 files changed

+313
-364
lines changed

‎.gitmodules‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323
branch = master
2424
[submodule "plugins/robots/thirdparty/Box2D/Box2D"]
2525
path = plugins/robots/thirdparty/Box2D/Box2D
26-
url = https://github.com/iakov/Box2D
26+
url = https://github.com/erincatto/box2d
2727
branch = "trik-studio"
2828
[submodule "thirdparty/qslog/qslog"]
2929
path = thirdparty/qslog/qslog

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

Lines changed: 58 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -41,10 +41,12 @@ Box2DPhysicsEngine::Box2DPhysicsEngine (const WorldModel &worldModel
4141
, const QList<RobotModel *> &robots)
4242
: PhysicsEngineBase(worldModel, robots)
4343
, mPixelsInCm(worldModel.pixelsInCm() * scaleCoeff)
44-
, mWorld(new b2World(b2Vec2(0, 0)))
45-
, mPrevPosition(b2Vec2(0, 0))
44+
, mPrevPosition(b2Vec2{0, 0})
4645
, mPrevAngle(0)
4746
{
47+
b2WorldDef worldDef = b2DefaultWorldDef();
48+
worldDef.gravity = b2Vec2{0, 0};
49+
mWorldId = b2CreateWorld(&worldDef);
4850
connect(&worldModel, &model::WorldModel::wallAdded,
4951
this, [this](const QSharedPointer<QGraphicsItem> &i) {itemAdded(i.data());});
5052
connect(&worldModel, &model::WorldModel::skittleAdded,
@@ -58,15 +60,16 @@ Box2DPhysicsEngine::Box2DPhysicsEngine (const WorldModel &worldModel
5860
Box2DPhysicsEngine::~Box2DPhysicsEngine(){
5961
qDeleteAll(mBox2DRobots);
6062
qDeleteAll(mBox2DResizableItems);
63+
b2DestroyWorld(mWorldId);
64+
mWorldId = b2_nullWorldId;
6165
}
6266

6367
QVector2D Box2DPhysicsEngine::positionShift(model::RobotModel &robot) const
6468
{
6569
if (!mBox2DRobots.contains(&robot)) {
6670
return QVector2D();
6771
}
68-
69-
return QVector2D(positionToScene(mBox2DRobots[&robot]->getBody()->GetPosition() - mPrevPosition));
72+
return QVector2D(positionToScene(b2Body_GetPosition(mBox2DRobots[&robot]->getBodyId()) - mPrevPosition));
7073
}
7174

7275
qreal Box2DPhysicsEngine::rotation(model::RobotModel &robot) const
@@ -75,15 +78,20 @@ qreal Box2DPhysicsEngine::rotation(model::RobotModel &robot) const
7578
return 0;
7679
}
7780

78-
return angleToScene(mBox2DRobots[&robot]->getBody()->GetAngle() - mPrevAngle);
81+
auto angle = b2Rot_GetAngle(b2Body_GetRotation(mBox2DRobots[&robot]->getBodyId()));
82+
return angleToScene(angle - mPrevAngle);
7983
}
8084

8185
void Box2DPhysicsEngine::onPressedReleasedSelectedItems(bool active)
8286
{
8387
for (auto *item : mScene->selectedItems()) {
8488
Box2DItem *bItem = mBox2DDynamicItems.value(item, nullptr);
8589
if (bItem) {
86-
bItem->getBody()->SetEnabled(active);
90+
if (active) {
91+
b2Body_Enable(bItem->getBodyId());
92+
} else {
93+
b2Body_Disable(bItem->getBodyId());
94+
}
8795
}
8896
}
8997
}
@@ -98,8 +106,9 @@ void Box2DPhysicsEngine::addRobot(model::RobotModel * const robot)
98106
PhysicsEngineBase::addRobot(robot);
99107
addRobot(robot, robot->robotCenter(), robot->rotation());
100108

101-
mPrevPosition = mBox2DRobots[robot]->getBody()->GetPosition();
102-
mPrevAngle = mBox2DRobots[robot]->getBody()->GetAngle();
109+
auto bodyId = mBox2DRobots[robot]->getBodyId();
110+
mPrevPosition = b2Body_GetPosition(bodyId);
111+
mPrevAngle = b2Rot_GetAngle(b2Body_GetRotation(bodyId));
103112

104113
connect(robot, &model::RobotModel::positionChanged, this, [&] (const QPointF &newPos) {
105114
onRobotStartPositionChanged(newPos, dynamic_cast<model::RobotModel *>(sender()));
@@ -202,14 +211,14 @@ void Box2DPhysicsEngine::onRecoverRobotPosition(const QPointF &pos)
202211
{
203212
clearForcesAndStop();
204213

205-
auto stop = [=](b2Body *body){
206-
body->SetAngularVelocity(0);
207-
body->SetLinearVelocity({0, 0});
214+
auto stop = [=](b2BodyId bodyId){
215+
b2Body_SetAngularVelocity(bodyId, 0);
216+
b2Body_SetLinearVelocity(bodyId, {0, 0});
208217
};
209218

210-
stop(mBox2DRobots.first()->getBody());
211-
stop(mBox2DRobots.first()->getWheelAt(0)->getBody());
212-
stop(mBox2DRobots.first()->getWheelAt(1)->getBody());
219+
stop(mBox2DRobots.first()->getBodyId());
220+
stop(mBox2DRobots.first()->getWheelAt(0)->getBodyId());
221+
stop(mBox2DRobots.first()->getWheelAt(1)->getBodyId());
213222

214223
onMouseReleased(pos, mBox2DRobots.keys().first()->startPositionMarker()->rotation());
215224
}
@@ -225,15 +234,14 @@ void Box2DPhysicsEngine::removeRobot(model::RobotModel * const robot)
225234

226235
void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
227236
{
228-
const int velocityIterations = 10;
229-
const int positionIterations = 6;
237+
const int subStepCount = 10;
230238

231239
model::RobotModel * const robot = mRobots.first();
232240
if (!mBox2DRobots[robot]) {
233241
return;
234242
}
235243

236-
b2Body *rBody = mBox2DRobots[robot]->getBody();
244+
b2BodyId rBodyId = mBox2DRobots[robot]->getBodyId();
237245
float secondsInterval = timeInterval / 1000.0f;
238246

239247
if (mBox2DRobots[robot]->isStopping()){
@@ -244,7 +252,7 @@ void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
244252
const qreal speed1 = pxToM(wheelLinearSpeed(*robot, robot->leftWheel())) / secondsInterval * sAdpt;
245253
const qreal speed2 = pxToM(wheelLinearSpeed(*robot, robot->rightWheel())) / secondsInterval * sAdpt;
246254

247-
if (qAbs(speed1) + qAbs(speed2) < b2_epsilon) {
255+
if (qAbs(speed1) + qAbs(speed2) < FLT_EPSILON) {
248256
mBox2DRobots[robot]->stop();
249257
mLeftWheels[robot]->stop();
250258
mRightWheels[robot]->stop();
@@ -255,11 +263,11 @@ void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
255263
}
256264
}
257265

258-
mPrevPosition = rBody->GetPosition();
259-
mPrevAngle = rBody->GetAngle();
260266

261-
mWorld->Step(secondsInterval, velocityIterations, positionIterations);
267+
mPrevPosition = b2Body_GetPosition(rBodyId);
268+
mPrevAngle = b2Rot_GetAngle(b2Body_GetRotation(rBodyId));
262269

270+
b2World_Step(mWorldId, secondsInterval, subStepCount);
263271
static volatile auto sThisFlagHelpsToAvoidClangError = true;
264272
if ("If you want debug BOX2D, fix this expression to be false" && sThisFlagHelpsToAvoidClangError) {
265273
return;
@@ -286,8 +294,12 @@ void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
286294
}
287295
}
288296

289-
qreal angleRobot= angleToScene(mBox2DRobots[robot]->getBody()->GetAngle());
290-
QPointF posRobot = positionToScene(mBox2DRobots[robot]->getBody()->GetPosition());
297+
auto bodyId = mBox2DRobots[robot]->getBodyId();
298+
auto angle = b2Rot_GetAngle(b2Body_GetRotation(bodyId));
299+
auto position = b2Body_GetPosition(bodyId);
300+
301+
qreal angleRobot = angleToScene(angle);
302+
QPointF posRobot = positionToScene(position);
291303
QGraphicsRectItem *rect1 = new QGraphicsRectItem(-25, -25, 60, 50);
292304
QGraphicsRectItem *rect2 = new QGraphicsRectItem(-10, -6, 20, 10);
293305
QGraphicsRectItem *rect3 = new QGraphicsRectItem(-10, -6, 20, 10);
@@ -296,11 +308,20 @@ void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
296308
rect1->setRotation(angleRobot);
297309
rect1->setPos(posRobot);
298310
rect2->setTransformOriginPoint(0, 0);
299-
rect2->setRotation(angleToScene(mBox2DRobots[robot]->getWheelAt(0)->getBody()->GetAngle()));
300-
rect2->setPos(positionToScene(mBox2DRobots[robot]->getWheelAt(0)->getBody()->GetPosition()));
311+
312+
auto wheelBodyId = mBox2DRobots[robot]->getWheelAt(0)->getBodyId();
313+
auto wheelBodyAngle = b2Rot_GetAngle(b2Body_GetRotation(wheelBodyId));
314+
auto wheelBodyPosition = b2Body_GetPosition(wheelBodyId);
315+
316+
auto wheel1BodyId = mBox2DRobots[robot]->getWheelAt(1)->getBodyId();
317+
auto wheel1BodyAngle = b2Rot_GetAngle(b2Body_GetRotation(wheel1BodyId));
318+
auto wheel1BodyPosition = b2Body_GetPosition(wheel1BodyId);
319+
320+
rect2->setRotation(angleToScene(wheelBodyAngle));
321+
rect2->setPos(positionToScene(wheelBodyPosition));
301322
rect3->setTransformOriginPoint(0, 0);
302-
rect3->setRotation(angleToScene(mBox2DRobots[robot]->getWheelAt(1)->getBody()->GetAngle()));
303-
rect3->setPos(positionToScene(mBox2DRobots[robot]->getWheelAt(1)->getBody()->GetPosition()));
323+
rect3->setRotation(angleToScene(wheel1BodyAngle));
324+
rect3->setPos(positionToScene(wheel1BodyPosition));
304325
mScene->addItem(rect1);
305326
mScene->addItem(rect2);
306327
mScene->addItem(rect3);
@@ -319,7 +340,7 @@ void Box2DPhysicsEngine::recalculateParameters(qreal timeInterval)
319340

320341
const QMap<const view::SensorItem *, Box2DItem *> sensors = mBox2DRobots[robot]->getSensors();
321342
for (Box2DItem * sensor : sensors.values()) {
322-
const b2Vec2 position = sensor->getBody()->GetPosition();
343+
const b2Vec2 position = b2Body_GetPosition(sensor->getBodyId());
323344
QPointF scenePos = positionToScene(position);
324345
path.addEllipse(scenePos, 10, 10);
325346
}
@@ -348,7 +369,8 @@ void Box2DPhysicsEngine::wakeUp()
348369
void Box2DPhysicsEngine::nextFrame()
349370
{
350371
for(QGraphicsItem *item : mBox2DDynamicItems.keys()) {
351-
if (mBox2DDynamicItems[item]->getBody()->IsEnabled() && mBox2DDynamicItems[item]->angleOrPositionChanged()) {
372+
auto isEnabled = b2Body_IsEnabled(mBox2DDynamicItems[item]->getBodyId());
373+
if (isEnabled && mBox2DDynamicItems[item]->angleOrPositionChanged()) {
352374
QPointF scenePos = positionToScene(mBox2DDynamicItems[item]->getPosition());
353375
item->setPos(scenePos - item->boundingRect().center());
354376
item->setRotation(angleToScene(mBox2DDynamicItems[item]->getRotation()));
@@ -358,11 +380,10 @@ void Box2DPhysicsEngine::nextFrame()
358380

359381
void Box2DPhysicsEngine::clearForcesAndStop()
360382
{
361-
mWorld->ClearForces();
362383
for (auto item : mBox2DDynamicItems) {
363-
b2Body *body = item->getBody();
364-
body->SetLinearVelocity({0, 0});
365-
body->SetAngularVelocity(0);
384+
b2BodyId bodyId = item->getBodyId();
385+
b2Body_SetLinearVelocity(bodyId, {0, 0});
386+
b2Body_SetAngularVelocity(bodyId, 0);
366387
}
367388
}
368389

@@ -449,9 +470,9 @@ void Box2DPhysicsEngine::itemRemoved(QGraphicsItem * const item)
449470
mBox2DDynamicItems.remove(item);
450471
}
451472

452-
b2World &Box2DPhysicsEngine::box2DWorld()
473+
b2WorldId Box2DPhysicsEngine::box2DWorldId()
453474
{
454-
return *mWorld.data();
475+
return mWorldId;
455476
}
456477

457478
float Box2DPhysicsEngine::pxToCm(qreal px) const
@@ -461,7 +482,7 @@ float Box2DPhysicsEngine::pxToCm(qreal px) const
461482

462483
b2Vec2 Box2DPhysicsEngine::pxToCm(const QPointF &posInPx) const
463484
{
464-
return b2Vec2(pxToCm(posInPx.x()), pxToCm(posInPx.y()));
485+
return b2Vec2{pxToCm(posInPx.x()), pxToCm(posInPx.y())};
465486
}
466487

467488
qreal Box2DPhysicsEngine::cmToPx(float cm) const
@@ -492,7 +513,7 @@ QPointF Box2DPhysicsEngine::positionToScene(b2Vec2 boxCoords) const
492513

493514
QPointF Box2DPhysicsEngine::positionToScene(float x, float y) const
494515
{
495-
b2Vec2 invertedCoords = b2Vec2(x, -y);
516+
b2Vec2 invertedCoords = b2Vec2{x, -y};
496517
return cmToPx(100.0f * invertedCoords);
497518
}
498519

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ class Box2DPhysicsEngine : public PhysicsEngineBase
7575
qreal computeDensity(const QPolygonF &shape, qreal mass);
7676
qreal computeDensity(qreal radius, qreal mass);
7777

78-
b2World &box2DWorld();
78+
b2WorldId box2DWorldId();
7979

8080
public slots:
8181
void onItemDragged(graphicsUtils::AbstractItem *item);
@@ -97,7 +97,7 @@ public slots:
9797

9898
twoDModel::view::TwoDModelScene *mScene {}; // Doesn't take ownership
9999
qreal mPixelsInCm;
100-
QScopedPointer<b2World> mWorld;
100+
b2WorldId mWorldId;
101101

102102
QMap<RobotModel *, parts::Box2DRobot *> mBox2DRobots; // Takes ownership on b2Body instances
103103
QMap<RobotModel *, parts::Box2DWheel *> mLeftWheels; // Does not take ownership

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

Lines changed: 35 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -25,9 +25,9 @@ Box2DItem::Box2DItem(twoDModel::model::physics::Box2DPhysicsEngine *engine
2525
: mIsCircle(false)
2626
, mEngine(*engine)
2727
{
28-
b2BodyDef bodyDef;
28+
b2BodyDef bodyDef = b2DefaultBodyDef();
2929
bodyDef.position = pos;
30-
bodyDef.angle = angle;
30+
bodyDef.rotation = b2MakeRot(angle);
3131
if (item->bodyType() == SolidItem::DYNAMIC) {
3232
bodyDef.type = b2_dynamicBody;
3333
} else if (item->bodyType() == SolidItem::KINEMATIC) {
@@ -36,22 +36,21 @@ Box2DItem::Box2DItem(twoDModel::model::physics::Box2DPhysicsEngine *engine
3636
bodyDef.type = b2_staticBody;
3737
}
3838

39-
mBody = this->mEngine.box2DWorld().CreateBody(&bodyDef);
40-
mBody->SetAngularDamping(item->angularDamping());
41-
mBody->SetLinearDamping(item->linearDamping());
42-
43-
b2FixtureDef fixture;
44-
fixture.restitution = 0.8;
39+
auto worldId = this->mEngine.box2DWorldId();
40+
mBodyId = b2CreateBody(worldId, &bodyDef);
41+
b2Body_SetAngularDamping(mBodyId, item->angularDamping());
42+
b2Body_SetLinearDamping(mBodyId, item->linearDamping());
43+
b2ShapeDef fixtureDef = b2DefaultShapeDef();
44+
fixtureDef.material.restitution = 0.8f;
4545
QPolygonF collidingPolygon = item->collidingPolygon();
4646
QPointF localCenter = collidingPolygon.boundingRect().center();
47-
b2CircleShape circleShape;
48-
b2PolygonShape polygonShape;
47+
b2Circle circleShape = {};
48+
b2Polygon polygonShape = {};
4949
if (item->isCircle()) {
5050
mIsCircle = true;
5151
qreal radius = collidingPolygon.boundingRect().height() / 2;
52-
circleShape.m_radius = this->mEngine.pxToM(radius);
53-
fixture.shape = &circleShape;
54-
fixture.density = engine->computeDensity(radius, item->mass());
52+
circleShape.radius = this->mEngine.pxToM(radius);
53+
fixtureDef.density = engine->computeDensity(radius, item->mass());
5554
} else {
5655
if (collidingPolygon.isClosed()) {
5756
collidingPolygon.removeLast();
@@ -62,55 +61,61 @@ Box2DItem::Box2DItem(twoDModel::model::physics::Box2DPhysicsEngine *engine
6261
mPolygon[i] = engine->positionToBox2D(collidingPolygon.at(i) - localCenter);
6362
}
6463

65-
polygonShape.Set(mPolygon, collidingPolygon.size());
66-
fixture.shape = &polygonShape;
67-
fixture.density = engine->computeDensity(collidingPolygon, item->mass());
64+
b2Hull hull = b2ComputeHull(mPolygon, collidingPolygon.size());
65+
polygonShape = b2MakePolygon(&hull, 0.0f);
66+
fixtureDef.density = engine->computeDensity(collidingPolygon, item->mass());
6867
}
6968

70-
fixture.friction = item->friction();
71-
mBody->CreateFixture(&fixture);
72-
mBody->SetUserData(this);
69+
fixtureDef.material.friction = item->friction();
70+
if (item->isCircle()) {
71+
b2CreateCircleShape(mBodyId, &fixtureDef, &circleShape);
72+
} else {
73+
b2CreatePolygonShape(mBodyId, &fixtureDef, &polygonShape);
74+
}
75+
b2Body_SetUserData(mBodyId, this);
7376
}
7477

7578
Box2DItem::~Box2DItem()
7679
{
77-
mBody->GetWorld()->DestroyBody(mBody);
80+
b2DestroyBody(mBodyId);
7881
if (!mIsCircle) {
7982
delete[] mPolygon;
8083
}
8184
}
8285

8386
void Box2DItem::moveToPosition(const b2Vec2 &pos)
8487
{
85-
mBody->SetTransform(pos, mBody->GetAngle());
86-
mPreviousPosition = mBody->GetPosition();
88+
b2Body_SetTransform(mBodyId, pos, b2Body_GetRotation(mBodyId));
89+
mPreviousPosition = b2Body_GetPosition(mBodyId);
8790
}
8891

8992
void Box2DItem::setRotation(float angle)
9093
{
91-
mBody->SetTransform(mBody->GetPosition(), angle);
92-
mPreviousRotation = mBody->GetAngle();
94+
b2Body_SetTransform(mBodyId, b2Body_GetPosition(mBodyId), b2MakeRot(angle));
95+
mPreviousRotation = b2Rot_GetAngle(b2Body_GetRotation(mBodyId));
9396
}
9497

9598
const b2Vec2 &Box2DItem::getPosition()
9699
{
97-
mPreviousPosition = mBody->GetPosition();
100+
mPreviousPosition = b2Body_GetPosition(mBodyId);
98101
return mPreviousPosition;
99102
}
100103

101104
float Box2DItem::getRotation()
102105
{
103-
mPreviousRotation = mBody->GetAngle();
106+
mPreviousRotation = b2Rot_GetAngle(b2Body_GetRotation(mBodyId));
104107
return mPreviousRotation;
105108
}
106109

107-
b2Body *Box2DItem::getBody() const
110+
b2BodyId Box2DItem::getBodyId() const
108111
{
109-
return mBody;
112+
return mBodyId;
110113
}
111114

112115
bool Box2DItem::angleOrPositionChanged() const
113116
{
114-
return b2Distance(mPreviousPosition, mBody->GetPosition()) > b2_epsilon
115-
|| qAbs(mPreviousRotation - mBody->GetAngle()) > b2_epsilon;
117+
auto angle = b2Rot_GetAngle(b2Body_GetRotation(mBodyId));
118+
auto position = b2Body_GetPosition(mBodyId);
119+
return b2Distance(mPreviousPosition, position) > FLT_EPSILON
120+
|| qAbs(mPreviousRotation - angle) > FLT_EPSILON;
116121
}

0 commit comments

Comments
 (0)