diff options
| -rw-r--r-- | servers/physics_2d/body_pair_2d_sw.cpp | 6 | ||||
| -rw-r--r-- | servers/physics_2d/step_2d_sw.cpp | 52 | ||||
| -rw-r--r-- | servers/physics_2d/step_2d_sw.h | 2 |
3 files changed, 55 insertions, 5 deletions
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp index b6006de140..db7869c6a6 100644 --- a/servers/physics_2d/body_pair_2d_sw.cpp +++ b/servers/physics_2d/body_pair_2d_sw.cpp @@ -364,6 +364,9 @@ bool BodyPair2DSW::setup(float p_step) { real_t inv_dt = 1.0/p_step; + + bool do_process=false; + for (int i = 0; i < contact_count; i++) { Contact& c = contacts[i]; @@ -459,10 +462,11 @@ bool BodyPair2DSW::setup(float p_step) { c.bounce = c.bounce * dv.dot(c.normal); } + do_process=true; } - return true; + return do_process; } void BodyPair2DSW::solve(float p_step) { diff --git a/servers/physics_2d/step_2d_sw.cpp b/servers/physics_2d/step_2d_sw.cpp index e3439fa109..94e1d26329 100644 --- a/servers/physics_2d/step_2d_sw.cpp +++ b/servers/physics_2d/step_2d_sw.cpp @@ -56,14 +56,29 @@ void Step2DSW::_populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2 } } -void Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) { +bool Step2DSW::_setup_island(Constraint2DSW *p_island,float p_delta) { Constraint2DSW *ci=p_island; + Constraint2DSW *prev_ci=NULL; + bool removed_root=false; while(ci) { bool process = ci->setup(p_delta); - //todo remove from island if process fails + + if (!process) { + //remove from island if process fails + if (prev_ci) { + prev_ci->set_island_next(ci->get_island_next()); + } else { + removed_root=true; + prev_ci=ci; + } + } else { + prev_ci=ci; + } ci=ci->get_island_next(); } + + return removed_root; } void Step2DSW::_solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta){ @@ -195,9 +210,40 @@ void Step2DSW::step(Space2DSW* p_space,float p_delta,int p_iterations) { { Constraint2DSW *ci=constraint_island_list; + Constraint2DSW *prev_ci=NULL; while(ci) { - _setup_island(ci,p_delta); + if (_setup_island(ci,p_delta)==true) { + + //removed the root from the island graph because it is not to be processed + + Constraint2DSW *next = ci->get_island_next(); + + if (next) { + //root from list being deleted no longer exists, replace by next + next->set_island_list_next(ci->get_island_list_next()); + if (prev_ci) { + prev_ci->set_island_list_next(next); + } else { + constraint_island_list=next; + + } + prev_ci=next; + } else { + + //list is empty, just skip + if (prev_ci) { + prev_ci->set_island_list_next(ci->get_island_list_next()); + + } else { + constraint_island_list=ci->get_island_list_next(); + } + + } + } else { + prev_ci=ci; + } + ci=ci->get_island_list_next(); } } diff --git a/servers/physics_2d/step_2d_sw.h b/servers/physics_2d/step_2d_sw.h index e8c6316886..0c374d7e12 100644 --- a/servers/physics_2d/step_2d_sw.h +++ b/servers/physics_2d/step_2d_sw.h @@ -36,7 +36,7 @@ class Step2DSW { uint64_t _step; void _populate_island(Body2DSW* p_body,Body2DSW** p_island,Constraint2DSW **p_constraint_island); - void _setup_island(Constraint2DSW *p_island,float p_delta); + bool _setup_island(Constraint2DSW *p_island,float p_delta); void _solve_island(Constraint2DSW *p_island,int p_iterations,float p_delta); void _check_suspend(Body2DSW *p_island,float p_delta); public: |