HiReconstruction.cpp

Go to the documentation of this file.

#include "moab/DiscreteGeometry/HiReconstruction.hpp"
#include "moab/DiscreteGeometry/DGMSolver.hpp"
#include "moab/HalfFacetRep.hpp"
 
#ifdef MOAB_HAVE_MPI
#include "moab/ParallelComm.hpp"
#endif
#include "MBTagConventions.hpp"
 
#define HIREC_USE_AHF
 
#include <cmath>
#include <deque>
#include <iostream>
 
namespace moab
{
HiReconstruction::HiReconstruction( Core* impl, ParallelComm* comm, EntityHandle meshIn, int minpnts, bool recwhole )
 : mbImpl( impl ), pcomm( comm ), _mesh2rec( meshIn ), _MINPNTS( minpnts )
{
 assert( NULL != impl );
 ErrorCode error;
 _MINEPS = 1e-12;
 _dim = 0;
 _hasfittings = false;
 _hasderiv = false;
#ifdef MOAB_HAVE_MPI
 if( !pcomm )
 {
 pcomm = moab::ParallelComm::get_pcomm( mbImpl, 0 );
 }
#endif
 
 error = initialize( recwhole );
 if( MB_SUCCESS != error )
 {
 std::cout << "Error initializing HiReconstruction\n" << std::endl;
 exit( 1 );
 }
}
 
HiReconstruction::~HiReconstruction()
{
#ifdef MOAB_HAVE_AHF
 ahf = NULL;
#else
 delete ahf;
#endif
}
 
ErrorCode HiReconstruction::initialize( bool recwhole )
{
 ErrorCode error;
 
#ifdef HIREC_USE_AHF
 std::cout << "HIREC_USE_AHF: Initializing" << std::endl;
 ahf = new HalfFacetRep( mbImpl, pcomm, _mesh2rec, false );
 if( !ahf )
 {
 return MB_MEMORY_ALLOCATION_FAILED;
 }
 error = ahf->initialize();MB_CHK_ERR( error );
#else
 ahf = NULL;
#endif
 
 // error = ahf->get_entity_ranges(_inverts,_inedges,_infaces,_incells); MB_CHK_ERR(error);
 error = mbImpl->get_entities_by_dimension( _mesh2rec, 0, _inverts );MB_CHK_ERR( error );
 error = mbImpl->get_entities_by_dimension( _mesh2rec, 1, _inedges );MB_CHK_ERR( error );
 error = mbImpl->get_entities_by_dimension( _mesh2rec, 2, _infaces );MB_CHK_ERR( error );
 error = mbImpl->get_entities_by_dimension( _mesh2rec, 3, _incells );MB_CHK_ERR( error );
 if( _inedges.size() && _infaces.empty() && _incells.empty() )
 {
 _dim = 1;
 _MAXPNTS = 13;
 }
 else if( _infaces.size() && _incells.empty() )
 {
 _dim = 2;
 _MAXPNTS = 128;
 }
 else
 {
 MB_SET_ERR( MB_FAILURE, "Encountered a non-manifold mesh or a mesh with volume elements" );
 }
 
 // get locally hosted vertices by filtering pstatus
#ifdef MOAB_HAVE_MPI
 if( pcomm )
 {
 error = pcomm->filter_pstatus( _inverts, PSTATUS_GHOST, PSTATUS_NOT, -1, &_verts2rec );MB_CHK_ERR( error );
 }
 else
 {
 _verts2rec = _inverts;
 }
#else
 _verts2rec = _inverts;
#endif
 _nv2rec = _verts2rec.size();
 
 if( recwhole )
 {
 // compute normals(surface) or tangent vector(curve) for all locally hosted vertices
 if( 2 == _dim )
 {
 compute_average_vertex_normals_surf();
 }
 else if( 1 == _dim )
 {
 compute_average_vertex_tangents_curve();
 }
 else
 {
 MB_SET_ERR( MB_FAILURE, "Unknow space dimension" );
 }
 _hasderiv = true;
 }
 return error;
}
 
/***************************************************
 * User Interface for Reconstruction of Geometry *
 ***************************************************/
 
ErrorCode HiReconstruction::reconstruct3D_surf_geom( int degree, bool interp, bool safeguard, bool reset )
{
 assert( 2 == _dim );
 if( _hasfittings && !reset )
 {
 // This object has precomputed fitting results and user don't want to reset
 return MB_SUCCESS;
 }
 else
 {
 _initfittings = _hasfittings = false;
 }
 // initialize for geometric information
 initialize_surf_geom( degree );
 ErrorCode error;
 double *coeffs, *coords;
 int* degree_out;
 int ncoeffs = ( degree + 2 ) * ( degree + 1 ) / 2;
 
 // DBG
 int dcount = 0;
 
 for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert )
 {
 int index = _verts2rec.index( *ivert );
 // for debug
 /*if(index==70){
 EntityHandle vid = *ivert;
 double vertcoords[3];
 error = mbImpl->get_coords(&vid,1,vertcoords);
 }*/
 
 size_t istr = _vertID2coeffID[index];
 coords = &( _local_coords[9 * index] );
 coeffs = &( _local_fit_coeffs[istr] );
 degree_out = &( _degrees_out[index] );
 _interps[index] = interp;
 error = polyfit3d_walf_surf_vertex( *ivert, interp, degree, _MINPNTS, safeguard, 9, coords, degree_out, ncoeffs,
 coeffs );MB_CHK_ERR( error );
 
 // DBG
 if( degree_out[0] < degree ) dcount += 1;
 }
 
 // DBG
 // std::cout<<"Total #points ="<<_verts2rec.size()<<", #degraded points = "<<dcount<<std::endl;
 
 _geom = HISURFACE;
 _hasfittings = true;
 return error;
}
 
ErrorCode HiReconstruction::reconstruct3D_surf_geom( size_t npts,
 int* degrees,
 bool* interps,
 bool safeguard,
 bool reset )
{
 assert( _dim == 2 );
 if( npts != _nv2rec )
 {
 MB_SET_ERR( MB_FAILURE, "Input number of degrees doesn't match number of vertices" );
 }
 if( _hasfittings && !reset )
 {
 return MB_SUCCESS;
 }
 else
 {
 _initfittings = _hasfittings = false;
 }
 ErrorCode error;
 // initialization for fitting
 initialize_surf_geom( npts, degrees );
 double *coeffs, *coords;
 int* degree_out;
 size_t i = 0;
 for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++i )
 {
 int index = _verts2rec.index( *ivert );
 assert( -1 != index );
 size_t istr = _vertID2coeffID[index];
 coords = &( _local_coords[9 * index] );
 coeffs = &( _local_fit_coeffs[istr] );
 degree_out = &( _degrees_out[index] );
 _interps[index] = interps[i];
 int ncoeffs = ( degrees[i] + 2 ) * ( degrees[i] + 1 ) / 2;
 error = polyfit3d_walf_surf_vertex( *ivert, interps[i], degrees[i], _MINPNTS, safeguard, 9, coords, degree_out,
 ncoeffs, coeffs );MB_CHK_ERR( error );
 }
 _geom = HISURFACE;
 _hasfittings = true;
 return error;
}
 
