void StepSW::step(SpaceSW *p_space, real_t p_delta, int p_iterations) {
p_space->lock(); // can't access space during this
p_space->setup(); //update inertias, etc
const SelfList<BodySW>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */
uint64_t profile_begtime = OS::get_singleton()->get_ticks_usec();
uint64_t profile_endtime = 0;
int active_count = 0;
const SelfList<BodySW> *b = body_list->first();
while (b) {
b->self()->integrate_forces(p_delta);
b = b->next();
active_count++;
}
p_space->set_active_objects(active_count);
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* GENERATE CONSTRAINT ISLANDS */
BodySW *island_list = NULL;
ConstraintSW *constraint_island_list = NULL;
b = body_list->first();
int island_count = 0;
while (b) {
BodySW *body = b->self();
if (body->get_island_step() != _step) {
BodySW *island = NULL;
ConstraintSW *constraint_island = NULL;
_populate_island(body, &island, &constraint_island);
island->set_island_list_next(island_list);
island_list = island;
if (constraint_island) {
constraint_island->set_island_list_next(constraint_island_list);
constraint_island_list = constraint_island;
island_count++;
}
}
b = b->next();
}
p_space->set_island_count(island_count);
const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list();
while (aml.first()) {
for (const Set<ConstraintSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
ConstraintSW *c = E->get();
if (c->get_island_step() == _step)
continue;
c->set_island_step(_step);
c->set_island_next(NULL);
c->set_island_list_next(constraint_island_list);
constraint_island_list = c;
}
p_space->area_remove_from_moved_list((SelfList<AreaSW> *)aml.first()); //faster to remove here
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
//print_line("island count: "+itos(island_count)+" active count: "+itos(active_count));
/* SETUP CONSTRAINT ISLANDS */
{
ConstraintSW *ci = constraint_island_list;
while (ci) {
_setup_island(ci, p_delta);
ci = ci->get_island_list_next();
}
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* SOLVE CONSTRAINT ISLANDS */
{
ConstraintSW *ci = constraint_island_list;
while (ci) {
//iterating each island separatedly improves cache efficiency
_solve_island(ci, p_iterations, p_delta);
ci = ci->get_island_list_next();
}
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
/* INTEGRATE VELOCITIES */
b = body_list->first();
while (b) {
const SelfList<BodySW> *n = b->next();
b->self()->integrate_velocities(p_delta);
b = n;
}
/* SLEEP / WAKE UP ISLANDS */
{
BodySW *bi = island_list;
while (bi) {
_check_suspend(bi, p_delta);
bi = bi->get_island_list_next();
}
}
{ //profile
profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(SpaceSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
profile_begtime = profile_endtime;
}
p_space->update();
p_space->unlock();
_step++;
}
