void
create_object_marker(geometry_msgs::Pose pose, std::pair<std::string,std::string> name, visualization_msgs::Marker &marker)
{
marker.header.stamp = ros::Time();
marker.ns= name.second.c_str();
if (name.second.compare(name.first) != 0){
std::vector<std::string> vst;
boost::split(vst, name.first, boost::is_any_of("_"), boost::token_compress_on);
int id = std::stoi(vst.at(vst.size()-1));
marker.id = id;
}
else{
marker.id = 1;
}
marker.scale.x=1;
marker.scale.y=1;
marker.scale.z=1;
marker.type = visualization_msgs::Marker::MESH_RESOURCE;
std::string mesh_path ("package://asus_scanner_models/" + name.second + "/" + name.second + ".stl");
marker.mesh_resource = mesh_path.c_str();
marker.action = visualization_msgs::Marker::ADD;
marker.pose = pose;
marker.lifetime = ros::Duration(1);
marker.color.r = 1.0f;
marker.color.g = 1.0f;
marker.color.b = 1.0f;
marker.color.a = 1.0f;
}
void
TestInterpolationHDiv<Dim>::testInterpolationOneElt( std::string one_element_mesh )
{
// expr to interpolate
int is3D = 0;
if( Dim == 3 )
is3D = 1;
auto myexpr = unitX() + unitY() + is3D*unitZ() ; //(1,1)
// one element mesh
auto mesh_name = one_element_mesh + ".msh"; //create the mesh and load it
fs::path mesh_path( mesh_name );
mesh_ptrtype oneelement_mesh = loadMesh( _mesh=new mesh_type,
_filename=mesh_name);
// refined mesh (export)
auto refine_level = std::floor(1 - math::log( 0.1 )); //Deduce refine level from meshSize (option)
mesh_ptrtype mesh = loadMesh( _mesh=new mesh_type,
_filename=mesh_name,
_refine=( int )refine_level);
space_ptrtype Xh = space_type::New( oneelement_mesh );
//std::cout << "nb dof = " << Xh->nDof() << std::endl;
std::vector<std::string> faces;
if(Dim == 2)
faces = { "hypo","vert","hor"};
else if (Dim == 3)
faces = {"xzFace","xyFace","xyzFace","yzFace"};
element_type U_h_int = Xh->element();
element_type U_h_on = Xh->element();
// handly computed interpolant coeff (in hdiv basis)
for ( int i = 0; i < Xh->nLocalDof(); ++i )
{
CHECK( mesh->hasMarkers( {faces[i]} ) );
U_h_int(i) = integrate( markedfaces( oneelement_mesh, faces[i] ), trans( N() )*myexpr ).evaluate()(0,0);
}
// raviart-thomas interpolant using on
U_h_on.zero();
U_h_on.on(_range=elements(oneelement_mesh), _expr=myexpr);
auto exporter_proj = exporter( _mesh=mesh, _name=( boost::format( "%1%-%2%" ) % this->about().appName() %mesh_path.stem().string() ).str() );
exporter_proj->step( 0 )->add( "U_interpolation_handly-" + mesh_path.stem().string(), U_h_int );
exporter_proj->step( 0 )->add( "U_interpolation_on-" + mesh_path.stem().string(), U_h_on );
exporter_proj->save();
U_h_int.printMatlab( "U_h_int_" + mesh_path.stem().string() + ".m" );
U_h_on.printMatlab( "U_h_on_" + mesh_path.stem().string() + ".m" );
//L2 norm of error
auto error = vf::project(_space=Xh, _range=elements(oneelement_mesh), _expr=idv(U_h_int) - idv(U_h_on) );
double L2error = error.l2Norm();
std::cout << "L2 error = " << L2error << std::endl;
}
void
TestInterpolationHCurl::testInterpolation( std::string one_element_mesh )
{
// expr to interpolate
auto myexpr = unitX() + unitY(); //(1,1)
// one element mesh
auto mesh_name = one_element_mesh + ".msh"; //create the mesh and load it
fs::path mesh_path( mesh_name );
mesh_ptrtype oneelement_mesh = loadMesh( _mesh=new mesh_type,
_filename=mesh_name);
// refined mesh (export)
auto refine_level = std::floor(1 - math::log( 0.1 )); //Deduce refine level from meshSize (option)
mesh_ptrtype mesh = loadMesh( _mesh=new mesh_type,
_filename=mesh_name,
_refine=( int )refine_level);
space_ptrtype Xh = space_type::New( oneelement_mesh );
std::vector<std::string> faces, edges; //list of edges
edges = {"hypo","vert","hor"};
element_type U_h_int = Xh->element();
element_type U_h_on = Xh->element();
element_type U_h_on_boundary = Xh->element();
// handly computed interpolant coeff (in hcurl basis)
for ( int i = 0; i < Xh->nLocalDof(); ++i )
{
CHECK( oneelement_mesh->hasMarkers( {edges[i]} ) );
U_h_int(i) = integrate( markedfaces( oneelement_mesh, edges[i] ), trans( T() )*myexpr ).evaluate()(0,0);
}
// nedelec interpolant using on
U_h_on.zero();
U_h_on.on(_range=elements(oneelement_mesh), _expr=myexpr);
U_h_on_boundary.on(_range=boundaryfaces(oneelement_mesh), _expr=myexpr);
auto exporter_proj = exporter( _mesh=mesh, _name=( boost::format( "%1%" ) % this->about().appName() ).str() );
exporter_proj->step( 0 )->add( "U_interpolation_handly_"+mesh_path.stem().string(), U_h_int );
exporter_proj->step( 0 )->add( "U_interpolation_on_"+mesh_path.stem().string(), U_h_on );
exporter_proj->save();
// print coefficient only for reference element
U_h_int.printMatlab( "U_h_int_" + mesh_path.stem().string() + ".m" );
U_h_on.printMatlab( "U_h_on_" + mesh_path.stem().string() + ".m" );
U_h_on_boundary.printMatlab( "U_h_on_boundary_" + mesh_path.stem().string() + ".m" );
//L2 norm of error
auto error = vf::project(_space=Xh, _range=elements(oneelement_mesh), _expr=idv(U_h_int) - idv(U_h_on) );
double L2error = error.l2Norm();
std::cout << "L2 error (elements) = " << L2error << std::endl;
auto error_boundary = vf::project(_space=Xh, _range=boundaryfaces(oneelement_mesh), _expr=idv(U_h_int) - idv(U_h_on_boundary) );
double L2error_boundary = error_boundary.l2Norm();
std::cout << "L2 error (boundary) = " << L2error_boundary << std::endl;
BOOST_CHECK_SMALL( L2error_boundary - L2error, 1e-13 );
}
// try to turn a path into a pipe
pipe_t link_path(link_t link, lob_t path)
{
pipe_t pipe;
if(!link || !path) return LOG("bad args");
if(!(pipe = mesh_path(link->mesh, link, path))) return NULL;
link_pipe(link, pipe);
return pipe;
}
bool LoadMeshData(const char* i_binaryMeshFile)
{
uint32_t vertexElementCount = 0;
VertexElement* pVertexElement = nullptr;
uint32_t vertexOffset = 0;
uint32_t vertexCount = 0;
uint32_t indexOffset = 0;
uint32_t indexCount = 0;
uint32_t subMeshOffset = 0;
uint32_t subMeshCount = 0;
uint8_t* pBuffer = LoadMeshInfo(i_binaryMeshFile, vertexElementCount, pVertexElement,
vertexOffset, vertexCount, indexOffset, indexCount, subMeshOffset, subMeshCount);
AOSMeshData* pAOSMeshData = new AOSMeshData();
sVertex* pVertices = (sVertex*)(pBuffer + vertexOffset);
for (uint32_t vertexIndex = 0; vertexIndex < vertexCount; ++vertexIndex)
{
sVertex& vertex = pVertices[vertexIndex];
pAOSMeshData->_vertices.push_back(vertex);
}
uint32_t* pIndices = (uint32_t*)(pBuffer + indexOffset);
for (uint32_t index = 0; index < indexCount; index += 3)
{
uint32_t& indexValue0 = pIndices[index + 0];
uint32_t& indexValue1 = pIndices[index + 1];
uint32_t& indexValue2 = pIndices[index + 2];
pAOSMeshData->_indices.push_back(indexValue0);
#if defined( EAEENGINE_PLATFORM_D3D9 )
pAOSMeshData->_indices.push_back(indexValue2);
pAOSMeshData->_indices.push_back(indexValue1);
#elif defined( EAEENGINE_PLATFORM_GL )
pAOSMeshData->_indices.push_back(indexValue1);
pAOSMeshData->_indices.push_back(indexValue2);
#endif
}
sSubMesh* pSubMeshes = (sSubMesh*)(pBuffer + subMeshOffset);
for (uint32_t subMeshIndex = 0; subMeshIndex < subMeshCount; ++subMeshIndex)
{
sSubMesh* pSubMesh = pSubMeshes + subMeshIndex;
pAOSMeshData->_subMeshes.push_back(*pSubMesh);
}
std::string mesh_path(i_binaryMeshFile);
std::string key = GetFileNameWithoutExtension(mesh_path.c_str());
SAFE_DELETE_ARRAY(pBuffer);
bool result = AOSMeshDataManager::GetInstance()->AddAOSMeshData(key.c_str(), pAOSMeshData);
return result;
}
void
TestInterpolationHCurl3D::testInterpolation( std::string one_element_mesh )
{
//auto myexpr = unitX() + unitY() + unitZ() ; //(1,1,1)
auto myexpr = vec( cst(1.), cst(1.), cst(1.));
// one element mesh
auto mesh_name = one_element_mesh + ".msh"; //create the mesh and load it
fs::path mesh_path( mesh_name );
mesh_ptrtype oneelement_mesh = loadMesh( _mesh=new mesh_type,
_filename=mesh_name);
// refined mesh (export)
auto refine_level = std::floor(1 - math::log( 0.1 )); //Deduce refine level from meshSize (option)
mesh_ptrtype mesh = loadMesh( _mesh=new mesh_type,
_filename=mesh_name,
_refine=( int )refine_level);
space_ptrtype Xh = space_type::New( oneelement_mesh );
std::vector<std::string> faces = {"yzFace","xyzFace","xyFace"};
std::vector<std::string> edges = {"zAxis","yAxis","yzAxis","xyAxis","xzAxis","xAxis"};
element_type U_h_int = Xh->element();
element_type U_h_on = Xh->element();
element_type U_h_on_boundary = Xh->element();
submesh1d_ptrtype edgeMesh( new submesh1d_type );
edgeMesh = createSubmesh(oneelement_mesh, boundaryedges(oneelement_mesh) ); //submesh of edges
// Tangents on ref element
auto t0 = vec(cst(0.),cst(0.),cst(-2.));
auto t1 = vec(cst(0.),cst(2.),cst(0.));
auto t2 = vec(cst(0.),cst(-2.),cst(2.));
auto t3 = vec(cst(2.),cst(-2.),cst(0.));
auto t4 = vec(cst(2.),cst(0.),cst(-2.));
auto t5 = vec(cst(2.),cst(0.),cst(0.));
// Jacobian of geometrical transforms
std::string jac;
if(mesh_path.stem().string() == "one-elt-ref-3d" || mesh_path.stem().string() == "one-elt-real-h**o-3d" )
jac = "{1,0,0,0,1,0,0,0,1}:x:y:z";
else if(mesh_path.stem().string() == "one-elt-real-rotx" )
jac = "{1,0,0,0,0,-1,0,1,0}:x:y:z";
else if(mesh_path.stem().string() == "one-elt-real-roty" )
jac = "{0,0,1,0,1,0,-1,0,0}:x:y:z";
else if(mesh_path.stem().string() == "one-elt-real-rotz" )
jac = "{0,-1,0,1,0,0,0,0,1}:x:y:z";
U_h_int(0) = integrate( markedelements(edgeMesh, edges[0]), trans(expr<3,3>(jac)*t0)*myexpr ).evaluate()(0,0);
U_h_int(1) = integrate( markedelements(edgeMesh, edges[1]), trans(expr<3,3>(jac)*t1)*myexpr ).evaluate()(0,0);
U_h_int(2) = integrate( markedelements(edgeMesh, edges[2]), trans(expr<3,3>(jac)*t2)*myexpr ).evaluate()(0,0);
U_h_int(3) = integrate( markedelements(edgeMesh, edges[3]), trans(expr<3,3>(jac)*t3)*myexpr ).evaluate()(0,0);
U_h_int(4) = integrate( markedelements(edgeMesh, edges[4]), trans(expr<3,3>(jac)*t4)*myexpr ).evaluate()(0,0);
U_h_int(5) = integrate( markedelements(edgeMesh, edges[5]), trans(expr<3,3>(jac)*t5)*myexpr ).evaluate()(0,0);
for(int i=0; i<edges.size(); i++)
{
double edgeLength = integrate( markedelements(edgeMesh, edges[i]), cst(1.) ).evaluate()(0,0);
U_h_int(i) /= edgeLength;
}
#if 0 //Doesn't work for now
for(int i=0; i<Xh->nLocalDof(); i++)
{
CHECK( edgeMesh->hasMarkers( {edges[i]} ) );
U_h_int(i) = integrate( markedelements(edgeMesh, edges[i]), trans( print(T(),"T=") )*myexpr ).evaluate()(0,0);
std::cout << "U_h_int(" << i << ")= " << U_h_int(i) << std::endl;
}
#endif
// nedelec interpolant using on keyword
// interpolate on element
U_h_on.zero();
U_h_on.on(_range=elements(oneelement_mesh), _expr=myexpr);
U_h_on_boundary.on(_range=boundaryfaces(oneelement_mesh), _expr=myexpr);
auto exporter_proj = exporter( _mesh=mesh, _name=( boost::format( "%1%" ) % this->about().appName() ).str() );
exporter_proj->step( 0 )->add( "U_interpolation_handly_"+mesh_path.stem().string(), U_h_int );
exporter_proj->step( 0 )->add( "U_interpolation_on_"+mesh_path.stem().string(), U_h_on );
exporter_proj->save();
// print coefficient only for reference element
U_h_int.printMatlab( "U_h_int_" + mesh_path.stem().string() + ".m" );
U_h_on.printMatlab( "U_h_on_" + mesh_path.stem().string() + ".m" );
U_h_on_boundary.printMatlab( "U_h_on_boundary_" + mesh_path.stem().string() + ".m" );
//L2 norm of error
auto error = vf::project(_space=Xh, _range=elements(oneelement_mesh), _expr=idv(U_h_int) - idv(U_h_on) );
double L2error = error.l2Norm();
std::cout << "L2 error (elements) = " << L2error << std::endl;
auto error_boundary = vf::project(_space=Xh, _range=elements(oneelement_mesh), _expr=idv(U_h_int) - idv(U_h_on_boundary) );
double L2error_boundary = error_boundary.l2Norm();
std::cout << "L2 error (boundary) = " << L2error_boundary << std::endl;
BOOST_CHECK_SMALL( L2error_boundary - L2error, 1e-13 );
}