ErrorCode HiReconstruction::reconstruct3D_curve_geom( int degree, bool interp, bool safeguard, bool reset )
{
 assert( _dim == 1 );
 if( _hasfittings && !reset )
 {
 return MB_SUCCESS;
 }
 else
 {
 _initfittings = _hasfittings = false;
 }
 initialize_3Dcurve_geom( degree );
 ErrorCode error;
 double *coords = 0, *coeffs;
 int* degree_out;
 int ncoeffs = 3 * ( degree + 1 );
 for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert )
 {
 int index = _verts2rec.index( *ivert );
 assert( index != -1 );
 size_t istr = _vertID2coeffID[index];
 coeffs = &( _local_fit_coeffs[istr] );
 degree_out = &( _degrees_out[index] );
 _interps[index] = interp;
 error = polyfit3d_walf_curve_vertex( *ivert, interp, degree, _MINPNTS, safeguard, 0, coords, degree_out,
 ncoeffs, coeffs );MB_CHK_ERR( error );
 }
 _geom = HI3DCURVE;
 _hasfittings = true;
 return error;
}
 
ErrorCode HiReconstruction::reconstruct3D_curve_geom( size_t npts,
 int* degrees,
 bool* interps,
 bool safeguard,
 bool reset )
{
 assert( _dim == 1 );
 ErrorCode error;
 if( npts != _nv2rec )
 {
 MB_SET_ERR( MB_FAILURE, "Input number of degrees doesn't match the number of vertices" );
 }
 if( _hasfittings && !reset )
 {
 return MB_SUCCESS;
 }
 else
 {
 _initfittings = _hasfittings = false;
 }
 // initialize
 initialize_3Dcurve_geom( npts, degrees );
 double *coords = 0, *coeffs;
 int* degree_out;
 size_t i = 0;
 for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++i )
 {
 int index = _verts2rec.index( *ivert );
 size_t istr = _vertID2coeffID[index];
 coeffs = &( _local_fit_coeffs[istr] );
 degree_out = &( _degrees_out[index] );
 _interps[index] = interps[i];
 int ncoeffs = 3 * ( degrees[i] + 1 );
 error = polyfit3d_walf_curve_vertex( *ivert, interps[i], degrees[i], _MINPNTS, safeguard, 0, coords, degree_out,
 ncoeffs, coeffs );MB_CHK_ERR( error );
 }
 _geom = HI3DCURVE;
 _hasfittings = true;
 return error;
}
 
ErrorCode HiReconstruction::polyfit3d_walf_surf_vertex( const EntityHandle vid,
 const bool interp,
 int degree,
 int minpnts,
 const bool safeguard,
 const int ncoords,
 double* coords,
 int* degree_out,
 const int ncoeffs,
 double* coeffs )
{
 assert( _dim == 2 );
 ErrorCode error;
 int ring = estimate_num_rings( degree, interp );
 // std::cout<<"ring = "<<ring<<std::endl;
 // get n-ring neighbors
 Range ngbvs;
 error = obtain_nring_ngbvs( vid, ring, minpnts, ngbvs );MB_CHK_ERR( error );
 // for debug
 /*if(_verts2rec.index(vid)==70){
 for(Range::iterator ingb=ngbvs.begin();ingb!=ngbvs.end();++ingb) std::cerr <<
 _verts2rec.index(*ingb) << " "; std::cout << std::endl;
 }*/
 
 // get coordinates;
 size_t nverts = ngbvs.size();
 assert( nverts );
 double* ngbcoords = new double[nverts * 3];
 error = mbImpl->get_coords( ngbvs, ngbcoords );MB_CHK_ERR( error );
 // get normals
 double* ngbnrms = new double[nverts * 3];
 error = get_normals_surf( ngbvs, ngbnrms );MB_CHK_ERR( error );
 // switch vid to first one
 int index = ngbvs.index( vid );
 assert( index != -1 );
 std::swap( ngbcoords[0], ngbcoords[3 * index] );
 std::swap( ngbcoords[1], ngbcoords[3 * index + 1] );
 std::swap( ngbcoords[2], ngbcoords[3 * index + 2] );
 std::swap( ngbnrms[0], ngbnrms[3 * index] );
 std::swap( ngbnrms[1], ngbnrms[3 * index + 1] );
 std::swap( ngbnrms[2], ngbnrms[3 * index + 2] );
 // local WLS fitting
 int degree_pnt, degree_qr;
 polyfit3d_surf_get_coeff( nverts, ngbcoords, ngbnrms, degree, interp, safeguard, ncoords, coords, ncoeffs, coeffs,
 degree_out, &degree_pnt, &degree_qr );
 delete[] ngbcoords;
 delete[] ngbnrms;
 return error;
}
 
ErrorCode HiReconstruction::polyfit3d_walf_curve_vertex( const EntityHandle vid,
 const bool interp,
 int degree,
 int minpnts,
 const bool safeguard,
 const int ncoords,
 double* coords,
 int* degree_out,
 const int ncoeffs,
 double* coeffs )
{
 ErrorCode error;
 int ring = estimate_num_rings( degree, interp );
 // get n-ring neighbors
 Range ngbvs;
 error = obtain_nring_ngbvs( vid, ring, minpnts, ngbvs );MB_CHK_ERR( error );
 // get coordinates
 size_t nverts = ngbvs.size();
 assert( nverts );
 double* ngbcoords = new double[nverts * 3];
 error = mbImpl->get_coords( ngbvs, ngbcoords );MB_CHK_ERR( error );
 // get tangent vectors
 double* ngbtangs = new double[nverts * 3];
 error = get_tangents_curve( ngbvs, ngbtangs );MB_CHK_ERR( error );
 // switch vid to first one
 int index = ngbvs.index( vid );
 assert( index != -1 );
 std::swap( ngbcoords[0], ngbcoords[3 * index] );
 std::swap( ngbcoords[1], ngbcoords[3 * index + 1] );
 std::swap( ngbcoords[2], ngbcoords[3 * index + 2] );
 std::swap( ngbtangs[0], ngbtangs[3 * index] );
 std::swap( ngbtangs[1], ngbtangs[3 * index + 1] );
 std::swap( ngbtangs[2], ngbtangs[3 * index + 2] );
 // local WLS fittings
 polyfit3d_curve_get_coeff( nverts, ngbcoords, ngbtangs, degree, interp, safeguard, ncoords, coords, ncoeffs, coeffs,
 degree_out );
 delete[] ngbcoords;
 delete[] ngbtangs;
 return error;
}
 
/**************************************************************
 * User Interface for Evaluation via Reconstructed Geometry *
 **************************************************************/
 
