void World::load_robot_shapes()

in roboschool/cpp-household/physics-bullet.cpp [272:343]

• 62 lines of code
• 11 McCabe index (conditional complexity)

void World::load_robot_shapes(const shared_ptr<Robot>& robot)
{
	b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(client, robot->bullet_handle);
	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(client, commandHandle);
	int statusType = b3GetStatusType(statusHandle);
	if (statusType != CMD_VISUAL_SHAPE_INFO_COMPLETED) return;

	b3VisualShapeInformation visualShapeInfo;
	b3GetVisualShapeInformation(client, &visualShapeInfo);
	for (int i=0; i<visualShapeInfo.m_numVisualShapes; i++) {
		int link_n = visualShapeInfo.m_visualShapeData[i].m_linkIndex;
		if (link_n < -1 || link_n > (int)robot->robot_parts.size()) {
			int set_breakpoint_here = 5;
			assert(0);
		}
		shared_ptr<Thingy> part;
		if (link_n==-1) {
			part = robot->root_part;
		} else {
			part = robot->robot_parts[link_n];
		}
		std::string fn = visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName;
		int geom = visualShapeInfo.m_visualShapeData[i].m_visualGeometryType;

		uint32_t color =
			(uint32_t(255*visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]) << 16) |
			(uint32_t(255*visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]) << 8) |
			(uint32_t(255*visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]) << 0) |
			(uint32_t(255*visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]) << 24);

		if (!part->klass) {
			char klass_name[1024];
			snprintf(klass_name, sizeof(klass_name), "%s:%03i", robot->original_urdf_name.c_str(), link_n);
			//fprintf(stderr, "allocating class=='%s' link==%i geom=%i fn=='%s'\n", klass_name, link_n, geom, fn.c_str());
			part->klass = klass_cache_find_or_create(klass_name);
		}

		btVector3 pos;
		btQuaternion quat;
		{
			pos[0] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0];
			pos[1] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1];
			pos[2] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2];
			quat[0] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3];
			quat[1] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4];
			quat[2] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5];
			quat[3] = visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6];
		}

		if (!part->klass->frozen) {
			//fprintf(stderr, "adding visual to link==%i geom=%i fn=='%s', already have %i viz shapes\n", link_n, geom, fn.c_str(),
			//	(int)part->klass->shapedet_visual->detail_levels[DETAIL_BEST].size());
			load_shape_into_class(part->klass, geom, fn,
				visualShapeInfo.m_visualShapeData[i].m_dimensions[0],
				visualShapeInfo.m_visualShapeData[i].m_dimensions[1],
				visualShapeInfo.m_visualShapeData[i].m_dimensions[2],
				color,
				btTransform(quat, pos)
				);
		}
		if (cx) cx->need_load_missing_textures = true;

		part->bullet_link_n = link_n;
		thingy_add_to_drawlist(part);
	}

	for (const shared_ptr<Thingy>& part: robot->robot_parts)
		if (part->klass)
			part->klass->frozen = true;
	if (robot->root_part->klass)
		robot->root_part->klass->frozen = true;
}