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