ErrorCode HiReconstruction::hiproj_walf_in_element( EntityHandle elem,
 const int nvpe,
 const int npts2fit,
 const double* naturalcoords2fit,
 double* newcoords )
{
 assert( newcoords );
 ErrorCode error;
 // get connectivity table
 std::vector< EntityHandle > elemconn;
 error = mbImpl->get_connectivity( &elem, 1, elemconn );MB_CHK_ERR( error );
 if( nvpe != (int)elemconn.size() )
 {
 MB_SET_ERR( MB_FAILURE, "element connectivity table size doesn't match input size" );
 }
 
 if( !_hasfittings )
 {
 MB_SET_ERR( MB_FAILURE, "There is no existing fitting results" );
 }
 else
 {
 std::ostringstream convert;
 convert << elem;
 std::string ID = convert.str();
 for( int i = 0; i < nvpe; ++i )
 {
 if( -1 == _verts2rec.index( elemconn[i] ) )
 {
 MB_SET_ERR( MB_FAILURE, "There is no existing fitting results for element " + ID );
 }
 }
 }
 // check correctness of input
 for( int i = 0; i < npts2fit; ++i )
 {
 if( !check_barycentric_coords( nvpe, naturalcoords2fit + i * nvpe ) )
 {
 MB_SET_ERR( MB_FAILURE, "Wrong barycentric coordinates" );
 }
 }
 
 double* elemcoords = new double[nvpe * 3];
 error = mbImpl->get_coords( &( elemconn[0] ), nvpe, elemcoords );MB_CHK_ERR( error );
 
 double* coords2fit = new double[3 * npts2fit]();
 for( int i = 0; i < npts2fit; ++i )
 {
 for( int j = 0; j < nvpe; ++j )
 {
 coords2fit[3 * i] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j];
 coords2fit[3 * i + 1] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j + 1];
 coords2fit[3 * i + 2] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j + 2];
 }
 }
 
 double* hiproj_new = new double[3 * npts2fit];
 // initialize output
 for( int i = 0; i < npts2fit; ++i )
 {
 newcoords[3 * i] = newcoords[3 * i + 1] = newcoords[3 * i + 2] = 0;
 }
 // for each input vertex, call nvpe fittings and take average
 for( int j = 0; j < nvpe; ++j )
 {
 error = hiproj_walf_around_vertex( elemconn[j], npts2fit, coords2fit, hiproj_new );MB_CHK_ERR( error );
 for( int i = 0; i < npts2fit; ++i )
 {
 newcoords[3 * i] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i];
 newcoords[3 * i + 1] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i + 1];
 newcoords[3 * i + 2] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i + 2];
 }
 }
 delete[] elemcoords;
 delete[] coords2fit;
 delete[] hiproj_new;
 return error;
}
 
ErrorCode HiReconstruction::hiproj_walf_around_vertex( EntityHandle vid,
 const int npts2fit,
 const double* coords2fit,
 double* hiproj_new )
{
 if( !_hasfittings )
 {
 MB_SET_ERR( MB_FAILURE, "There is no existing fitting results" );
 }
 else if( -1 == _verts2rec.index( vid ) )
 {
 std::ostringstream convert;
 convert << vid;
 std::string VID = convert.str();
 MB_SET_ERR( MB_FAILURE, "There is no existing fitting results for vertex " + VID );
 }
 ErrorCode error;
 // get center of local coordinates system
 double local_origin[3];
 error = mbImpl->get_coords( &vid, 1, local_origin );MB_CHK_ERR( error );
 // get local fitting parameters
 int index = _verts2rec.index( vid );
 bool interp = _interps[index];
 int local_deg = _degrees_out[index];
 double *uvw_coords, *local_coeffs;
 if( _geom == HISURFACE )
 {
 uvw_coords = &( _local_coords[9 * index] );
 // int ncoeffs = (local_deg+2)*(local_deg+1)>>1;
 size_t istr = _vertID2coeffID[index];
 local_coeffs = &( _local_fit_coeffs[istr] );
 walf3d_surf_vertex_eval( local_origin, uvw_coords, local_deg, local_coeffs, interp, npts2fit, coords2fit,
 hiproj_new );
 }
 else if( _geom == HI3DCURVE )
 {
 uvw_coords = &( _local_coords[3 * index] );
 size_t istr = _vertID2coeffID[index];
 local_coeffs = &( _local_fit_coeffs[istr] );
 walf3d_curve_vertex_eval( local_origin, uvw_coords, local_deg, local_coeffs, interp, npts2fit, coords2fit,
 hiproj_new );
 }
 return error;
}
 
void HiReconstruction::walf3d_surf_vertex_eval( const double* local_origin,
 const double* local_coords,
 const int local_deg,
 const double* local_coeffs,
 const bool interp,
 const int npts2fit,
 const double* coords2fit,
 double* hiproj_new )
{
 double xaxis[3], yaxis[3], zaxis[3];
 for( int i = 0; i < 3; ++i )
 {
 xaxis[i] = local_coords[i];
 yaxis[i] = local_coords[3 + i];
 zaxis[i] = local_coords[6 + i];
 }
 // double *basis = new double[(local_deg+2)*(local_deg+1)/2-1];
 std::vector< double > basis( ( local_deg + 2 ) * ( local_deg + 1 ) / 2 - 1 );
 for( int i = 0; i < npts2fit; ++i )
 {
 double local_pos[3];
 for( int j = 0; j < 3; ++j )
 {
 local_pos[j] = coords2fit[3 * i + j] - local_origin[j];
 }
 double u, v, height = 0;
 u = DGMSolver::vec_innerprod( 3, local_pos, xaxis );
 v = DGMSolver::vec_innerprod( 3, local_pos, yaxis );
 basis[0] = u;
 basis[1] = v;
 int l = 1;
 for( int k = 2; k <= local_deg; ++k )
 {
 ++l;
 basis[l] = u * basis[l - k];
 for( int id = 0; id < k; ++id )
 {
 ++l;
 basis[l] = basis[l - k - 1] * v;
 }
 }
 if( !interp )
 {
 height = local_coeffs[0];
 }
 for( int p = 0; p <= l; ++p )
 {
 height += local_coeffs[p + 1] * basis[p];
 }
 hiproj_new[3 * i] = local_origin[0] + u * xaxis[0] + v * yaxis[0] + height * zaxis[0];
 hiproj_new[3 * i + 1] = local_origin[1] + u * xaxis[1] + v * yaxis[1] + height * zaxis[1];
 hiproj_new[3 * i + 2] = local_origin[2] + u * xaxis[2] + v * yaxis[2] + height * zaxis[2];
 }
 // delete [] basis;
}
 
