void World::query_body_position()

in roboschool/cpp-household/physics-bullet.cpp [440:495]

• 50 lines of code
• 8 McCabe index (conditional complexity)

void World::query_body_position(const shared_ptr<Robot>& robot)
{
	if (!robot->root_part) return;

	b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(client, robot->bullet_handle);
	b3RequestActualStateCommandComputeLinkVelocity(cmd_handle, ACTUAL_STATE_COMPUTE_LINKVELOCITY);
	b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(client, cmd_handle);

	const double* root_inertial_frame;
	const double* q;
	const double* q_dot;
	b3GetStatusActualState(
		status_handle, 0 /* body_unique_id */,
		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
		&root_inertial_frame /*root_local_inertial_frame*/, &q,
		&q_dot, 0 /* joint_reaction_forces */);
	//printf("%f ", jointReactionForces[i]);
	assert(robot->root_part->bullet_link_n==-1);
	robot->root_part->bullet_position = transform_from_doubles(q, q+3);
	robot->root_part->bullet_speed[0] = q_dot[0];
	robot->root_part->bullet_speed[1] = q_dot[1];
	robot->root_part->bullet_speed[2] = q_dot[2];
	robot->root_part->bullet_angular_speed[0] = q_dot[3];
	robot->root_part->bullet_angular_speed[1] = q_dot[4];
	robot->root_part->bullet_angular_speed[2] = q_dot[5];
	robot->root_part->bullet_local_inertial_frame = transform_from_doubles(root_inertial_frame, root_inertial_frame+3);
	robot->root_part->bullet_link_position = transform_from_doubles(q, q+3);
	robot->root_part->bullet_queried_at_least_once = true;

	int status_type = b3GetStatusType(status_handle);
	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
		return;

	for (const shared_ptr<Thingy>& part: robot->robot_parts) {
		struct b3LinkState linkstate;
		if (!part) continue;
		if (part->bullet_link_n==-1) continue;
		b3GetLinkState(client, status_handle, part->bullet_link_n, &linkstate);
		part->bullet_position = transform_from_doubles(linkstate.m_worldPosition, linkstate.m_worldOrientation);
		part->bullet_local_inertial_frame = transform_from_doubles(linkstate.m_localInertialPosition, linkstate.m_localInertialOrientation);
		part->bullet_link_position = transform_from_doubles(linkstate.m_worldLinkFramePosition, linkstate.m_worldLinkFrameOrientation);
		part->bullet_speed[0] = linkstate.m_worldLinearVelocity[0];
		part->bullet_speed[1] = linkstate.m_worldLinearVelocity[1];
		part->bullet_speed[2] = linkstate.m_worldLinearVelocity[2];
		part->bullet_angular_speed[0] = linkstate.m_worldAngularVelocity[0];
		part->bullet_angular_speed[1] = linkstate.m_worldAngularVelocity[1];
		part->bullet_angular_speed[2] = linkstate.m_worldAngularVelocity[2];
		part->bullet_queried_at_least_once = true;
	}

	for (const shared_ptr<Joint>& j: robot->joints) {
		if (!j) continue;
		j->joint_current_position = q[j->bullet_qindex];
		j->joint_current_speed = q_dot[j->bullet_uindex];
	}
}