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);
}
}
}
}
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;
}
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;
}
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];
}
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;
}
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;
}
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)));
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
}
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;
}
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;
}
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;
}