void HiReconstruction::walf3d_curve_vertex_eval( const double* local_origin,
 const double* local_coords,
 const int local_deg,
 const double* local_coeffs,
 const bool interp,
 const int npts2fit,
 const double* coords2fit,
 double* hiproj_new )
{
 assert( local_origin && local_coords && local_coeffs );
 int ncoeffspvpd = local_deg + 1;
 for( int i = 0; i < npts2fit; ++i )
 {
 // get the vector from center to current point, project to tangent line
 double vec[3], ans[3] = { 0, 0, 0 };
 DGMSolver::vec_linear_operation( 3, 1, coords2fit + 3 * i, -1, local_origin, vec );
 double u = DGMSolver::vec_innerprod( 3, local_coords, vec );
 // evaluate polynomials
 if( !interp )
 {
 ans[0] = local_coeffs[0];
 ans[1] = local_coeffs[ncoeffspvpd];
 ans[2] = local_coeffs[2 * ncoeffspvpd];
 }
 double uk = 1; // degree_out and degree different, stored in columnwise contiguously
 for( int j = 1; j < ncoeffspvpd; ++j )
 {
 uk *= u;
 ans[0] += uk * local_coeffs[j];
 ans[1] += uk * local_coeffs[j + ncoeffspvpd];
 ans[2] += uk * local_coeffs[j + 2 * ncoeffspvpd];
 }
 hiproj_new[3 * i] = ans[0] + local_origin[0];
 hiproj_new[3 * i + 1] = ans[1] + local_origin[1];
 hiproj_new[3 * i + 2] = ans[2] + local_origin[2];
 }
}
 
bool HiReconstruction::get_fittings_data( EntityHandle vid,
 GEOMTYPE& geomtype,
 std::vector< double >& coords,
 int& degree_out,
 std::vector< double >& coeffs,
 bool& interp )
{
 if( !_hasfittings )
 {
 return false;
 }
 else
 {
 int index = _verts2rec.index( vid );
 if( -1 == index )
 {
 std::cout << "Input vertex is not locally hosted vertex in this mesh set" << std::endl;
 return false;
 }
 geomtype = _geom;
 if( HISURFACE == _geom )
 {
 coords.insert( coords.end(), _local_coords.begin() + 9 * index, _local_coords.begin() + 9 * index + 9 );
 degree_out = _degrees_out[index];
 interp = _interps[index];
 int ncoeffs = ( degree_out + 2 ) * ( degree_out + 1 ) >> 1;
 size_t istr = _vertID2coeffID[index];
 coeffs.insert( coeffs.end(), _local_fit_coeffs.begin() + istr, _local_fit_coeffs.begin() + istr + ncoeffs );
 }
 else if( HI3DCURVE == _geom )
 {
 coords.insert( coords.end(), _local_coords.begin() + 3 * index, _local_coords.begin() + 3 * index + 3 );
 degree_out = _degrees_out[index];
 interp = _interps[index];
 int ncoeffs = 3 * ( degree_out + 1 );
 size_t istr = _vertID2coeffID[index];
 coeffs.insert( coeffs.end(), _local_fit_coeffs.begin() + istr, _local_fit_coeffs.begin() + istr + ncoeffs );
 }
 return true;
 }
}
 
/****************************************************************
 * Basic Internal Routines to initialize and set fitting data *
 ****************************************************************/
 
int HiReconstruction::estimate_num_rings( int degree, bool interp )
{
 return interp ? ( ( degree + 1 ) >> 1 ) + ( ( degree + 1 ) & 1 ) : ( ( degree + 2 ) >> 1 ) + ( ( degree + 2 ) & 1 );
}
 
ErrorCode HiReconstruction::vertex_get_incident_elements( const EntityHandle& vid,
 const int elemdim,
 std::vector< EntityHandle >& adjents )
{
 ErrorCode error;
 assert( elemdim == _dim );
#ifdef HIREC_USE_AHF
 error = ahf->get_up_adjacencies( vid, elemdim, adjents );MB_CHK_ERR( error );
#else
 error = mbImpl->get_adjacencies( &vid, 1, elemdim, false, adjents );MB_CHK_ERR( error );
#endif
 return error;
}
 
ErrorCode HiReconstruction::obtain_nring_ngbvs( const EntityHandle vid, int ring, const int minpnts, Range& ngbvs )
{
 ErrorCode error;
 std::deque< EntityHandle > todo;
 todo.push_back( vid );
 ngbvs.insert( vid );
 EntityHandle pre, nxt;
 for( int i = 1; i <= ring; ++i )
 {
 int count = todo.size();
 while( count )
 {
 EntityHandle center = todo.front();
 todo.pop_front();
 --count;
 std::vector< EntityHandle > adjents;
 error = vertex_get_incident_elements( center, _dim, adjents );MB_CHK_ERR( error );
 for( size_t j = 0; j < adjents.size(); ++j )
 {
 std::vector< EntityHandle > elemconn;
 error = mbImpl->get_connectivity( &adjents[j], 1, elemconn );MB_CHK_ERR( error );
 int nvpe = elemconn.size();
 for( int k = 0; k < nvpe; ++k )
 {
 if( elemconn[k] == center )
 {
 pre = k == 0 ? elemconn[nvpe - 1] : elemconn[k - 1];
 nxt = elemconn[( k + 1 ) % nvpe];
 if( ngbvs.find( pre ) == ngbvs.end() )
 {
 ngbvs.insert( pre );
 todo.push_back( pre );
 }
 if( ngbvs.find( nxt ) == ngbvs.end() )
 {
 ngbvs.insert( nxt );
 todo.push_back( nxt );
 }
 break;
 }
 }
 }
 }
 if( _MAXPNTS <= (int)ngbvs.size() )
 {
 // obtain enough points
 return error;
 }
 if( !todo.size() )
 {
 // current ring cannot introduce any points, return incase deadlock
 return error;
 }
 if( ( i == ring ) && ( minpnts > (int)ngbvs.size() ) )
 {
 // reach maximum ring but not enough points
 ++ring;
 }
 }
 return error;
}
 
void HiReconstruction::initialize_surf_geom( const int degree )
{
 if( !_hasderiv )
 {
 compute_average_vertex_normals_surf();
 _hasderiv = true;
 }
 if( !_initfittings )
 {
 int ncoeffspv = ( degree + 2 ) * ( degree + 1 ) / 2;
 _degrees_out.assign( _nv2rec, 0 );
 _interps.assign( _nv2rec, false );
 _vertID2coeffID.resize( _nv2rec );
 _local_fit_coeffs.assign( _nv2rec * ncoeffspv, 0 );
 for( size_t i = 0; i < _nv2rec; ++i )
 {
 _vertID2coeffID[i] = i * ncoeffspv;
 }
 _initfittings = true;
 }
}
 
void HiReconstruction::initialize_surf_geom( const size_t npts, const int* degrees )
{
 if( !_hasderiv )
 {
 compute_average_vertex_normals_surf();
 _hasderiv = true;
 }
 if( !_initfittings )
 {
 assert( _nv2rec == npts );
 _degrees_out.assign( _nv2rec, 0 );
 _interps.assign( _nv2rec, false );
 _vertID2coeffID.resize( _nv2rec );
 size_t index = 0;
 for( size_t i = 0; i < _nv2rec; ++i )
 {
 _vertID2coeffID[i] = index;
 index += ( degrees[i] + 2 ) * ( degrees[i] + 1 ) / 2;
 }
 _local_fit_coeffs.assign( index, 0 );
 _initfittings = true;
 }
}
 
void HiReconstruction::initialize_3Dcurve_geom( const int degree )
{
 if( !_hasderiv )
 {
 compute_average_vertex_tangents_curve();
 _hasderiv = true;
 }
 if( !_initfittings )
 {
 int ncoeffspvpd = degree + 1;
 _degrees_out.assign( _nv2rec, 0 );
 _interps.assign( _nv2rec, false );
 _vertID2coeffID.resize( _nv2rec );
 _local_fit_coeffs.assign( _nv2rec * ncoeffspvpd * 3, 0 );
 for( size_t i = 0; i < _nv2rec; ++i )
 {
 _vertID2coeffID[i] = i * ncoeffspvpd * 3;
 }
 _initfittings = true;
 }
}
 
void HiReconstruction::initialize_3Dcurve_geom( const size_t npts, const int* degrees )
{
 if( !_hasderiv )
 {
 compute_average_vertex_tangents_curve();
 _hasderiv = true;
 }
 if( !_hasfittings )
 {
 assert( _nv2rec == npts );
 _degrees_out.assign( _nv2rec, 0 );
 _interps.assign( _nv2rec, false );
 _vertID2coeffID.reserve( _nv2rec );
 size_t index = 0;
 for( size_t i = 0; i < _nv2rec; ++i )
 {
 _vertID2coeffID[i] = index;
 index += 3 * ( degrees[i] + 1 );
 }
 _local_fit_coeffs.assign( index, 0 );
 _initfittings = true;
 }
}
 
/* ErrorCode HiReconstruction::set_geom_data_surf(const EntityHandle vid, const double* coords,
 const double degree_out, const double* coeffs, bool interp)
 {
 return MB_SUCCESS;
 }
 
 ErrorCode HiReconstruction::set_geom_data_3Dcurve(const EntityHandle vid, const double* coords,
 const double degree_out, const double* coeffs, bool interp)
 {
 return MB_SUCCESS;
 } */
 
/*********************************************************
 * Routines for vertex normal/tangent vector estimation *
 *********************************************************/
ErrorCode HiReconstruction::average_vertex_normal( const EntityHandle vid, double* nrm )
{
 ErrorCode error;
 std::vector< EntityHandle > adjfaces;
 error = vertex_get_incident_elements( vid, 2, adjfaces );MB_CHK_ERR( error );
 int npolys = adjfaces.size();
 if( !npolys )
 {
 MB_SET_ERR( MB_FAILURE, "Vertex has no incident 2D entities" );
 }
 else
 {
 double v1[3], v2[3], v3[3], a[3], b[3], c[3];
 nrm[0] = nrm[1] = nrm[2] = 0;
 for( int i = 0; i < npolys; ++i )
 {
 // get incident "triangles"
 std::vector< EntityHandle > elemconn;
 error = mbImpl->get_connectivity( &adjfaces[i], 1, elemconn );MB_CHK_ERR( error );
 EntityHandle pre, nxt;
 int nvpe = elemconn.size();
 for( int j = 0; j < nvpe; ++j )
 {
 if( vid == elemconn[j] )
 {
 pre = j == 0 ? elemconn[nvpe - 1] : elemconn[j - 1];
 nxt = elemconn[( j + 1 ) % nvpe];
 break;
 }
 }
 // compute area weighted normals
 error = mbImpl->get_coords( &pre, 1, a );MB_CHK_ERR( error );
 error = mbImpl->get_coords( &vid, 1, b );MB_CHK_ERR( error );
 error = mbImpl->get_coords( &nxt, 1, c );MB_CHK_ERR( error );
 DGMSolver::vec_linear_operation( 3, 1, c, -1, b, v1 );
 DGMSolver::vec_linear_operation( 3, 1, a, -1, b, v2 );
 DGMSolver::vec_crossprod( v1, v2, v3 );
 DGMSolver::vec_linear_operation( 3, 1, nrm, 1, v3, nrm );
 }
#ifndef NDEBUG
 assert( DGMSolver::vec_normalize( 3, nrm, nrm ) );
#endif
 }
 return error;
}
 
ErrorCode HiReconstruction::compute_average_vertex_normals_surf()
{
 if( _hasderiv )
 {
 return MB_SUCCESS;
 }
 ErrorCode error;
 _local_coords.assign( 9 * _nv2rec, 0 );
 size_t index = 0;
 for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++index )
 {
 error = average_vertex_normal( *ivert, &( _local_coords[9 * index + 6] ) );MB_CHK_ERR( error );
 }
 return error;
}
 
ErrorCode HiReconstruction::get_normals_surf( const Range& vertsh, double* nrms )
{
 ErrorCode error = MB_SUCCESS;
 if( _hasderiv )
 {
 size_t id = 0;
 for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
 {
 int index = _verts2rec.index( *ivert );
#ifdef MOAB_HAVE_MPI
 if( -1 == index )
 {
 // ghost vertex
 error = average_vertex_normal( *ivert, nrms + 3 * id );MB_CHK_ERR( error );
 }
 else
 {
 nrms[3 * id] = _local_coords[9 * index + 6];
 nrms[3 * id + 1] = _local_coords[9 * index + 7];
 nrms[3 * id + 2] = _local_coords[9 * index + 8];
 }
#else
 assert( -1 != index );
 nrms[3 * id] = _local_coords[9 * index + 6];
 nrms[3 * id + 1] = _local_coords[9 * index + 7];
 nrms[3 * id + 2] = _local_coords[9 * index + 8];
#endif
 }
 }
 else
 {
 size_t id = 0;
 for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
 {
 error = average_vertex_normal( *ivert, nrms + 3 * id );MB_CHK_ERR( error );
 }
 }
 return error;
}
 
ErrorCode HiReconstruction::average_vertex_tangent( const EntityHandle vid, double* tang )
{
 ErrorCode error;
 std::vector< EntityHandle > adjedges;
 error = vertex_get_incident_elements( vid, 1, adjedges );MB_CHK_ERR( error );
 int nedges = adjedges.size();
 if( !nedges )
 {
 MB_SET_ERR( MB_FAILURE, "Vertex has no incident edges" );
 }
 else
 {
 assert( nedges <= 2 );
 tang[0] = tang[1] = tang[2] = 0;
 for( int i = 0; i < nedges; ++i )
 {
 std::vector< EntityHandle > edgeconn;
 error = mbImpl->get_connectivity( &adjedges[i], 1, edgeconn );
 double istr[3], iend[3], t[3];
 error = mbImpl->get_coords( &( edgeconn[0] ), 1, istr );
 error = mbImpl->get_coords( &( edgeconn[1] ), 1, iend );
 DGMSolver::vec_linear_operation( 3, 1, iend, -1, istr, t );
 DGMSolver::vec_linear_operation( 3, 1, tang, 1, t, tang );
 }
#ifndef NDEBUG
 assert( DGMSolver::vec_normalize( 3, tang, tang ) );
#endif
 }
 return error;
}
 
