void World::bullet_step()

in roboschool/cpp-household/physics-bullet.cpp [358:412]

• 46 lines of code
• 10 McCabe index (conditional complexity)

void World::bullet_step(int skip_frames)
{
	QElapsedTimer elapsed;
	elapsed.start();

	float need_timestep = settings_timestep*skip_frames;
	if (
		settings_timestep_sent != need_timestep ||
		settings_skip_frames_sent != skip_frames
	) {
		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(client);
		b3PhysicsParamSetGravity(command, 0, 0, -settings_gravity);
		b3PhysicsParamSetNumSolverIterations(command, 5);
		b3PhysicsParamSetDefaultContactERP(command, 0.9);
		b3PhysicsParamSetTimeStep(command, need_timestep);
		settings_timestep_sent = need_timestep;
		b3PhysicsParamSetNumSubSteps(command, skip_frames);
		settings_skip_frames_sent = skip_frames;
		b3SubmitClientCommandAndWaitStatus(client, command);
	}

	for (const boost::weak_ptr<Robot>& wr: robotlist) {
		boost::shared_ptr<Robot> robot = wr.lock();
		if (!robot) continue;
		b3SharedMemoryCommandHandle cmd = 0;
		for (const shared_ptr<Joint>& j: robot->joints) {
			if (!j) continue;
			if (j->torque_need_repeat) {
				if (!cmd) cmd = b3JointControlCommandInit2(client, robot->bullet_handle, CONTROL_MODE_TORQUE);
				b3JointControlSetDesiredForceTorque(cmd, j->bullet_uindex, j->torque_repeat_val);
			}
		}
		if (cmd) b3SubmitClientCommandAndWaitStatus(client, cmd);
	}

	double ms_post_joints = elapsed.nsecsElapsed() / 1000000.0;
	elapsed.start();

	b3SharedMemoryCommandHandle cmd = b3InitStepSimulationCommand(client);
	b3SubmitClientCommandAndWaitStatus(client, cmd);

	ts += settings_timestep*skip_frames;

	double ms_step = elapsed.nsecsElapsed() / 1000000.0;
	elapsed.start();
	query_positions();
	double ms_query = elapsed.nsecsElapsed() / 1000000.0;

	static double mean;
	mean *= 0.95;
	mean += 0.05*ms_step;
	//fprintf(stderr, "j=%0.2lf, step=%0.2lf (mean=%0.2f), query=%0.2lf\n", ms_post_joints, ms_step, mean, ms_query);

	performance_bullet_ms = ms_post_joints + ms_step + ms_query;
}