 |
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/DGMSolver.hpp"
2 #include "moab/ErrorHandler.hpp"
3 #include <iostream>
4 #include <cassert>
5 #include <vector>
6 #include <limits>
7 #include <cmath>
8 #include <algorithm>
9
10 namespace moab
11 {
12
13 /* This class implements the lowest level solvers required by polynomial fitting for high-order
14 * reconstruction. An underlying assumption of the matrices passed are that they are given in a
15 * column-major form. So, the first mrows values of V is the first column, and so on. This
16 * assumption is made because most of the operations are column-based in the current scenario.
17 * */
18
19 unsigned int DGMSolver::nchoosek( unsigned int n, unsigned int k )
20 {
21 if( k > n )
22 {
23 return 0;
24 }
25 unsigned long long ans = 1;
26 if( k > ( n >> 1 ) )
27 {
28 k = n - k;
29 }
30 for( unsigned int i = 1; i <= k; ++i )
31 {
32 ans *= n--;
33 ans /= i;
34 if( ans > std::numeric_limits< unsigned int >::max() )
35 {
36 return 0;
37 }
38 }
39 return ans;
40 }
41
42 unsigned int DGMSolver::compute_numcols_vander_multivar( unsigned int kvars, unsigned int degree )
43 {
44 unsigned int mcols = 0;
45 for( unsigned int i = 0; i <= degree; ++i )
46 {
47 unsigned int temp = nchoosek( kvars - 1 + i, kvars - 1 );
48 if( !temp )
49 {
50 std::cout << "overflow to compute nchoosek n= " << kvars - 1 + i << " k= " << kvars - 1 << std::endl;
51 return 0;
52 }
53 mcols += temp;
54 }
55 return mcols;
56 }
57
58 void DGMSolver::gen_multivar_monomial_basis( const int kvars,
59 const double* vars,
60 const int degree,
61 std::vector< double >& basis )
62 {
63 unsigned int len = compute_numcols_vander_multivar( kvars, degree );
64 basis.reserve( len - basis.capacity() + basis.size() );
65 size_t iend = basis.size();
66 #ifndef NDEBUG
67 size_t istr = basis.size();
68 #endif
69 basis.push_back( 1 );
70 ++iend;
71 if( !degree )
72 {
73 return;
74 }
75 std::vector< size_t > varspos( kvars );
76 // degree 1
77 for( int ivar = 0; ivar < kvars; ++ivar )
78 {
79 basis.push_back( vars[ivar] );
80 varspos[ivar] = iend++;
81 }
82 // degree 2 to degree
83 for( int ideg = 2; ideg <= degree; ++ideg )
84 {
85 size_t preend = iend;
86 for( int ivar = 0; ivar < kvars; ++ivar )
87 {
88 size_t varpreend = iend;
89 for( size_t ilast = varspos[ivar]; ilast < preend; ++ilast )
90 {
91 basis.push_back( vars[ivar] * basis[ilast] );
92 ++iend;
93 }
94 varspos[ivar] = varpreend;
95 }
96 }
97 assert( len == iend - istr );
98 }
99
100 void DGMSolver::gen_vander_multivar( const int mrows,
101 const int kvars,
102 const double* us,
103 const int degree,
104 std::vector< double >& V )
105 {
106 unsigned int ncols = compute_numcols_vander_multivar( kvars, degree );
107 V.reserve( mrows * ncols - V.capacity() + V.size() );
108 size_t istr = V.size(), icol = 0;
109 // add ones, V is stored in an single array, elements placed in columnwise order
110 for( int irow = 0; irow < mrows; ++irow )
111 {
112 V.push_back( 1 );
113 }
114 ++icol;
115 if( !degree )
116 {
117 return;
118 }
119 std::vector< size_t > varspos( kvars );
120 // degree 1
121 for( int ivar = 0; ivar < kvars; ++ivar )
122 {
123 for( int irow = 0; irow < mrows; ++irow )
124 {
125 V.push_back( us[irow * kvars + ivar] ); // us stored in row-wise
126 }
127 varspos[ivar] = icol++;
128 }
129 // from 2 to degree
130 for( int ideg = 2; ideg <= degree; ++ideg )
131 {
132 size_t preendcol = icol;
133 for( int ivar = 0; ivar < kvars; ++ivar )
134 {
135 size_t varpreend = icol;
136 for( size_t ilast = varspos[ivar]; ilast < preendcol; ++ilast )
137 {
138 for( int irow = 0; irow < mrows; ++irow )
139 {
140 V.push_back( us[irow * kvars + ivar] * V[istr + irow + ilast * mrows] );
141 }
142 ++icol;
143 }
144 varspos[ivar] = varpreend;
145 }
146 }
147 assert( icol == ncols );
148 }
149
150 void DGMSolver::rescale_matrix( int mrows, int ncols, double* V, double* ts )
151 {
152 // This function rescales the input matrix using the norm of each column.
153 double* v = new double[mrows];
154 for( int i = 0; i < ncols; i++ )
155 {
156 for( int j = 0; j < mrows; j++ )
157 v[j] = V[mrows * i + j];
158
159 // Compute norm of the column vector
160 double w = vec_2norm( mrows, v );
161
162 if( fabs( w ) == 0 )
163 ts[i] = 1;
164 else
165 {
166 ts[i] = w;
167 for( int j = 0; j < mrows; j++ )
168 V[mrows * i + j] = V[mrows * i + j] / ts[i];
169 }
170 }
171 delete[] v;
172 }
173
174 void DGMSolver::compute_qtransposeB( int mrows, int ncols, const double* Q, int bncols, double* bs )
175 {
176 for( int k = 0; k < ncols; k++ )
177 {
178 for( int j = 0; j < bncols; j++ )
179 {
180 double t2 = 0;
181 for( int i = k; i < mrows; i++ )
182 t2 += Q[mrows * k + i] * bs[mrows * j + i];
183 t2 = t2 + t2;
184
185 for( int i = k; i < mrows; i++ )
186 bs[mrows * j + i] -= t2 * Q[mrows * k + i];
187 }
188 }
189 }
190
191 void DGMSolver::qr_polyfit_safeguarded( const int mrows, const int ncols, double* V, double* D, int* rank )
192 {
193 double tol = 1e-8;
194 *rank = ncols;
195 double* v = new double[mrows];
196
197 for( int k = 0; k < ncols; k++ )
198 {
199 int nv = mrows - k;
200
201 for( int j = 0; j < nv; j++ )
202 v[j] = V[mrows * k + ( j + k )];
203
204 double t2 = 0;
205
206 for( int j = 0; j < nv; j++ )
207 t2 = t2 + v[j] * v[j];
208
209 double t = sqrt( t2 );
210 double vnrm = 0;
211
212 if( v[0] >= 0 )
213 {
214 vnrm = sqrt( 2 * ( t2 + v[0] * t ) );
215 v[0] = v[0] + t;
216 }
217 else
218 {
219 vnrm = sqrt( 2 * ( t2 - v[0] * t ) );
220 v[0] = v[0] - t;
221 }
222
223 if( vnrm > 0 )
224 {
225 for( int j = 0; j < nv; j++ )
226 v[j] = v[j] / vnrm;
227 }
228
229 for( int j = k; j < ncols; j++ )
230 {
231 t2 = 0;
232 for( int i = 0; i < nv; i++ )
233 t2 = t2 + v[i] * V[mrows * j + ( i + k )];
234
235 t2 = t2 + t2;
236
237 for( int i = 0; i < nv; i++ )
238 V[mrows * j + ( i + k )] = V[mrows * j + ( i + k )] - t2 * v[i];
239 }
240
241 D[k] = V[mrows * k + k];
242
243 for( int i = 0; i < nv; i++ )
244 V[mrows * k + ( i + k )] = v[i];
245
246 if( ( fabs( D[k] ) ) < tol && ( *rank == ncols ) )
247 {
248 *rank = k;
249 break;
250 }
251 }
252
253 delete[] v;
254 }
255
256 void DGMSolver::backsolve( int mrows, int ncols, double* R, int bncols, double* bs, double* ws )
257 {
258 #if 0
259 std::cout.precision(16);
260 std::cout<<"Before backsolve "<<std::endl;
261 std::cout<<" V = "<<std::endl;
262 for (int k=0; k< ncols; k++){
263 for (int j=0; j<mrows; ++j){
264 std::cout<<R[mrows*k+j]<<std::endl;
265 }
266 std::cout<<std::endl;
267 }
268 std::cout<<std::endl;
269
270 //std::cout<<"#pnts = "<<mrows<<std::endl;
271 std::cout<<"bs = "<<std::endl;
272 std::cout<<std::endl;
273 for (int k=0; k< bncols; k++){
274 for (int j=0; j<mrows; ++j){
275 std::cout<<" "<<bs[mrows*k+j]<<std::endl;
276 }
277 }
278 std::cout<<std::endl;
279 #endif
280
281 for( int k = 0; k < bncols; k++ )
282 {
283 for( int j = ncols - 1; j >= 0; j-- )
284 {
285 for( int i = j + 1; i < ncols; ++i )
286 bs[mrows * k + j] = bs[mrows * k + j] - R[mrows * i + j] * bs[mrows * k + i];
287
288 assert( R[mrows * j + j] != 0 );
289 bs[mrows * k + j] = bs[mrows * k + j] / R[mrows * j + j];
290 }
291 }
292
293 for( int k = 0; k < bncols; k++ )
294 {
295 for( int j = 0; j < ncols; ++j )
296 bs[mrows * k + j] = bs[mrows * k + j] / ws[j];
297 }
298 }
299
300 void DGMSolver::backsolve_polyfit_safeguarded( int dim,
301 int degree,
302 const bool interp,
303 int mrows,
304 int ncols,
305 double* R,
306 int bncols,
307 double* bs,
308 const double* ws,
309 int* degree_out )
310 {
311 if( ncols < 1 ) std::cout << "ERROR: Invalid input to safeguarded polyfit backsolve routine.\n";
312 #if 0
313 std::cout.precision(12);
314 std::cout<<"Before backsolve "<<std::endl;
315 std::cout<<" V = "<<std::endl;
316 for (int k=0; k< ncols; k++){
317 for (int j=0; j<mrows; ++j){
318 std::cout<<R[mrows*k+j]<<std::endl;
319 }
320 std::cout<<std::endl;
321 }
322 std::cout<<std::endl;
323
324 //std::cout<<"#pnts = "<<mrows<<std::endl;
325 std::cout<<"bs = "<<std::endl;
326 std::cout<<std::endl;
327 for (int k=0; k< bncols; k++){
328 for (int j=0; j<mrows; ++j){
329 std::cout<<" "<<bs[mrows*k+j]<<std::endl;
330 }
331 }
332 std::cout<<std::endl;
333
334 //std::cout<<" ] "<<std::endl;
335
336 std::cout<<"Input ws = [ ";
337 for (int k=0; k< ncols; k++){
338 std::cout<<ws[k]<<", ";
339 }
340 std::cout<<" ] "<<std::endl;
341
342 std::cout << "R: " << R << "size: [" << mrows << "," << ncols << "]" << std::endl;
343 std::cout << "bs: " << bs << "size: [" << mrows << "," << bncols << "]" << std::endl;
344 std::cout << "ws: " << ws << "size: [" << ncols << "," << 1 << "]" << std::endl;
345 std::cout << "degree_out: " << degree_out << std::endl;
346 #endif
347
348 int deg, numcols = 0;
349
350 for( int k = 0; k < bncols; k++ )
351 {
352 deg = degree;
353 /* I think we should consider interp = true/false -Xinglin*/
354 if( dim == 1 )
355 numcols = deg + 1 - interp;
356 else if( dim == 2 )
357 numcols = ( deg + 2 ) * ( deg + 1 ) / 2 - interp;
358
359 assert( numcols <= ncols );
360
361 std::vector< double > bs_bak( numcols );
362
363 if( deg >= 2 )
364 {
365 for( int i = 0; i < numcols; i++ )
366 {
367 assert( mrows * k + i < mrows * bncols );
368 bs_bak.at( i ) = bs[mrows * k + i];
369 }
370 }
371
372 while( deg >= 1 )
373 {
374 int cend = numcols - 1;
375 bool downgrade = false;
376
377 // The reconstruction can be applied only on edges (2-d) or faces (3-d)
378 assert( cend >= 0 );
379 assert( dim > 0 && dim < 3 );
380
381 for( int d = deg; d >= 0; d-- )
382 {
383 int cstart = 0;
384 if( dim == 1 )
385 {
386 cstart = d;
387 }
388 else if( dim == 2 )
389 {
390 cstart = ( ( d + 1 ) * d ) / 2;
391 // cstart = ((d*(d+1))>>1)-interp;
392 }
393
394 // Solve for bs
395 for( int j = cend; j >= cstart; j-- )
396 {
397 assert( mrows * k + j < mrows * bncols );
398 for( int i = j + 1; i < numcols; ++i )
399 {
400 assert( mrows * k + i < mrows * bncols );
401 assert( mrows * i + j < mrows * ncols ); // check R
402 bs[mrows * k + j] = bs[mrows * k + j] - R[mrows * i + j] * bs[mrows * k + i];
403 }
404 assert( mrows * j + j < mrows * ncols ); // check R
405 bs[mrows * k + j] = bs[mrows * k + j] / R[mrows * j + j];
406 }
407
408 // Checking for change in the coefficient
409 if( d >= 2 && d < deg )
410 {
411 double tol;
412
413 if( dim == 1 )
414 {
415 tol = 1e-06;
416 assert( mrows * cstart + cstart < mrows * ncols ); // check R
417 double tb = bs_bak.at( cstart ) / R[mrows * cstart + cstart];
418 assert( mrows * k + cstart < mrows * bncols );
419 if( fabs( bs[mrows * k + cstart] - tb ) > ( 1 + tol ) * fabs( tb ) )
420 {
421 downgrade = true;
422 break;
423 }
424 }
425
426 else if( dim == 2 )
427 {
428 tol = 0.05;
429
430 std::vector< double > tb( cend - cstart + 1 );
431 for( int j = 0; j <= ( cend - cstart ); j++ )
432 {
433 tb.at( j ) = bs_bak.at( cstart + j );
434 }
435
436 for( int j = cend; j >= cstart; j-- )
437 {
438 int jind = j - cstart;
439
440 for( int i = j + 1; i <= cend; ++i )
441 {
442 assert( mrows * i + j < mrows * ncols ); // check R
443 tb.at( jind ) = tb.at( jind ) - R[mrows * i + j] * tb.at( i - cstart );
444 }
445 assert( mrows * j + j < mrows * ncols ); // check R
446 tb.at( jind ) = tb.at( jind ) / R[mrows * j + j];
447 assert( mrows * k + j < mrows * bncols );
448 double err = fabs( bs[mrows * k + j] - tb.at( jind ) );
449
450 if( ( err > tol ) && ( err >= ( 1 + tol ) * fabs( tb.at( jind ) ) ) )
451 {
452 downgrade = true;
453 break;
454 }
455 }
456
457 if( downgrade ) break;
458 }
459 }
460
461 cend = cstart - 1;
462 }
463
464 if( !downgrade )
465 break;
466 else
467 {
468 deg = deg - 1;
469 if( dim == 1 )
470 numcols = deg + 1;
471 else if( dim == 2 )
472 numcols = ( deg + 2 ) * ( deg + 1 ) / 2;
473
474 for( int i = 0; i < numcols; i++ )
475 {
476 assert( mrows * k + i < mrows * bncols );
477 bs[mrows * k + i] = bs_bak.at( i );
478 }
479 }
480 }
481 assert( k < bncols );
482 degree_out[k] = deg;
483
484 for( int i = 0; i < numcols; i++ )
485 {
486 // assert(mrows*k+i < mrows*bncols);
487 // assert(i < ncols);
488 bs[mrows * k + i] = bs[mrows * k + i] / ws[i];
489 }
490
491 for( int i = numcols; i < mrows; i++ )
492 {
493 // assert(mrows*k+i < mrows*bncols);
494 bs[mrows * k + i] = 0;
495 }
496 }
497 }
498
499 void DGMSolver::vec_dotprod( const int len, const double* a, const double* b, double* c )
500 {
501 if( !a || !b || !c )
502 {
503 MB_SET_ERR_RET( "NULL Pointer" );
504 }
505 for( int i = 0; i < len; ++i )
506 {
507 c[i] = a[i] * b[i];
508 }
509 }
510
511 void DGMSolver::vec_scalarprod( const int len, const double* a, const double c, double* b )
512 {
513 if( !a || !b )
514 {
515 MB_SET_ERR_RET( "NULL Pointer" );
516 }
517 for( int i = 0; i < len; ++i )
518 {
519 b[i] = c * a[i];
520 }
521 }
522
523 void DGMSolver::vec_crossprod( const double a[3], const double b[3], double ( &c )[3] )
524 {
525 c[0] = a[1] * b[2] - a[2] * b[1];
526 c[1] = a[2] * b[0] - a[0] * b[2];
527 c[2] = a[0] * b[1] - a[1] * b[0];
528 }
529
530 double DGMSolver::vec_innerprod( const int len, const double* a, const double* b )
531 {
532 if( !a || !b )
533 {
534 MB_SET_ERR_RET_VAL( "NULL Pointer", 0.0 );
535 }
536 double ans = 0;
537 for( int i = 0; i < len; ++i )
538 {
539 ans += a[i] * b[i];
540 }
541 return ans;
542 }
543
544 double DGMSolver::vec_2norm( const int len, const double* a )
545 {
546 if( !a )
547 {
548 MB_SET_ERR_RET_VAL( "NULL Pointer", 0.0 );
549 }
550 double w = 0, s = 0;
551 for( int k = 0; k < len; k++ )
552 w = std::max( w, fabs( a[k] ) );
553
554 if( w == 0 )
555 {
556 return 0;
557 }
558 else
559 {
560 for( int k = 0; k < len; k++ )
561 {
562 s += ( a[k] / w ) * ( a[k] / w );
563 }
564 s = w * sqrt( s );
565 }
566 return s;
567 }
568
569 double DGMSolver::vec_normalize( const int len, const double* a, double* b )
570 {
571 if( !a || !b )
572 {
573 MB_SET_ERR_RET_VAL( "NULL Pointer", 0.0 );
574 }
575 double nrm = 0, mx = 0;
576 for( int i = 0; i < len; ++i )
577 {
578 mx = std::max( fabs( a[i] ), mx );
579 }
580 if( mx == 0 )
581 {
582 for( int i = 0; i < len; ++i )
583 {
584 b[i] = 0;
585 }
586 return 0;
587 }
588 for( int i = 0; i < len; ++i )
589 {
590 nrm += ( a[i] / mx ) * ( a[i] / mx );
591 }
592 nrm = mx * sqrt( nrm );
593 if( nrm == 0 )
594 {
595 return nrm;
596 }
597 for( int i = 0; i < len; ++i )
598 {
599 b[i] = a[i] / nrm;
600 }
601 return nrm;
602 }
603
604 double DGMSolver::vec_distance( const int len, const double* a, const double* b )
605 {
606 double res = 0;
607 for( int i = 0; i < len; ++i )
608 {
609 res += ( a[i] - b[i] ) * ( a[i] - b[i] );
610 }
611 return sqrt( res );
612 }
613
614 void DGMSolver::vec_projoff( const int len, const double* a, const double* b, double* c )
615 {
616 if( !a || !b || !c )
617 {
618 MB_SET_ERR_RET( "NULL Pointer" );
619 }
620 // c = a-<a,b>b/<b,b>;
621 double bnrm = vec_2norm( len, b );
622 if( bnrm == 0 )
623 {
624 for( int i = 0; i < len; ++i )
625 {
626 c[i] = a[i];
627 }
628 return;
629 }
630 double innerp = vec_innerprod( len, a, b ) / bnrm;
631
632 if( innerp == 0 )
633 {
634 if( c != a )
635 {
636 for( int i = 0; i < len; ++i )
637 {
638 c[i] = a[i];
639 }
640 }
641 return;
642 }
643
644 for( int i = 0; i < len; ++i )
645 {
646 c[i] = a[i] - innerp * b[i] / bnrm;
647 }
648 }
649
650 void DGMSolver::vec_linear_operation( const int len,
651 const double mu,
652 const double* a,
653 const double psi,
654 const double* b,
655 double* c )
656 {
657 if( !a || !b || !c )
658 {
659 MB_SET_ERR_RET( "NULL Pointer" );
660 }
661 for( int i = 0; i < len; ++i )
662 {
663 c[i] = mu * a[i] + psi * b[i];
664 }
665 }
666
667 void DGMSolver::get_tri_natural_coords( const int dim,
668 const double* cornercoords,
669 const int npts,
670 const double* currcoords,
671 double* naturalcoords )
672 {
673 assert( dim == 2 || dim == 3 );
674 double a = 0, b = 0, d = 0, tol = 1e-12;
675 for( int i = 0; i < dim; ++i )
676 {
677 a += ( cornercoords[dim + i] - cornercoords[i] ) * ( cornercoords[dim + i] - cornercoords[i] );
678 b += ( cornercoords[dim + i] - cornercoords[i] ) * ( cornercoords[2 * dim + i] - cornercoords[i] );
679 d += ( cornercoords[2 * dim + i] - cornercoords[i] ) * ( cornercoords[2 * dim + i] - cornercoords[i] );
680 }
681 double det = a * d - b * b;
682 assert( det > 0 );
683 for( int ipt = 0; ipt < npts; ++ipt )
684 {
685 double e = 0, f = 0;
686 for( int i = 0; i < dim; ++i )
687 {
688 e += ( cornercoords[dim + i] - cornercoords[i] ) * ( currcoords[ipt * dim + i] - cornercoords[i] );
689 f += ( cornercoords[2 * dim + i] - cornercoords[i] ) * ( currcoords[ipt * dim + i] - cornercoords[i] );
690 }
691 naturalcoords[ipt * 3 + 1] = ( e * d - b * f ) / det;
692 naturalcoords[ipt * 3 + 2] = ( a * f - b * e ) / det;
693 naturalcoords[ipt * 3] = 1 - naturalcoords[ipt * 3 + 1] - naturalcoords[ipt * 3 + 2];
694 if( naturalcoords[ipt * 3] < -tol || naturalcoords[ipt * 3 + 1] < -tol || naturalcoords[ipt * 3 + 2] < -tol )
695 {
696 std::cout << "Corners: \n";
697 std::cout << cornercoords[0] << "\t" << cornercoords[1] << "\t" << cornercoords[3] << std::endl;
698 std::cout << cornercoords[3] << "\t" << cornercoords[4] << "\t" << cornercoords[5] << std::endl;
699 std::cout << cornercoords[6] << "\t" << cornercoords[7] << "\t" << cornercoords[8] << std::endl;
700 std::cout << "Candidate: \n";
701 std::cout << currcoords[ipt * dim] << "\t" << currcoords[ipt * dim + 1] << "\t" << currcoords[ipt * dim + 2]
702 << std::endl;
703 exit( 0 );
704 }
705 assert( fabs( naturalcoords[ipt * 3] + naturalcoords[ipt * 3 + 1] + naturalcoords[ipt * 3 + 2] - 1 ) < tol );
706 for( int i = 0; i < dim; ++i )
707 {
708 assert( fabs( naturalcoords[ipt * 3] * cornercoords[i] +
709 naturalcoords[ipt * 3 + 1] * cornercoords[dim + i] +
710 naturalcoords[ipt * 3 + 2] * cornercoords[2 * dim + i] - currcoords[ipt * dim + i] ) < tol );
711 }
712 }
713 }
714 } // namespace moab
1.9.1.