ErrorCode HiReconstruction::compute_average_vertex_tangents_curve()
{
 if( _hasderiv )
 {
 return MB_SUCCESS;
 }
 ErrorCode error;
 _local_coords.assign( 3 * _nv2rec, 0 );
 size_t index = 0;
 for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++index )
 {
 error = average_vertex_tangent( *ivert, &( _local_coords[3 * index] ) );MB_CHK_ERR( error );
 }
 return error;
}
 
ErrorCode HiReconstruction::get_tangents_curve( const Range& vertsh, double* tangs )
{
 ErrorCode error = MB_SUCCESS;
 if( _hasderiv )
 {
 size_t id = 0;
 for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
 {
 int index = _verts2rec.index( *ivert );
#ifdef MOAB_HAVE_MPI
 if( -1 != index )
 {
 tangs[3 * id] = _local_coords[3 * index];
 tangs[3 * id + 1] = _local_coords[3 * index + 1];
 tangs[3 * id + 2] = _local_coords[3 * index + 2];
 }
 else
 {
 error = average_vertex_tangent( *ivert, tangs + 3 * id );MB_CHK_ERR( error );
 }
#else
 assert( -1 != index );
 tangs[3 * id] = _local_coords[3 * index];
 tangs[3 * id + 1] = _local_coords[3 * index + 1];
 tangs[3 * id + 2] = _local_coords[3 * index + 2];
#endif
 }
 }
 else
 {
 size_t id = 0;
 for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
 {
 error = average_vertex_tangent( *ivert, tangs + 3 * id );MB_CHK_ERR( error );
 }
 }
 return error;
}
 
/************************************************
 * Internal Routines for local WLS fittings *
 *************************************************/
 
void HiReconstruction::polyfit3d_surf_get_coeff( const int nverts,
 const double* ngbcoords,
 const double* ngbnrms,
 int degree,
 const bool interp,
 const bool safeguard,
 const int ncoords,
 double* coords,
 const int ncoeffs,
 double* coeffs,
 int* degree_out,
 int* degree_pnt,
 int* degree_qr )
{
 if( nverts <= 0 )
 {
 return;
 }
 
 // std::cout << "npnts in initial stencil = " << nverts << std::endl;
 // std::cout << "centered at (" << ngbcoords[0] << "," << ngbcoords[1] << "," << ngbcoords[2] <<
 // ")" << std::endl;
 
 // step 1. copmute local coordinate system
 double nrm[3] = { ngbnrms[0], ngbnrms[1], ngbnrms[2] }, tang1[3] = { 0, 0, 0 }, tang2[3] = { 0, 0, 0 };
 if( fabs( nrm[0] ) > fabs( nrm[1] ) && fabs( nrm[0] ) > fabs( nrm[2] ) )
 {
 tang1[1] = 1.0;
 }
 else
 {
 tang1[0] = 1.0;
 }
 
 DGMSolver::vec_projoff( 3, tang1, nrm, tang1 );
#ifndef NDEBUG
 assert( DGMSolver::vec_normalize( 3, tang1, tang1 ) );
#endif
 DGMSolver::vec_crossprod( nrm, tang1, tang2 );
 if( 9 <= ncoords && coords )
 {
 coords[0] = tang1[0];
 coords[1] = tang1[1];
 coords[2] = tang1[2];
 coords[3] = tang2[0];
 coords[4] = tang2[1];
 coords[5] = tang2[2];
 coords[6] = nrm[0];
 coords[7] = nrm[1];
 coords[8] = nrm[2];
 }
 if( !ncoeffs || !coeffs )
 {
 return;
 }
 else
 {
 assert( ncoeffs >= ( degree + 2 ) * ( degree + 1 ) / 2 );
 }
 
 // step 2. project onto local coordinates system
 int npts2fit = nverts - interp;
 if( 0 == npts2fit )
 {
 *degree_out = *degree_pnt = *degree_qr = 0;
 for( int i = 0; i < ncoeffs; ++i )
 {
 coeffs[i] = 0;
 }
 // coeffs[0] = 0;
 return;
 }
 std::vector< double > us( npts2fit * 2 ); // double *us = new double[npts2fit*2];
 std::vector< double > bs( npts2fit ); // double *bs = new double[npts2fit];
 for( int i = interp; i < nverts; ++i )
 {
 int k = i - interp;
 double uu[3];
 DGMSolver::vec_linear_operation( 3, 1, ngbcoords + 3 * i, -1, ngbcoords, uu );
 us[k * 2] = DGMSolver::vec_innerprod( 3, tang1, uu );
 us[k * 2 + 1] = DGMSolver::vec_innerprod( 3, tang2, uu );
 bs[k] = DGMSolver::vec_innerprod( 3, nrm, uu );
 }
 
 // step 3. compute weights
 std::vector< double > ws( npts2fit ); // double *ws = new double[npts2fit];
 int nzeros = compute_weights( npts2fit, 2, &( us[0] ), nverts, ngbnrms, degree, _MINEPS, &( ws[0] ) );
 
 // step 4. adjust according to zero-weights
 if( nzeros )
 {
 if( nzeros == npts2fit )
 {
 *degree_out = *degree_pnt = *degree_qr = 0;
 for( int i = 0; i < ncoeffs; ++i )
 {
 coeffs[i] = 0;
 }
 // coeffs[0] = 0;
 return;
 }
 int index = 0;
 for( int i = 0; i < npts2fit; ++i )
 {
 if( ws[i] )
 {
 if( i > index )
 {
 us[index * 2] = us[i * 2];
 us[index * 2 + 1] = us[i * 2 + 1];
 bs[index] = bs[i];
 ws[index] = ws[i];
 }
 ++index;
 }
 }
 npts2fit -= nzeros;
 assert( index == npts2fit );
 us.resize( npts2fit * 2 );
 bs.resize( npts2fit );
 ws.resize( npts2fit );
 /*us = realloc(us,npts2fit*2*sizeof(double));
 bs = realloc(bs,npts2fit*sizeof(double));
 ws = realloc(ws,npts2fit*sizeof(double));*/
 }
 
 // std::cout<<"npnts after weighting = "<<npts2fit<<std::endl;
 
 // step 5. fitting
 eval_vander_bivar_cmf( npts2fit, &( us[0] ), 1, &( bs[0] ), degree, &( ws[0] ), interp, safeguard, degree_out,
 degree_pnt, degree_qr );
 
 // step 6. organize output
 int ncoeffs_out = ( *degree_out + 2 ) * ( *degree_out + 1 ) / 2;
 assert( ncoeffs_out <= ncoeffs );
 coeffs[0] = 0;
 for( int j = 0; j < ncoeffs_out - interp; ++j )
 {
 coeffs[j + interp] = bs[j];
 }
 // delete [] us; delete [] bs; delete [] ws;
}
 
