1 #include "moab/DiscreteGeometry/HiReconstruction.hpp" 2 #include "moab/DiscreteGeometry/DGMSolver.hpp" 3 #include "moab/HalfFacetRep.hpp" 4 5 #ifdef MOAB_HAVE_MPI 6 #include "moab/ParallelComm.hpp" 7 #endif 8 #include "MBTagConventions.hpp" 9 10 #define HIREC_USE_AHF 11 12 #include <math.h> 13 #include <deque> 14 #include <iostream> 15 16 namespace moab 17 { HiReconstruction(Core * impl,ParallelComm * comm,EntityHandle meshIn,int minpnts,bool recwhole)18 HiReconstruction::HiReconstruction(Core *impl, ParallelComm *comm, EntityHandle meshIn, int minpnts, bool recwhole) 19 : mbImpl(impl),pcomm(comm),_mesh2rec(meshIn),_MINPNTS(minpnts){ 20 assert(NULL!=impl); 21 ErrorCode error; 22 _MINEPS = 1e-12; 23 _dim = 0; 24 _hasfittings = false; 25 _hasderiv = false; 26 #ifdef MOAB_HAVE_MPI 27 if(!pcomm){ 28 pcomm = moab::ParallelComm::get_pcomm(mbImpl,0); 29 } 30 #endif 31 32 error = initialize(recwhole); 33 if(MB_SUCCESS!=error){ 34 std::cout << "Error initializing HiReconstruction\n" << std::endl; 35 exit(1); 36 } 37 } 38 ~HiReconstruction()39 HiReconstruction::~HiReconstruction(){ 40 #ifdef MOAB_HAVE_AHF 41 ahf = NULL; 42 #else 43 delete ahf; 44 #endif 45 } 46 initialize(bool recwhole)47 ErrorCode HiReconstruction::initialize(bool recwhole){ 48 ErrorCode error; 49 50 #ifdef HIREC_USE_AHF 51 std::cout << "HIREC_USE_AHF: Initializing" << std::endl; 52 ahf = new HalfFacetRep(mbImpl,pcomm,_mesh2rec, false); 53 if(!ahf){ 54 return MB_MEMORY_ALLOCATION_FAILED; 55 } 56 error = ahf->initialize(); MB_CHK_ERR(error); 57 #else 58 ahf = NULL; 59 #endif 60 61 //error = ahf->get_entity_ranges(_inverts,_inedges,_infaces,_incells); MB_CHK_ERR(error); 62 error = mbImpl->get_entities_by_dimension(_mesh2rec,0,_inverts); MB_CHK_ERR(error); 63 error = mbImpl->get_entities_by_dimension(_mesh2rec,1,_inedges); MB_CHK_ERR(error); 64 error = mbImpl->get_entities_by_dimension(_mesh2rec,2,_infaces); MB_CHK_ERR(error); 65 error = mbImpl->get_entities_by_dimension(_mesh2rec,3,_incells); MB_CHK_ERR(error); 66 if(_inedges.size()&&_infaces.empty()&&_incells.empty()){ 67 _dim = 1; _MAXPNTS = 13; 68 }else if(_infaces.size()&&_incells.empty()){ 69 _dim = 2; _MAXPNTS = 128; 70 }else{ 71 MB_SET_ERR(MB_FAILURE,"Encountered a non-manifold mesh or a mesh with volume elements"); 72 } 73 74 75 //get locally hosted vertices by filtering pstatus 76 #ifdef MOAB_HAVE_MPI 77 if(pcomm){ 78 error = pcomm->filter_pstatus(_inverts,PSTATUS_GHOST,PSTATUS_NOT,-1,&_verts2rec); MB_CHK_ERR(error); 79 }else{ 80 _verts2rec = _inverts; 81 } 82 #else 83 _verts2rec = _inverts; 84 #endif 85 _nv2rec = _verts2rec.size(); 86 87 if(recwhole){ 88 //compute normals(surface) or tangent vector(curve) for all locally hosted vertices 89 if(2==_dim){ 90 compute_average_vertex_normals_surf(); 91 }else if(1==_dim){ 92 compute_average_vertex_tangents_curve(); 93 }else{ 94 MB_SET_ERR(MB_FAILURE,"Unknow space dimension"); 95 } 96 _hasderiv = true; 97 } 98 return error; 99 } 100 101 /*************************************************** 102 * User Interface for Reconstruction of Geometry * 103 ***************************************************/ 104 reconstruct3D_surf_geom(int degree,bool interp,bool safeguard,bool reset)105 ErrorCode HiReconstruction::reconstruct3D_surf_geom(int degree, bool interp, bool safeguard, bool reset){ 106 assert(2==_dim); 107 if(_hasfittings&&!reset){ 108 //This object has precomputed fitting results and user don't want to reset 109 return MB_SUCCESS; 110 }else{ 111 _initfittings = _hasfittings = false; 112 } 113 //initialize for geometric information 114 initialize_surf_geom(degree); 115 ErrorCode error; 116 double *coeffs,*coords; 117 int *degree_out; 118 int ncoeffs = (degree+2)*(degree+1)/2; 119 120 //DBG 121 int dcount = 0; 122 123 for(Range::iterator ivert=_verts2rec.begin();ivert!=_verts2rec.end();++ivert){ 124 int index = _verts2rec.index(*ivert); 125 //for debug 126 /*if(index==70){ 127 EntityHandle vid = *ivert; 128 double vertcoords[3]; 129 error = mbImpl->get_coords(&vid,1,vertcoords); 130 }*/ 131 132 size_t istr = _vertID2coeffID[index]; 133 coords = &(_local_coords[9*index]); 134 coeffs = &(_local_fit_coeffs[istr]); 135 degree_out = &(_degrees_out[index]); 136 _interps[index] = interp; 137 error = polyfit3d_walf_surf_vertex(*ivert,interp,degree,_MINPNTS,safeguard,9,coords,degree_out,ncoeffs,coeffs);MB_CHK_ERR(error); 138 139 //DBG 140 if (degree_out[0] < degree) 141 dcount += 1; 142 143 } 144 145 //DBG 146 //std::cout<<"Total #points ="<<_verts2rec.size()<<", #degraded points = "<<dcount<<std::endl; 147 148 _geom = HISURFACE; 149 _hasfittings = true; 150 return error; 151 } 152 reconstruct3D_surf_geom(size_t npts,int * degrees,bool * interps,bool safeguard,bool reset)153 ErrorCode HiReconstruction::reconstruct3D_surf_geom(size_t npts, int* degrees, bool* interps, bool safeguard, bool reset){ 154 assert(_dim==2); 155 if(npts!=_nv2rec){ 156 MB_SET_ERR(MB_FAILURE,"Input number of degrees doesn't match number of vertices"); 157 } 158 if(_hasfittings&&!reset){ 159 return MB_SUCCESS; 160 }else{ 161 _initfittings = _hasfittings = false; 162 } 163 ErrorCode error; 164 //initialization for fitting 165 initialize_surf_geom(npts,degrees); 166 double *coeffs,*coords; 167 int *degree_out; 168 size_t i=0; 169 for(Range::iterator ivert=_verts2rec.begin();ivert!=_verts2rec.end();++ivert,++i){ 170 int index = _verts2rec.index(*ivert); assert(-1!=index); 171 size_t istr = _vertID2coeffID[index]; 172 coords = &(_local_coords[9*index]); 173 coeffs = &(_local_fit_coeffs[istr]); 174 degree_out = &(_degrees_out[index]); 175 _interps[index] = interps[i]; 176 int ncoeffs = (degrees[i]+2)*(degrees[i]+1)/2; 177 error = polyfit3d_walf_surf_vertex(*ivert,interps[i],degrees[i],_MINPNTS,safeguard,9,coords,degree_out,ncoeffs,coeffs); 178 MB_CHK_ERR(error); 179 } 180 _geom = HISURFACE; 181 _hasfittings = true; 182 return error; 183 } 184 reconstruct3D_curve_geom(int degree,bool interp,bool safeguard,bool reset)185 ErrorCode HiReconstruction::reconstruct3D_curve_geom(int degree, bool interp, bool safeguard, bool reset){ 186 assert(_dim==1); 187 if(_hasfittings&&!reset){ 188 return MB_SUCCESS; 189 }else{ 190 _initfittings = _hasfittings = false; 191 } 192 initialize_3Dcurve_geom(degree); 193 ErrorCode error; 194 double *coords=0,*coeffs; 195 int *degree_out; 196 int ncoeffs = 3*(degree+1); 197 for(Range::iterator ivert=_verts2rec.begin();ivert!=_verts2rec.end();++ivert){ 198 int index = _verts2rec.index(*ivert); assert(index!=-1); 199 size_t istr = _vertID2coeffID[index]; 200 coeffs = &(_local_fit_coeffs[istr]); 201 degree_out = &(_degrees_out[index]); 202 _interps[index] = interp; 203 error = polyfit3d_walf_curve_vertex(*ivert,interp,degree,_MINPNTS,safeguard,0,coords,degree_out,ncoeffs,coeffs); MB_CHK_ERR(error); 204 } 205 _geom = HI3DCURVE; 206 _hasfittings = true; 207 return error; 208 } 209 reconstruct3D_curve_geom(size_t npts,int * degrees,bool * interps,bool safeguard,bool reset)210 ErrorCode HiReconstruction::reconstruct3D_curve_geom(size_t npts, int* degrees, bool* interps, bool safeguard, bool reset){ 211 assert(_dim==1); 212 ErrorCode error; 213 if(npts!=_nv2rec){ 214 MB_SET_ERR(MB_FAILURE,"Input number of degrees doesn't match the number of vertices"); 215 } 216 if(_hasfittings&&!reset){ 217 return MB_SUCCESS; 218 }else{ 219 _initfittings = _hasfittings = false; 220 } 221 //initialize 222 initialize_3Dcurve_geom(npts,degrees); 223 double *coords=0,*coeffs; 224 int *degree_out; 225 size_t i=0; 226 for(Range::iterator ivert=_verts2rec.begin();ivert!=_verts2rec.end();++ivert,++i){ 227 int index = _verts2rec.index(*ivert); 228 size_t istr = _vertID2coeffID[index]; 229 coeffs = &(_local_fit_coeffs[istr]); 230 degree_out = &(_degrees_out[index]); 231 _interps[index] = interps[i]; 232 int ncoeffs = 3*(degrees[i]+1); 233 error = polyfit3d_walf_curve_vertex(*ivert,interps[i],degrees[i],_MINPNTS,safeguard,0,coords,degree_out,ncoeffs,coeffs); 234 MB_CHK_ERR(error); 235 } 236 _geom = HI3DCURVE; 237 _hasfittings = true; 238 return error; 239 } 240 polyfit3d_walf_surf_vertex(const EntityHandle vid,const bool interp,int degree,int minpnts,const bool safeguard,const int ncoords,double * coords,int * degree_out,const int ncoeffs,double * coeffs)241 ErrorCode HiReconstruction::polyfit3d_walf_surf_vertex(const EntityHandle vid, const bool interp, int degree, int minpnts, const bool safeguard, const int ncoords, double* coords, int* degree_out, const int ncoeffs, double* coeffs){ 242 assert(_dim==2); 243 ErrorCode error; 244 int ring = estimate_num_rings(degree,interp); 245 //std::cout<<"ring = "<<ring<<std::endl; 246 //get n-ring neighbors 247 Range ngbvs; 248 error = obtain_nring_ngbvs(vid,ring,minpnts,ngbvs); MB_CHK_ERR(error); 249 //for debug 250 /*if(_verts2rec.index(vid)==70){ 251 for(Range::iterator ingb=ngbvs.begin();ingb!=ngbvs.end();++ingb) std::cerr << _verts2rec.index(*ingb) << " "; 252 std::cout << std::endl; 253 }*/ 254 255 //get coordinates; 256 size_t nverts = ngbvs.size(); assert(nverts); 257 double *ngbcoords = new double[nverts*3]; 258 error = mbImpl->get_coords(ngbvs,ngbcoords); MB_CHK_ERR(error); 259 //get normals 260 double *ngbnrms = new double[nverts*3]; 261 error = get_normals_surf(ngbvs,ngbnrms); MB_CHK_ERR(error); 262 //switch vid to first one 263 int index = ngbvs.index(vid); assert(index!=-1); 264 std::swap(ngbcoords[0],ngbcoords[3*index]);std::swap(ngbcoords[1],ngbcoords[3*index+1]);std::swap(ngbcoords[2],ngbcoords[3*index+2]); 265 std::swap(ngbnrms[0],ngbnrms[3*index]);std::swap(ngbnrms[1],ngbnrms[3*index+1]);std::swap(ngbnrms[2],ngbnrms[3*index+2]); 266 //local WLS fitting 267 int degree_pnt,degree_qr; 268 polyfit3d_surf_get_coeff(nverts,ngbcoords,ngbnrms,degree,interp,safeguard,ncoords,coords,ncoeffs,coeffs,degree_out,°ree_pnt,°ree_qr); 269 delete [] ngbcoords; delete [] ngbnrms; 270 return error; 271 } 272 polyfit3d_walf_curve_vertex(const EntityHandle vid,const bool interp,int degree,int minpnts,const bool safeguard,const int ncoords,double * coords,int * degree_out,const int ncoeffs,double * coeffs)273 ErrorCode HiReconstruction::polyfit3d_walf_curve_vertex(const EntityHandle vid, const bool interp, int degree, int minpnts, const bool safeguard, const int ncoords, double* coords, int* degree_out, const int ncoeffs, double* coeffs){ 274 ErrorCode error; 275 int ring = estimate_num_rings(degree,interp); 276 //get n-ring neighbors 277 Range ngbvs; 278 error = obtain_nring_ngbvs(vid,ring,minpnts,ngbvs); MB_CHK_ERR(error); 279 //get coordinates 280 size_t nverts = ngbvs.size(); assert(nverts); 281 double *ngbcoords = new double[nverts*3]; 282 error = mbImpl->get_coords(ngbvs,ngbcoords); MB_CHK_ERR(error); 283 //get tangent vectors 284 double *ngbtangs = new double[nverts*3]; 285 error = get_tangents_curve(ngbvs,ngbtangs); MB_CHK_ERR(error); 286 //switch vid to first one 287 int index = ngbvs.index(vid); assert(index!=-1); 288 std::swap(ngbcoords[0],ngbcoords[3*index]);std::swap(ngbcoords[1],ngbcoords[3*index+1]);std::swap(ngbcoords[2],ngbcoords[3*index+2]); 289 std::swap(ngbtangs[0],ngbtangs[3*index]);std::swap(ngbtangs[1],ngbtangs[3*index+1]);std::swap(ngbtangs[2],ngbtangs[3*index+2]); 290 //local WLS fittings 291 polyfit3d_curve_get_coeff(nverts,ngbcoords,ngbtangs,degree,interp,safeguard,ncoords,coords,ncoeffs,coeffs,degree_out); 292 delete [] ngbcoords; delete [] ngbtangs; 293 return error; 294 } 295 296 /************************************************************** 297 * User Interface for Evaluation via Reconstructed Geometry * 298 **************************************************************/ 299 hiproj_walf_in_element(EntityHandle elem,const int nvpe,const int npts2fit,const double * naturalcoords2fit,double * newcoords)300 ErrorCode HiReconstruction::hiproj_walf_in_element(EntityHandle elem, const int nvpe, const int npts2fit, const double* naturalcoords2fit, double* newcoords){ 301 assert(newcoords); 302 ErrorCode error; 303 //get connectivity table 304 std::vector<EntityHandle> elemconn; 305 error = mbImpl->get_connectivity(&elem,1,elemconn); MB_CHK_ERR(error); 306 if(nvpe!=(int)elemconn.size()){ 307 MB_SET_ERR(MB_FAILURE,"element connectivity table size doesn't match input size"); 308 } 309 310 if(!_hasfittings){ 311 MB_SET_ERR(MB_FAILURE,"There is no existing fitting results"); 312 }else{ 313 std::ostringstream convert; convert << elem; std::string ID = convert.str(); 314 for(int i=0;i<nvpe;++i){ 315 if(-1==_verts2rec.index(elemconn[i])){ 316 MB_SET_ERR(MB_FAILURE,"There is no existing fitting results for element "+ID); 317 } 318 } 319 } 320 //check correctness of input 321 for(int i=0;i<npts2fit;++i){ 322 if(!check_barycentric_coords(nvpe,naturalcoords2fit+i*nvpe)){ 323 MB_SET_ERR(MB_FAILURE,"Wrong barycentric coordinates"); 324 } 325 } 326 327 double *elemcoords = new double[nvpe*3]; 328 error = mbImpl->get_coords(&(elemconn[0]),nvpe,elemcoords); MB_CHK_ERR(error); 329 330 double *coords2fit = new double[3*npts2fit](); 331 for(int i=0;i<npts2fit;++i){ 332 for(int j=0;j<nvpe;++j){ 333 coords2fit[3*i] += naturalcoords2fit[i*nvpe+j]*elemcoords[3*j]; 334 coords2fit[3*i+1] += naturalcoords2fit[i*nvpe+j]*elemcoords[3*j+1]; 335 coords2fit[3*i+2] += naturalcoords2fit[i*nvpe+j]*elemcoords[3*j+2]; 336 } 337 } 338 339 double *hiproj_new = new double[3*npts2fit]; 340 //initialize output 341 for(int i=0;i<npts2fit;++i){ 342 newcoords[3*i] = newcoords[3*i+1] = newcoords[3*i+2] = 0; 343 } 344 //for each input vertex, call nvpe fittings and take average 345 for(int j=0;j<nvpe;++j){ 346 error = hiproj_walf_around_vertex(elemconn[j],npts2fit,coords2fit,hiproj_new); MB_CHK_ERR(error); 347 for(int i=0;i<npts2fit;++i){ 348 newcoords[3*i] += naturalcoords2fit[i*nvpe+j]*hiproj_new[3*i]; 349 newcoords[3*i+1] += naturalcoords2fit[i*nvpe+j]*hiproj_new[3*i+1]; 350 newcoords[3*i+2] += naturalcoords2fit[i*nvpe+j]*hiproj_new[3*i+2]; 351 } 352 } 353 delete [] elemcoords; delete [] coords2fit; delete [] hiproj_new; 354 return error; 355 } 356 hiproj_walf_around_vertex(EntityHandle vid,const int npts2fit,const double * coords2fit,double * hiproj_new)357 ErrorCode HiReconstruction::hiproj_walf_around_vertex(EntityHandle vid, const int npts2fit, const double* coords2fit, double* hiproj_new){ 358 if(!_hasfittings){ 359 MB_SET_ERR(MB_FAILURE,"There is no existing fitting results"); 360 }else if(-1==_verts2rec.index(vid)){ 361 std::ostringstream convert; convert << vid; std::string VID = convert.str(); 362 MB_SET_ERR(MB_FAILURE,"There is no existing fitting results for vertex "+VID); 363 } 364 ErrorCode error; 365 //get center of local coordinates system 366 double local_origin[3]; 367 error = mbImpl->get_coords(&vid,1,local_origin); MB_CHK_ERR(error); 368 //get local fitting parameters 369 int index = _verts2rec.index(vid); 370 bool interp = _interps[index]; 371 int local_deg = _degrees_out[index]; 372 double *uvw_coords,*local_coeffs; 373 if(_geom==HISURFACE){ 374 uvw_coords = &(_local_coords[9*index]); 375 //int ncoeffs = (local_deg+2)*(local_deg+1)>>1; 376 size_t istr = _vertID2coeffID[index]; 377 local_coeffs = &(_local_fit_coeffs[istr]); 378 walf3d_surf_vertex_eval(local_origin,uvw_coords,local_deg,local_coeffs,interp,npts2fit,coords2fit,hiproj_new); 379 }else if(_geom==HI3DCURVE){ 380 uvw_coords = &(_local_coords[3*index]); 381 size_t istr = _vertID2coeffID[index]; 382 local_coeffs = &(_local_fit_coeffs[istr]); 383 walf3d_curve_vertex_eval(local_origin,uvw_coords,local_deg,local_coeffs,interp,npts2fit,coords2fit,hiproj_new); 384 } 385 return error; 386 } 387 walf3d_surf_vertex_eval(const double * local_origin,const double * local_coords,const int local_deg,const double * local_coeffs,const bool interp,const int npts2fit,const double * coords2fit,double * hiproj_new)388 void HiReconstruction::walf3d_surf_vertex_eval(const double* local_origin, const double* local_coords, const int local_deg, const double* local_coeffs, const bool interp, const int npts2fit, const double* coords2fit, double* hiproj_new){ 389 double xaxis[3], yaxis[3], zaxis[3]; 390 for(int i=0;i<3;++i){ 391 xaxis[i] = local_coords[i]; 392 yaxis[i] = local_coords[3+i]; 393 zaxis[i] = local_coords[6+i]; 394 } 395 //double *basis = new double[(local_deg+2)*(local_deg+1)/2-1]; 396 std::vector<double> basis((local_deg+2)*(local_deg+1)/2-1); 397 for(int i=0;i<npts2fit;++i){ 398 double local_pos[3]; 399 for(int j=0;j<3;++j){ 400 local_pos[j] = coords2fit[3*i+j]-local_origin[j]; 401 } 402 double u,v,height=0; 403 u = DGMSolver::vec_innerprod(3,local_pos,xaxis); 404 v = DGMSolver::vec_innerprod(3,local_pos,yaxis); 405 basis[0] = u; 406 basis[1] = v; 407 int l=1; 408 for(int k=2;k<=local_deg;++k){ 409 ++l; 410 basis[l] = u*basis[l-k]; 411 for(int id=0;id<k;++id){ 412 ++l; 413 basis[l] = basis[l-k-1]*v; 414 } 415 } 416 if(!interp){ 417 height = local_coeffs[0]; 418 } 419 for(int p=0;p<=l;++p){ 420 height += local_coeffs[p+1]*basis[p]; 421 } 422 hiproj_new[3*i] = local_origin[0]+u*xaxis[0]+v*yaxis[0]+height*zaxis[0]; 423 hiproj_new[3*i+1] = local_origin[1]+u*xaxis[1]+v*yaxis[1]+height*zaxis[1]; 424 hiproj_new[3*i+2] = local_origin[2]+u*xaxis[2]+v*yaxis[2]+height*zaxis[2]; 425 } 426 //delete [] basis; 427 } 428 walf3d_curve_vertex_eval(const double * local_origin,const double * local_coords,const int local_deg,const double * local_coeffs,const bool interp,const int npts2fit,const double * coords2fit,double * hiproj_new)429 void HiReconstruction::walf3d_curve_vertex_eval(const double* local_origin, const double* local_coords, const int local_deg, const double* local_coeffs, const bool interp, const int npts2fit, const double* coords2fit, double* hiproj_new){ 430 assert(local_origin&&local_coords&&local_coeffs); 431 int ncoeffspvpd = local_deg+1; 432 for(int i=0;i<npts2fit;++i){ 433 //get the vector from center to current point, project to tangent line 434 double vec[3],ans[3]={0,0,0}; 435 DGMSolver::vec_linear_operation(3,1,coords2fit+3*i,-1,local_origin,vec); 436 double u = DGMSolver::vec_innerprod(3,local_coords,vec); 437 //evaluate polynomials 438 if(!interp){ 439 ans[0] = local_coeffs[0]; ans[1] = local_coeffs[ncoeffspvpd]; ans[2] = local_coeffs[2*ncoeffspvpd]; 440 } 441 double uk=1;//degree_out and degree different, stored in columnwise contiguously 442 for(int j=1;j<ncoeffspvpd;++j){ 443 uk *= u; 444 ans[0] += uk*local_coeffs[j]; ans[1] += uk*local_coeffs[j+ncoeffspvpd]; ans[2] += uk*local_coeffs[j+2*ncoeffspvpd]; 445 } 446 hiproj_new[3*i] = ans[0]+local_origin[0]; 447 hiproj_new[3*i+1] = ans[1]+local_origin[1]; 448 hiproj_new[3*i+2] = ans[2]+local_origin[2]; 449 } 450 } 451 get_fittings_data(EntityHandle vid,GEOMTYPE & geomtype,std::vector<double> & coords,int & degree_out,std::vector<double> & coeffs,bool & interp)452 bool HiReconstruction::get_fittings_data(EntityHandle vid, GEOMTYPE& geomtype, std::vector<double>& coords, int& degree_out, std::vector<double>& coeffs, bool& interp){ 453 if(!_hasfittings){ 454 return false; 455 }else{ 456 int index = _verts2rec.index(vid); 457 if(-1==index){ 458 std::cout << "Input vertex is not locally hosted vertex in this mesh set" << std::endl; 459 return false; 460 } 461 geomtype = _geom; 462 if(HISURFACE==_geom){ 463 coords.insert(coords.end(),_local_coords.begin()+9*index,_local_coords.begin()+9*index+9); 464 degree_out = _degrees_out[index]; interp = _interps[index]; 465 int ncoeffs = (degree_out+2)*(degree_out+1)>>1; 466 size_t istr = _vertID2coeffID[index]; 467 coeffs.insert(coeffs.end(),_local_fit_coeffs.begin()+istr,_local_fit_coeffs.begin()+istr+ncoeffs); 468 }else if(HI3DCURVE==_geom){ 469 coords.insert(coords.end(),_local_coords.begin()+3*index,_local_coords.begin()+3*index+3); 470 degree_out = _degrees_out[index]; interp = _interps[index]; 471 int ncoeffs = 3*(degree_out+1); 472 size_t istr = _vertID2coeffID[index]; 473 coeffs.insert(coeffs.end(),_local_fit_coeffs.begin()+istr,_local_fit_coeffs.begin()+istr+ncoeffs); 474 } 475 return true; 476 } 477 } 478 479 /**************************************************************** 480 * Basic Internal Routines to initialize and set fitting data * 481 ****************************************************************/ 482 estimate_num_rings(int degree,bool interp)483 int HiReconstruction::estimate_num_rings(int degree, bool interp){ 484 return interp?((degree+1)>>1)+((degree+1)&1):((degree+2)>>1)+((degree+2)&1); 485 } 486 vertex_get_incident_elements(const EntityHandle & vid,const int elemdim,std::vector<EntityHandle> & adjents)487 ErrorCode HiReconstruction::vertex_get_incident_elements(const EntityHandle& vid, const int elemdim, std::vector<EntityHandle>& adjents){ 488 ErrorCode error; 489 assert(elemdim==_dim); 490 #ifdef HIREC_USE_AHF 491 error = ahf->get_up_adjacencies(vid,elemdim,adjents); MB_CHK_ERR(error); 492 #else 493 error = mbImpl->get_adjacencies(&vid,1,elemdim,false,adjents); MB_CHK_ERR(error); 494 #endif 495 return error; 496 } 497 obtain_nring_ngbvs(const EntityHandle vid,int ring,const int minpnts,Range & ngbvs)498 ErrorCode HiReconstruction::obtain_nring_ngbvs(const EntityHandle vid, int ring, const int minpnts, Range& ngbvs){ 499 ErrorCode error; 500 std::deque<EntityHandle> todo; 501 todo.push_back(vid); ngbvs.insert(vid); 502 EntityHandle pre,nxt; 503 for(int i=1;i<=ring;++i){ 504 int count = todo.size(); 505 while(count){ 506 EntityHandle center = todo.front(); 507 todo.pop_front(); --count; 508 std::vector<EntityHandle> adjents; 509 error = vertex_get_incident_elements(center,_dim,adjents); MB_CHK_ERR(error); 510 for(size_t j=0;j<adjents.size();++j){ 511 std::vector<EntityHandle> elemconn; 512 error = mbImpl->get_connectivity(&adjents[j],1,elemconn); MB_CHK_ERR(error); 513 int nvpe = elemconn.size(); 514 for(int k=0;k<nvpe;++k){ 515 if(elemconn[k]==center){ 516 pre = k==0?elemconn[nvpe-1]:elemconn[k-1]; 517 nxt = elemconn[(k+1)%nvpe]; 518 if(ngbvs.find(pre)==ngbvs.end()){ 519 ngbvs.insert(pre); 520 todo.push_back(pre); 521 } 522 if(ngbvs.find(nxt)==ngbvs.end()){ 523 ngbvs.insert(nxt); 524 todo.push_back(nxt); 525 } 526 break; 527 } 528 } 529 } 530 } 531 if(_MAXPNTS<=(int)ngbvs.size()){ 532 //obtain enough points 533 return error; 534 } 535 if(!todo.size()){ 536 //current ring cannot introduce any points, return incase deadlock 537 return error; 538 } 539 if((i==ring)&& (minpnts>(int)ngbvs.size())){ 540 //reach maximum ring but not enough points 541 ++ring; 542 } 543 } 544 return error; 545 } 546 initialize_surf_geom(const int degree)547 void HiReconstruction::initialize_surf_geom(const int degree){ 548 if(!_hasderiv){ 549 compute_average_vertex_normals_surf(); 550 _hasderiv = true; 551 } 552 if(!_initfittings){ 553 int ncoeffspv = (degree+2)*(degree+1)/2; 554 _degrees_out.assign(_nv2rec,0); 555 _interps.assign(_nv2rec,false); 556 _vertID2coeffID.resize(_nv2rec); 557 _local_fit_coeffs.assign(_nv2rec*ncoeffspv,0); 558 for(size_t i=0;i<_nv2rec;++i){ 559 _vertID2coeffID[i] = i*ncoeffspv; 560 } 561 _initfittings = true; 562 } 563 } 564 initialize_surf_geom(const size_t npts,const int * degrees)565 void HiReconstruction::initialize_surf_geom(const size_t npts, const int* degrees){ 566 if(!_hasderiv){ 567 compute_average_vertex_normals_surf(); 568 _hasderiv = true; 569 } 570 if(!_initfittings){ 571 assert(_nv2rec==npts); 572 _degrees_out.assign(_nv2rec,0); 573 _interps.assign(_nv2rec,false); 574 _vertID2coeffID.resize(_nv2rec); 575 size_t index=0; 576 for(size_t i=0;i<_nv2rec;++i){ 577 _vertID2coeffID[i] = index; 578 index += (degrees[i]+2)*(degrees[i]+1)/2; 579 } 580 _local_fit_coeffs.assign(index,0); 581 _initfittings = true; 582 } 583 } 584 initialize_3Dcurve_geom(const int degree)585 void HiReconstruction::initialize_3Dcurve_geom(const int degree){ 586 if(!_hasderiv){ 587 compute_average_vertex_tangents_curve(); 588 _hasderiv = true; 589 } 590 if(!_initfittings){ 591 int ncoeffspvpd = degree+1; 592 _degrees_out.assign(_nv2rec,0); 593 _interps.assign(_nv2rec,false); 594 _vertID2coeffID.resize(_nv2rec); 595 _local_fit_coeffs.assign(_nv2rec*ncoeffspvpd*3,0); 596 for(size_t i=0;i<_nv2rec;++i){ 597 _vertID2coeffID[i] = i*ncoeffspvpd*3; 598 } 599 _initfittings = true; 600 } 601 } 602 initialize_3Dcurve_geom(const size_t npts,const int * degrees)603 void HiReconstruction::initialize_3Dcurve_geom(const size_t npts, const int* degrees){ 604 if(!_hasderiv){ 605 compute_average_vertex_tangents_curve(); 606 _hasderiv = true; 607 } 608 if(!_hasfittings){ 609 assert(_nv2rec==npts); 610 _degrees_out.assign(_nv2rec,0); 611 _interps.assign(_nv2rec,false); 612 _vertID2coeffID.reserve(_nv2rec); 613 size_t index=0; 614 for(size_t i=0;i<_nv2rec;++i){ 615 _vertID2coeffID[i] = index; 616 index += 3*(degrees[i]+1); 617 } 618 _local_fit_coeffs.assign(index,0); 619 _initfittings = true; 620 } 621 } 622 623 /* ErrorCode HiReconstruction::set_geom_data_surf(const EntityHandle vid, const double* coords, const double degree_out, const double* coeffs, bool interp) 624 { 625 return MB_SUCCESS; 626 } 627 628 ErrorCode HiReconstruction::set_geom_data_3Dcurve(const EntityHandle vid, const double* coords, const double degree_out, const double* coeffs, bool interp) 629 { 630 return MB_SUCCESS; 631 } */ 632 633 /********************************************************* 634 * Routines for vertex normal/tangent vector estimation * 635 *********************************************************/ average_vertex_normal(const EntityHandle vid,double * nrm)636 ErrorCode HiReconstruction::average_vertex_normal(const EntityHandle vid, double* nrm){ 637 ErrorCode error; 638 std::vector<EntityHandle> adjfaces; 639 error = vertex_get_incident_elements(vid,2,adjfaces); MB_CHK_ERR(error); 640 int npolys = adjfaces.size(); 641 if(!npolys){ 642 MB_SET_ERR(MB_FAILURE,"Vertex has no incident 2D entities"); 643 }else{ 644 double v1[3],v2[3],v3[3],a[3],b[3],c[3]; 645 nrm[0] = nrm[1] = nrm[2] = 0; 646 for(int i=0;i<npolys;++i){ 647 //get incident "triangles" 648 std::vector<EntityHandle> elemconn; 649 error = mbImpl->get_connectivity(&adjfaces[i],1,elemconn); MB_CHK_ERR(error); 650 EntityHandle pre,nxt; 651 int nvpe = elemconn.size(); 652 for(int j=0;j<nvpe;++j){ 653 if(vid==elemconn[j]){ 654 pre = j==0?elemconn[nvpe-1]:elemconn[j-1]; 655 nxt = elemconn[(j+1)%nvpe]; 656 break; 657 } 658 } 659 //compute area weighted normals 660 error = mbImpl->get_coords(&pre,1,a); MB_CHK_ERR(error); 661 error = mbImpl->get_coords(&vid,1,b); MB_CHK_ERR(error); 662 error = mbImpl->get_coords(&nxt,1,c); MB_CHK_ERR(error); 663 DGMSolver::vec_linear_operation(3,1,c,-1,b,v1); 664 DGMSolver::vec_linear_operation(3,1,a,-1,b,v2); 665 DGMSolver::vec_crossprod(v1,v2,v3); 666 DGMSolver::vec_linear_operation(3,1,nrm,1,v3,nrm); 667 } 668 #ifndef NDEBUG 669 assert ( DGMSolver::vec_normalize(3,nrm,nrm) ); 670 #endif 671 } 672 return error; 673 } 674 compute_average_vertex_normals_surf()675 ErrorCode HiReconstruction::compute_average_vertex_normals_surf(){ 676 if(_hasderiv){ 677 return MB_SUCCESS; 678 } 679 ErrorCode error; 680 _local_coords.assign(9*_nv2rec,0); 681 size_t index=0; 682 for(Range::iterator ivert=_verts2rec.begin();ivert!=_verts2rec.end();++ivert,++index){ 683 error = average_vertex_normal(*ivert,&(_local_coords[9*index+6])); MB_CHK_ERR(error); 684 } 685 return error; 686 } 687 get_normals_surf(const Range & vertsh,double * nrms)688 ErrorCode HiReconstruction::get_normals_surf(const Range& vertsh, double* nrms){ 689 ErrorCode error = MB_SUCCESS; 690 if(_hasderiv){ 691 size_t id=0; 692 for(Range::iterator ivert=vertsh.begin();ivert!=vertsh.end();++ivert,++id){ 693 int index = _verts2rec.index(*ivert); 694 #ifdef MOAB_HAVE_MPI 695 if(-1==index){ 696 //ghost vertex 697 error = average_vertex_normal(*ivert,nrms+3*id); MB_CHK_ERR(error); 698 }else{ 699 nrms[3*id] = _local_coords[9*index+6]; 700 nrms[3*id+1] = _local_coords[9*index+7]; 701 nrms[3*id+2] = _local_coords[9*index+8]; 702 } 703 #else 704 assert(-1!=index); 705 nrms[3*id] = _local_coords[9*index+6]; 706 nrms[3*id+1] = _local_coords[9*index+7]; 707 nrms[3*id+2] = _local_coords[9*index+8]; 708 #endif 709 } 710 }else{ 711 size_t id=0; 712 for(Range::iterator ivert=vertsh.begin();ivert!=vertsh.end();++ivert,++id){ 713 error = average_vertex_normal(*ivert,nrms+3*id); MB_CHK_ERR(error); 714 } 715 } 716 return error; 717 } 718 average_vertex_tangent(const EntityHandle vid,double * tang)719 ErrorCode HiReconstruction::average_vertex_tangent(const EntityHandle vid, double* tang){ 720 ErrorCode error; 721 std::vector<EntityHandle> adjedges; 722 error = vertex_get_incident_elements(vid,1,adjedges); MB_CHK_ERR(error); 723 int nedges = adjedges.size(); 724 if(!nedges){ 725 MB_SET_ERR(MB_FAILURE,"Vertex has no incident edges"); 726 }else{ 727 assert(nedges<=2); 728 tang[0] = tang[1] = tang[2] = 0; 729 for(int i=0;i<nedges;++i){ 730 std::vector<EntityHandle> edgeconn; 731 error = mbImpl->get_connectivity(&adjedges[i],1,edgeconn); 732 double istr[3],iend[3],t[3]; 733 error = mbImpl->get_coords(&(edgeconn[0]),1,istr); 734 error = mbImpl->get_coords(&(edgeconn[1]),1,iend); 735 DGMSolver::vec_linear_operation(3,1,iend,-1,istr,t); 736 DGMSolver::vec_linear_operation(3,1,tang,1,t,tang); 737 } 738 #ifndef NDEBUG 739 assert ( DGMSolver::vec_normalize(3,tang,tang) ); 740 #endif 741 } 742 return error; 743 } 744 compute_average_vertex_tangents_curve()745 ErrorCode HiReconstruction::compute_average_vertex_tangents_curve(){ 746 if(_hasderiv){ 747 return MB_SUCCESS; 748 } 749 ErrorCode error; 750 _local_coords.assign(3*_nv2rec,0); 751 size_t index=0; 752 for(Range::iterator ivert=_verts2rec.begin();ivert!=_verts2rec.end();++ivert,++index){ 753 error = average_vertex_tangent(*ivert,&(_local_coords[3*index])); MB_CHK_ERR(error); 754 } 755 return error; 756 } 757 get_tangents_curve(const Range & vertsh,double * tangs)758 ErrorCode HiReconstruction::get_tangents_curve(const Range& vertsh, double* tangs){ 759 ErrorCode error=MB_SUCCESS; 760 if(_hasderiv){ 761 size_t id=0; 762 for(Range::iterator ivert=vertsh.begin();ivert!=vertsh.end();++ivert,++id){ 763 int index = _verts2rec.index(*ivert); 764 #ifdef MOAB_HAVE_MPI 765 if(-1!=index){ 766 tangs[3*id] = _local_coords[3*index]; 767 tangs[3*id+1] = _local_coords[3*index+1]; 768 tangs[3*id+2] = _local_coords[3*index+2]; 769 }else{ 770 error = average_vertex_tangent(*ivert,tangs+3*id); MB_CHK_ERR(error); 771 } 772 #else 773 assert(-1!=index); 774 tangs[3*id] = _local_coords[3*index]; 775 tangs[3*id+1] = _local_coords[3*index+1]; 776 tangs[3*id+2] = _local_coords[3*index+2]; 777 #endif 778 } 779 }else{ 780 size_t id=0; 781 for(Range::iterator ivert=vertsh.begin();ivert!=vertsh.end();++ivert,++id){ 782 error = average_vertex_tangent(*ivert,tangs+3*id); MB_CHK_ERR(error); 783 } 784 } 785 return error; 786 } 787 788 /************************************************ 789 * Internal Routines for local WLS fittings * 790 *************************************************/ 791 polyfit3d_surf_get_coeff(const int nverts,const double * ngbcoords,const double * ngbnrms,int degree,const bool interp,const bool safeguard,const int ncoords,double * coords,const int ncoeffs,double * coeffs,int * degree_out,int * degree_pnt,int * degree_qr)792 void HiReconstruction::polyfit3d_surf_get_coeff(const int nverts, const double* ngbcoords, const double* ngbnrms, int degree, const bool interp, const bool safeguard, const int ncoords, double* coords, const int ncoeffs, double* coeffs, int* degree_out, int* degree_pnt, int* degree_qr){ 793 if(nverts<=0){ 794 return; 795 } 796 797 //std::cout << "npnts in initial stencil = " << nverts << std::endl; 798 //std::cout << "centered at (" << ngbcoords[0] << "," << ngbcoords[1] << "," << ngbcoords[2] << ")" << std::endl; 799 800 //step 1. copmute local coordinate system 801 double nrm[3] = {ngbnrms[0],ngbnrms[1],ngbnrms[2]}, tang1[3] = {0,0,0}, tang2[3] = {0,0,0}; 802 if(fabs(nrm[0])>fabs(nrm[1])&&fabs(nrm[0])>fabs(nrm[2])){ 803 tang1[1] = 1.0; 804 }else{ 805 tang1[0] = 1.0; 806 } 807 808 DGMSolver::vec_projoff(3,tang1,nrm,tang1); 809 #ifndef NDEBUG 810 assert (DGMSolver::vec_normalize(3,tang1,tang1)); 811 #endif 812 DGMSolver::vec_crossprod(nrm,tang1,tang2); 813 if(9<=ncoords&&coords){ 814 coords[0] = tang1[0]; coords[1] = tang1[1]; coords[2] = tang1[2]; 815 coords[3] = tang2[0]; coords[4] = tang2[1]; coords[5] = tang2[2]; 816 coords[6] = nrm[0]; coords[7] = nrm[1]; coords[8] = nrm[2]; 817 } 818 if(!ncoeffs||!coeffs){ 819 return; 820 }else{ 821 assert(ncoeffs>=(degree+2)*(degree+1)/2); 822 } 823 824 //step 2. project onto local coordinates system 825 int npts2fit = nverts-interp; 826 if(0==npts2fit){ 827 *degree_out = *degree_pnt = *degree_qr = 0; 828 for(int i=0;i<ncoeffs;++i){ 829 coeffs[i] = 0; 830 } 831 //coeffs[0] = 0; 832 return; 833 } 834 std::vector<double> us(npts2fit*2); //double *us = new double[npts2fit*2]; 835 std::vector<double> bs(npts2fit); //double *bs = new double[npts2fit]; 836 for(int i=interp;i<nverts;++i){ 837 int k = i-interp; 838 double uu[3]; 839 DGMSolver::vec_linear_operation(3,1,ngbcoords+3*i,-1,ngbcoords,uu); 840 us[k*2] = DGMSolver::vec_innerprod(3,tang1,uu); us[k*2+1] = DGMSolver::vec_innerprod(3,tang2,uu); 841 bs[k] = DGMSolver::vec_innerprod(3,nrm,uu); 842 } 843 844 //step 3. compute weights 845 std::vector<double> ws(npts2fit); //double *ws = new double[npts2fit]; 846 int nzeros = compute_weights(npts2fit,2,&(us[0]),nverts,ngbnrms,degree,_MINEPS,&(ws[0])); 847 848 //step 4. adjust according to zero-weights 849 if(nzeros){ 850 if(nzeros==npts2fit){ 851 *degree_out = *degree_pnt = *degree_qr = 0; 852 for(int i=0;i<ncoeffs;++i){ 853 coeffs[i] = 0; 854 } 855 //coeffs[0] = 0; 856 return; 857 } 858 int index=0; 859 for(int i=0;i<npts2fit;++i){ 860 if(ws[i]){ 861 if(i>index){ 862 us[index*2] = us[i*2]; us[index*2+1] = us[i*2+1]; 863 bs[index] = bs[i]; ws[index] = ws[i]; 864 } 865 ++index; 866 } 867 } 868 npts2fit -= nzeros; assert(index==npts2fit); 869 us.resize(npts2fit*2); bs.resize(npts2fit); ws.resize(npts2fit); 870 /*us = realloc(us,npts2fit*2*sizeof(double)); 871 bs = realloc(bs,npts2fit*sizeof(double)); 872 ws = realloc(ws,npts2fit*sizeof(double));*/ 873 } 874 875 //std::cout<<"npnts after weighting = "<<npts2fit<<std::endl; 876 877 //step 5. fitting 878 eval_vander_bivar_cmf(npts2fit,&(us[0]),1,&(bs[0]),degree,&(ws[0]),interp,safeguard,degree_out,degree_pnt,degree_qr); 879 880 //step 6. organize output 881 int ncoeffs_out = (*degree_out+2)*(*degree_out+1)/2; 882 assert(ncoeffs_out<=ncoeffs); 883 coeffs[0] = 0; 884 for(int j=0;j<ncoeffs_out-interp;++j){ 885 coeffs[j+interp] = bs[j]; 886 } 887 //delete [] us; delete [] bs; delete [] ws; 888 } 889 eval_vander_bivar_cmf(const int npts2fit,const double * us,const int ndim,double * bs,int degree,const double * ws,const bool interp,const bool safeguard,int * degree_out,int * degree_pnt,int * degree_qr)890 void HiReconstruction::eval_vander_bivar_cmf(const int npts2fit, const double* us, const int ndim, double* bs, int degree, const double* ws, const bool interp, const bool safeguard, int* degree_out, int* degree_pnt, int* degree_qr){ 891 //step 1. adjust the degree according to number of points to fit 892 int ncols = (((degree+2)*(degree+1))>>1)-interp; 893 while(1<degree&&npts2fit<ncols){ 894 --degree; 895 ncols = (((degree+2)*(degree+1))>>1)-interp; 896 } 897 *degree_pnt = degree; 898 899 //std::cout << "degree_pnt: " << *degree_pnt << std::endl; 900 901 //step 2. construct Vandermonde matrix, stored in columnwise 902 std::vector<double> V;//V(npts2fit*(ncols+interp)); //double *V_init = new double[npts2fit*(ncols+interp)]; 903 DGMSolver::gen_vander_multivar(npts2fit,2,us,degree,V); 904 //remove the first column of 1s if interpolation 905 if(interp){ 906 V.erase(V.begin(),V.begin()+npts2fit); 907 } 908 /*double* V; 909 if(interp){ 910 V = new double[npts2fit*ncols]; 911 std::memcpy(V,V_init+npts2fit,ncols*npts2fit*sizeof(double)); 912 delete [] V_init; V_init = 0; 913 }else{ 914 V = V_init; 915 }*/ 916 917 //step 3. Scale rows to assign different weights to different points 918 for(int i=0;i<npts2fit;++i){ 919 for(int j=0;j<ncols;++j){ 920 V[j*npts2fit+i] *= ws[i]; 921 } 922 for(int k=0;k<ndim;++k){ 923 bs[k*npts2fit+i] *= ws[i]; 924 } 925 } 926 927 //step 4. scale columns to reduce condition number 928 std::vector<double> ts(ncols); //double *ts = new double[ncols]; 929 DGMSolver::rescale_matrix(npts2fit,ncols,&(V[0]),&(ts[0])); 930 931 //step 5. Perform Householder QR factorization 932 std::vector<double> D(ncols); //double *D = new double[ncols]; 933 int rank; 934 DGMSolver::qr_polyfit_safeguarded(npts2fit,ncols,&(V[0]),&(D[0]),&rank); 935 936 //step 6. adjust degree of fitting according to rank of Vandermonde matrix 937 int ncols_sub = ncols; 938 while(rank<ncols_sub){ 939 --degree; 940 if(degree==0){ 941 //surface is flat, return 0 942 *degree_out = *degree_qr = degree; 943 for(int i=0;i<npts2fit;++i){ 944 for(int k=0;k<ndim;++k){ 945 bs[k*npts2fit+i] = 0; 946 } 947 } 948 return; 949 }else{ 950 ncols_sub = (((degree+2)*(degree+1))>>1)-interp; 951 } 952 } 953 *degree_qr = degree; 954 955 //std::cout << "degree_qr: " << *degree_qr << std::endl; 956 957 /* DBG 958 * std::cout<<"before Qtb"<<std::endl; 959 std::cout<<std::endl; 960 std::cout<<"bs = "<<std::endl; 961 std::cout<<std::endl; 962 for (int k=0; k< ndim; k++){ 963 for (int j=0; j<npts2fit; ++j){ 964 std::cout<<" "<<bs[npts2fit*k+j]<<std::endl; 965 } 966 } 967 std::cout<<std::endl;*/ 968 969 //step 7. compute Q'b 970 DGMSolver::compute_qtransposeB(npts2fit,ncols_sub,&(V[0]),ndim,bs); 971 972 /* DBG 973 * std::cout<<"after Qtb"<<std::endl; 974 std::cout<<"bs = "<<std::endl; 975 std::cout<<std::endl; 976 for (int k=0; k< ndim; k++){ 977 for (int j=0; j<npts2fit; ++j){ 978 std::cout<<" "<<bs[npts2fit*k+j]<<std::endl; 979 } 980 } 981 std::cout<<std::endl;*/ 982 983 //step 8. perform backward substitution and scale the solution 984 //assign diagonals of V 985 for(int i=0;i<ncols_sub;++i){ 986 V[i*npts2fit+i] = D[i]; 987 } 988 989 //backsolve 990 if(safeguard){ 991 //for debug 992 //std::cout << "ts size " << ts.size() << std::endl; 993 DGMSolver::backsolve_polyfit_safeguarded(2,degree,interp,npts2fit,ncols_sub,&(V[0]),ndim,bs,&(ts[0]),degree_out); 994 }else{ 995 DGMSolver::backsolve(npts2fit,ncols_sub,&(V[0]),1,bs,&(ts[0])); 996 *degree_out = degree; 997 } 998 /*if(V_init){ 999 delete [] V_init; 1000 }else{ 1001 delete [] V; 1002 }*/ 1003 } 1004 polyfit3d_curve_get_coeff(const int nverts,const double * ngbcors,const double * ngbtangs,int degree,const bool interp,const bool safeguard,const int ncoords,double * coords,const int ncoeffs,double * coeffs,int * degree_out)1005 void HiReconstruction::polyfit3d_curve_get_coeff(const int nverts, const double* ngbcors, const double* ngbtangs, int degree, const bool interp, const bool safeguard, const int ncoords, double* coords, const int ncoeffs, double* coeffs, int* degree_out){ 1006 if(!nverts){ 1007 return; 1008 } 1009 //step 1. compute local coordinates system 1010 double tang[3] = {ngbtangs[0],ngbtangs[1],ngbtangs[2]}; 1011 if(coords&&ncoords>2){ 1012 coords[0] = tang[0]; coords[1] = tang[1]; coords[2] = tang[2]; 1013 } 1014 if(!coeffs||!ncoeffs){ 1015 return; 1016 }else{ 1017 assert(ncoeffs>=3*(degree+1)); 1018 } 1019 //step 2. project onto local coordinate system 1020 int npts2fit = nverts-interp; 1021 if(!npts2fit){ 1022 *degree_out = 0; 1023 for(int i=0;i<ncoeffs;++i){ 1024 coeffs[0] = 0; 1025 } 1026 return; 1027 } 1028 std::vector<double> us(npts2fit); //double *us = new double[npts2fit]; 1029 std::vector<double> bs(npts2fit*3); //double *bs = new double[npts2fit*3]; 1030 double uu[3]; 1031 for(int i=interp;i<nverts;++i){ 1032 int k=i-interp; 1033 DGMSolver::vec_linear_operation(3,1,ngbcors+3*i,-1,ngbcors,uu); 1034 us[k] = DGMSolver::vec_innerprod(3,uu,tang); 1035 bs[k] = uu[0]; bs[npts2fit+k] = uu[1]; bs[2*npts2fit+k] = uu[2]; 1036 } 1037 1038 //step 3. copmute weights 1039 std::vector<double> ws(npts2fit); 1040 int nzeros = compute_weights(npts2fit,1,&(us[0]),nverts,ngbtangs,degree,_MINEPS,&(ws[0])); assert(nzeros<=npts2fit); 1041 1042 //step 4. adjust according to number of zero-weights 1043 if(nzeros){ 1044 if(nzeros==npts2fit){ 1045 //singular case 1046 *degree_out = 0; 1047 for(int i=0;i<ncoeffs;++i){ 1048 coeffs[i] =0; 1049 } 1050 return; 1051 } 1052 int npts_new = npts2fit-nzeros; 1053 std::vector<double> bs_temp(3*npts_new); 1054 int index=0; 1055 for(int i=0;i<npts2fit;++i){ 1056 if(ws[i]){ 1057 if(i>index){ 1058 us[index] = us[i]; ws[index] = ws[i]; 1059 } 1060 bs_temp[index] = bs[i]; bs_temp[index+npts_new] = bs[i+npts2fit]; bs_temp[index+2*npts_new] = bs[i+2*npts2fit]; 1061 ++index; 1062 } 1063 } 1064 assert(index==npts_new); 1065 npts2fit = npts_new; 1066 us.resize(index); ws.resize(index); bs = bs_temp; 1067 //destroy bs_temp; 1068 std::vector<double>().swap(bs_temp); 1069 } 1070 1071 //step 5. fitting 1072 eval_vander_univar_cmf(npts2fit,&(us[0]),3,&(bs[0]),degree,&(ws[0]),interp,safeguard,degree_out); 1073 //step 6. write results to output pointers 1074 int ncoeffs_out_pvpd = *degree_out+1; 1075 assert(ncoeffs>=3*ncoeffs_out_pvpd); 1076 //write to coeffs consecutively, bs's size is not changed by eval_vander_univar_cmf 1077 coeffs[0] = coeffs[ncoeffs_out_pvpd] = coeffs[2*ncoeffs_out_pvpd] = 0; 1078 for(int i=0;i<ncoeffs_out_pvpd-interp;++i){ 1079 coeffs[i+interp] = bs[i]; 1080 coeffs[i+interp+ncoeffs_out_pvpd] = bs[i+npts2fit]; 1081 coeffs[i+interp+2*ncoeffs_out_pvpd] = bs[i+2*npts2fit]; 1082 } 1083 } 1084 eval_vander_univar_cmf(const int npts2fit,const double * us,const int ndim,double * bs,int degree,const double * ws,const bool interp,const bool safeguard,int * degree_out)1085 void HiReconstruction::eval_vander_univar_cmf(const int npts2fit, const double* us, const int ndim, double* bs, int degree, const double* ws, const bool interp, const bool safeguard, int* degree_out){ 1086 //step 1. determine degree of polynomials to fit according to number of points 1087 int ncols = degree+1-interp; 1088 while(npts2fit<ncols&°ree>=1){ 1089 --degree; 1090 ncols = degree+1-interp; 1091 } 1092 if(!degree){ 1093 if(interp){ 1094 for(int icol=0;icol<ndim;++icol){ 1095 bs[icol*npts2fit] = 0; 1096 } 1097 } 1098 for(int irow=1;irow<npts2fit;++irow){ 1099 for(int icol=0;icol<ndim;++icol){ 1100 bs[icol*npts2fit+irow] = 0; 1101 } 1102 } 1103 *degree_out = 0; 1104 return; 1105 } 1106 //step 2. construct Vandermonde matrix 1107 std::vector<double> V;//V(npts2fit*(ncols+interp)); 1108 DGMSolver::gen_vander_multivar(npts2fit,1,us,degree,V); 1109 1110 if(interp){ 1111 V.erase(V.begin(),V.begin()+npts2fit); 1112 } 1113 1114 //step 3. scale rows with respect to weights 1115 for(int i=0;i<npts2fit;++i){ 1116 for(int j=0;j<ncols;++j){ 1117 V[j*npts2fit+i] *= ws[i]; 1118 } 1119 for(int k=0;k<ndim;++k){ 1120 bs[k*npts2fit+i] *= ws[i]; 1121 } 1122 } 1123 1124 //step 4. scale columns to reduce condition number 1125 std::vector<double> ts(ncols); 1126 DGMSolver::rescale_matrix(npts2fit,ncols,&(V[0]),&(ts[0])); 1127 1128 //step 5. perform Householder QR factorization 1129 std::vector<double> D(ncols); 1130 int rank; 1131 DGMSolver::qr_polyfit_safeguarded(npts2fit,ncols,&(V[0]),&(D[0]),&rank); 1132 1133 //step 6. adjust degree of fitting 1134 int ncols_sub = ncols; 1135 while(rank<ncols_sub){ 1136 --degree; 1137 if(!degree){ 1138 //matrix is singular, consider curve on flat plane passing center and orthogonal to tangent line 1139 *degree_out = 0; 1140 for(int i=0;i<npts2fit;++i){ 1141 for(int k=0;k<ndim;++k){ 1142 bs[k*npts2fit+i] = 0; 1143 } 1144 } 1145 } 1146 ncols_sub = degree+1-interp; 1147 } 1148 1149 //step 7. compute Q'*bs 1150 DGMSolver::compute_qtransposeB(npts2fit,ncols_sub,&(V[0]),ndim,bs); 1151 1152 //step 8. perform backward substitution and scale solutions 1153 //assign diagonals of V 1154 for(int i=0;i<ncols_sub;++i){ 1155 V[i*npts2fit+i] = D[i]; 1156 } 1157 //backsolve 1158 if(safeguard){ 1159 DGMSolver::backsolve_polyfit_safeguarded(1,degree,interp,npts2fit,ncols,&(V[0]),ndim,bs,ws,degree_out); 1160 }else{ 1161 DGMSolver::backsolve(npts2fit,ncols_sub,&(V[0]),ndim,bs,&(ts[0])); 1162 *degree_out = degree; 1163 } 1164 } 1165 compute_weights(const int nrows,const int ncols,const double * us,const int nngbs,const double * ngbnrms,const int degree,const double toler,double * ws)1166 int HiReconstruction::compute_weights(const int nrows, const int ncols, const double* us, const int nngbs, const double* ngbnrms, const int degree, const double toler, double* ws){ 1167 assert(nrows<=_MAXPNTS&&ws); 1168 bool interp=false; 1169 if(nngbs!=nrows){ 1170 assert(nngbs==nrows+1); 1171 interp = true; 1172 } 1173 double epsilon = 1e-2; 1174 1175 //First, compute squared distance from each input piont to the center 1176 for(int i=0;i<nrows;++i){ 1177 ws[i] = DGMSolver::vec_innerprod(ncols,us+i*ncols,us+i*ncols); 1178 } 1179 1180 //Second, compute a small correction termt o guard against zero 1181 double h=0; 1182 for(int i=0;i<nrows;++i){ 1183 h += ws[i]; 1184 } 1185 h /= (double) nrows; 1186 1187 //Finally, compute the weights for each vertex 1188 int nzeros = 0; 1189 for(int i=0;i<nrows;++i){ 1190 double costheta = DGMSolver::vec_innerprod(3,ngbnrms,ngbnrms+3*(i+interp)); 1191 if(costheta>toler){ 1192 ws[i] = costheta*pow(ws[i]/h+epsilon,-1*(double) degree/2.0); 1193 }else{ 1194 ws[i] = 0; 1195 ++nzeros; 1196 } 1197 } 1198 return nzeros; 1199 } check_barycentric_coords(const int nws,const double * naturalcoords)1200 bool HiReconstruction::check_barycentric_coords(const int nws, const double* naturalcoords){ 1201 double sum=0; 1202 for(int i=0;i<nws;++i){ 1203 if(naturalcoords[i]<-_MINEPS){ 1204 return false; 1205 } 1206 sum += naturalcoords[i]; 1207 } 1208 if(fabs(1-sum)>_MINEPS){ 1209 return false; 1210 }else{ 1211 return true; 1212 } 1213 } 1214 }//namespace moab 1215