Beispiel #1
0
bool do_GetMeshSkeleton(meshproc_msgs::GetMeshSkeleton::Request &req,
                        meshproc_msgs::GetMeshSkeleton::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.vertices.clear();
    res.edge_ends_L.clear();
    res.edge_ends_R.clear();
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    std::vector<double> x, y, z;
    x.clear();
    y.clear();
    z.clear();
    itA->second->getMeshSkeleton(req.approximate, req.duplicate_distance, x, y, z, res.edge_ends_L, res.edge_ends_R);
    int maxK = x.size();
    res.vertices.reserve(maxK);
    for(int k = 0; k < maxK; k++)
    {
        geometry_msgs::Point aux;
        aux.x = x[k];
        aux.y = y[k];
        aux.z = z[k];
        res.vertices.push_back(aux);
    }
    return true;
}
void UVWChannel::SetWithMesh(Mesh *mesh,int channel)
{
	MeshMap *map  = &mesh->Map(channel);
	if(map->getNumVerts()>0 &&map->getNumFaces()>0)
	{
		int i;
		mNumVerts = map->getNumVerts();
		mVerts = new UVVert[mNumVerts];
		for(i=0;i<mNumVerts;++i)
		{
			mVerts[i] = map->tv[i];
		}

		mNumFaces = map->getNumFaces();
		mFaces = new UVWChannel::Face[mNumFaces];
		TVFace * faces = map->tf;
		for(i=0;i<mNumFaces;++i)
		{
			mFaces[i].AllocateVerts(3);
			for(int j=0;j<mFaces[i].mNumVerts;++j)
			{
				mFaces[i].mTVVerts[j] = faces[i].getTVert(j);
			}
		}
	}
}
Beispiel #3
0
bool do_GetNearMeshVertices(meshproc_msgs::GetNearMeshVertices::Request &req,
                            meshproc_msgs::GetNearMeshVertices::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.neighbors.clear();
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    std::vector<MeshEntry::XYZTriplet> points;
    points.clear();
    itA->second->getNearVertices(req.point.x, req.point.y, req.point.z, req.distance, points);
    int maxK = points.size();
    for(int k = 0; k < maxK; k++)
    {
        geometry_msgs::Point aux;
        aux.x = points[k].x;
        aux.y = points[k].y;
        aux.z = points[k].z;
        res.neighbors.push_back(aux);
    }
    return true;
}
Beispiel #4
0
int gr_mesh_cmd(lua_State* L)
{
    GRLUA_DEBUG_CALL;

    gr_node_ud* data = (gr_node_ud*)lua_newuserdata(L, sizeof(gr_node_ud));
    data->node = 0;

    const char* name = luaL_checkstring(L, 1);
    const char* obj_fname = luaL_checkstring(L, 2);

	std::string sfname( obj_fname );

	// Use a dictionary structure to make sure every mesh is loaded
	// at most once.
	auto i = mesh_map.find( sfname );
	Mesh *mesh = nullptr;

	if( i == mesh_map.end() ) {
		mesh = new Mesh( obj_fname );
	} else {
		mesh = i->second;
	}

	data->node = new GeometryNode( name, mesh );

    luaL_getmetatable(L, "gr.node");
    lua_setmetatable(L, -2);

    return 1;
}
Beispiel #5
0
const Mesh & GlUtils::getMesh(Resource res) {
  typedef std::map<Resource, MeshPtr> MeshMap;
  static MeshMap meshes;
  if (0 == meshes.count(res)) {
    std::string meshData = Platform::getResourceData(res);
    meshes[res] = MeshPtr(new Mesh(meshData));
  }
  return *meshes[res];
}
Beispiel #6
0
bool do_GetLoadedMeshNames(meshproc_msgs::GetLoadedMeshNames::Request &req,
                           meshproc_msgs::GetLoadedMeshNames::Response &res)
{
    res.mesh_names.clear();
    MeshMap::iterator it = loadedMeshes.begin();
    for(it = loadedMeshes.begin(); it != loadedMeshes.end(); it++)
    {
        res.mesh_names.push_back(it->first);
    }
    return true;
}
Beispiel #7
0
bool do_UnloadMesh(meshproc_msgs::UnloadMesh::Request &req, meshproc_msgs::UnloadMesh::Response &res)
{
    res.unloaded_mesh = 0;
    MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
    if(loadedMeshes.end() != it)
    {
        delete it->second;
        res.unloaded_mesh = 1;
        loadedMeshes.erase(it);
    }
    return true;
}
Beispiel #8
0
MeshEntry* checkMeshAvailability(std::string const& meshName,
                                 meshproc_msgs::CSGRequest::Response::_mesh_A_loaded_type &isLoaded,
                                 meshproc_msgs::CSGRequest::Response::_mesh_A_csg_safe_type &isCSGSafe)
{
    isLoaded = isCSGSafe = false;
    MeshMap::iterator it = loadedMeshes.find(meshName);
    if(it != loadedMeshes.end())
    {
        isLoaded = true;
        isCSGSafe = it->second->isCSGSafe();
        return it->second;
    }
    return NULL;
}
void init()
{
	//	Assimp::Importer importer;
	const aiScene* scene = aiImportFile("/home/arprice/catkin_workspace/src/ap_robot_utils/test/resources/cube.stla",
										aiProcess_ValidateDataStructure |
										aiProcess_JoinIdenticalVertices |
										aiProcess_ImproveCacheLocality |
										aiProcess_Triangulate |
										aiProcess_OptimizeGraph |
										aiProcess_OptimizeMeshes |
										aiProcess_FindDegenerates |
										aiProcess_SortByPType);
//	std::cerr << importer.GetErrorString() << std::endl;
	if (scene == NULL) { return; }

	scenes.insert(MeshMap::value_type("cube", meshAItoAP(scene)));
	transformedScenes.insert(MeshMap::value_type("cube", meshAItoAP(scene)));
}
Beispiel #10
0
bool do_ProjectMesh(meshproc_msgs::ProjectMesh::Request &req,
                    meshproc_msgs::ProjectMesh::Response &res)
{
    bool already_had_R = true;
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.operation_performed = false;
    res.file_written = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    if(itR == loadedMeshes.end())
    {
        already_had_R = false;
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    MeshEntry *A, *R;
    A = itA->second;
    R = itR->second;
    res.operation_performed = R->setFromProjection(*A, req.normal.x, req.normal.y, req.normal.z, req.fill_holes);
    if(!res.operation_performed)
    {
        if(!already_had_R)
        {
            delete R;
            loadedMeshes.erase(req.mesh_R);
        }
        return true;
    }
    if(req.return_result_as_mesh)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.return_result_as_polygon)
    {
        std::vector<double> x, y, z;
        R->getBoundaryPolygon(x, y, z, res.edge_L, res.edge_R);
        int maxK = x.size();
        for(int k = 0; k < maxK; k++)
        {
            geometry_msgs::Point aux;
            aux.x = x[k];
            aux.y = y[k];
            aux.z = z[k];
            res.points.push_back(aux);
        }
    }
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
Beispiel #11
0
bool do_GetMeshAABB(meshproc_msgs::GetMeshAABB::Request &req,
                    meshproc_msgs::GetMeshAABB::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    double maxX, minX, maxY, minY, maxZ, minZ;
    itA->second->getBoundingBox(maxX, minX, maxY, minY, maxZ, minZ);
    res.max_x = maxX;
    res.min_x = minX;
    res.max_y = maxY;
    res.min_y = minY;
    res.max_z = maxZ;
    res.min_z = minZ;
    return true;
}
Beispiel #12
0
bool do_PathOnMesh(meshproc_msgs::PathOnMesh::Request &req,
                   meshproc_msgs::PathOnMesh::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.operation_performed = false;

    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }

    MeshEntry *A;
    A = itA->second;

    res.operation_performed = A->getGeodesicPath(req.lina_start, req.lina_end, req.linb_start, req.linb_end,
                              res.path);

    return true;
}
Beispiel #13
0
bool do_GetMesh(meshproc_msgs::GetMesh::Request &req,
                meshproc_msgs::GetMesh::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.file_written = false;
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    if(req.return_result)
        itA->second->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = itA->second->writeToFile(req.result_filename);
    }
    return true;
}
Beispiel #14
0
bool do_ConvexHull(meshproc_msgs::ConvexHull::Request &req,
                   meshproc_msgs::ConvexHull::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.file_written = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    if(itR == loadedMeshes.end())
    {
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    MeshEntry *A, *R;
    A = itA->second;
    R = itR->second;
    R->setFromConvexHull(*A);
    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
Beispiel #15
0
bool do_LoadMesh(meshproc_msgs::LoadMesh::Request &req, meshproc_msgs::LoadMesh::Response &res)
{
    ROS_INFO("Loading mesh %s", req.mesh_name.c_str());
    res.loaded_mesh = res.io_error = res.mesh_already_loaded = false;
    MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
    if(it != loadedMeshes.end())
        res.mesh_already_loaded = true;
    else
    {
        if((0 == req.mesh_filenames.size()) && (0 == req.mesh_msgs.size()))
            return true;
        it = loadedMeshes.insert(loadedMeshes.begin(),
                                 std::pair<std::string, MeshEntry*>(req.mesh_name, new MeshEntry()));
        MeshEntry *R = it->second;
        int maxK = req.mesh_filenames.size();
        bool goOn = true;
        for(int k = 0; (k < maxK) && goOn; k++)
        {
            ROS_INFO("    loading part from file %s", req.mesh_filenames[k].c_str());
            goOn = R->loadFromFile(req.mesh_filenames[k], req.duplicate_dist);
        }
        if(!goOn)
        {
            ROS_INFO("    Encountered I/O error (file might not exist or is inaccessible), cancelling load.");
            res.io_error = true;
            delete R;
            loadedMeshes.erase(it);
            return true;
        }
        res.loaded_mesh = true;
        maxK = req.mesh_msgs.size();
        for(int k = 0; k < maxK; k++)
        {
            ROS_INFO("    loading part from message");
            R->loadFromMsg(req.mesh_msgs[k], req.duplicate_dist);
        }
    }
    ROS_INFO("    Loading done.");
    return true;
}
Beispiel #16
0
bool do_SolidifyMesh(meshproc_msgs::SolidifyMesh::Request &req,
                     meshproc_msgs::SolidifyMesh::Response &res)
{
    bool already_had_R = true;
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.file_written = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    if(itR == loadedMeshes.end())
    {
        already_had_R = false;
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    MeshEntry *A, *R;
    A = itA->second;
    R = itR->second;
    R->setFromSolidification(*A, req.thickness);
    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
Beispiel #17
0
bool do_AffineTransformMesh(meshproc_msgs::AffineTransformMesh::Request &req,
                            meshproc_msgs::AffineTransformMesh::Response &res)
{
    MeshEntry *A, *R;
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    res.mesh_A_loaded = true;
    res.file_written = false;
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }
    if(itR == loadedMeshes.end())
    {
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    A = itA->second;
    R = itR->second;
    R->setFromMeshEntry(*A);
    Eigen::Affine3d M = Eigen::Translation3d(req.transform.translation.x,
                        req.transform.translation.y,
                        req.transform.translation.z)*
                        Eigen::Quaterniond(req.transform.rotation.w,
                                           req.transform.rotation.x,
                                           req.transform.rotation.y,
                                           req.transform.rotation.z);
    R->applyTransform(M);
    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
Beispiel #18
0
bool do_GetMeshProps(meshproc_msgs::GetMeshProps::Request &req, meshproc_msgs::GetMeshProps::Response &res)
{
    MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
    if(it != loadedMeshes.end())
    {
        res.is_loaded = true;
        res.is_closed = it->second->isClosed();
        res.is_manifold = it->second->isManifold();
        res.is_orientable = it->second->isOrientable();
        res.is_safe_for_csg = it->second->isCSGSafe();
        res.num_connected_components = it->second->getNrConnectedComponents();
        res.num_vertices = it->second->getNrVertices();
        res.num_edges = it->second->getNrEdges();
        res.num_faces = it->second->getNrFaces();
        res.Euler_characteristic = it->second->getEulerCharacteristic();
        res.volume = it->second->getVolume();
    }
    else
    {
        res.is_loaded = false;
    }
    return true;
}
Beispiel #19
0
bool do_SelectByNormal(meshproc_msgs::SelectByNormal::Request &req,
                       meshproc_msgs::SelectByNormal::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.file_written = false;
    res.operation_performed = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshEntry *A;
    A = itA->second;

    res.operation_performed = A->filterByNormal(req.result_filename,
                              req.normal.x, req.normal.y, req.normal.z,
                              req.tolerated_angle);

    res.file_written = res.operation_performed;
    return true;
}
Beispiel #20
0
	void end() { 
		xform_map.clear();
		xform_schema_map.clear();
		mesh_map.clear();
		schema_map.clear();
		camera_map.clear();
		camera_schema_map.clear();
		face_to_vertex_index_map.clear();
		trivi_to_vertex_index_map.clear();
		quadvi_count_map.clear();
		surface_size_map.clear();
		temporary_uv_list.clear();
		temporary_normal_list.clear();
		temporary_vertex_list.clear();
		{
			delete archive; archive = NULL;
		}
	}
Beispiel #21
0
bool do_ConvexDecomposition(meshproc_msgs::ConvexDecomposition::Request &req,
                            meshproc_msgs::ConvexDecomposition::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_loaded = true;
    res.nr_components = 0;
    res.mesh_names.clear();
    res.file_written.clear();
    res.components.clear();
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    std::vector<MeshEntry*> components;
    components.clear();
    std::cerr << "Getting components" << std::endl;
    itA->second->getConvexComponents(components);
    std::cerr << "Components obtained" << std::endl;
    int maxK = res.nr_components = components.size();
    for(int k = 0; k < maxK; k++)
    {
        char nr_str[20];
        std::sprintf(nr_str, "%d", k);
        std::string cName = req.mesh_R + nr_str;
        MeshMap::iterator itR = loadedMeshes.find(cName);
        if(itR == loadedMeshes.end())
        {
            itR = loadedMeshes.insert(loadedMeshes.begin(),
                                      std::pair<std::string, MeshEntry*>(cName, components[k]));
        }
        else
        {
            delete itR->second;
            itR->second = components[k];
        }
        MeshEntry* R = itR->second;
        res.mesh_names.push_back(cName);
        if(req.return_result)
        {
            res.components.push_back(shape_msgs::Mesh());
            R->writeToMsg(res.components[k]);
        }
        if(req.result_to_file)
        {
            res.file_written.push_back(R->writeToFile(req.result_filename + nr_str + req.result_filename_extension));
            res.file_names.push_back(req.result_filename + nr_str + req.result_filename_extension);
        }

    }
    return true;
}
cv::Mat projectWithEigen()
{
	// Transform meshes into camera frame

	// For each frame in vector
	for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
	{
		// Lookup current transform
		Eigen::Isometry3 transform;
		transform = transforms.at(mMeshFrameIDs[frame]);

		// Get copy of mesh for each frame
		ap::Mesh* sourceMesh;
		ap::Mesh* transformedMesh;
		//std::cerr << "Getting frame " << frame << " : " << mMeshFrameIDs[frame] << std::endl;
		MeshMap::iterator scene_i = scenes.find(mMeshFrameIDs[frame]);
		if (scenes.end() == scene_i) { continue; }
		sourceMesh = scene_i->second;

		MeshMap::iterator scene_t = transformedScenes.find(mMeshFrameIDs[frame]);
		if (transformedScenes.end() == scene_t) { continue; }
		transformedMesh = scene_t->second;

		// Transform mesh into camera frame
		for (int i = 0; i < sourceMesh->vertices.size(); i++)
		{
			Eigen::Vector3 newVertex = transform * sourceMesh->vertices[i];
			//std::cerr << mesh->vertices[i].transpose() << "\t->\t" << newVertex.transpose() << std::endl;
			transformedMesh->vertices[i] = newVertex;
		}
	}

	// For each pixel in camera image
	cv::Mat robotImage(mCameraModel.cameraInfo().height, mCameraModel.cameraInfo().width, CV_32F);
	float* pixelPtr = (float*)robotImage.data;
	float maxDepth = 0;
	for (int v = 0; v < robotImage.rows; v++)
	{
		for (int u = 0; u < robotImage.cols; u++)
		{
			// Create a ray through the pixel
			int pixelIdx = u + (v * robotImage.cols);
			//std::cerr << "Pixel (" << u << "," << v << ")" << std::endl;
			cv::Point2d pixel = cv::Point2d(u, v);
			cv::Point3d cvRay = mCameraModel.projectPixelTo3dRay(pixel);
			// Convert cvRay to ap::Ray
			ap::Ray ray;
			ray.point = Eigen::Vector3::Zero();
			ray.vector.x() = cvRay.x; ray.vector.y() = cvRay.y; ray.vector.z() = cvRay.z;
			ray.vector.normalize();
			//std::cerr << ray.vector.transpose() << std::endl;

			// For each frame in vector
			for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
			{
				MeshMap::iterator scene_i = transformedScenes.find(mMeshFrameIDs[frame]);
				if (transformedScenes.end() == scene_i)
				{
					continue;
				}
				ap::Mesh* mesh = scene_i->second;

				// For each triangle in mesh
				for (int i = 0; i < mesh->faces.size(); i++)
				{
					// Check for intersection. If finite, set distance

					ap::Triangle triangle(mesh->vertices[mesh->faces[i].vertices[0]],
										  mesh->vertices[mesh->faces[i].vertices[1]],
										  mesh->vertices[mesh->faces[i].vertices[2]]);

					Eigen::Vector3 intersection = ap::intersectRayTriangle(ray, triangle);
					if (std::isfinite(intersection.x()))
					{
						float d = intersection.norm();
						float val = pixelPtr[pixelIdx];
						if (val == 0 || val > d)
						{
							pixelPtr[pixelIdx] = d;
						}
						if (d > maxDepth)
						{
							maxDepth = d;
						}
					}
				}
			}
		}
	}

	// Return the matrix
	if (maxDepth == 0) { maxDepth = 1;}
	return robotImage/maxDepth;
}
Beispiel #23
0
bool load_scene( Scene* scene, const char* filename )
{
    TiXmlDocument doc( filename );
    const TiXmlElement* root = 0;
    const TiXmlElement* elem = 0;
    MaterialMap materials;
    MeshMap meshes;
    TriVertMap triverts;

    assert( scene );

    // load the document

    if ( !doc.LoadFile() ) {
        std::cout << "ERROR, " << doc.ErrorRow() << ":" << doc.ErrorCol() << "; "
            << "parse error: " << doc.ErrorDesc() << "\n";
        return false;
    }

    // check for root element

    root = doc.RootElement();
    if ( !root ) {
        std::cout << "No root element.\n";
        return false;
    }

    // reset the scene

    scene->reset();

    try {
        // parse the camera
        elem = get_unique_child( root, true, STR_CAMERA );
        parse_camera( elem, &scene->camera );
        // parse background color
        parse_elem( root, true,  STR_BACKGROUND, &scene->background_color );
        // parse refractive index
        parse_elem( root, true,  STR_REFRACT, &scene->refractive_index );
        // parse ambient light
        parse_elem( root, false, STR_AMLIGHT, &scene->ambient_light );

        // parse the lights
        elem = root->FirstChildElement( STR_PLIGHT );
        while ( elem ) {
            PointLight pl;
            parse_point_light( elem, &pl );
            scene->add_light( pl );
            elem = elem->NextSiblingElement( STR_PLIGHT );
        }

        // parse the materials
        elem = root->FirstChildElement( STR_MATERIAL );
        while ( elem ) {
            Material* mat = new Material();
            check_mem( mat );
            scene->add_material( mat );
            const char* name = parse_material( elem, mat );
            assert( name );
            // place each material in map by it's name, so we can associate geometries
            // with them when loading geometries
            // check for repeat name
            if ( !materials.insert( std::make_pair( name, mat ) ).second ) {
                print_error_header( elem );
                std::cout << "Material '" << name << "' multiply defined.\n";
                throw std::exception();
            }
            elem = elem->NextSiblingElement( STR_MATERIAL );
        }

        // parse the meshes
        elem = root->FirstChildElement( STR_MESH );
        while ( elem ) {
            Mesh* mesh = new Mesh();
            check_mem( mesh );
            scene->add_mesh( mesh );
            const char* name = parse_mesh( elem, mesh );
            assert( name );
            // place each mesh in map by it's name, so we can associate geometries
            // with them when loading geometries
            if ( !meshes.insert( std::make_pair( name, mesh ) ).second ) {
                print_error_header( elem );
                std::cout << "Mesh '" << name << "' multiply defined.\n";
                throw std::exception();
            }
            elem = elem->NextSiblingElement( STR_MESH );
        }

        // parse vertices (used by triangles)
        elem = root->FirstChildElement( STR_VERTEX );
        while ( elem ) {
            Triangle::Vertex v;
            const char* name = parse_triangle_vertex( materials, elem, &v );
            assert( name );
            // place each vertex in map by it's name, so we can associate triangles
            // with them when loading geometries
            if ( !triverts.insert( std::make_pair( name, v ) ).second ) {
                print_error_header( elem );
                std::cout << "Triangle vertex '" << name << "' multiply defined.\n";
                throw std::exception();
            }
            elem = elem->NextSiblingElement( STR_VERTEX );
        }

        // parse the geometries

        // spheres
        elem = root->FirstChildElement( STR_SPHERE );
        while ( elem ) {
            Sphere* geom = new Sphere();
            check_mem( geom );
            scene->add_geometry( geom );
            parse_geom_sphere( materials, elem, geom );
            elem = elem->NextSiblingElement( STR_SPHERE );
        }

        // triangles
        elem = root->FirstChildElement( STR_TRIANGLE );
        while ( elem ) {
            Triangle* geom = new Triangle();
            check_mem( geom );
            scene->add_geometry( geom );
            parse_geom_triangle( materials, triverts, elem, geom );
            elem = elem->NextSiblingElement( STR_TRIANGLE );
        }

        // models
        elem = root->FirstChildElement( STR_MODEL );
        while ( elem ) {
            Model* geom = new Model();
            check_mem( geom );
            scene->add_geometry( geom );
            parse_geom_model( materials, meshes, elem, geom );
            elem = elem->NextSiblingElement( STR_MODEL );
        }

        // TODO add you own geometries here

    } catch ( std::bad_alloc const& ) {
        std::cout << "Out of memory error while loading scene\n.";
        scene->reset();
        return false;
    } catch ( ... ) {
        scene->reset();
        return false;
    }

    return true;

}
Beispiel #24
0
bool do_CSGRequest(meshproc_msgs::CSGRequest::Request &req, meshproc_msgs::CSGRequest::Response &res)
{
    res.operation_performed = false;
    res.mesh_A_csg_safe = res.mesh_A_loaded = res.mesh_B_csg_safe = res.mesh_B_loaded = false;
    res.result = shape_msgs::Mesh();
    bool already_had_R = true;
    MeshEntry *A, *B, *R;
    A = checkMeshAvailability(req.mesh_A, res.mesh_A_loaded, res.mesh_A_csg_safe);
    B = checkMeshAvailability(req.mesh_B, res.mesh_B_loaded, res.mesh_B_csg_safe);
    MeshMap::iterator it = loadedMeshes.find(req.mesh_R);
    if(it == loadedMeshes.end())
    {
        already_had_R = false;
        it = loadedMeshes.insert(loadedMeshes.begin(),
                                 std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    R = it->second;
    if(!canPerform(res.mesh_A_loaded, res.mesh_B_loaded, res.mesh_A_csg_safe, res.mesh_B_csg_safe, req.operation))
    {
        //Operation can't be performed, so let's not leave a result mesh (if created just now) loaded as a side-effect.
        if(!already_had_R)
        {
            delete R;
            loadedMeshes.erase(req.mesh_R);
        }
        return true;
    }
    switch(req.operation)
    {
    case 0:
        res.operation_performed = R->setFromUnion(*A, *B);
        break;
    case 1:
        res.operation_performed = R->setFromIntersection(*A, *B);
        break;
    case 2:
        res.operation_performed = R->setFromDifference(*A, *B);
        break;
    case 3:
        res.operation_performed = R->setFromSymmetricDifference(*A, *B);
        break;
    case 4:
        res.operation_performed = R->setFromMinkowskiSum(*A, *B);
        break;
    case 5:
        res.operation_performed = R->setFromMinkowskiErosion(*A, *B);
        break;
    }

    if(!res.operation_performed)
    {
        //Operation failed, so let's not leave a result mesh (if created just now) loaded as a side-effect.
        if(!already_had_R)
        {
            delete R;
            loadedMeshes.erase(req.mesh_R);
        }
        return true;
    }

    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
IGeometryChecker::ReturnVal MissingUVCoordinatesChecker::GeometryCheck(TimeValue t,INode *nodeToCheck, IGeometryChecker::OutputVal &val)
{
	val.mIndex.ZeroCount();
	if(IsSupported(nodeToCheck))
	{
		LARGE_INTEGER	ups,startTime;
		QueryPerformanceFrequency(&ups);
		QueryPerformanceCounter(&startTime); //used to see if we need to pop up a dialog 
		bool compute = true;
		bool checkTime = GetIGeometryCheckerManager()->GetAutoUpdate();//only check time for the dialog if auto update is active!
		UVWChannel uvmesh;
		ObjectState os  = nodeToCheck->EvalWorldState(t);
		Object *obj = os.obj;
		if(os.obj->IsSubClassOf(triObjectClassID))
		{
			TriObject *tri = dynamic_cast<TriObject *>(os.obj);
			if(tri)
			{
				BitArray arrayOfVertices;
				arrayOfVertices.SetSize(tri->mesh.numVerts);
				arrayOfVertices.ClearAll();
				IGeometryChecker::ReturnVal returnval=IGeometryChecker::eFail;
				int numChannels = tri->mesh.getNumMaps();
				int index;
				for(int i=0;i<numChannels;++i)
				{
					if(tri->mesh.mapSupport(i))
					{

						MeshMap *map  = &tri->mesh.Map(i);
						if(map->getNumVerts()>0 &&map->getNumFaces()>0)
						{
							returnval= TypeReturned();
							int numFaces = map->getNumFaces();
							TVFace * tvFaces = map->tf;
							UVVert *  uvVerts= map->tv;
						
							#pragma omp parallel for
							for(int faceIndex =0;faceIndex<numFaces;++faceIndex)
							{
								if(compute)
								{
									TVFace& tvFace = tvFaces[faceIndex];
									Point3 tv = uvVerts[tvFace.t[0]];
									if(_isnan(tv.x) || !_finite(tv.x)||_isnan(tv.y) || !_finite(tv.y)||_isnan(tv.z) || !_finite(tv.z))
									{
										index = tri->mesh.faces[faceIndex].v[0];
										if(index>=0&&index<tri->mesh.numVerts)
										{
											#pragma omp critical
											{
												arrayOfVertices.Set(index);
											}
										}
									}
									tv = uvVerts[tvFace.t[1]];
									if(_isnan(tv.x) || !_finite(tv.x)||_isnan(tv.y) || !_finite(tv.y)||_isnan(tv.z) || !_finite(tv.z))
									{
										index = tri->mesh.faces[faceIndex].v[1];
										if(index>=0&&index<tri->mesh.numVerts)
										{
											#pragma omp critical
											{
												arrayOfVertices.Set(index);
											}
										}
									}
									tv = uvVerts[tvFace.t[2]];
									if(_isnan(tv.x) || !_finite(tv.x)||_isnan(tv.y) || !_finite(tv.y)||_isnan(tv.z) || !_finite(tv.z))
									{
										index = tri->mesh.faces[faceIndex].v[2];
										if(index>=0&&index<tri->mesh.numVerts)
										{
											#pragma omp critical
											{
												arrayOfVertices.Set(index);
											}
										}
									}
									if(checkTime==true)
									{
										#pragma omp critical
										{
											DialogChecker::Check(checkTime,compute,numFaces,startTime,ups);
										}
									}
								}
							}
						}
					}
				}
				if(arrayOfVertices.IsEmpty()==false) //we have overlapping faces
				{
					int localsize= arrayOfVertices.GetSize();
					for(int i=0;i<localsize;++i)
					{
						if(arrayOfVertices[i])
							val.mIndex.Append(1,&i);
					}
				}
				return returnval;
			}
			else return IGeometryChecker::eFail;
		}
		else if(os.obj->IsSubClassOf(polyObjectClassID))
		{
			PolyObject *poly = dynamic_cast<PolyObject *>(os.obj);
			if(poly)
			{
				BitArray arrayOfVertices;
				arrayOfVertices.SetSize(poly->GetMesh().numv);
				arrayOfVertices.ClearAll();
				IGeometryChecker::ReturnVal returnval=IGeometryChecker::eFail;
				int numChannels= poly->GetMesh().MNum();
				int index;
				for(int i=0;i<numChannels;++i)
				{
					if(poly->GetMesh().M(i))
					{
						MNMesh *mesh = &(poly->GetMesh());
						MNMap *mnmap = mesh->M(i);
						if(mnmap&&mnmap->numv>0&&mnmap->numf>0)
						{
							returnval= TypeReturned();
							int numFaces = mnmap->numf;
							#pragma omp parallel for
							for(int faceIndex =0;faceIndex<numFaces;++faceIndex)
							{
								if(compute)
								{
									Point3 tv;
									int a,b,c;
									MNMapFace *face = mnmap->F(faceIndex);
									UVVert *  uvVerts=  mnmap->v;
									for(int j=0;j<(face->deg);++j)
									{
										if(j==(face->deg-2))
										{
											a  = j; b = j+1; c = 0;
										}
										else if(j==(face->deg-1))
										{
											a  = j; b = 0; c = 1;
										}
										else
										{
											a  = j; b = j+1; c = j+2;
										}
										tv = uvVerts[face->tv[a]];
										if(_isnan(tv.x) || !_finite(tv.x)||_isnan(tv.y) || !_finite(tv.y)||_isnan(tv.z) || !_finite(tv.z))
										{
											index = mesh->f->vtx[a];
											if(index>=0&&index<mesh->numv)
											{
												#pragma omp critical
												{
													arrayOfVertices.Set(index);
												}
											}
										}
										tv = uvVerts[face->tv[b]];
										if(_isnan(tv.x) || !_finite(tv.x)||_isnan(tv.y) || !_finite(tv.y)||_isnan(tv.z) || !_finite(tv.z))
										{
											index = mesh->f->vtx[b];
											if(index>=0&&index<mesh->numv)
											{
												#pragma omp critical
												{
													arrayOfVertices.Set(index);
												}
											}
										}
										tv = uvVerts[face->tv[c]];
										if(_isnan(tv.x) || !_finite(tv.x)||_isnan(tv.y) || !_finite(tv.y)||_isnan(tv.z) || !_finite(tv.z))
										{
											index = mesh->f->vtx[c];
											if(index>=0&&index<mesh->numv)
											{
												#pragma omp critical
												{
													arrayOfVertices.Set(index);
												}
											}
										}
										if(checkTime==true)
										{
											#pragma omp critical
											{
												DialogChecker::Check(checkTime,compute,numFaces,startTime,ups);
											}
										}
									}
								}
							}	
						}
					}
				}
				if(arrayOfVertices.IsEmpty()==false) //we have overlapping faces
				{
					int localsize= arrayOfVertices.GetSize();
					for(int i=0;i<localsize;++i)
					{
						if(arrayOfVertices[i])
							val.mIndex.Append(1,&i);
					}
				}
				return returnval;
			}
			else return IGeometryChecker::eFail;
		}
	}
	return IGeometryChecker::eFail;
}