void HiReconstruction::eval_vander_bivar_cmf( const int npts2fit,
 const double* us,
 const int ndim,
 double* bs,
 int degree,
 const double* ws,
 const bool interp,
 const bool safeguard,
 int* degree_out,
 int* degree_pnt,
 int* degree_qr )
{
 // step 1. adjust the degree according to number of points to fit
 int ncols = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
 while( 1 < degree && npts2fit < ncols )
 {
 --degree;
 ncols = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
 }
 *degree_pnt = degree;
 
 // std::cout << "degree_pnt: " << *degree_pnt << std::endl;
 
 // step 2. construct Vandermonde matrix, stored in columnwise
 std::vector< double > V; // V(npts2fit*(ncols+interp)); //double *V_init = new double[npts2fit*(ncols+interp)];
 DGMSolver::gen_vander_multivar( npts2fit, 2, us, degree, V );
 // remove the first column of 1s if interpolation
 if( interp )
 {
 V.erase( V.begin(), V.begin() + npts2fit );
 }
 /*double* V;
 if(interp){
 V = new double[npts2fit*ncols];
 std::memcpy(V,V_init+npts2fit,ncols*npts2fit*sizeof(double));
 delete [] V_init; V_init = 0;
 }else{
 V = V_init;
 }*/
 
 // step 3. Scale rows to assign different weights to different points
 for( int i = 0; i < npts2fit; ++i )
 {
 for( int j = 0; j < ncols; ++j )
 {
 V[j * npts2fit + i] *= ws[i];
 }
 for( int k = 0; k < ndim; ++k )
 {
 bs[k * npts2fit + i] *= ws[i];
 }
 }
 
 // step 4. scale columns to reduce condition number
 std::vector< double > ts( ncols ); // double *ts = new double[ncols];
 DGMSolver::rescale_matrix( npts2fit, ncols, &( V[0] ), &( ts[0] ) );
 
 // step 5. Perform Householder QR factorization
 std::vector< double > D( ncols ); // double *D = new double[ncols];
 int rank;
 DGMSolver::qr_polyfit_safeguarded( npts2fit, ncols, &( V[0] ), &( D[0] ), &rank );
 
 // step 6. adjust degree of fitting according to rank of Vandermonde matrix
 int ncols_sub = ncols;
 while( rank < ncols_sub )
 {
 --degree;
 if( degree == 0 )
 {
 // surface is flat, return 0
 *degree_out = *degree_qr = degree;
 for( int i = 0; i < npts2fit; ++i )
 {
 for( int k = 0; k < ndim; ++k )
 {
 bs[k * npts2fit + i] = 0;
 }
 }
 return;
 }
 else
 {
 ncols_sub = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
 }
 }
 *degree_qr = degree;
 
 // std::cout << "degree_qr: " << *degree_qr << std::endl;
 
 /* DBG
 * std::cout<<"before Qtb"<<std::endl;
 std::cout<<std::endl;
 std::cout<<"bs = "<<std::endl;
 std::cout<<std::endl;
 for (int k=0; k< ndim; k++){
 for (int j=0; j<npts2fit; ++j){
 std::cout<<" "<<bs[npts2fit*k+j]<<std::endl;
 }
 }
 std::cout<<std::endl;*/
 
 // step 7. compute Q'b
 DGMSolver::compute_qtransposeB( npts2fit, ncols_sub, &( V[0] ), ndim, bs );
 
 /* DBG
 * std::cout<<"after Qtb"<<std::endl;
 std::cout<<"bs = "<<std::endl;
 std::cout<<std::endl;
 for (int k=0; k< ndim; k++){
 for (int j=0; j<npts2fit; ++j){
 std::cout<<" "<<bs[npts2fit*k+j]<<std::endl;
 }
 }
 std::cout<<std::endl;*/
 
 // step 8. perform backward substitution and scale the solution
 // assign diagonals of V
 for( int i = 0; i < ncols_sub; ++i )
 {
 V[i * npts2fit + i] = D[i];
 }
 
 // backsolve
 if( safeguard )
 {
 // for debug
 // std::cout << "ts size " << ts.size() << std::endl;
 DGMSolver::backsolve_polyfit_safeguarded( 2, degree, interp, npts2fit, ncols_sub, &( V[0] ), ndim, bs,
 &( ts[0] ), degree_out );
 }
 else
 {
 DGMSolver::backsolve( npts2fit, ncols_sub, &( V[0] ), 1, bs, &( ts[0] ) );
 *degree_out = degree;
 }
 /*if(V_init){
 delete [] V_init;
 }else{
 delete [] V;
 }*/
}
 
void HiReconstruction::polyfit3d_curve_get_coeff( const int nverts,
 const double* ngbcors,
 const double* ngbtangs,
 int degree,
 const bool interp,
 const bool safeguard,
 const int ncoords,
 double* coords,
 const int ncoeffs,
 double* coeffs,
 int* degree_out )
{
 if( !nverts )
 {
 return;
 }
 // step 1. compute local coordinates system
 double tang[3] = { ngbtangs[0], ngbtangs[1], ngbtangs[2] };
 if( coords && ncoords > 2 )
 {
 coords[0] = tang[0];
 coords[1] = tang[1];
 coords[2] = tang[2];
 }
 if( !coeffs || !ncoeffs )
 {
 return;
 }
 else
 {
 assert( ncoeffs >= 3 * ( degree + 1 ) );
 }
 // step 2. project onto local coordinate system
 int npts2fit = nverts - interp;
 if( !npts2fit )
 {
 *degree_out = 0;
 for( int i = 0; i < ncoeffs; ++i )
 {
 coeffs[0] = 0;
 }
 return;
 }
 std::vector< double > us( npts2fit ); // double *us = new double[npts2fit];
 std::vector< double > bs( npts2fit * 3 ); // double *bs = new double[npts2fit*3];
 double uu[3];
 for( int i = interp; i < nverts; ++i )
 {
 int k = i - interp;
 DGMSolver::vec_linear_operation( 3, 1, ngbcors + 3 * i, -1, ngbcors, uu );
 us[k] = DGMSolver::vec_innerprod( 3, uu, tang );
 bs[k] = uu[0];
 bs[npts2fit + k] = uu[1];
 bs[2 * npts2fit + k] = uu[2];
 }
 
 // step 3. copmute weights
 std::vector< double > ws( npts2fit );
 int nzeros = compute_weights( npts2fit, 1, &( us[0] ), nverts, ngbtangs, degree, _MINEPS, &( ws[0] ) );
 assert( nzeros <= npts2fit );
 
 // step 4. adjust according to number of zero-weights
 if( nzeros )
 {
 if( nzeros == npts2fit )
 {
 // singular case
 *degree_out = 0;
 for( int i = 0; i < ncoeffs; ++i )
 {
 coeffs[i] = 0;
 }
 return;
 }
 int npts_new = npts2fit - nzeros;
 std::vector< double > bs_temp( 3 * npts_new );
 int index = 0;
 for( int i = 0; i < npts2fit; ++i )
 {
 if( ws[i] )
 {
 if( i > index )
 {
 us[index] = us[i];
 ws[index] = ws[i];
 }
 bs_temp[index] = bs[i];
 bs_temp[index + npts_new] = bs[i + npts2fit];
 bs_temp[index + 2 * npts_new] = bs[i + 2 * npts2fit];
 ++index;
 }
 }
 assert( index == npts_new );
 npts2fit = npts_new;
 us.resize( index );
 ws.resize( index );
 bs = bs_temp;
 // destroy bs_temp;
 std::vector< double >().swap( bs_temp );
 }
 
 // step 5. fitting
 eval_vander_univar_cmf( npts2fit, &( us[0] ), 3, &( bs[0] ), degree, &( ws[0] ), interp, safeguard, degree_out );
 // step 6. write results to output pointers
 int ncoeffs_out_pvpd = *degree_out + 1;
 assert( ncoeffs >= 3 * ncoeffs_out_pvpd );
 // write to coeffs consecutively, bs's size is not changed by eval_vander_univar_cmf
 coeffs[0] = coeffs[ncoeffs_out_pvpd] = coeffs[2 * ncoeffs_out_pvpd] = 0;
 for( int i = 0; i < ncoeffs_out_pvpd - interp; ++i )
 {
 coeffs[i + interp] = bs[i];
 coeffs[i + interp + ncoeffs_out_pvpd] = bs[i + npts2fit];
 coeffs[i + interp + 2 * ncoeffs_out_pvpd] = bs[i + 2 * npts2fit];
 }
}
 
void HiReconstruction::eval_vander_univar_cmf( const int npts2fit,
 const double* us,
 const int ndim,
 double* bs,
 int degree,
 const double* ws,
 const bool interp,
 const bool safeguard,
 int* degree_out )
{
 // step 1. determine degree of polynomials to fit according to number of points
 int ncols = degree + 1 - interp;
 while( npts2fit < ncols && degree >= 1 )
 {
 --degree;
 ncols = degree + 1 - interp;
 }
 if( !degree )
 {
 if( interp )
 {
 for( int icol = 0; icol < ndim; ++icol )
 {
 bs[icol * npts2fit] = 0;
 }
 }
 for( int irow = 1; irow < npts2fit; ++irow )
 {
 for( int icol = 0; icol < ndim; ++icol )
 {
 bs[icol * npts2fit + irow] = 0;
 }
 }
 *degree_out = 0;
 return;
 }
 // step 2. construct Vandermonde matrix
 std::vector< double > V; // V(npts2fit*(ncols+interp));
 DGMSolver::gen_vander_multivar( npts2fit, 1, us, degree, V );
 
 if( interp )
 {
 V.erase( V.begin(), V.begin() + npts2fit );
 }
 
 // step 3. scale rows with respect to weights
 for( int i = 0; i < npts2fit; ++i )
 {
 for( int j = 0; j < ncols; ++j )
 {
 V[j * npts2fit + i] *= ws[i];
 }
 for( int k = 0; k < ndim; ++k )
 {
 bs[k * npts2fit + i] *= ws[i];
 }
 }
 
 // step 4. scale columns to reduce condition number
 std::vector< double > ts( ncols );
 DGMSolver::rescale_matrix( npts2fit, ncols, &( V[0] ), &( ts[0] ) );
 
 // step 5. perform Householder QR factorization
 std::vector< double > D( ncols );
 int rank;
 DGMSolver::qr_polyfit_safeguarded( npts2fit, ncols, &( V[0] ), &( D[0] ), &rank );
 
 // step 6. adjust degree of fitting
 int ncols_sub = ncols;
 while( rank < ncols_sub )
 {
 --degree;
 if( !degree )
 {
 // matrix is singular, consider curve on flat plane passing center and orthogonal to
 // tangent line
 *degree_out = 0;
 for( int i = 0; i < npts2fit; ++i )
 {
 for( int k = 0; k < ndim; ++k )
 {
 bs[k * npts2fit + i] = 0;
 }
 }
 }
 ncols_sub = degree + 1 - interp;
 }
 
 // step 7. compute Q'*bs
 DGMSolver::compute_qtransposeB( npts2fit, ncols_sub, &( V[0] ), ndim, bs );
 
 // step 8. perform backward substitution and scale solutions
 // assign diagonals of V
 for( int i = 0; i < ncols_sub; ++i )
 {
 V[i * npts2fit + i] = D[i];
 }
 // backsolve
 if( safeguard )
 {
 DGMSolver::backsolve_polyfit_safeguarded( 1, degree, interp, npts2fit, ncols, &( V[0] ), ndim, bs, ws,
 degree_out );
 }
 else
 {
 DGMSolver::backsolve( npts2fit, ncols_sub, &( V[0] ), ndim, bs, &( ts[0] ) );
 *degree_out = degree;
 }
}
 
int HiReconstruction::compute_weights( const int nrows,
 const int ncols,
 const double* us,
 const int nngbs,
 const double* ngbnrms,
 const int degree,
 const double toler,
 double* ws )
{
 assert( nrows <= _MAXPNTS && ws );
 bool interp = false;
 if( nngbs != nrows )
 {
 assert( nngbs == nrows + 1 );
 interp = true;
 }
 double epsilon = 1e-2;
 
 // First, compute squared distance from each input piont to the center
 for( int i = 0; i < nrows; ++i )
 {
 ws[i] = DGMSolver::vec_innerprod( ncols, us + i * ncols, us + i * ncols );
 }
 
 // Second, compute a small correction termt o guard against zero
 double h = 0;
 for( int i = 0; i < nrows; ++i )
 {
 h += ws[i];
 }
 h /= (double)nrows;
 
 // Finally, compute the weights for each vertex
 int nzeros = 0;
 for( int i = 0; i < nrows; ++i )
 {
 double costheta = DGMSolver::vec_innerprod( 3, ngbnrms, ngbnrms + 3 * ( i + interp ) );
 if( costheta > toler )
 {
 ws[i] = costheta * pow( ws[i] / h + epsilon, -1 * (double)degree / 2.0 );
 }
 else
 {
 ws[i] = 0;
 ++nzeros;
 }
 }
 return nzeros;
}
bool HiReconstruction::check_barycentric_coords( const int nws, const double* naturalcoords )
{
 double sum = 0;
 for( int i = 0; i < nws; ++i )
 {
 if( naturalcoords[i] < -_MINEPS )
 {
 return false;
 }
 sum += naturalcoords[i];
 }
 if( fabs( 1 - sum ) > _MINEPS )
 {
 return false;
 }
 else
 {
 return true;
 }
}
} // namespace moab