Mesh Oriented datABase  (version 5.5.1)
An array-based unstructured mesh library
DGMSolver.cpp
Go to the documentation of this file.
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 
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 
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 
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 
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