MOAB: Mesh Oriented datABase  (version 5.3.1)
HiReconstruction.cpp
Go to the documentation of this file.
00001 #include "moab/DiscreteGeometry/HiReconstruction.hpp"
00002 #include "moab/DiscreteGeometry/DGMSolver.hpp"
00003 #include "moab/HalfFacetRep.hpp"
00004 
00005 #ifdef MOAB_HAVE_MPI
00006 #include "moab/ParallelComm.hpp"
00007 #endif
00008 #include "MBTagConventions.hpp"
00009 
00010 #define HIREC_USE_AHF
00011 
00012 #include <cmath>
00013 #include <deque>
00014 #include <iostream>
00015 
00016 namespace moab
00017 {
00018 HiReconstruction::HiReconstruction( Core* impl, ParallelComm* comm, EntityHandle meshIn, int minpnts, bool recwhole )
00019     : mbImpl( impl ), pcomm( comm ), _mesh2rec( meshIn ), _MINPNTS( minpnts )
00020 {
00021     assert( NULL != impl );
00022     ErrorCode error;
00023     _MINEPS      = 1e-12;
00024     _dim         = 0;
00025     _hasfittings = false;
00026     _hasderiv    = false;
00027 #ifdef MOAB_HAVE_MPI
00028     if( !pcomm ) { pcomm = moab::ParallelComm::get_pcomm( mbImpl, 0 ); }
00029 #endif
00030 
00031     error = initialize( recwhole );
00032     if( MB_SUCCESS != error )
00033     {
00034         std::cout << "Error initializing HiReconstruction\n" << std::endl;
00035         exit( 1 );
00036     }
00037 }
00038 
00039 HiReconstruction::~HiReconstruction()
00040 {
00041 #ifdef MOAB_HAVE_AHF
00042     ahf = NULL;
00043 #else
00044     delete ahf;
00045 #endif
00046 }
00047 
00048 ErrorCode HiReconstruction::initialize( bool recwhole )
00049 {
00050     ErrorCode error;
00051 
00052 #ifdef HIREC_USE_AHF
00053     std::cout << "HIREC_USE_AHF: Initializing" << std::endl;
00054     ahf = new HalfFacetRep( mbImpl, pcomm, _mesh2rec, false );
00055     if( !ahf ) { return MB_MEMORY_ALLOCATION_FAILED; }
00056     error = ahf->initialize();MB_CHK_ERR( error );
00057 #else
00058     ahf        = NULL;
00059 #endif
00060 
00061     // error = ahf->get_entity_ranges(_inverts,_inedges,_infaces,_incells); MB_CHK_ERR(error);
00062     error = mbImpl->get_entities_by_dimension( _mesh2rec, 0, _inverts );MB_CHK_ERR( error );
00063     error = mbImpl->get_entities_by_dimension( _mesh2rec, 1, _inedges );MB_CHK_ERR( error );
00064     error = mbImpl->get_entities_by_dimension( _mesh2rec, 2, _infaces );MB_CHK_ERR( error );
00065     error = mbImpl->get_entities_by_dimension( _mesh2rec, 3, _incells );MB_CHK_ERR( error );
00066     if( _inedges.size() && _infaces.empty() && _incells.empty() )
00067     {
00068         _dim     = 1;
00069         _MAXPNTS = 13;
00070     }
00071     else if( _infaces.size() && _incells.empty() )
00072     {
00073         _dim     = 2;
00074         _MAXPNTS = 128;
00075     }
00076     else
00077     {
00078         MB_SET_ERR( MB_FAILURE, "Encountered a non-manifold mesh or a mesh with volume elements" );
00079     }
00080 
00081     // get locally hosted vertices by filtering pstatus
00082 #ifdef MOAB_HAVE_MPI
00083     if( pcomm )
00084     {
00085         error = pcomm->filter_pstatus( _inverts, PSTATUS_GHOST, PSTATUS_NOT, -1, &_verts2rec );MB_CHK_ERR( error );
00086     }
00087     else
00088     {
00089         _verts2rec = _inverts;
00090     }
00091 #else
00092     _verts2rec = _inverts;
00093 #endif
00094     _nv2rec = _verts2rec.size();
00095 
00096     if( recwhole )
00097     {
00098         // compute normals(surface) or tangent vector(curve) for all locally hosted vertices
00099         if( 2 == _dim ) { compute_average_vertex_normals_surf(); }
00100         else if( 1 == _dim )
00101         {
00102             compute_average_vertex_tangents_curve();
00103         }
00104         else
00105         {
00106             MB_SET_ERR( MB_FAILURE, "Unknow space dimension" );
00107         }
00108         _hasderiv = true;
00109     }
00110     return error;
00111 }
00112 
00113 /***************************************************
00114  *  User Interface for Reconstruction of Geometry  *
00115  ***************************************************/
00116 
00117 ErrorCode HiReconstruction::reconstruct3D_surf_geom( int degree, bool interp, bool safeguard, bool reset )
00118 {
00119     assert( 2 == _dim );
00120     if( _hasfittings && !reset )
00121     {
00122         // This object has precomputed fitting results and user don't want to reset
00123         return MB_SUCCESS;
00124     }
00125     else
00126     {
00127         _initfittings = _hasfittings = false;
00128     }
00129     // initialize for geometric information
00130     initialize_surf_geom( degree );
00131     ErrorCode error;
00132     double *coeffs, *coords;
00133     int* degree_out;
00134     int ncoeffs = ( degree + 2 ) * ( degree + 1 ) / 2;
00135 
00136     // DBG
00137     int dcount = 0;
00138 
00139     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert )
00140     {
00141         int index = _verts2rec.index( *ivert );
00142         // for debug
00143         /*if(index==70){
00144             EntityHandle vid = *ivert;
00145             double vertcoords[3];
00146             error = mbImpl->get_coords(&vid,1,vertcoords);
00147         }*/
00148 
00149         size_t istr     = _vertID2coeffID[index];
00150         coords          = &( _local_coords[9 * index] );
00151         coeffs          = &( _local_fit_coeffs[istr] );
00152         degree_out      = &( _degrees_out[index] );
00153         _interps[index] = interp;
00154         error = polyfit3d_walf_surf_vertex( *ivert, interp, degree, _MINPNTS, safeguard, 9, coords, degree_out, ncoeffs,
00155                                             coeffs );MB_CHK_ERR( error );
00156 
00157         // DBG
00158         if( degree_out[0] < degree ) dcount += 1;
00159     }
00160 
00161     // DBG
00162     // std::cout<<"Total #points ="<<_verts2rec.size()<<", #degraded points = "<<dcount<<std::endl;
00163 
00164     _geom        = HISURFACE;
00165     _hasfittings = true;
00166     return error;
00167 }
00168 
00169 ErrorCode HiReconstruction::reconstruct3D_surf_geom( size_t npts, int* degrees, bool* interps, bool safeguard,
00170                                                      bool reset )
00171 {
00172     assert( _dim == 2 );
00173     if( npts != _nv2rec ) { MB_SET_ERR( MB_FAILURE, "Input number of degrees doesn't match number of vertices" ); }
00174     if( _hasfittings && !reset ) { return MB_SUCCESS; }
00175     else
00176     {
00177         _initfittings = _hasfittings = false;
00178     }
00179     ErrorCode error;
00180     // initialization for fitting
00181     initialize_surf_geom( npts, degrees );
00182     double *coeffs, *coords;
00183     int* degree_out;
00184     size_t i = 0;
00185     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++i )
00186     {
00187         int index = _verts2rec.index( *ivert );
00188         assert( -1 != index );
00189         size_t istr     = _vertID2coeffID[index];
00190         coords          = &( _local_coords[9 * index] );
00191         coeffs          = &( _local_fit_coeffs[istr] );
00192         degree_out      = &( _degrees_out[index] );
00193         _interps[index] = interps[i];
00194         int ncoeffs     = ( degrees[i] + 2 ) * ( degrees[i] + 1 ) / 2;
00195         error = polyfit3d_walf_surf_vertex( *ivert, interps[i], degrees[i], _MINPNTS, safeguard, 9, coords, degree_out,
00196                                             ncoeffs, coeffs );MB_CHK_ERR( error );
00197     }
00198     _geom        = HISURFACE;
00199     _hasfittings = true;
00200     return error;
00201 }
00202 
00203 ErrorCode HiReconstruction::reconstruct3D_curve_geom( int degree, bool interp, bool safeguard, bool reset )
00204 {
00205     assert( _dim == 1 );
00206     if( _hasfittings && !reset ) { return MB_SUCCESS; }
00207     else
00208     {
00209         _initfittings = _hasfittings = false;
00210     }
00211     initialize_3Dcurve_geom( degree );
00212     ErrorCode error;
00213     double *coords = 0, *coeffs;
00214     int* degree_out;
00215     int ncoeffs = 3 * ( degree + 1 );
00216     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert )
00217     {
00218         int index = _verts2rec.index( *ivert );
00219         assert( index != -1 );
00220         size_t istr     = _vertID2coeffID[index];
00221         coeffs          = &( _local_fit_coeffs[istr] );
00222         degree_out      = &( _degrees_out[index] );
00223         _interps[index] = interp;
00224         error = polyfit3d_walf_curve_vertex( *ivert, interp, degree, _MINPNTS, safeguard, 0, coords, degree_out,
00225                                              ncoeffs, coeffs );MB_CHK_ERR( error );
00226     }
00227     _geom        = HI3DCURVE;
00228     _hasfittings = true;
00229     return error;
00230 }
00231 
00232 ErrorCode HiReconstruction::reconstruct3D_curve_geom( size_t npts, int* degrees, bool* interps, bool safeguard,
00233                                                       bool reset )
00234 {
00235     assert( _dim == 1 );
00236     ErrorCode error;
00237     if( npts != _nv2rec ) { MB_SET_ERR( MB_FAILURE, "Input number of degrees doesn't match the number of vertices" ); }
00238     if( _hasfittings && !reset ) { return MB_SUCCESS; }
00239     else
00240     {
00241         _initfittings = _hasfittings = false;
00242     }
00243     // initialize
00244     initialize_3Dcurve_geom( npts, degrees );
00245     double *coords = 0, *coeffs;
00246     int* degree_out;
00247     size_t i = 0;
00248     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++i )
00249     {
00250         int index       = _verts2rec.index( *ivert );
00251         size_t istr     = _vertID2coeffID[index];
00252         coeffs          = &( _local_fit_coeffs[istr] );
00253         degree_out      = &( _degrees_out[index] );
00254         _interps[index] = interps[i];
00255         int ncoeffs     = 3 * ( degrees[i] + 1 );
00256         error = polyfit3d_walf_curve_vertex( *ivert, interps[i], degrees[i], _MINPNTS, safeguard, 0, coords, degree_out,
00257                                              ncoeffs, coeffs );MB_CHK_ERR( error );
00258     }
00259     _geom        = HI3DCURVE;
00260     _hasfittings = true;
00261     return error;
00262 }
00263 
00264 ErrorCode HiReconstruction::polyfit3d_walf_surf_vertex( const EntityHandle vid, const bool interp, int degree,
00265                                                         int minpnts, const bool safeguard, const int ncoords,
00266                                                         double* coords, int* degree_out, const int ncoeffs,
00267                                                         double* coeffs )
00268 {
00269     assert( _dim == 2 );
00270     ErrorCode error;
00271     int ring = estimate_num_rings( degree, interp );
00272     // std::cout<<"ring = "<<ring<<std::endl;
00273     // get n-ring neighbors
00274     Range ngbvs;
00275     error = obtain_nring_ngbvs( vid, ring, minpnts, ngbvs );MB_CHK_ERR( error );
00276     // for debug
00277     /*if(_verts2rec.index(vid)==70){
00278         for(Range::iterator ingb=ngbvs.begin();ingb!=ngbvs.end();++ingb) std::cerr <<
00279     _verts2rec.index(*ingb) << " "; std::cout << std::endl;
00280     }*/
00281 
00282     // get coordinates;
00283     size_t nverts = ngbvs.size();
00284     assert( nverts );
00285     double* ngbcoords = new double[nverts * 3];
00286     error             = mbImpl->get_coords( ngbvs, ngbcoords );MB_CHK_ERR( error );
00287     // get normals
00288     double* ngbnrms = new double[nverts * 3];
00289     error           = get_normals_surf( ngbvs, ngbnrms );MB_CHK_ERR( error );
00290     // switch vid to first one
00291     int index = ngbvs.index( vid );
00292     assert( index != -1 );
00293     std::swap( ngbcoords[0], ngbcoords[3 * index] );
00294     std::swap( ngbcoords[1], ngbcoords[3 * index + 1] );
00295     std::swap( ngbcoords[2], ngbcoords[3 * index + 2] );
00296     std::swap( ngbnrms[0], ngbnrms[3 * index] );
00297     std::swap( ngbnrms[1], ngbnrms[3 * index + 1] );
00298     std::swap( ngbnrms[2], ngbnrms[3 * index + 2] );
00299     // local WLS fitting
00300     int degree_pnt, degree_qr;
00301     polyfit3d_surf_get_coeff( nverts, ngbcoords, ngbnrms, degree, interp, safeguard, ncoords, coords, ncoeffs, coeffs,
00302                               degree_out, &degree_pnt, &degree_qr );
00303     delete[] ngbcoords;
00304     delete[] ngbnrms;
00305     return error;
00306 }
00307 
00308 ErrorCode HiReconstruction::polyfit3d_walf_curve_vertex( const EntityHandle vid, const bool interp, int degree,
00309                                                          int minpnts, const bool safeguard, const int ncoords,
00310                                                          double* coords, int* degree_out, const int ncoeffs,
00311                                                          double* coeffs )
00312 {
00313     ErrorCode error;
00314     int ring = estimate_num_rings( degree, interp );
00315     // get n-ring neighbors
00316     Range ngbvs;
00317     error = obtain_nring_ngbvs( vid, ring, minpnts, ngbvs );MB_CHK_ERR( error );
00318     // get coordinates
00319     size_t nverts = ngbvs.size();
00320     assert( nverts );
00321     double* ngbcoords = new double[nverts * 3];
00322     error             = mbImpl->get_coords( ngbvs, ngbcoords );MB_CHK_ERR( error );
00323     // get tangent vectors
00324     double* ngbtangs = new double[nverts * 3];
00325     error            = get_tangents_curve( ngbvs, ngbtangs );MB_CHK_ERR( error );
00326     // switch vid to first one
00327     int index = ngbvs.index( vid );
00328     assert( index != -1 );
00329     std::swap( ngbcoords[0], ngbcoords[3 * index] );
00330     std::swap( ngbcoords[1], ngbcoords[3 * index + 1] );
00331     std::swap( ngbcoords[2], ngbcoords[3 * index + 2] );
00332     std::swap( ngbtangs[0], ngbtangs[3 * index] );
00333     std::swap( ngbtangs[1], ngbtangs[3 * index + 1] );
00334     std::swap( ngbtangs[2], ngbtangs[3 * index + 2] );
00335     // local WLS fittings
00336     polyfit3d_curve_get_coeff( nverts, ngbcoords, ngbtangs, degree, interp, safeguard, ncoords, coords, ncoeffs, coeffs,
00337                                degree_out );
00338     delete[] ngbcoords;
00339     delete[] ngbtangs;
00340     return error;
00341 }
00342 
00343 /**************************************************************
00344  *  User Interface for Evaluation via Reconstructed Geometry  *
00345  **************************************************************/
00346 
00347 ErrorCode HiReconstruction::hiproj_walf_in_element( EntityHandle elem, const int nvpe, const int npts2fit,
00348                                                     const double* naturalcoords2fit, double* newcoords )
00349 {
00350     assert( newcoords );
00351     ErrorCode error;
00352     // get connectivity table
00353     std::vector< EntityHandle > elemconn;
00354     error = mbImpl->get_connectivity( &elem, 1, elemconn );MB_CHK_ERR( error );
00355     if( nvpe != (int)elemconn.size() )
00356     {
00357         MB_SET_ERR( MB_FAILURE, "element connectivity table size doesn't match input size" );
00358     }
00359 
00360     if( !_hasfittings ) { MB_SET_ERR( MB_FAILURE, "There is no existing fitting results" ); }
00361     else
00362     {
00363         std::ostringstream convert;
00364         convert << elem;
00365         std::string ID = convert.str();
00366         for( int i = 0; i < nvpe; ++i )
00367         {
00368             if( -1 == _verts2rec.index( elemconn[i] ) )
00369             {
00370                 MB_SET_ERR( MB_FAILURE, "There is no existing fitting results for element " + ID );
00371             }
00372         }
00373     }
00374     // check correctness of input
00375     for( int i = 0; i < npts2fit; ++i )
00376     {
00377         if( !check_barycentric_coords( nvpe, naturalcoords2fit + i * nvpe ) )
00378         {
00379             MB_SET_ERR( MB_FAILURE, "Wrong barycentric coordinates" );
00380         }
00381     }
00382 
00383     double* elemcoords = new double[nvpe * 3];
00384     error              = mbImpl->get_coords( &( elemconn[0] ), nvpe, elemcoords );MB_CHK_ERR( error );
00385 
00386     double* coords2fit = new double[3 * npts2fit]();
00387     for( int i = 0; i < npts2fit; ++i )
00388     {
00389         for( int j = 0; j < nvpe; ++j )
00390         {
00391             coords2fit[3 * i] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j];
00392             coords2fit[3 * i + 1] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j + 1];
00393             coords2fit[3 * i + 2] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j + 2];
00394         }
00395     }
00396 
00397     double* hiproj_new = new double[3 * npts2fit];
00398     // initialize output
00399     for( int i = 0; i < npts2fit; ++i )
00400     {
00401         newcoords[3 * i] = newcoords[3 * i + 1] = newcoords[3 * i + 2] = 0;
00402     }
00403     // for each input vertex, call nvpe fittings and take average
00404     for( int j = 0; j < nvpe; ++j )
00405     {
00406         error = hiproj_walf_around_vertex( elemconn[j], npts2fit, coords2fit, hiproj_new );MB_CHK_ERR( error );
00407         for( int i = 0; i < npts2fit; ++i )
00408         {
00409             newcoords[3 * i] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i];
00410             newcoords[3 * i + 1] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i + 1];
00411             newcoords[3 * i + 2] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i + 2];
00412         }
00413     }
00414     delete[] elemcoords;
00415     delete[] coords2fit;
00416     delete[] hiproj_new;
00417     return error;
00418 }
00419 
00420 ErrorCode HiReconstruction::hiproj_walf_around_vertex( EntityHandle vid, const int npts2fit, const double* coords2fit,
00421                                                        double* hiproj_new )
00422 {
00423     if( !_hasfittings ) { MB_SET_ERR( MB_FAILURE, "There is no existing fitting results" ); }
00424     else if( -1 == _verts2rec.index( vid ) )
00425     {
00426         std::ostringstream convert;
00427         convert << vid;
00428         std::string VID = convert.str();
00429         MB_SET_ERR( MB_FAILURE, "There is no existing fitting results for vertex " + VID );
00430     }
00431     ErrorCode error;
00432     // get center of local coordinates system
00433     double local_origin[3];
00434     error = mbImpl->get_coords( &vid, 1, local_origin );MB_CHK_ERR( error );
00435     // get local fitting parameters
00436     int index     = _verts2rec.index( vid );
00437     bool interp   = _interps[index];
00438     int local_deg = _degrees_out[index];
00439     double *uvw_coords, *local_coeffs;
00440     if( _geom == HISURFACE )
00441     {
00442         uvw_coords = &( _local_coords[9 * index] );
00443         // int ncoeffs = (local_deg+2)*(local_deg+1)>>1;
00444         size_t istr  = _vertID2coeffID[index];
00445         local_coeffs = &( _local_fit_coeffs[istr] );
00446         walf3d_surf_vertex_eval( local_origin, uvw_coords, local_deg, local_coeffs, interp, npts2fit, coords2fit,
00447                                  hiproj_new );
00448     }
00449     else if( _geom == HI3DCURVE )
00450     {
00451         uvw_coords   = &( _local_coords[3 * index] );
00452         size_t istr  = _vertID2coeffID[index];
00453         local_coeffs = &( _local_fit_coeffs[istr] );
00454         walf3d_curve_vertex_eval( local_origin, uvw_coords, local_deg, local_coeffs, interp, npts2fit, coords2fit,
00455                                   hiproj_new );
00456     }
00457     return error;
00458 }
00459 
00460 void HiReconstruction::walf3d_surf_vertex_eval( const double* local_origin, const double* local_coords,
00461                                                 const int local_deg, const double* local_coeffs, const bool interp,
00462                                                 const int npts2fit, const double* coords2fit, double* hiproj_new )
00463 {
00464     double xaxis[3], yaxis[3], zaxis[3];
00465     for( int i = 0; i < 3; ++i )
00466     {
00467         xaxis[i] = local_coords[i];
00468         yaxis[i] = local_coords[3 + i];
00469         zaxis[i] = local_coords[6 + i];
00470     }
00471     // double *basis = new double[(local_deg+2)*(local_deg+1)/2-1];
00472     std::vector< double > basis( ( local_deg + 2 ) * ( local_deg + 1 ) / 2 - 1 );
00473     for( int i = 0; i < npts2fit; ++i )
00474     {
00475         double local_pos[3];
00476         for( int j = 0; j < 3; ++j )
00477         {
00478             local_pos[j] = coords2fit[3 * i + j] - local_origin[j];
00479         }
00480         double u, v, height = 0;
00481         u        = DGMSolver::vec_innerprod( 3, local_pos, xaxis );
00482         v        = DGMSolver::vec_innerprod( 3, local_pos, yaxis );
00483         basis[0] = u;
00484         basis[1] = v;
00485         int l    = 1;
00486         for( int k = 2; k <= local_deg; ++k )
00487         {
00488             ++l;
00489             basis[l] = u * basis[l - k];
00490             for( int id = 0; id < k; ++id )
00491             {
00492                 ++l;
00493                 basis[l] = basis[l - k - 1] * v;
00494             }
00495         }
00496         if( !interp ) { height = local_coeffs[0]; }
00497         for( int p = 0; p <= l; ++p )
00498         {
00499             height += local_coeffs[p + 1] * basis[p];
00500         }
00501         hiproj_new[3 * i]     = local_origin[0] + u * xaxis[0] + v * yaxis[0] + height * zaxis[0];
00502         hiproj_new[3 * i + 1] = local_origin[1] + u * xaxis[1] + v * yaxis[1] + height * zaxis[1];
00503         hiproj_new[3 * i + 2] = local_origin[2] + u * xaxis[2] + v * yaxis[2] + height * zaxis[2];
00504     }
00505     // delete [] basis;
00506 }
00507 
00508 void HiReconstruction::walf3d_curve_vertex_eval( const double* local_origin, const double* local_coords,
00509                                                  const int local_deg, const double* local_coeffs, const bool interp,
00510                                                  const int npts2fit, const double* coords2fit, double* hiproj_new )
00511 {
00512     assert( local_origin && local_coords && local_coeffs );
00513     int ncoeffspvpd = local_deg + 1;
00514     for( int i = 0; i < npts2fit; ++i )
00515     {
00516         // get the vector from center to current point, project to tangent line
00517         double vec[3], ans[3] = { 0, 0, 0 };
00518         DGMSolver::vec_linear_operation( 3, 1, coords2fit + 3 * i, -1, local_origin, vec );
00519         double u = DGMSolver::vec_innerprod( 3, local_coords, vec );
00520         // evaluate polynomials
00521         if( !interp )
00522         {
00523             ans[0] = local_coeffs[0];
00524             ans[1] = local_coeffs[ncoeffspvpd];
00525             ans[2] = local_coeffs[2 * ncoeffspvpd];
00526         }
00527         double uk = 1;  // degree_out and degree different, stored in columnwise contiguously
00528         for( int j = 1; j < ncoeffspvpd; ++j )
00529         {
00530             uk *= u;
00531             ans[0] += uk * local_coeffs[j];
00532             ans[1] += uk * local_coeffs[j + ncoeffspvpd];
00533             ans[2] += uk * local_coeffs[j + 2 * ncoeffspvpd];
00534         }
00535         hiproj_new[3 * i]     = ans[0] + local_origin[0];
00536         hiproj_new[3 * i + 1] = ans[1] + local_origin[1];
00537         hiproj_new[3 * i + 2] = ans[2] + local_origin[2];
00538     }
00539 }
00540 
00541 bool HiReconstruction::get_fittings_data( EntityHandle vid, GEOMTYPE& geomtype, std::vector< double >& coords,
00542                                           int& degree_out, std::vector< double >& coeffs, bool& interp )
00543 {
00544     if( !_hasfittings ) { return false; }
00545     else
00546     {
00547         int index = _verts2rec.index( vid );
00548         if( -1 == index )
00549         {
00550             std::cout << "Input vertex is not locally hosted vertex in this mesh set" << std::endl;
00551             return false;
00552         }
00553         geomtype = _geom;
00554         if( HISURFACE == _geom )
00555         {
00556             coords.insert( coords.end(), _local_coords.begin() + 9 * index, _local_coords.begin() + 9 * index + 9 );
00557             degree_out  = _degrees_out[index];
00558             interp      = _interps[index];
00559             int ncoeffs = ( degree_out + 2 ) * ( degree_out + 1 ) >> 1;
00560             size_t istr = _vertID2coeffID[index];
00561             coeffs.insert( coeffs.end(), _local_fit_coeffs.begin() + istr, _local_fit_coeffs.begin() + istr + ncoeffs );
00562         }
00563         else if( HI3DCURVE == _geom )
00564         {
00565             coords.insert( coords.end(), _local_coords.begin() + 3 * index, _local_coords.begin() + 3 * index + 3 );
00566             degree_out  = _degrees_out[index];
00567             interp      = _interps[index];
00568             int ncoeffs = 3 * ( degree_out + 1 );
00569             size_t istr = _vertID2coeffID[index];
00570             coeffs.insert( coeffs.end(), _local_fit_coeffs.begin() + istr, _local_fit_coeffs.begin() + istr + ncoeffs );
00571         }
00572         return true;
00573     }
00574 }
00575 
00576 /****************************************************************
00577  *  Basic Internal Routines to initialize and set fitting data  *
00578  ****************************************************************/
00579 
00580 int HiReconstruction::estimate_num_rings( int degree, bool interp )
00581 {
00582     return interp ? ( ( degree + 1 ) >> 1 ) + ( ( degree + 1 ) & 1 ) : ( ( degree + 2 ) >> 1 ) + ( ( degree + 2 ) & 1 );
00583 }
00584 
00585 ErrorCode HiReconstruction::vertex_get_incident_elements( const EntityHandle& vid, const int elemdim,
00586                                                           std::vector< EntityHandle >& adjents )
00587 {
00588     ErrorCode error;
00589     assert( elemdim == _dim );
00590 #ifdef HIREC_USE_AHF
00591     error = ahf->get_up_adjacencies( vid, elemdim, adjents );MB_CHK_ERR( error );
00592 #else
00593     error      = mbImpl->get_adjacencies( &vid, 1, elemdim, false, adjents );MB_CHK_ERR( error );
00594 #endif
00595     return error;
00596 }
00597 
00598 ErrorCode HiReconstruction::obtain_nring_ngbvs( const EntityHandle vid, int ring, const int minpnts, Range& ngbvs )
00599 {
00600     ErrorCode error;
00601     std::deque< EntityHandle > todo;
00602     todo.push_back( vid );
00603     ngbvs.insert( vid );
00604     EntityHandle pre, nxt;
00605     for( int i = 1; i <= ring; ++i )
00606     {
00607         int count = todo.size();
00608         while( count )
00609         {
00610             EntityHandle center = todo.front();
00611             todo.pop_front();
00612             --count;
00613             std::vector< EntityHandle > adjents;
00614             error = vertex_get_incident_elements( center, _dim, adjents );MB_CHK_ERR( error );
00615             for( size_t j = 0; j < adjents.size(); ++j )
00616             {
00617                 std::vector< EntityHandle > elemconn;
00618                 error = mbImpl->get_connectivity( &adjents[j], 1, elemconn );MB_CHK_ERR( error );
00619                 int nvpe = elemconn.size();
00620                 for( int k = 0; k < nvpe; ++k )
00621                 {
00622                     if( elemconn[k] == center )
00623                     {
00624                         pre = k == 0 ? elemconn[nvpe - 1] : elemconn[k - 1];
00625                         nxt = elemconn[( k + 1 ) % nvpe];
00626                         if( ngbvs.find( pre ) == ngbvs.end() )
00627                         {
00628                             ngbvs.insert( pre );
00629                             todo.push_back( pre );
00630                         }
00631                         if( ngbvs.find( nxt ) == ngbvs.end() )
00632                         {
00633                             ngbvs.insert( nxt );
00634                             todo.push_back( nxt );
00635                         }
00636                         break;
00637                     }
00638                 }
00639             }
00640         }
00641         if( _MAXPNTS <= (int)ngbvs.size() )
00642         {
00643             // obtain enough points
00644             return error;
00645         }
00646         if( !todo.size() )
00647         {
00648             // current ring cannot introduce any points, return incase deadlock
00649             return error;
00650         }
00651         if( ( i == ring ) && ( minpnts > (int)ngbvs.size() ) )
00652         {
00653             // reach maximum ring but not enough points
00654             ++ring;
00655         }
00656     }
00657     return error;
00658 }
00659 
00660 void HiReconstruction::initialize_surf_geom( const int degree )
00661 {
00662     if( !_hasderiv )
00663     {
00664         compute_average_vertex_normals_surf();
00665         _hasderiv = true;
00666     }
00667     if( !_initfittings )
00668     {
00669         int ncoeffspv = ( degree + 2 ) * ( degree + 1 ) / 2;
00670         _degrees_out.assign( _nv2rec, 0 );
00671         _interps.assign( _nv2rec, false );
00672         _vertID2coeffID.resize( _nv2rec );
00673         _local_fit_coeffs.assign( _nv2rec * ncoeffspv, 0 );
00674         for( size_t i = 0; i < _nv2rec; ++i )
00675         {
00676             _vertID2coeffID[i] = i * ncoeffspv;
00677         }
00678         _initfittings = true;
00679     }
00680 }
00681 
00682 void HiReconstruction::initialize_surf_geom( const size_t npts, const int* degrees )
00683 {
00684     if( !_hasderiv )
00685     {
00686         compute_average_vertex_normals_surf();
00687         _hasderiv = true;
00688     }
00689     if( !_initfittings )
00690     {
00691         assert( _nv2rec == npts );
00692         _degrees_out.assign( _nv2rec, 0 );
00693         _interps.assign( _nv2rec, false );
00694         _vertID2coeffID.resize( _nv2rec );
00695         size_t index = 0;
00696         for( size_t i = 0; i < _nv2rec; ++i )
00697         {
00698             _vertID2coeffID[i] = index;
00699             index += ( degrees[i] + 2 ) * ( degrees[i] + 1 ) / 2;
00700         }
00701         _local_fit_coeffs.assign( index, 0 );
00702         _initfittings = true;
00703     }
00704 }
00705 
00706 void HiReconstruction::initialize_3Dcurve_geom( const int degree )
00707 {
00708     if( !_hasderiv )
00709     {
00710         compute_average_vertex_tangents_curve();
00711         _hasderiv = true;
00712     }
00713     if( !_initfittings )
00714     {
00715         int ncoeffspvpd = degree + 1;
00716         _degrees_out.assign( _nv2rec, 0 );
00717         _interps.assign( _nv2rec, false );
00718         _vertID2coeffID.resize( _nv2rec );
00719         _local_fit_coeffs.assign( _nv2rec * ncoeffspvpd * 3, 0 );
00720         for( size_t i = 0; i < _nv2rec; ++i )
00721         {
00722             _vertID2coeffID[i] = i * ncoeffspvpd * 3;
00723         }
00724         _initfittings = true;
00725     }
00726 }
00727 
00728 void HiReconstruction::initialize_3Dcurve_geom( const size_t npts, const int* degrees )
00729 {
00730     if( !_hasderiv )
00731     {
00732         compute_average_vertex_tangents_curve();
00733         _hasderiv = true;
00734     }
00735     if( !_hasfittings )
00736     {
00737         assert( _nv2rec == npts );
00738         _degrees_out.assign( _nv2rec, 0 );
00739         _interps.assign( _nv2rec, false );
00740         _vertID2coeffID.reserve( _nv2rec );
00741         size_t index = 0;
00742         for( size_t i = 0; i < _nv2rec; ++i )
00743         {
00744             _vertID2coeffID[i] = index;
00745             index += 3 * ( degrees[i] + 1 );
00746         }
00747         _local_fit_coeffs.assign( index, 0 );
00748         _initfittings = true;
00749     }
00750 }
00751 
00752 /* ErrorCode HiReconstruction::set_geom_data_surf(const EntityHandle vid, const double* coords,
00753  const double degree_out, const double* coeffs, bool interp)
00754  {
00755    return MB_SUCCESS;
00756  }
00757 
00758  ErrorCode HiReconstruction::set_geom_data_3Dcurve(const EntityHandle vid, const double* coords,
00759  const double degree_out, const double* coeffs, bool interp)
00760  {
00761    return MB_SUCCESS;
00762  } */
00763 
00764 /*********************************************************
00765  * Routines for vertex normal/tangent vector estimation *
00766  *********************************************************/
00767 ErrorCode HiReconstruction::average_vertex_normal( const EntityHandle vid, double* nrm )
00768 {
00769     ErrorCode error;
00770     std::vector< EntityHandle > adjfaces;
00771     error = vertex_get_incident_elements( vid, 2, adjfaces );MB_CHK_ERR( error );
00772     int npolys = adjfaces.size();
00773     if( !npolys ) { MB_SET_ERR( MB_FAILURE, "Vertex has no incident 2D entities" ); }
00774     else
00775     {
00776         double v1[3], v2[3], v3[3], a[3], b[3], c[3];
00777         nrm[0] = nrm[1] = nrm[2] = 0;
00778         for( int i = 0; i < npolys; ++i )
00779         {
00780             // get incident "triangles"
00781             std::vector< EntityHandle > elemconn;
00782             error = mbImpl->get_connectivity( &adjfaces[i], 1, elemconn );MB_CHK_ERR( error );
00783             EntityHandle pre, nxt;
00784             int nvpe = elemconn.size();
00785             for( int j = 0; j < nvpe; ++j )
00786             {
00787                 if( vid == elemconn[j] )
00788                 {
00789                     pre = j == 0 ? elemconn[nvpe - 1] : elemconn[j - 1];
00790                     nxt = elemconn[( j + 1 ) % nvpe];
00791                     break;
00792                 }
00793             }
00794             // compute area weighted normals
00795             error = mbImpl->get_coords( &pre, 1, a );MB_CHK_ERR( error );
00796             error = mbImpl->get_coords( &vid, 1, b );MB_CHK_ERR( error );
00797             error = mbImpl->get_coords( &nxt, 1, c );MB_CHK_ERR( error );
00798             DGMSolver::vec_linear_operation( 3, 1, c, -1, b, v1 );
00799             DGMSolver::vec_linear_operation( 3, 1, a, -1, b, v2 );
00800             DGMSolver::vec_crossprod( v1, v2, v3 );
00801             DGMSolver::vec_linear_operation( 3, 1, nrm, 1, v3, nrm );
00802         }
00803 #ifndef NDEBUG
00804         assert( DGMSolver::vec_normalize( 3, nrm, nrm ) );
00805 #endif
00806     }
00807     return error;
00808 }
00809 
00810 ErrorCode HiReconstruction::compute_average_vertex_normals_surf()
00811 {
00812     if( _hasderiv ) { return MB_SUCCESS; }
00813     ErrorCode error;
00814     _local_coords.assign( 9 * _nv2rec, 0 );
00815     size_t index = 0;
00816     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++index )
00817     {
00818         error = average_vertex_normal( *ivert, &( _local_coords[9 * index + 6] ) );MB_CHK_ERR( error );
00819     }
00820     return error;
00821 }
00822 
00823 ErrorCode HiReconstruction::get_normals_surf( const Range& vertsh, double* nrms )
00824 {
00825     ErrorCode error = MB_SUCCESS;
00826     if( _hasderiv )
00827     {
00828         size_t id = 0;
00829         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00830         {
00831             int index = _verts2rec.index( *ivert );
00832 #ifdef MOAB_HAVE_MPI
00833             if( -1 == index )
00834             {
00835                 // ghost vertex
00836                 error = average_vertex_normal( *ivert, nrms + 3 * id );MB_CHK_ERR( error );
00837             }
00838             else
00839             {
00840                 nrms[3 * id]     = _local_coords[9 * index + 6];
00841                 nrms[3 * id + 1] = _local_coords[9 * index + 7];
00842                 nrms[3 * id + 2] = _local_coords[9 * index + 8];
00843             }
00844 #else
00845             assert( -1 != index );
00846             nrms[3 * id]     = _local_coords[9 * index + 6];
00847             nrms[3 * id + 1] = _local_coords[9 * index + 7];
00848             nrms[3 * id + 2] = _local_coords[9 * index + 8];
00849 #endif
00850         }
00851     }
00852     else
00853     {
00854         size_t id = 0;
00855         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00856         {
00857             error = average_vertex_normal( *ivert, nrms + 3 * id );MB_CHK_ERR( error );
00858         }
00859     }
00860     return error;
00861 }
00862 
00863 ErrorCode HiReconstruction::average_vertex_tangent( const EntityHandle vid, double* tang )
00864 {
00865     ErrorCode error;
00866     std::vector< EntityHandle > adjedges;
00867     error = vertex_get_incident_elements( vid, 1, adjedges );MB_CHK_ERR( error );
00868     int nedges = adjedges.size();
00869     if( !nedges ) { MB_SET_ERR( MB_FAILURE, "Vertex has no incident edges" ); }
00870     else
00871     {
00872         assert( nedges <= 2 );
00873         tang[0] = tang[1] = tang[2] = 0;
00874         for( int i = 0; i < nedges; ++i )
00875         {
00876             std::vector< EntityHandle > edgeconn;
00877             error = mbImpl->get_connectivity( &adjedges[i], 1, edgeconn );
00878             double istr[3], iend[3], t[3];
00879             error = mbImpl->get_coords( &( edgeconn[0] ), 1, istr );
00880             error = mbImpl->get_coords( &( edgeconn[1] ), 1, iend );
00881             DGMSolver::vec_linear_operation( 3, 1, iend, -1, istr, t );
00882             DGMSolver::vec_linear_operation( 3, 1, tang, 1, t, tang );
00883         }
00884 #ifndef NDEBUG
00885         assert( DGMSolver::vec_normalize( 3, tang, tang ) );
00886 #endif
00887     }
00888     return error;
00889 }
00890 
00891 ErrorCode HiReconstruction::compute_average_vertex_tangents_curve()
00892 {
00893     if( _hasderiv ) { return MB_SUCCESS; }
00894     ErrorCode error;
00895     _local_coords.assign( 3 * _nv2rec, 0 );
00896     size_t index = 0;
00897     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++index )
00898     {
00899         error = average_vertex_tangent( *ivert, &( _local_coords[3 * index] ) );MB_CHK_ERR( error );
00900     }
00901     return error;
00902 }
00903 
00904 ErrorCode HiReconstruction::get_tangents_curve( const Range& vertsh, double* tangs )
00905 {
00906     ErrorCode error = MB_SUCCESS;
00907     if( _hasderiv )
00908     {
00909         size_t id = 0;
00910         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00911         {
00912             int index = _verts2rec.index( *ivert );
00913 #ifdef MOAB_HAVE_MPI
00914             if( -1 != index )
00915             {
00916                 tangs[3 * id]     = _local_coords[3 * index];
00917                 tangs[3 * id + 1] = _local_coords[3 * index + 1];
00918                 tangs[3 * id + 2] = _local_coords[3 * index + 2];
00919             }
00920             else
00921             {
00922                 error = average_vertex_tangent( *ivert, tangs + 3 * id );MB_CHK_ERR( error );
00923             }
00924 #else
00925             assert( -1 != index );
00926             tangs[3 * id]     = _local_coords[3 * index];
00927             tangs[3 * id + 1] = _local_coords[3 * index + 1];
00928             tangs[3 * id + 2] = _local_coords[3 * index + 2];
00929 #endif
00930         }
00931     }
00932     else
00933     {
00934         size_t id = 0;
00935         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00936         {
00937             error = average_vertex_tangent( *ivert, tangs + 3 * id );MB_CHK_ERR( error );
00938         }
00939     }
00940     return error;
00941 }
00942 
00943 /************************************************
00944  *  Internal Routines for local WLS fittings    *
00945  *************************************************/
00946 
00947 void HiReconstruction::polyfit3d_surf_get_coeff( const int nverts, const double* ngbcoords, const double* ngbnrms,
00948                                                  int degree, const bool interp, const bool safeguard, const int ncoords,
00949                                                  double* coords, const int ncoeffs, double* coeffs, int* degree_out,
00950                                                  int* degree_pnt, int* degree_qr )
00951 {
00952     if( nverts <= 0 ) { return; }
00953 
00954     // std::cout << "npnts in initial stencil = " << nverts << std::endl;
00955     // std::cout << "centered at (" << ngbcoords[0] << "," << ngbcoords[1] << "," << ngbcoords[2] <<
00956     // ")" << std::endl;
00957 
00958     // step 1. copmute local coordinate system
00959     double nrm[3] = { ngbnrms[0], ngbnrms[1], ngbnrms[2] }, tang1[3] = { 0, 0, 0 }, tang2[3] = { 0, 0, 0 };
00960     if( fabs( nrm[0] ) > fabs( nrm[1] ) && fabs( nrm[0] ) > fabs( nrm[2] ) ) { tang1[1] = 1.0; }
00961     else
00962     {
00963         tang1[0] = 1.0;
00964     }
00965 
00966     DGMSolver::vec_projoff( 3, tang1, nrm, tang1 );
00967 #ifndef NDEBUG
00968     assert( DGMSolver::vec_normalize( 3, tang1, tang1 ) );
00969 #endif
00970     DGMSolver::vec_crossprod( nrm, tang1, tang2 );
00971     if( 9 <= ncoords && coords )
00972     {
00973         coords[0] = tang1[0];
00974         coords[1] = tang1[1];
00975         coords[2] = tang1[2];
00976         coords[3] = tang2[0];
00977         coords[4] = tang2[1];
00978         coords[5] = tang2[2];
00979         coords[6] = nrm[0];
00980         coords[7] = nrm[1];
00981         coords[8] = nrm[2];
00982     }
00983     if( !ncoeffs || !coeffs ) { return; }
00984     else
00985     {
00986         assert( ncoeffs >= ( degree + 2 ) * ( degree + 1 ) / 2 );
00987     }
00988 
00989     // step 2. project onto local coordinates system
00990     int npts2fit = nverts - interp;
00991     if( 0 == npts2fit )
00992     {
00993         *degree_out = *degree_pnt = *degree_qr = 0;
00994         for( int i = 0; i < ncoeffs; ++i )
00995         {
00996             coeffs[i] = 0;
00997         }
00998         // coeffs[0] = 0;
00999         return;
01000     }
01001     std::vector< double > us( npts2fit * 2 );  // double *us = new double[npts2fit*2];
01002     std::vector< double > bs( npts2fit );      // double *bs = new double[npts2fit];
01003     for( int i = interp; i < nverts; ++i )
01004     {
01005         int k = i - interp;
01006         double uu[3];
01007         DGMSolver::vec_linear_operation( 3, 1, ngbcoords + 3 * i, -1, ngbcoords, uu );
01008         us[k * 2]     = DGMSolver::vec_innerprod( 3, tang1, uu );
01009         us[k * 2 + 1] = DGMSolver::vec_innerprod( 3, tang2, uu );
01010         bs[k]         = DGMSolver::vec_innerprod( 3, nrm, uu );
01011     }
01012 
01013     // step 3. compute weights
01014     std::vector< double > ws( npts2fit );  // double *ws = new double[npts2fit];
01015     int nzeros = compute_weights( npts2fit, 2, &( us[0] ), nverts, ngbnrms, degree, _MINEPS, &( ws[0] ) );
01016 
01017     // step 4. adjust according to zero-weights
01018     if( nzeros )
01019     {
01020         if( nzeros == npts2fit )
01021         {
01022             *degree_out = *degree_pnt = *degree_qr = 0;
01023             for( int i = 0; i < ncoeffs; ++i )
01024             {
01025                 coeffs[i] = 0;
01026             }
01027             // coeffs[0] = 0;
01028             return;
01029         }
01030         int index = 0;
01031         for( int i = 0; i < npts2fit; ++i )
01032         {
01033             if( ws[i] )
01034             {
01035                 if( i > index )
01036                 {
01037                     us[index * 2]     = us[i * 2];
01038                     us[index * 2 + 1] = us[i * 2 + 1];
01039                     bs[index]         = bs[i];
01040                     ws[index]         = ws[i];
01041                 }
01042                 ++index;
01043             }
01044         }
01045         npts2fit -= nzeros;
01046         assert( index == npts2fit );
01047         us.resize( npts2fit * 2 );
01048         bs.resize( npts2fit );
01049         ws.resize( npts2fit );
01050         /*us = realloc(us,npts2fit*2*sizeof(double));
01051         bs = realloc(bs,npts2fit*sizeof(double));
01052         ws = realloc(ws,npts2fit*sizeof(double));*/
01053     }
01054 
01055     // std::cout<<"npnts after weighting = "<<npts2fit<<std::endl;
01056 
01057     // step 5. fitting
01058     eval_vander_bivar_cmf( npts2fit, &( us[0] ), 1, &( bs[0] ), degree, &( ws[0] ), interp, safeguard, degree_out,
01059                            degree_pnt, degree_qr );
01060 
01061     // step 6. organize output
01062     int ncoeffs_out = ( *degree_out + 2 ) * ( *degree_out + 1 ) / 2;
01063     assert( ncoeffs_out <= ncoeffs );
01064     coeffs[0] = 0;
01065     for( int j = 0; j < ncoeffs_out - interp; ++j )
01066     {
01067         coeffs[j + interp] = bs[j];
01068     }
01069     // delete [] us; delete [] bs; delete [] ws;
01070 }
01071 
01072 void HiReconstruction::eval_vander_bivar_cmf( const int npts2fit, const double* us, const int ndim, double* bs,
01073                                               int degree, const double* ws, const bool interp, const bool safeguard,
01074                                               int* degree_out, int* degree_pnt, int* degree_qr )
01075 {
01076     // step 1. adjust the degree according to number of points to fit
01077     int ncols = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
01078     while( 1 < degree && npts2fit < ncols )
01079     {
01080         --degree;
01081         ncols = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
01082     }
01083     *degree_pnt = degree;
01084 
01085     // std::cout << "degree_pnt: " << *degree_pnt << std::endl;
01086 
01087     // step 2. construct Vandermonde matrix, stored in columnwise
01088     std::vector< double > V;  // V(npts2fit*(ncols+interp)); //double *V_init = new double[npts2fit*(ncols+interp)];
01089     DGMSolver::gen_vander_multivar( npts2fit, 2, us, degree, V );
01090     // remove the first column of 1s if interpolation
01091     if( interp ) { V.erase( V.begin(), V.begin() + npts2fit ); }
01092     /*double* V;
01093     if(interp){
01094         V = new double[npts2fit*ncols];
01095         std::memcpy(V,V_init+npts2fit,ncols*npts2fit*sizeof(double));
01096         delete [] V_init; V_init = 0;
01097     }else{
01098         V = V_init;
01099     }*/
01100 
01101     // step 3. Scale rows to assign different weights to different points
01102     for( int i = 0; i < npts2fit; ++i )
01103     {
01104         for( int j = 0; j < ncols; ++j )
01105         {
01106             V[j * npts2fit + i] *= ws[i];
01107         }
01108         for( int k = 0; k < ndim; ++k )
01109         {
01110             bs[k * npts2fit + i] *= ws[i];
01111         }
01112     }
01113 
01114     // step 4. scale columns to reduce condition number
01115     std::vector< double > ts( ncols );  // double *ts = new double[ncols];
01116     DGMSolver::rescale_matrix( npts2fit, ncols, &( V[0] ), &( ts[0] ) );
01117 
01118     // step 5. Perform Householder QR factorization
01119     std::vector< double > D( ncols );  // double *D = new double[ncols];
01120     int rank;
01121     DGMSolver::qr_polyfit_safeguarded( npts2fit, ncols, &( V[0] ), &( D[0] ), &rank );
01122 
01123     // step 6. adjust degree of fitting according to rank of Vandermonde matrix
01124     int ncols_sub = ncols;
01125     while( rank < ncols_sub )
01126     {
01127         --degree;
01128         if( degree == 0 )
01129         {
01130             // surface is flat, return 0
01131             *degree_out = *degree_qr = degree;
01132             for( int i = 0; i < npts2fit; ++i )
01133             {
01134                 for( int k = 0; k < ndim; ++k )
01135                 {
01136                     bs[k * npts2fit + i] = 0;
01137                 }
01138             }
01139             return;
01140         }
01141         else
01142         {
01143             ncols_sub = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
01144         }
01145     }
01146     *degree_qr = degree;
01147 
01148     // std::cout << "degree_qr: " << *degree_qr << std::endl;
01149 
01150     /* DBG
01151      * std::cout<<"before Qtb"<<std::endl;
01152     std::cout<<std::endl;
01153     std::cout<<"bs = "<<std::endl;
01154     std::cout<<std::endl;
01155     for (int k=0; k< ndim; k++){
01156         for (int j=0; j<npts2fit; ++j){
01157         std::cout<<"  "<<bs[npts2fit*k+j]<<std::endl;
01158           }
01159       }
01160     std::cout<<std::endl;*/
01161 
01162     // step 7. compute Q'b
01163     DGMSolver::compute_qtransposeB( npts2fit, ncols_sub, &( V[0] ), ndim, bs );
01164 
01165     /* DBG
01166      * std::cout<<"after Qtb"<<std::endl;
01167     std::cout<<"bs = "<<std::endl;
01168     std::cout<<std::endl;
01169     for (int k=0; k< ndim; k++){
01170         for (int j=0; j<npts2fit; ++j){
01171         std::cout<<"  "<<bs[npts2fit*k+j]<<std::endl;
01172           }
01173       }
01174     std::cout<<std::endl;*/
01175 
01176     // step 8. perform backward substitution and scale the solution
01177     // assign diagonals of V
01178     for( int i = 0; i < ncols_sub; ++i )
01179     {
01180         V[i * npts2fit + i] = D[i];
01181     }
01182 
01183     // backsolve
01184     if( safeguard )
01185     {
01186         // for debug
01187         // std::cout << "ts size " << ts.size() << std::endl;
01188         DGMSolver::backsolve_polyfit_safeguarded( 2, degree, interp, npts2fit, ncols_sub, &( V[0] ), ndim, bs,
01189                                                   &( ts[0] ), degree_out );
01190     }
01191     else
01192     {
01193         DGMSolver::backsolve( npts2fit, ncols_sub, &( V[0] ), 1, bs, &( ts[0] ) );
01194         *degree_out = degree;
01195     }
01196     /*if(V_init){
01197         delete [] V_init;
01198     }else{
01199         delete [] V;
01200     }*/
01201 }
01202 
01203 void HiReconstruction::polyfit3d_curve_get_coeff( const int nverts, const double* ngbcors, const double* ngbtangs,
01204                                                   int degree, const bool interp, const bool safeguard,
01205                                                   const int ncoords, double* coords, const int ncoeffs, double* coeffs,
01206                                                   int* degree_out )
01207 {
01208     if( !nverts ) { return; }
01209     // step 1. compute local coordinates system
01210     double tang[3] = { ngbtangs[0], ngbtangs[1], ngbtangs[2] };
01211     if( coords && ncoords > 2 )
01212     {
01213         coords[0] = tang[0];
01214         coords[1] = tang[1];
01215         coords[2] = tang[2];
01216     }
01217     if( !coeffs || !ncoeffs ) { return; }
01218     else
01219     {
01220         assert( ncoeffs >= 3 * ( degree + 1 ) );
01221     }
01222     // step 2. project onto local coordinate system
01223     int npts2fit = nverts - interp;
01224     if( !npts2fit )
01225     {
01226         *degree_out = 0;
01227         for( int i = 0; i < ncoeffs; ++i )
01228         {
01229             coeffs[0] = 0;
01230         }
01231         return;
01232     }
01233     std::vector< double > us( npts2fit );      // double *us = new double[npts2fit];
01234     std::vector< double > bs( npts2fit * 3 );  // double *bs = new double[npts2fit*3];
01235     double uu[3];
01236     for( int i = interp; i < nverts; ++i )
01237     {
01238         int k = i - interp;
01239         DGMSolver::vec_linear_operation( 3, 1, ngbcors + 3 * i, -1, ngbcors, uu );
01240         us[k]                = DGMSolver::vec_innerprod( 3, uu, tang );
01241         bs[k]                = uu[0];
01242         bs[npts2fit + k]     = uu[1];
01243         bs[2 * npts2fit + k] = uu[2];
01244     }
01245 
01246     // step 3. copmute weights
01247     std::vector< double > ws( npts2fit );
01248     int nzeros = compute_weights( npts2fit, 1, &( us[0] ), nverts, ngbtangs, degree, _MINEPS, &( ws[0] ) );
01249     assert( nzeros <= npts2fit );
01250 
01251     // step 4. adjust according to number of zero-weights
01252     if( nzeros )
01253     {
01254         if( nzeros == npts2fit )
01255         {
01256             // singular case
01257             *degree_out = 0;
01258             for( int i = 0; i < ncoeffs; ++i )
01259             {
01260                 coeffs[i] = 0;
01261             }
01262             return;
01263         }
01264         int npts_new = npts2fit - nzeros;
01265         std::vector< double > bs_temp( 3 * npts_new );
01266         int index = 0;
01267         for( int i = 0; i < npts2fit; ++i )
01268         {
01269             if( ws[i] )
01270             {
01271                 if( i > index )
01272                 {
01273                     us[index] = us[i];
01274                     ws[index] = ws[i];
01275                 }
01276                 bs_temp[index]                = bs[i];
01277                 bs_temp[index + npts_new]     = bs[i + npts2fit];
01278                 bs_temp[index + 2 * npts_new] = bs[i + 2 * npts2fit];
01279                 ++index;
01280             }
01281         }
01282         assert( index == npts_new );
01283         npts2fit = npts_new;
01284         us.resize( index );
01285         ws.resize( index );
01286         bs = bs_temp;
01287         // destroy bs_temp;
01288         std::vector< double >().swap( bs_temp );
01289     }
01290 
01291     // step 5. fitting
01292     eval_vander_univar_cmf( npts2fit, &( us[0] ), 3, &( bs[0] ), degree, &( ws[0] ), interp, safeguard, degree_out );
01293     // step 6. write results to output pointers
01294     int ncoeffs_out_pvpd = *degree_out + 1;
01295     assert( ncoeffs >= 3 * ncoeffs_out_pvpd );
01296     // write to coeffs consecutively, bs's size is not changed by eval_vander_univar_cmf
01297     coeffs[0] = coeffs[ncoeffs_out_pvpd] = coeffs[2 * ncoeffs_out_pvpd] = 0;
01298     for( int i = 0; i < ncoeffs_out_pvpd - interp; ++i )
01299     {
01300         coeffs[i + interp]                        = bs[i];
01301         coeffs[i + interp + ncoeffs_out_pvpd]     = bs[i + npts2fit];
01302         coeffs[i + interp + 2 * ncoeffs_out_pvpd] = bs[i + 2 * npts2fit];
01303     }
01304 }
01305 
01306 void HiReconstruction::eval_vander_univar_cmf( const int npts2fit, const double* us, const int ndim, double* bs,
01307                                                int degree, const double* ws, const bool interp, const bool safeguard,
01308                                                int* degree_out )
01309 {
01310     // step 1. determine degree of polynomials to fit according to number of points
01311     int ncols = degree + 1 - interp;
01312     while( npts2fit < ncols && degree >= 1 )
01313     {
01314         --degree;
01315         ncols = degree + 1 - interp;
01316     }
01317     if( !degree )
01318     {
01319         if( interp )
01320         {
01321             for( int icol = 0; icol < ndim; ++icol )
01322             {
01323                 bs[icol * npts2fit] = 0;
01324             }
01325         }
01326         for( int irow = 1; irow < npts2fit; ++irow )
01327         {
01328             for( int icol = 0; icol < ndim; ++icol )
01329             {
01330                 bs[icol * npts2fit + irow] = 0;
01331             }
01332         }
01333         *degree_out = 0;
01334         return;
01335     }
01336     // step 2. construct Vandermonde matrix
01337     std::vector< double > V;  // V(npts2fit*(ncols+interp));
01338     DGMSolver::gen_vander_multivar( npts2fit, 1, us, degree, V );
01339 
01340     if( interp ) { V.erase( V.begin(), V.begin() + npts2fit ); }
01341 
01342     // step 3. scale rows with respect to weights
01343     for( int i = 0; i < npts2fit; ++i )
01344     {
01345         for( int j = 0; j < ncols; ++j )
01346         {
01347             V[j * npts2fit + i] *= ws[i];
01348         }
01349         for( int k = 0; k < ndim; ++k )
01350         {
01351             bs[k * npts2fit + i] *= ws[i];
01352         }
01353     }
01354 
01355     // step 4. scale columns to reduce condition number
01356     std::vector< double > ts( ncols );
01357     DGMSolver::rescale_matrix( npts2fit, ncols, &( V[0] ), &( ts[0] ) );
01358 
01359     // step 5. perform Householder QR factorization
01360     std::vector< double > D( ncols );
01361     int rank;
01362     DGMSolver::qr_polyfit_safeguarded( npts2fit, ncols, &( V[0] ), &( D[0] ), &rank );
01363 
01364     // step 6. adjust degree of fitting
01365     int ncols_sub = ncols;
01366     while( rank < ncols_sub )
01367     {
01368         --degree;
01369         if( !degree )
01370         {
01371             // matrix is singular, consider curve on flat plane passing center and orthogonal to
01372             // tangent line
01373             *degree_out = 0;
01374             for( int i = 0; i < npts2fit; ++i )
01375             {
01376                 for( int k = 0; k < ndim; ++k )
01377                 {
01378                     bs[k * npts2fit + i] = 0;
01379                 }
01380             }
01381         }
01382         ncols_sub = degree + 1 - interp;
01383     }
01384 
01385     // step 7. compute Q'*bs
01386     DGMSolver::compute_qtransposeB( npts2fit, ncols_sub, &( V[0] ), ndim, bs );
01387 
01388     // step 8. perform backward substitution and scale solutions
01389     // assign diagonals of V
01390     for( int i = 0; i < ncols_sub; ++i )
01391     {
01392         V[i * npts2fit + i] = D[i];
01393     }
01394     // backsolve
01395     if( safeguard )
01396     {
01397         DGMSolver::backsolve_polyfit_safeguarded( 1, degree, interp, npts2fit, ncols, &( V[0] ), ndim, bs, ws,
01398                                                   degree_out );
01399     }
01400     else
01401     {
01402         DGMSolver::backsolve( npts2fit, ncols_sub, &( V[0] ), ndim, bs, &( ts[0] ) );
01403         *degree_out = degree;
01404     }
01405 }
01406 
01407 int HiReconstruction::compute_weights( const int nrows, const int ncols, const double* us, const int nngbs,
01408                                        const double* ngbnrms, const int degree, const double toler, double* ws )
01409 {
01410     assert( nrows <= _MAXPNTS && ws );
01411     bool interp = false;
01412     if( nngbs != nrows )
01413     {
01414         assert( nngbs == nrows + 1 );
01415         interp = true;
01416     }
01417     double epsilon = 1e-2;
01418 
01419     // First, compute squared distance from each input piont to the center
01420     for( int i = 0; i < nrows; ++i )
01421     {
01422         ws[i] = DGMSolver::vec_innerprod( ncols, us + i * ncols, us + i * ncols );
01423     }
01424 
01425     // Second, compute a small correction termt o guard against zero
01426     double h = 0;
01427     for( int i = 0; i < nrows; ++i )
01428     {
01429         h += ws[i];
01430     }
01431     h /= (double)nrows;
01432 
01433     // Finally, compute the weights for each vertex
01434     int nzeros = 0;
01435     for( int i = 0; i < nrows; ++i )
01436     {
01437         double costheta = DGMSolver::vec_innerprod( 3, ngbnrms, ngbnrms + 3 * ( i + interp ) );
01438         if( costheta > toler ) { ws[i] = costheta * pow( ws[i] / h + epsilon, -1 * (double)degree / 2.0 ); }
01439         else
01440         {
01441             ws[i] = 0;
01442             ++nzeros;
01443         }
01444     }
01445     return nzeros;
01446 }
01447 bool HiReconstruction::check_barycentric_coords( const int nws, const double* naturalcoords )
01448 {
01449     double sum = 0;
01450     for( int i = 0; i < nws; ++i )
01451     {
01452         if( naturalcoords[i] < -_MINEPS ) { return false; }
01453         sum += naturalcoords[i];
01454     }
01455     if( fabs( 1 - sum ) > _MINEPS ) { return false; }
01456     else
01457     {
01458         return true;
01459     }
01460 }
01461 }  // namespace moab
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines