diff options
Diffstat (limited to 'servers/physics_2d/step_2d_sw.cpp')
-rw-r--r-- | servers/physics_2d/step_2d_sw.cpp | 36 |
1 files changed, 12 insertions, 24 deletions
diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index 6f3bcfec13..c7711bcd1d 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -32,33 +32,33 @@ #include "core/os/os.h" void Step2DSW::_populate_island(Body2DSW *p_body, Body2DSW **p_island, Constraint2DSW **p_constraint_island) { - p_body->set_island_step(_step); p_body->set_island_next(*p_island); *p_island = p_body; for (Map<Constraint2DSW *, int>::Element *E = p_body->get_constraint_map().front(); E; E = E->next()) { - Constraint2DSW *c = (Constraint2DSW *)E->key(); - if (c->get_island_step() == _step) + if (c->get_island_step() == _step) { continue; //already processed + } c->set_island_step(_step); c->set_island_next(*p_constraint_island); *p_constraint_island = c; for (int i = 0; i < c->get_body_count(); i++) { - if (i == E->get()) + if (i == E->get()) { continue; + } Body2DSW *b = c->get_body_ptr()[i]; - if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) + if (b->get_island_step() == _step || b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { continue; //no go + } _populate_island(c->get_body_ptr()[i], p_island, p_constraint_island); } } } bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { - Constraint2DSW *ci = p_island; Constraint2DSW *prev_ci = nullptr; bool removed_root = false; @@ -83,9 +83,7 @@ bool Step2DSW::_setup_island(Constraint2DSW *p_island, real_t p_delta) { } void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, real_t p_delta) { - for (int i = 0; i < p_iterations; i++) { - Constraint2DSW *ci = p_island; while (ci) { ci->solve(p_delta); @@ -95,19 +93,18 @@ void Step2DSW::_solve_island(Constraint2DSW *p_island, int p_iterations, real_t } void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { - bool can_sleep = true; Body2DSW *b = p_island; while (b) { - if (b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { b = b->get_island_next(); continue; //ignore for static } - if (!b->sleep_test(p_delta)) + if (!b->sleep_test(p_delta)) { can_sleep = false; + } b = b->get_island_next(); } @@ -116,7 +113,6 @@ void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { b = p_island; while (b) { - if (b->get_mode() == PhysicsServer2D::BODY_MODE_STATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC) { b = b->get_island_next(); continue; //ignore for static @@ -124,15 +120,15 @@ void Step2DSW::_check_suspend(Body2DSW *p_island, real_t p_delta) { bool active = b->is_active(); - if (active == can_sleep) + if (active == can_sleep) { b->set_active(!can_sleep); + } b = b->get_island_next(); } } void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { - p_space->lock(); // can't access space during this p_space->setup(); //update inertias, etc @@ -148,7 +144,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { const SelfList<Body2DSW> *b = body_list->first(); while (b) { - b->self()->integrate_forces(p_delta); b = b->next(); active_count++; @@ -174,7 +169,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { Body2DSW *body = b->self(); if (body->get_island_step() != _step) { - Body2DSW *island = nullptr; Constraint2DSW *constraint_island = nullptr; _populate_island(body, &island, &constraint_island); @@ -197,10 +191,10 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { while (aml.first()) { for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { - Constraint2DSW *c = E->get(); - if (c->get_island_step() == _step) + if (c->get_island_step() == _step) { continue; + } c->set_island_step(_step); c->set_island_next(nullptr); c->set_island_list_next(constraint_island_list); @@ -221,9 +215,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { Constraint2DSW *ci = constraint_island_list; Constraint2DSW *prev_ci = nullptr; while (ci) { - if (_setup_island(ci, p_delta)) { - //removed the root from the island graph because it is not to be processed Constraint2DSW *next = ci->get_island_next(); @@ -238,7 +230,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { } prev_ci = next; } else { - //list is empty, just skip if (prev_ci) { prev_ci->set_island_list_next(ci->get_island_list_next()); @@ -282,7 +273,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { b = body_list->first(); while (b) { - const SelfList<Body2DSW> *n = b->next(); b->self()->integrate_velocities(p_delta); b = n; // in case it shuts itself down @@ -293,7 +283,6 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { { Body2DSW *bi = island_list; while (bi) { - _check_suspend(bi, p_delta); bi = bi->get_island_list_next(); } @@ -311,6 +300,5 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { } Step2DSW::Step2DSW() { - _step = 1; } |