MOAB: Mesh Oriented datABase  (version 5.2.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 <math.h>
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     { MB_SET_ERR( MB_FAILURE, "element connectivity table size doesn't match input size" ); }
00357 
00358     if( !_hasfittings ) { MB_SET_ERR( MB_FAILURE, "There is no existing fitting results" ); }
00359     else
00360     {
00361         std::ostringstream convert;
00362         convert << elem;
00363         std::string ID = convert.str();
00364         for( int i = 0; i < nvpe; ++i )
00365         {
00366             if( -1 == _verts2rec.index( elemconn[i] ) )
00367             { MB_SET_ERR( MB_FAILURE, "There is no existing fitting results for element " + ID ); }
00368         }
00369     }
00370     // check correctness of input
00371     for( int i = 0; i < npts2fit; ++i )
00372     {
00373         if( !check_barycentric_coords( nvpe, naturalcoords2fit + i * nvpe ) )
00374         { MB_SET_ERR( MB_FAILURE, "Wrong barycentric coordinates" ); }
00375     }
00376 
00377     double* elemcoords = new double[nvpe * 3];
00378     error              = mbImpl->get_coords( &( elemconn[0] ), nvpe, elemcoords );MB_CHK_ERR( error );
00379 
00380     double* coords2fit = new double[3 * npts2fit]();
00381     for( int i = 0; i < npts2fit; ++i )
00382     {
00383         for( int j = 0; j < nvpe; ++j )
00384         {
00385             coords2fit[3 * i] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j];
00386             coords2fit[3 * i + 1] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j + 1];
00387             coords2fit[3 * i + 2] += naturalcoords2fit[i * nvpe + j] * elemcoords[3 * j + 2];
00388         }
00389     }
00390 
00391     double* hiproj_new = new double[3 * npts2fit];
00392     // initialize output
00393     for( int i = 0; i < npts2fit; ++i )
00394     {
00395         newcoords[3 * i] = newcoords[3 * i + 1] = newcoords[3 * i + 2] = 0;
00396     }
00397     // for each input vertex, call nvpe fittings and take average
00398     for( int j = 0; j < nvpe; ++j )
00399     {
00400         error = hiproj_walf_around_vertex( elemconn[j], npts2fit, coords2fit, hiproj_new );MB_CHK_ERR( error );
00401         for( int i = 0; i < npts2fit; ++i )
00402         {
00403             newcoords[3 * i] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i];
00404             newcoords[3 * i + 1] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i + 1];
00405             newcoords[3 * i + 2] += naturalcoords2fit[i * nvpe + j] * hiproj_new[3 * i + 2];
00406         }
00407     }
00408     delete[] elemcoords;
00409     delete[] coords2fit;
00410     delete[] hiproj_new;
00411     return error;
00412 }
00413 
00414 ErrorCode HiReconstruction::hiproj_walf_around_vertex( EntityHandle vid, const int npts2fit, const double* coords2fit,
00415                                                        double* hiproj_new )
00416 {
00417     if( !_hasfittings ) { MB_SET_ERR( MB_FAILURE, "There is no existing fitting results" ); }
00418     else if( -1 == _verts2rec.index( vid ) )
00419     {
00420         std::ostringstream convert;
00421         convert << vid;
00422         std::string VID = convert.str();
00423         MB_SET_ERR( MB_FAILURE, "There is no existing fitting results for vertex " + VID );
00424     }
00425     ErrorCode error;
00426     // get center of local coordinates system
00427     double local_origin[3];
00428     error = mbImpl->get_coords( &vid, 1, local_origin );MB_CHK_ERR( error );
00429     // get local fitting parameters
00430     int index     = _verts2rec.index( vid );
00431     bool interp   = _interps[index];
00432     int local_deg = _degrees_out[index];
00433     double *uvw_coords, *local_coeffs;
00434     if( _geom == HISURFACE )
00435     {
00436         uvw_coords = &( _local_coords[9 * index] );
00437         // int ncoeffs = (local_deg+2)*(local_deg+1)>>1;
00438         size_t istr  = _vertID2coeffID[index];
00439         local_coeffs = &( _local_fit_coeffs[istr] );
00440         walf3d_surf_vertex_eval( local_origin, uvw_coords, local_deg, local_coeffs, interp, npts2fit, coords2fit,
00441                                  hiproj_new );
00442     }
00443     else if( _geom == HI3DCURVE )
00444     {
00445         uvw_coords   = &( _local_coords[3 * index] );
00446         size_t istr  = _vertID2coeffID[index];
00447         local_coeffs = &( _local_fit_coeffs[istr] );
00448         walf3d_curve_vertex_eval( local_origin, uvw_coords, local_deg, local_coeffs, interp, npts2fit, coords2fit,
00449                                   hiproj_new );
00450     }
00451     return error;
00452 }
00453 
00454 void HiReconstruction::walf3d_surf_vertex_eval( const double* local_origin, const double* local_coords,
00455                                                 const int local_deg, const double* local_coeffs, const bool interp,
00456                                                 const int npts2fit, const double* coords2fit, double* hiproj_new )
00457 {
00458     double xaxis[3], yaxis[3], zaxis[3];
00459     for( int i = 0; i < 3; ++i )
00460     {
00461         xaxis[i] = local_coords[i];
00462         yaxis[i] = local_coords[3 + i];
00463         zaxis[i] = local_coords[6 + i];
00464     }
00465     // double *basis = new double[(local_deg+2)*(local_deg+1)/2-1];
00466     std::vector< double > basis( ( local_deg + 2 ) * ( local_deg + 1 ) / 2 - 1 );
00467     for( int i = 0; i < npts2fit; ++i )
00468     {
00469         double local_pos[3];
00470         for( int j = 0; j < 3; ++j )
00471         {
00472             local_pos[j] = coords2fit[3 * i + j] - local_origin[j];
00473         }
00474         double u, v, height = 0;
00475         u        = DGMSolver::vec_innerprod( 3, local_pos, xaxis );
00476         v        = DGMSolver::vec_innerprod( 3, local_pos, yaxis );
00477         basis[0] = u;
00478         basis[1] = v;
00479         int l    = 1;
00480         for( int k = 2; k <= local_deg; ++k )
00481         {
00482             ++l;
00483             basis[l] = u * basis[l - k];
00484             for( int id = 0; id < k; ++id )
00485             {
00486                 ++l;
00487                 basis[l] = basis[l - k - 1] * v;
00488             }
00489         }
00490         if( !interp ) { height = local_coeffs[0]; }
00491         for( int p = 0; p <= l; ++p )
00492         {
00493             height += local_coeffs[p + 1] * basis[p];
00494         }
00495         hiproj_new[3 * i]     = local_origin[0] + u * xaxis[0] + v * yaxis[0] + height * zaxis[0];
00496         hiproj_new[3 * i + 1] = local_origin[1] + u * xaxis[1] + v * yaxis[1] + height * zaxis[1];
00497         hiproj_new[3 * i + 2] = local_origin[2] + u * xaxis[2] + v * yaxis[2] + height * zaxis[2];
00498     }
00499     // delete [] basis;
00500 }
00501 
00502 void HiReconstruction::walf3d_curve_vertex_eval( const double* local_origin, const double* local_coords,
00503                                                  const int local_deg, const double* local_coeffs, const bool interp,
00504                                                  const int npts2fit, const double* coords2fit, double* hiproj_new )
00505 {
00506     assert( local_origin && local_coords && local_coeffs );
00507     int ncoeffspvpd = local_deg + 1;
00508     for( int i = 0; i < npts2fit; ++i )
00509     {
00510         // get the vector from center to current point, project to tangent line
00511         double vec[3], ans[3] = { 0, 0, 0 };
00512         DGMSolver::vec_linear_operation( 3, 1, coords2fit + 3 * i, -1, local_origin, vec );
00513         double u = DGMSolver::vec_innerprod( 3, local_coords, vec );
00514         // evaluate polynomials
00515         if( !interp )
00516         {
00517             ans[0] = local_coeffs[0];
00518             ans[1] = local_coeffs[ncoeffspvpd];
00519             ans[2] = local_coeffs[2 * ncoeffspvpd];
00520         }
00521         double uk = 1;  // degree_out and degree different, stored in columnwise contiguously
00522         for( int j = 1; j < ncoeffspvpd; ++j )
00523         {
00524             uk *= u;
00525             ans[0] += uk * local_coeffs[j];
00526             ans[1] += uk * local_coeffs[j + ncoeffspvpd];
00527             ans[2] += uk * local_coeffs[j + 2 * ncoeffspvpd];
00528         }
00529         hiproj_new[3 * i]     = ans[0] + local_origin[0];
00530         hiproj_new[3 * i + 1] = ans[1] + local_origin[1];
00531         hiproj_new[3 * i + 2] = ans[2] + local_origin[2];
00532     }
00533 }
00534 
00535 bool HiReconstruction::get_fittings_data( EntityHandle vid, GEOMTYPE& geomtype, std::vector< double >& coords,
00536                                           int& degree_out, std::vector< double >& coeffs, bool& interp )
00537 {
00538     if( !_hasfittings ) { return false; }
00539     else
00540     {
00541         int index = _verts2rec.index( vid );
00542         if( -1 == index )
00543         {
00544             std::cout << "Input vertex is not locally hosted vertex in this mesh set" << std::endl;
00545             return false;
00546         }
00547         geomtype = _geom;
00548         if( HISURFACE == _geom )
00549         {
00550             coords.insert( coords.end(), _local_coords.begin() + 9 * index, _local_coords.begin() + 9 * index + 9 );
00551             degree_out  = _degrees_out[index];
00552             interp      = _interps[index];
00553             int ncoeffs = ( degree_out + 2 ) * ( degree_out + 1 ) >> 1;
00554             size_t istr = _vertID2coeffID[index];
00555             coeffs.insert( coeffs.end(), _local_fit_coeffs.begin() + istr, _local_fit_coeffs.begin() + istr + ncoeffs );
00556         }
00557         else if( HI3DCURVE == _geom )
00558         {
00559             coords.insert( coords.end(), _local_coords.begin() + 3 * index, _local_coords.begin() + 3 * index + 3 );
00560             degree_out  = _degrees_out[index];
00561             interp      = _interps[index];
00562             int ncoeffs = 3 * ( degree_out + 1 );
00563             size_t istr = _vertID2coeffID[index];
00564             coeffs.insert( coeffs.end(), _local_fit_coeffs.begin() + istr, _local_fit_coeffs.begin() + istr + ncoeffs );
00565         }
00566         return true;
00567     }
00568 }
00569 
00570 /****************************************************************
00571  *  Basic Internal Routines to initialize and set fitting data  *
00572  ****************************************************************/
00573 
00574 int HiReconstruction::estimate_num_rings( int degree, bool interp )
00575 {
00576     return interp ? ( ( degree + 1 ) >> 1 ) + ( ( degree + 1 ) & 1 ) : ( ( degree + 2 ) >> 1 ) + ( ( degree + 2 ) & 1 );
00577 }
00578 
00579 ErrorCode HiReconstruction::vertex_get_incident_elements( const EntityHandle& vid, const int elemdim,
00580                                                           std::vector< EntityHandle >& adjents )
00581 {
00582     ErrorCode error;
00583     assert( elemdim == _dim );
00584 #ifdef HIREC_USE_AHF
00585     error = ahf->get_up_adjacencies( vid, elemdim, adjents );MB_CHK_ERR( error );
00586 #else
00587     error      = mbImpl->get_adjacencies( &vid, 1, elemdim, false, adjents );MB_CHK_ERR( error );
00588 #endif
00589     return error;
00590 }
00591 
00592 ErrorCode HiReconstruction::obtain_nring_ngbvs( const EntityHandle vid, int ring, const int minpnts, Range& ngbvs )
00593 {
00594     ErrorCode error;
00595     std::deque< EntityHandle > todo;
00596     todo.push_back( vid );
00597     ngbvs.insert( vid );
00598     EntityHandle pre, nxt;
00599     for( int i = 1; i <= ring; ++i )
00600     {
00601         int count = todo.size();
00602         while( count )
00603         {
00604             EntityHandle center = todo.front();
00605             todo.pop_front();
00606             --count;
00607             std::vector< EntityHandle > adjents;
00608             error = vertex_get_incident_elements( center, _dim, adjents );MB_CHK_ERR( error );
00609             for( size_t j = 0; j < adjents.size(); ++j )
00610             {
00611                 std::vector< EntityHandle > elemconn;
00612                 error = mbImpl->get_connectivity( &adjents[j], 1, elemconn );MB_CHK_ERR( error );
00613                 int nvpe = elemconn.size();
00614                 for( int k = 0; k < nvpe; ++k )
00615                 {
00616                     if( elemconn[k] == center )
00617                     {
00618                         pre = k == 0 ? elemconn[nvpe - 1] : elemconn[k - 1];
00619                         nxt = elemconn[( k + 1 ) % nvpe];
00620                         if( ngbvs.find( pre ) == ngbvs.end() )
00621                         {
00622                             ngbvs.insert( pre );
00623                             todo.push_back( pre );
00624                         }
00625                         if( ngbvs.find( nxt ) == ngbvs.end() )
00626                         {
00627                             ngbvs.insert( nxt );
00628                             todo.push_back( nxt );
00629                         }
00630                         break;
00631                     }
00632                 }
00633             }
00634         }
00635         if( _MAXPNTS <= (int)ngbvs.size() )
00636         {
00637             // obtain enough points
00638             return error;
00639         }
00640         if( !todo.size() )
00641         {
00642             // current ring cannot introduce any points, return incase deadlock
00643             return error;
00644         }
00645         if( ( i == ring ) && ( minpnts > (int)ngbvs.size() ) )
00646         {
00647             // reach maximum ring but not enough points
00648             ++ring;
00649         }
00650     }
00651     return error;
00652 }
00653 
00654 void HiReconstruction::initialize_surf_geom( const int degree )
00655 {
00656     if( !_hasderiv )
00657     {
00658         compute_average_vertex_normals_surf();
00659         _hasderiv = true;
00660     }
00661     if( !_initfittings )
00662     {
00663         int ncoeffspv = ( degree + 2 ) * ( degree + 1 ) / 2;
00664         _degrees_out.assign( _nv2rec, 0 );
00665         _interps.assign( _nv2rec, false );
00666         _vertID2coeffID.resize( _nv2rec );
00667         _local_fit_coeffs.assign( _nv2rec * ncoeffspv, 0 );
00668         for( size_t i = 0; i < _nv2rec; ++i )
00669         {
00670             _vertID2coeffID[i] = i * ncoeffspv;
00671         }
00672         _initfittings = true;
00673     }
00674 }
00675 
00676 void HiReconstruction::initialize_surf_geom( const size_t npts, const int* degrees )
00677 {
00678     if( !_hasderiv )
00679     {
00680         compute_average_vertex_normals_surf();
00681         _hasderiv = true;
00682     }
00683     if( !_initfittings )
00684     {
00685         assert( _nv2rec == npts );
00686         _degrees_out.assign( _nv2rec, 0 );
00687         _interps.assign( _nv2rec, false );
00688         _vertID2coeffID.resize( _nv2rec );
00689         size_t index = 0;
00690         for( size_t i = 0; i < _nv2rec; ++i )
00691         {
00692             _vertID2coeffID[i] = index;
00693             index += ( degrees[i] + 2 ) * ( degrees[i] + 1 ) / 2;
00694         }
00695         _local_fit_coeffs.assign( index, 0 );
00696         _initfittings = true;
00697     }
00698 }
00699 
00700 void HiReconstruction::initialize_3Dcurve_geom( const int degree )
00701 {
00702     if( !_hasderiv )
00703     {
00704         compute_average_vertex_tangents_curve();
00705         _hasderiv = true;
00706     }
00707     if( !_initfittings )
00708     {
00709         int ncoeffspvpd = degree + 1;
00710         _degrees_out.assign( _nv2rec, 0 );
00711         _interps.assign( _nv2rec, false );
00712         _vertID2coeffID.resize( _nv2rec );
00713         _local_fit_coeffs.assign( _nv2rec * ncoeffspvpd * 3, 0 );
00714         for( size_t i = 0; i < _nv2rec; ++i )
00715         {
00716             _vertID2coeffID[i] = i * ncoeffspvpd * 3;
00717         }
00718         _initfittings = true;
00719     }
00720 }
00721 
00722 void HiReconstruction::initialize_3Dcurve_geom( const size_t npts, const int* degrees )
00723 {
00724     if( !_hasderiv )
00725     {
00726         compute_average_vertex_tangents_curve();
00727         _hasderiv = true;
00728     }
00729     if( !_hasfittings )
00730     {
00731         assert( _nv2rec == npts );
00732         _degrees_out.assign( _nv2rec, 0 );
00733         _interps.assign( _nv2rec, false );
00734         _vertID2coeffID.reserve( _nv2rec );
00735         size_t index = 0;
00736         for( size_t i = 0; i < _nv2rec; ++i )
00737         {
00738             _vertID2coeffID[i] = index;
00739             index += 3 * ( degrees[i] + 1 );
00740         }
00741         _local_fit_coeffs.assign( index, 0 );
00742         _initfittings = true;
00743     }
00744 }
00745 
00746 /* ErrorCode HiReconstruction::set_geom_data_surf(const EntityHandle vid, const double* coords,
00747  const double degree_out, const double* coeffs, bool interp)
00748  {
00749    return MB_SUCCESS;
00750  }
00751 
00752  ErrorCode HiReconstruction::set_geom_data_3Dcurve(const EntityHandle vid, const double* coords,
00753  const double degree_out, const double* coeffs, bool interp)
00754  {
00755    return MB_SUCCESS;
00756  } */
00757 
00758 /*********************************************************
00759  * Routines for vertex normal/tangent vector estimation *
00760  *********************************************************/
00761 ErrorCode HiReconstruction::average_vertex_normal( const EntityHandle vid, double* nrm )
00762 {
00763     ErrorCode error;
00764     std::vector< EntityHandle > adjfaces;
00765     error = vertex_get_incident_elements( vid, 2, adjfaces );MB_CHK_ERR( error );
00766     int npolys = adjfaces.size();
00767     if( !npolys ) { MB_SET_ERR( MB_FAILURE, "Vertex has no incident 2D entities" ); }
00768     else
00769     {
00770         double v1[3], v2[3], v3[3], a[3], b[3], c[3];
00771         nrm[0] = nrm[1] = nrm[2] = 0;
00772         for( int i = 0; i < npolys; ++i )
00773         {
00774             // get incident "triangles"
00775             std::vector< EntityHandle > elemconn;
00776             error = mbImpl->get_connectivity( &adjfaces[i], 1, elemconn );MB_CHK_ERR( error );
00777             EntityHandle pre, nxt;
00778             int nvpe = elemconn.size();
00779             for( int j = 0; j < nvpe; ++j )
00780             {
00781                 if( vid == elemconn[j] )
00782                 {
00783                     pre = j == 0 ? elemconn[nvpe - 1] : elemconn[j - 1];
00784                     nxt = elemconn[( j + 1 ) % nvpe];
00785                     break;
00786                 }
00787             }
00788             // compute area weighted normals
00789             error = mbImpl->get_coords( &pre, 1, a );MB_CHK_ERR( error );
00790             error = mbImpl->get_coords( &vid, 1, b );MB_CHK_ERR( error );
00791             error = mbImpl->get_coords( &nxt, 1, c );MB_CHK_ERR( error );
00792             DGMSolver::vec_linear_operation( 3, 1, c, -1, b, v1 );
00793             DGMSolver::vec_linear_operation( 3, 1, a, -1, b, v2 );
00794             DGMSolver::vec_crossprod( v1, v2, v3 );
00795             DGMSolver::vec_linear_operation( 3, 1, nrm, 1, v3, nrm );
00796         }
00797 #ifndef NDEBUG
00798         assert( DGMSolver::vec_normalize( 3, nrm, nrm ) );
00799 #endif
00800     }
00801     return error;
00802 }
00803 
00804 ErrorCode HiReconstruction::compute_average_vertex_normals_surf()
00805 {
00806     if( _hasderiv ) { return MB_SUCCESS; }
00807     ErrorCode error;
00808     _local_coords.assign( 9 * _nv2rec, 0 );
00809     size_t index = 0;
00810     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++index )
00811     {
00812         error = average_vertex_normal( *ivert, &( _local_coords[9 * index + 6] ) );MB_CHK_ERR( error );
00813     }
00814     return error;
00815 }
00816 
00817 ErrorCode HiReconstruction::get_normals_surf( const Range& vertsh, double* nrms )
00818 {
00819     ErrorCode error = MB_SUCCESS;
00820     if( _hasderiv )
00821     {
00822         size_t id = 0;
00823         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00824         {
00825             int index = _verts2rec.index( *ivert );
00826 #ifdef MOAB_HAVE_MPI
00827             if( -1 == index )
00828             {
00829                 // ghost vertex
00830                 error = average_vertex_normal( *ivert, nrms + 3 * id );MB_CHK_ERR( error );
00831             }
00832             else
00833             {
00834                 nrms[3 * id]     = _local_coords[9 * index + 6];
00835                 nrms[3 * id + 1] = _local_coords[9 * index + 7];
00836                 nrms[3 * id + 2] = _local_coords[9 * index + 8];
00837             }
00838 #else
00839             assert( -1 != index );
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 #endif
00844         }
00845     }
00846     else
00847     {
00848         size_t id = 0;
00849         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00850         {
00851             error = average_vertex_normal( *ivert, nrms + 3 * id );MB_CHK_ERR( error );
00852         }
00853     }
00854     return error;
00855 }
00856 
00857 ErrorCode HiReconstruction::average_vertex_tangent( const EntityHandle vid, double* tang )
00858 {
00859     ErrorCode error;
00860     std::vector< EntityHandle > adjedges;
00861     error = vertex_get_incident_elements( vid, 1, adjedges );MB_CHK_ERR( error );
00862     int nedges = adjedges.size();
00863     if( !nedges ) { MB_SET_ERR( MB_FAILURE, "Vertex has no incident edges" ); }
00864     else
00865     {
00866         assert( nedges <= 2 );
00867         tang[0] = tang[1] = tang[2] = 0;
00868         for( int i = 0; i < nedges; ++i )
00869         {
00870             std::vector< EntityHandle > edgeconn;
00871             error = mbImpl->get_connectivity( &adjedges[i], 1, edgeconn );
00872             double istr[3], iend[3], t[3];
00873             error = mbImpl->get_coords( &( edgeconn[0] ), 1, istr );
00874             error = mbImpl->get_coords( &( edgeconn[1] ), 1, iend );
00875             DGMSolver::vec_linear_operation( 3, 1, iend, -1, istr, t );
00876             DGMSolver::vec_linear_operation( 3, 1, tang, 1, t, tang );
00877         }
00878 #ifndef NDEBUG
00879         assert( DGMSolver::vec_normalize( 3, tang, tang ) );
00880 #endif
00881     }
00882     return error;
00883 }
00884 
00885 ErrorCode HiReconstruction::compute_average_vertex_tangents_curve()
00886 {
00887     if( _hasderiv ) { return MB_SUCCESS; }
00888     ErrorCode error;
00889     _local_coords.assign( 3 * _nv2rec, 0 );
00890     size_t index = 0;
00891     for( Range::iterator ivert = _verts2rec.begin(); ivert != _verts2rec.end(); ++ivert, ++index )
00892     {
00893         error = average_vertex_tangent( *ivert, &( _local_coords[3 * index] ) );MB_CHK_ERR( error );
00894     }
00895     return error;
00896 }
00897 
00898 ErrorCode HiReconstruction::get_tangents_curve( const Range& vertsh, double* tangs )
00899 {
00900     ErrorCode error = MB_SUCCESS;
00901     if( _hasderiv )
00902     {
00903         size_t id = 0;
00904         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00905         {
00906             int index = _verts2rec.index( *ivert );
00907 #ifdef MOAB_HAVE_MPI
00908             if( -1 != index )
00909             {
00910                 tangs[3 * id]     = _local_coords[3 * index];
00911                 tangs[3 * id + 1] = _local_coords[3 * index + 1];
00912                 tangs[3 * id + 2] = _local_coords[3 * index + 2];
00913             }
00914             else
00915             {
00916                 error = average_vertex_tangent( *ivert, tangs + 3 * id );MB_CHK_ERR( error );
00917             }
00918 #else
00919             assert( -1 != index );
00920             tangs[3 * id]     = _local_coords[3 * index];
00921             tangs[3 * id + 1] = _local_coords[3 * index + 1];
00922             tangs[3 * id + 2] = _local_coords[3 * index + 2];
00923 #endif
00924         }
00925     }
00926     else
00927     {
00928         size_t id = 0;
00929         for( Range::iterator ivert = vertsh.begin(); ivert != vertsh.end(); ++ivert, ++id )
00930         {
00931             error = average_vertex_tangent( *ivert, tangs + 3 * id );MB_CHK_ERR( error );
00932         }
00933     }
00934     return error;
00935 }
00936 
00937 /************************************************
00938  *  Internal Routines for local WLS fittings    *
00939  *************************************************/
00940 
00941 void HiReconstruction::polyfit3d_surf_get_coeff( const int nverts, const double* ngbcoords, const double* ngbnrms,
00942                                                  int degree, const bool interp, const bool safeguard, const int ncoords,
00943                                                  double* coords, const int ncoeffs, double* coeffs, int* degree_out,
00944                                                  int* degree_pnt, int* degree_qr )
00945 {
00946     if( nverts <= 0 ) { return; }
00947 
00948     // std::cout << "npnts in initial stencil = " << nverts << std::endl;
00949     // std::cout << "centered at (" << ngbcoords[0] << "," << ngbcoords[1] << "," << ngbcoords[2] <<
00950     // ")" << std::endl;
00951 
00952     // step 1. copmute local coordinate system
00953     double nrm[3] = { ngbnrms[0], ngbnrms[1], ngbnrms[2] }, tang1[3] = { 0, 0, 0 }, tang2[3] = { 0, 0, 0 };
00954     if( fabs( nrm[0] ) > fabs( nrm[1] ) && fabs( nrm[0] ) > fabs( nrm[2] ) ) { tang1[1] = 1.0; }
00955     else
00956     {
00957         tang1[0] = 1.0;
00958     }
00959 
00960     DGMSolver::vec_projoff( 3, tang1, nrm, tang1 );
00961 #ifndef NDEBUG
00962     assert( DGMSolver::vec_normalize( 3, tang1, tang1 ) );
00963 #endif
00964     DGMSolver::vec_crossprod( nrm, tang1, tang2 );
00965     if( 9 <= ncoords && coords )
00966     {
00967         coords[0] = tang1[0];
00968         coords[1] = tang1[1];
00969         coords[2] = tang1[2];
00970         coords[3] = tang2[0];
00971         coords[4] = tang2[1];
00972         coords[5] = tang2[2];
00973         coords[6] = nrm[0];
00974         coords[7] = nrm[1];
00975         coords[8] = nrm[2];
00976     }
00977     if( !ncoeffs || !coeffs ) { return; }
00978     else
00979     {
00980         assert( ncoeffs >= ( degree + 2 ) * ( degree + 1 ) / 2 );
00981     }
00982 
00983     // step 2. project onto local coordinates system
00984     int npts2fit = nverts - interp;
00985     if( 0 == npts2fit )
00986     {
00987         *degree_out = *degree_pnt = *degree_qr = 0;
00988         for( int i = 0; i < ncoeffs; ++i )
00989         {
00990             coeffs[i] = 0;
00991         }
00992         // coeffs[0] = 0;
00993         return;
00994     }
00995     std::vector< double > us( npts2fit * 2 );  // double *us = new double[npts2fit*2];
00996     std::vector< double > bs( npts2fit );      // double *bs = new double[npts2fit];
00997     for( int i = interp; i < nverts; ++i )
00998     {
00999         int k = i - interp;
01000         double uu[3];
01001         DGMSolver::vec_linear_operation( 3, 1, ngbcoords + 3 * i, -1, ngbcoords, uu );
01002         us[k * 2]     = DGMSolver::vec_innerprod( 3, tang1, uu );
01003         us[k * 2 + 1] = DGMSolver::vec_innerprod( 3, tang2, uu );
01004         bs[k]         = DGMSolver::vec_innerprod( 3, nrm, uu );
01005     }
01006 
01007     // step 3. compute weights
01008     std::vector< double > ws( npts2fit );  // double *ws = new double[npts2fit];
01009     int nzeros = compute_weights( npts2fit, 2, &( us[0] ), nverts, ngbnrms, degree, _MINEPS, &( ws[0] ) );
01010 
01011     // step 4. adjust according to zero-weights
01012     if( nzeros )
01013     {
01014         if( nzeros == npts2fit )
01015         {
01016             *degree_out = *degree_pnt = *degree_qr = 0;
01017             for( int i = 0; i < ncoeffs; ++i )
01018             {
01019                 coeffs[i] = 0;
01020             }
01021             // coeffs[0] = 0;
01022             return;
01023         }
01024         int index = 0;
01025         for( int i = 0; i < npts2fit; ++i )
01026         {
01027             if( ws[i] )
01028             {
01029                 if( i > index )
01030                 {
01031                     us[index * 2]     = us[i * 2];
01032                     us[index * 2 + 1] = us[i * 2 + 1];
01033                     bs[index]         = bs[i];
01034                     ws[index]         = ws[i];
01035                 }
01036                 ++index;
01037             }
01038         }
01039         npts2fit -= nzeros;
01040         assert( index == npts2fit );
01041         us.resize( npts2fit * 2 );
01042         bs.resize( npts2fit );
01043         ws.resize( npts2fit );
01044         /*us = realloc(us,npts2fit*2*sizeof(double));
01045         bs = realloc(bs,npts2fit*sizeof(double));
01046         ws = realloc(ws,npts2fit*sizeof(double));*/
01047     }
01048 
01049     // std::cout<<"npnts after weighting = "<<npts2fit<<std::endl;
01050 
01051     // step 5. fitting
01052     eval_vander_bivar_cmf( npts2fit, &( us[0] ), 1, &( bs[0] ), degree, &( ws[0] ), interp, safeguard, degree_out,
01053                            degree_pnt, degree_qr );
01054 
01055     // step 6. organize output
01056     int ncoeffs_out = ( *degree_out + 2 ) * ( *degree_out + 1 ) / 2;
01057     assert( ncoeffs_out <= ncoeffs );
01058     coeffs[0] = 0;
01059     for( int j = 0; j < ncoeffs_out - interp; ++j )
01060     {
01061         coeffs[j + interp] = bs[j];
01062     }
01063     // delete [] us; delete [] bs; delete [] ws;
01064 }
01065 
01066 void HiReconstruction::eval_vander_bivar_cmf( const int npts2fit, const double* us, const int ndim, double* bs,
01067                                               int degree, const double* ws, const bool interp, const bool safeguard,
01068                                               int* degree_out, int* degree_pnt, int* degree_qr )
01069 {
01070     // step 1. adjust the degree according to number of points to fit
01071     int ncols = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
01072     while( 1 < degree && npts2fit < ncols )
01073     {
01074         --degree;
01075         ncols = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
01076     }
01077     *degree_pnt = degree;
01078 
01079     // std::cout << "degree_pnt: " << *degree_pnt << std::endl;
01080 
01081     // step 2. construct Vandermonde matrix, stored in columnwise
01082     std::vector< double > V;  // V(npts2fit*(ncols+interp)); //double *V_init = new double[npts2fit*(ncols+interp)];
01083     DGMSolver::gen_vander_multivar( npts2fit, 2, us, degree, V );
01084     // remove the first column of 1s if interpolation
01085     if( interp ) { V.erase( V.begin(), V.begin() + npts2fit ); }
01086     /*double* V;
01087     if(interp){
01088         V = new double[npts2fit*ncols];
01089         std::memcpy(V,V_init+npts2fit,ncols*npts2fit*sizeof(double));
01090         delete [] V_init; V_init = 0;
01091     }else{
01092         V = V_init;
01093     }*/
01094 
01095     // step 3. Scale rows to assign different weights to different points
01096     for( int i = 0; i < npts2fit; ++i )
01097     {
01098         for( int j = 0; j < ncols; ++j )
01099         {
01100             V[j * npts2fit + i] *= ws[i];
01101         }
01102         for( int k = 0; k < ndim; ++k )
01103         {
01104             bs[k * npts2fit + i] *= ws[i];
01105         }
01106     }
01107 
01108     // step 4. scale columns to reduce condition number
01109     std::vector< double > ts( ncols );  // double *ts = new double[ncols];
01110     DGMSolver::rescale_matrix( npts2fit, ncols, &( V[0] ), &( ts[0] ) );
01111 
01112     // step 5. Perform Householder QR factorization
01113     std::vector< double > D( ncols );  // double *D = new double[ncols];
01114     int rank;
01115     DGMSolver::qr_polyfit_safeguarded( npts2fit, ncols, &( V[0] ), &( D[0] ), &rank );
01116 
01117     // step 6. adjust degree of fitting according to rank of Vandermonde matrix
01118     int ncols_sub = ncols;
01119     while( rank < ncols_sub )
01120     {
01121         --degree;
01122         if( degree == 0 )
01123         {
01124             // surface is flat, return 0
01125             *degree_out = *degree_qr = degree;
01126             for( int i = 0; i < npts2fit; ++i )
01127             {
01128                 for( int k = 0; k < ndim; ++k )
01129                 {
01130                     bs[k * npts2fit + i] = 0;
01131                 }
01132             }
01133             return;
01134         }
01135         else
01136         {
01137             ncols_sub = ( ( ( degree + 2 ) * ( degree + 1 ) ) >> 1 ) - interp;
01138         }
01139     }
01140     *degree_qr = degree;
01141 
01142     // std::cout << "degree_qr: " << *degree_qr << std::endl;
01143 
01144     /* DBG
01145      * std::cout<<"before Qtb"<<std::endl;
01146     std::cout<<std::endl;
01147     std::cout<<"bs = "<<std::endl;
01148     std::cout<<std::endl;
01149     for (int k=0; k< ndim; k++){
01150         for (int j=0; j<npts2fit; ++j){
01151         std::cout<<"  "<<bs[npts2fit*k+j]<<std::endl;
01152           }
01153       }
01154     std::cout<<std::endl;*/
01155 
01156     // step 7. compute Q'b
01157     DGMSolver::compute_qtransposeB( npts2fit, ncols_sub, &( V[0] ), ndim, bs );
01158 
01159     /* DBG
01160      * std::cout<<"after Qtb"<<std::endl;
01161     std::cout<<"bs = "<<std::endl;
01162     std::cout<<std::endl;
01163     for (int k=0; k< ndim; k++){
01164         for (int j=0; j<npts2fit; ++j){
01165         std::cout<<"  "<<bs[npts2fit*k+j]<<std::endl;
01166           }
01167       }
01168     std::cout<<std::endl;*/
01169 
01170     // step 8. perform backward substitution and scale the solution
01171     // assign diagonals of V
01172     for( int i = 0; i < ncols_sub; ++i )
01173     {
01174         V[i * npts2fit + i] = D[i];
01175     }
01176 
01177     // backsolve
01178     if( safeguard )
01179     {
01180         // for debug
01181         // std::cout << "ts size " << ts.size() << std::endl;
01182         DGMSolver::backsolve_polyfit_safeguarded( 2, degree, interp, npts2fit, ncols_sub, &( V[0] ), ndim, bs,
01183                                                   &( ts[0] ), degree_out );
01184     }
01185     else
01186     {
01187         DGMSolver::backsolve( npts2fit, ncols_sub, &( V[0] ), 1, bs, &( ts[0] ) );
01188         *degree_out = degree;
01189     }
01190     /*if(V_init){
01191         delete [] V_init;
01192     }else{
01193         delete [] V;
01194     }*/
01195 }
01196 
01197 void HiReconstruction::polyfit3d_curve_get_coeff( const int nverts, const double* ngbcors, const double* ngbtangs,
01198                                                   int degree, const bool interp, const bool safeguard,
01199                                                   const int ncoords, double* coords, const int ncoeffs, double* coeffs,
01200                                                   int* degree_out )
01201 {
01202     if( !nverts ) { return; }
01203     // step 1. compute local coordinates system
01204     double tang[3] = { ngbtangs[0], ngbtangs[1], ngbtangs[2] };
01205     if( coords && ncoords > 2 )
01206     {
01207         coords[0] = tang[0];
01208         coords[1] = tang[1];
01209         coords[2] = tang[2];
01210     }
01211     if( !coeffs || !ncoeffs ) { return; }
01212     else
01213     {
01214         assert( ncoeffs >= 3 * ( degree + 1 ) );
01215     }
01216     // step 2. project onto local coordinate system
01217     int npts2fit = nverts - interp;
01218     if( !npts2fit )
01219     {
01220         *degree_out = 0;
01221         for( int i = 0; i < ncoeffs; ++i )
01222         {
01223             coeffs[0] = 0;
01224         }
01225         return;
01226     }
01227     std::vector< double > us( npts2fit );      // double *us = new double[npts2fit];
01228     std::vector< double > bs( npts2fit * 3 );  // double *bs = new double[npts2fit*3];
01229     double uu[3];
01230     for( int i = interp; i < nverts; ++i )
01231     {
01232         int k = i - interp;
01233         DGMSolver::vec_linear_operation( 3, 1, ngbcors + 3 * i, -1, ngbcors, uu );
01234         us[k]                = DGMSolver::vec_innerprod( 3, uu, tang );
01235         bs[k]                = uu[0];
01236         bs[npts2fit + k]     = uu[1];
01237         bs[2 * npts2fit + k] = uu[2];
01238     }
01239 
01240     // step 3. copmute weights
01241     std::vector< double > ws( npts2fit );
01242     int nzeros = compute_weights( npts2fit, 1, &( us[0] ), nverts, ngbtangs, degree, _MINEPS, &( ws[0] ) );
01243     assert( nzeros <= npts2fit );
01244 
01245     // step 4. adjust according to number of zero-weights
01246     if( nzeros )
01247     {
01248         if( nzeros == npts2fit )
01249         {
01250             // singular case
01251             *degree_out = 0;
01252             for( int i = 0; i < ncoeffs; ++i )
01253             {
01254                 coeffs[i] = 0;
01255             }
01256             return;
01257         }
01258         int npts_new = npts2fit - nzeros;
01259         std::vector< double > bs_temp( 3 * npts_new );
01260         int index = 0;
01261         for( int i = 0; i < npts2fit; ++i )
01262         {
01263             if( ws[i] )
01264             {
01265                 if( i > index )
01266                 {
01267                     us[index] = us[i];
01268                     ws[index] = ws[i];
01269                 }
01270                 bs_temp[index]                = bs[i];
01271                 bs_temp[index + npts_new]     = bs[i + npts2fit];
01272                 bs_temp[index + 2 * npts_new] = bs[i + 2 * npts2fit];
01273                 ++index;
01274             }
01275         }
01276         assert( index == npts_new );
01277         npts2fit = npts_new;
01278         us.resize( index );
01279         ws.resize( index );
01280         bs = bs_temp;
01281         // destroy bs_temp;
01282         std::vector< double >().swap( bs_temp );
01283     }
01284 
01285     // step 5. fitting
01286     eval_vander_univar_cmf( npts2fit, &( us[0] ), 3, &( bs[0] ), degree, &( ws[0] ), interp, safeguard, degree_out );
01287     // step 6. write results to output pointers
01288     int ncoeffs_out_pvpd = *degree_out + 1;
01289     assert( ncoeffs >= 3 * ncoeffs_out_pvpd );
01290     // write to coeffs consecutively, bs's size is not changed by eval_vander_univar_cmf
01291     coeffs[0] = coeffs[ncoeffs_out_pvpd] = coeffs[2 * ncoeffs_out_pvpd] = 0;
01292     for( int i = 0; i < ncoeffs_out_pvpd - interp; ++i )
01293     {
01294         coeffs[i + interp]                        = bs[i];
01295         coeffs[i + interp + ncoeffs_out_pvpd]     = bs[i + npts2fit];
01296         coeffs[i + interp + 2 * ncoeffs_out_pvpd] = bs[i + 2 * npts2fit];
01297     }
01298 }
01299 
01300 void HiReconstruction::eval_vander_univar_cmf( const int npts2fit, const double* us, const int ndim, double* bs,
01301                                                int degree, const double* ws, const bool interp, const bool safeguard,
01302                                                int* degree_out )
01303 {
01304     // step 1. determine degree of polynomials to fit according to number of points
01305     int ncols = degree + 1 - interp;
01306     while( npts2fit < ncols && degree >= 1 )
01307     {
01308         --degree;
01309         ncols = degree + 1 - interp;
01310     }
01311     if( !degree )
01312     {
01313         if( interp )
01314         {
01315             for( int icol = 0; icol < ndim; ++icol )
01316             {
01317                 bs[icol * npts2fit] = 0;
01318             }
01319         }
01320         for( int irow = 1; irow < npts2fit; ++irow )
01321         {
01322             for( int icol = 0; icol < ndim; ++icol )
01323             {
01324                 bs[icol * npts2fit + irow] = 0;
01325             }
01326         }
01327         *degree_out = 0;
01328         return;
01329     }
01330     // step 2. construct Vandermonde matrix
01331     std::vector< double > V;  // V(npts2fit*(ncols+interp));
01332     DGMSolver::gen_vander_multivar( npts2fit, 1, us, degree, V );
01333 
01334     if( interp ) { V.erase( V.begin(), V.begin() + npts2fit ); }
01335 
01336     // step 3. scale rows with respect to weights
01337     for( int i = 0; i < npts2fit; ++i )
01338     {
01339         for( int j = 0; j < ncols; ++j )
01340         {
01341             V[j * npts2fit + i] *= ws[i];
01342         }
01343         for( int k = 0; k < ndim; ++k )
01344         {
01345             bs[k * npts2fit + i] *= ws[i];
01346         }
01347     }
01348 
01349     // step 4. scale columns to reduce condition number
01350     std::vector< double > ts( ncols );
01351     DGMSolver::rescale_matrix( npts2fit, ncols, &( V[0] ), &( ts[0] ) );
01352 
01353     // step 5. perform Householder QR factorization
01354     std::vector< double > D( ncols );
01355     int rank;
01356     DGMSolver::qr_polyfit_safeguarded( npts2fit, ncols, &( V[0] ), &( D[0] ), &rank );
01357 
01358     // step 6. adjust degree of fitting
01359     int ncols_sub = ncols;
01360     while( rank < ncols_sub )
01361     {
01362         --degree;
01363         if( !degree )
01364         {
01365             // matrix is singular, consider curve on flat plane passing center and orthogonal to
01366             // tangent line
01367             *degree_out = 0;
01368             for( int i = 0; i < npts2fit; ++i )
01369             {
01370                 for( int k = 0; k < ndim; ++k )
01371                 {
01372                     bs[k * npts2fit + i] = 0;
01373                 }
01374             }
01375         }
01376         ncols_sub = degree + 1 - interp;
01377     }
01378 
01379     // step 7. compute Q'*bs
01380     DGMSolver::compute_qtransposeB( npts2fit, ncols_sub, &( V[0] ), ndim, bs );
01381 
01382     // step 8. perform backward substitution and scale solutions
01383     // assign diagonals of V
01384     for( int i = 0; i < ncols_sub; ++i )
01385     {
01386         V[i * npts2fit + i] = D[i];
01387     }
01388     // backsolve
01389     if( safeguard )
01390     {
01391         DGMSolver::backsolve_polyfit_safeguarded( 1, degree, interp, npts2fit, ncols, &( V[0] ), ndim, bs, ws,
01392                                                   degree_out );
01393     }
01394     else
01395     {
01396         DGMSolver::backsolve( npts2fit, ncols_sub, &( V[0] ), ndim, bs, &( ts[0] ) );
01397         *degree_out = degree;
01398     }
01399 }
01400 
01401 int HiReconstruction::compute_weights( const int nrows, const int ncols, const double* us, const int nngbs,
01402                                        const double* ngbnrms, const int degree, const double toler, double* ws )
01403 {
01404     assert( nrows <= _MAXPNTS && ws );
01405     bool interp = false;
01406     if( nngbs != nrows )
01407     {
01408         assert( nngbs == nrows + 1 );
01409         interp = true;
01410     }
01411     double epsilon = 1e-2;
01412 
01413     // First, compute squared distance from each input piont to the center
01414     for( int i = 0; i < nrows; ++i )
01415     {
01416         ws[i] = DGMSolver::vec_innerprod( ncols, us + i * ncols, us + i * ncols );
01417     }
01418 
01419     // Second, compute a small correction termt o guard against zero
01420     double h = 0;
01421     for( int i = 0; i < nrows; ++i )
01422     {
01423         h += ws[i];
01424     }
01425     h /= (double)nrows;
01426 
01427     // Finally, compute the weights for each vertex
01428     int nzeros = 0;
01429     for( int i = 0; i < nrows; ++i )
01430     {
01431         double costheta = DGMSolver::vec_innerprod( 3, ngbnrms, ngbnrms + 3 * ( i + interp ) );
01432         if( costheta > toler ) { ws[i] = costheta * pow( ws[i] / h + epsilon, -1 * (double)degree / 2.0 ); }
01433         else
01434         {
01435             ws[i] = 0;
01436             ++nzeros;
01437         }
01438     }
01439     return nzeros;
01440 }
01441 bool HiReconstruction::check_barycentric_coords( const int nws, const double* naturalcoords )
01442 {
01443     double sum = 0;
01444     for( int i = 0; i < nws; ++i )
01445     {
01446         if( naturalcoords[i] < -_MINEPS ) { return false; }
01447         sum += naturalcoords[i];
01448     }
01449     if( fabs( 1 - sum ) > _MINEPS ) { return false; }
01450     else
01451     {
01452         return true;
01453     }
01454 }
01455 }  // namespace moab
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines