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